CN111896008A - Improved robust unscented Kalman filtering integrated navigation method - Google Patents

Abstract

An improved robust unscented Kalman filtering integrated navigation method relates to the technical field of integrated navigation, and aims to solve the problems that in the prior art, the filtering precision is low, the positioning precision is poor, and the processing capacity of the unscented Kalman filtering integrated navigation method on outliers is poor. Aiming at measuring mutation noise and outliers, the invention introduces a measurement error criterion on the basis of the traditional robust algorithm, thereby enhancing the processing capability of outliers. The invention provides a calculation method of a multiple robust adaptive matrix aiming at the problem that the conventional robust algorithm has poor regulation capability on partial observation interfered and the actual situation of UWB ranging errors is combined, and the algorithm regulates a filter gain matrix in real time aiming at the situation of different errors measured, so that a filter has stronger adaptability.

Description

Improved robust unscented Kalman filtering integrated navigation method

Technical Field

The invention relates to the technical field of integrated navigation, in particular to an improved robust unscented Kalman filtering integrated navigation method.

Background

In recent years, with the development of technology and the change of people's lives, Location-based services (LBS) have been widely used in industry and in markets. Nowadays, Global Navigation Satellite Systems (GNSS) are rapidly developed, which can provide a user with a high-precision location service in an outdoor environment. However, in an indoor environment, GNSS signals are extremely vulnerable to occlusion, so that a user cannot realize normal navigation positioning through GNSS. Meanwhile, most of human activities occur in indoor environments, and high-precision indoor positioning services can provide network services such as route navigation, commodity shopping guide, resource guide and the like for users, so that the high-precision indoor positioning technology has a wide application prospect.

As one of the core contents of LBS, the design of an indoor positioning system and the like become hot spots in this field. Nowadays, wireless positioning is commonly used in indoor positioning technology, and a wireless positioning system has a defect that the positioning effect is easily influenced by environments such as non-line of sight (NLOS). The widely applied indoor positioning technology mainly includes Wireless Local Area Networks (WLAN), Radio Frequency Identification (RFID), Ultra-wideband (UWB), bluetooth, etc., where UWB has the advantages of Ultra-large bandwidth, good anti-multipath interference capability, and is more suitable for providing indoor high-precision positioning service. However, UWB positioning accuracy is susceptible to non-line-of-sight environments. Therefore, many scholars propose to combine two or more sensors to compensate for the disadvantages of a single sensor navigation system. With the rise and development of Micro Electro Mechanical System (MEMS) technology, the volume and cost of conventional inertial navigation devices are effectively reduced. The combined navigation system based on the UWB/MEMS can make full use of respective advantages, improve the positioning accuracy and stability of the system and has wide development prospect.

In the field of indoor integrated navigation, UWB/MEMS integrated navigation research is roughly divided into two categories: one is to improve indoor positioning accuracy and solve the problem of positioning divergence when the UWB fails by researching a UWB/MEMS combined mode. Although these multi-sensor combinations improve the positioning accuracy of the combination to some extent, the introduction of other sensors makes it more difficult to implement in engineering. The other is to solve the noise and outlier problem of UWB from a filtering perspective. Although some learners propose to solve the problem of UWB positioning noise by adopting strong tracking Sage adaptive filtering from the filtering perspective, if the fading factor in strong tracking is not properly selected, the filtering precision is reduced, and the processing capability of the strong tracking on the outlier is poor.

Disclosure of Invention

The purpose of the invention is: aiming at the problems that in the prior art, the filtering precision is low, the positioning precision is poor, and the processing capacity of the filtering precision on the outlier is poor, an improved robust unscented Kalman filtering integrated navigation method is provided.

The technical scheme adopted by the invention to solve the technical problems is as follows:

an improved robust unscented Kalman filter integrated navigation method comprises the following steps:

the method comprises the following steps: collecting data output by the UWB system and the MEMS system;

step two: selecting state quantity and observed quantity of a system, and establishing a state space model of the UWB/MEMS combined navigation system;

step three: carrying out filtering initialization on the UWB/MEMS combined navigation system;

step four: time updating and measurement updating are carried out on the UWB/MEMS combined navigation system at the moment k, and error covariance of one-step prediction and cross covariance of one-step prediction of the system state are obtained;

step five: introducing a measurement error criterion, if the measurement has an error, calculating a robust matrix of multiple factors, correcting the error covariance predicted by one step of state, and then entering the step six, and if the measurement has no error, directly entering the step six;

step six: if the measurement has errors, the corrected state one-step prediction error covariance is used for calculating the state estimation value and the state error covariance of the system at the moment k, and if the measurement has no errors, the state one-step prediction error covariance in the step four is used for calculating the state estimation value and the state error covariance of the system at the moment k;

step seven: and acquiring constant drift of the gyroscope and the accelerometer of the MEMS inertial navigation according to the state estimation value obtained in the sixth step, feeding the constant drift back to the combined system, and outputting attitude, speed and position information.

Further, the system state equation of the UWB/MEMS integrated navigation system state space model is expressed as:

X_k+1＝F_kX_k+w_k

wherein, X_kError of inertial navigation solution information in 15 dimensions, expressed as

Wherein phi is_k＝[φ_k,xφ_k,yφ_k,z]^TAttitude error, Δ V, resolved for moment k inertial navigation_k＝[ΔV_k,xΔV_k,yΔV_k,z]^TVelocity error, Δ P, in a geographic coordinate system resolved for moment k inertial navigation_k＝[ΔL_kΔλ_kΔh_k]^TFor the longitude and latitude height error of the inertial navigation solution at the moment k,_k＝[_{k,x k,y k,z}]^Tfor the moment k the gyro is constantly drifting,

zero offset, w, of the accelerometer at time k_kCombining the systematic process noise for time k, w_kNoise covariance matrix of Q_k，F_kAnd obtaining a state transition matrix of the inertial navigation system at the moment k.

Further, the system measurement equation of the UWB/MEMS integrated navigation system state space model is expressed as:

Z_k＝h(X'_k)+V_k

wherein Z is_kFor system measurement input, h (X'_k) As a function of the system non-linearity, V_kIs a combination systemMeasuring noise, V, collectively_kThe noise covariance matrix is R_k，h(X'_k) And V_kRespectively expressed as:

V_k＝-2ρ_Ui,k+²

wherein, X'_kIncluding the position error Deltax of inertial navigation in the geocentric geostationary coordinate system_k、Δy_kWherein Δ x_k、Δy_kAnd the state quantity X_kPosition error delta L of medium inertial navigation in geographic coordinate system_k、Δλ_k、Δh_kThe relationship of (c) is expressed as:

Δx_k＝-(R_n+h_k)cosλ_ksinL_kΔL_k-(R_n+h_k)cosL_ksinλ_kΔλ_k+cosL_kcosλ_kΔh_k

Δy_k＝-(R_n+h_k)sinλ_ksinL_kΔL_k+(R_n+h_k)cosL_kcosλ_kΔλ_k+cosL_ksinλ_kΔh_k

wherein e is the earth's eccentricity, R_nThe radius of the earth meridian.

Further, the measurement error criterion in the fifth step is specifically expressed as:

λ_kwhen beta is larger than beta, the measurement has errors;

λ_kwhen the beta is less than the beta, no error exists in the measurement;

wherein beta is a threshold value and lambda_kFor the detection function, λ_kObeying x degree of freedom of m²Distribution, i.e. λ_k～χ²(m)，λ_kThe concrete expression is as follows:

wherein v is_kFor the sequence of innovation at time k of the system,

is an innovation sequence v_kThe theoretical covariance of (2).

Further, the robust matrix of the multiple factors in the step five is represented as:

wherein S is_kFor robust adjustment of the matrix by multiple factors, s_k(i, i) a robust adjustment factor, s, representing the ith observation_k(i, i) the calculation method is as follows:

wherein,

is the value of the innovation theory covariance diagonal, Y_k(i, i) is the value of the innovation real covariance diagonal.

The invention has the beneficial effects that:

(1) according to the UWB and MEMS inertial navigation positioning principle, the UWB/MEMS tight combination mathematical model is established, and the model has higher positioning precision and stronger environmental adaptability than a conventional UWB/MEMS loose combination model.

(2) Aiming at measuring mutation noise and outliers, the invention introduces a measurement error criterion on the basis of the traditional robust algorithm, thereby enhancing the processing capability of outliers.

(3) The invention provides a calculation method of a multiple robust adaptive matrix aiming at the problem that the conventional robust algorithm has poor regulation capability on partial observation interfered and the actual situation of UWB ranging errors is combined, and the algorithm regulates a filter gain matrix in real time aiming at the situation of different errors measured, so that a filter has stronger adaptability.

Drawings

FIG. 1 is a schematic diagram of the present invention;

FIG. 2 is a graph of the course of the guideline movement of the present invention;

FIG. 3 is a comparison of carrier traces for UWB, MEMS, UWB/MEMS loose combination and UWB/MEMS tight combination algorithm solutions;

FIG. 4(a) is a comparison of attitude angle errors to FIG. 1;

FIG. 4(b) is a comparison of attitude angle error with FIG. 2;

FIG. 4(c) is a comparison of attitude angle errors to FIG. 3;

FIG. 5(a) is a comparison of velocity error with FIG. 1;

FIG. 5(b) is a comparison of velocity error with FIG. 2;

FIG. 6(a) is a comparison of velocity error with FIG. 1;

FIG. 6(b) is a comparison of velocity error with FIG. 2;

FIG. 7(a) is a comparison of attitude angle errors to FIG. 1;

FIG. 7(b) is a comparison of attitude angle error with FIG. 2;

FIG. 7(c) is a comparison of attitude angle errors to FIG. 3;

FIG. 8(a) is a comparison of velocity error with FIG. 1;

FIG. 8(b) is a comparison of velocity error with FIG. 2;

FIG. 9(a) is a comparison of position error with FIG. 1;

FIG. 9(b) is a comparison of position error with FIG. 2;

FIG. 10(a) is a comparison of attitude angle errors to FIG. 1;

FIG. 10(b) is a comparison of attitude angle error with FIG. 2;

FIG. 10(c) is a comparison of attitude angle errors to FIG. 3;

FIG. 11(a) is a comparison of velocity error with FIG. 1;

FIG. 11(b) is a comparison of velocity error with FIG. 2;

FIG. 12(a) is a comparison of position error with FIG. 1;

fig. 12(b) is a comparison of the position error with fig. 2.

Detailed Description

In order to improve the precision and robustness of an indoor positioning system, the invention deduces a UWB/MEMS tight combination algorithm according to UWB and MEMS positioning models, introduces a measurement error criterion on the basis of the traditional robust algorithm, and provides a multiple robust self-adaptive filtering algorithm aiming at the problem that the traditional robust algorithm is poor in regulating energy for partial observation under the influence of errors, so that the regulation of correct quantity measurement is avoided, and the precision and robustness of the combination system are improved.

The first embodiment is as follows: specifically describing the embodiment with reference to fig. 1, the method for improved robust unscented kalman filter integrated navigation in the embodiment includes the following steps:

(1) collecting data output by the UWB system and the MEMS system;

(2) selecting the state quantity and the observed quantity, and establishing a state space model of the UWB/MEMS combined navigation system;

(3) initializing filtering of the UWB/MEMS combined system;

(4) and (5) updating the time. Calculating a system state predicted value and covariance at the moment k according to the state estimated value and covariance at the moment k-1;

(5) sigma dot selection. Selecting a group of weighted Sigma points according to the state predicted value and the covariance thereof;

(6) and (6) updating the measurement. Measuring and updating by using the Sigma point after nonlinear measurement equation transformation to obtain error covariance of one-step state prediction and cross covariance of one-step prediction;

(7) introducing a measurement error criterion, if the measurement has an error, calculating a robust matrix and correcting the error covariance of the one-step state prediction;

(8) calculating state estimation and state error covariance at the moment k by using the corrected state error covariance in one step;

(9) and (4) feeding back and compensating the error information output by filtering to the UWB system and the MEMS system, and outputting high-precision attitude, speed and position information.

The invention discloses an improved robust unscented Kalman filtering integrated navigation method, which further comprises the following steps:

establishing UWB/MEMS tight combination system model

(1) The system state equation:

X_k+1＝F_kX_k+w_k

wherein, X_kError of inertial navigation solution information for 15D, which can be written as

Wherein phi is_k＝[φ_k,xφ_k,yφ_k,z]^TAttitude error, Δ V, resolved for moment k inertial navigation_k＝[ΔV_k,xΔV_k,yΔV_k,z]^TVelocity error, Δ P, in a geographic coordinate system resolved for moment k inertial navigation_k＝[ΔL_kΔλ_kΔh_k]^TFor the longitude and latitude height error of the inertial navigation solution at the moment k,_k＝[_{k,x k,y k,z}]^Tfor the moment k the gyro is constantly drifting,

the accelerometer is zero offset for time k. w is a_I,kIs the process noise of the inertial navigation system at the moment k and satisfies E [ w_I,k]＝0，F_I,kAnd obtaining a state transition matrix of the inertial navigation system at the moment k.

(2) And (3) derivation of a system measurement mathematical model:

in the geocentric earth-fixed coordinate system, the real position of the carrier at the k moment is assumed to be (x)_k,y_k) The position of the carrier obtained by inertial navigation is (x)_I,k,y_I,k) Ith base station S_iIs true at

Thus solving the resulting carrier to base station S by MEMS inertial navigation_iA pseudo range p therebetween_Ii,kComprises the following steps:

carrier to base station S_iTrue distance ρ of_iIs composed of

Subtracting the two formulas to obtain

Position (x) due to inertial navigation solution_I,k,y_I,k) There is an error, which is compared with the true value (x)_k,y_k) The relationship between them is:

x_k＝x_I,k-Δx_k

y_k＝y_I,k-Δy_k

wherein, Δ x_kAnd Δ y_kAnd solving the position error of the carrier for the MEMS inertial navigation at the moment k.

Finishing to obtain:

carrier to base station S assuming time k UWB measurements_iIs a distance of rho_Ui,kWith carriers to base station S_iTrue distance ρ of_i,kThe relationship between can be expressed as:

ρ_Ui,k＝ρ_i,k+

where noise is measured for UWB. Arranging to obtain a system measurement model:

in the formula,

for measurement input, -2 rho_Ui,k+²For the system to observe noise, its covariance matrix is R_k。

(3) The system measurement equation:

Z_k＝h(X'_k)+V_k

wherein Z is_kFor system measurement input, h (X'_k) As a function of the system non-linearity, V_kFor systematic measurement of noise, the noise covariance matrix is R_k，h(X'_k) AndV_kcan be expressed as:

V_k＝-2ρ_Ui,k+²

wherein, X'_kIncluding the position error Deltax of inertial navigation in the geocentric geostationary coordinate system_k、Δy_kWherein Δ x_k、Δy_kAnd the state quantity X_kPosition error delta L of medium inertial navigation in geographic coordinate system_k、Δλ_k、Δh_kCan be expressed as:

Δx_k＝-(R_n+h_k)cosλ_ksinL_kΔL_k-(R_n+h_k)cosL_ksinλ_kΔλ_k+cosL_kcosλ_kΔh_k

Δy_k＝-(R_n+h_k)sinλ_ksinL_kΔL_k+(R_n+h_k)cosL_kcosλ_kΔλ_k+cosL_ksinλ_kΔh_k

wherein e is the earth's eccentricity, R_nThe radius of the earth meridian.

2. Derivative Unscented Kalman Filter (DUKF) Algorithm

Assume a nonlinear discrete-time system as:

wherein x is_k∈RⁿAnd z_k∈R^mRespectively, a state vector and a measurement vector of the k-time system, phi_k/k-1For a discrete state transition matrix, h (-) is a nonlinear function describing the metrology model, w_kAnd v_kThe white noise is zero mean Gaussian white noise which is irrelevant, and the variance is as follows:

according to the DUKF filtering algorithm, system state updating and measurement updating are carried out to obtain navigation information of the carrier, and the method specifically comprises the following steps:

(1) given state estimates

And its covariance

(2) And (5) updating the time. Since the system state equation is linear, the state prediction value

And its covariance

Can be expressed as:

(3) sigma dot selection. Selecting a group of weighted Sigma points according to the state predicted value and the covariance thereof, wherein the selected Sigma points are as follows:

(4) and (6) updating the measurement. The Sigma point after the transformation of the nonlinear measurement equation h (-) is:

γ_i,k/k-1＝h(ξ_i,k/k-1),i＝0,1,...,2n

calculating a measurement predicted value and covariance thereof:

calculating the cross covariance between the state predicted value and the measurement predicted value:

wherein, ω is_iAs a weight, it can be expressed as

(5) Determining a Kalman filtering gain matrix:

(6) updating state estimates

And its covariance

As can be seen from the algorithm steps of the DUKF, in the time updating process, the time updating method has the same form as that of the linear KF, can simply and effectively calculate the state prediction value and the covariance thereof, and avoids the additional calculation of the standard UKF caused by UT conversion; in the measurement updating process, the DUKF selects a group of weighted Sigma point capture state predicted values and variance information thereof, and then updates a state estimated value and covariance thereof according to the selected Sigma point and the statistic information of the Sigma point transformed by a nonlinear measurement equation, so that the excellent characteristic of the standard UKF for processing the nonlinear filtering problem is inherited.

3. Criterion for measuring noise and outlier

In the kalman filtering framework, the influence of the measurement error on the filtering result is introduced through innovation, so that the measurement error can be detected on line from the innovation, and the innovation sequence of the system can be expressed as:

under the condition of no noise and outlier, innovation v at the time k_kIs zero mean gaussian white noise with covariance as:

when there is an error in the measurement, the innovation v_kThe mean value is no longer zero, therefore, whether noise and outlier exist can be judged by detecting the innovation sequence, and innovation v is carried out_kMaking a binary assumption:

H₀error-free Ev_k]＝0，

H₁There is an error E [ v ]_k]＝μ_k，E[(v_k-μ_k)(v_k-μ_k)^T]＝A_k

By the above-mentioned pair of innovation v_kThe hypothesis testing of (2) can yield a detection function:

wherein λ is_kObeying x degree of freedom of m²Distribution, i.e. λ_k～χ²(m) of the reaction mixture. When there is an error in the measurement, λ_kWill change if lambda is taken_kThe probability of being greater than a threshold value beta is alpha, i.e. p { lambda_k＞β}＝α, the criterion of the measurement error of the combined system is:

λ_kwhen beta is greater than beta, the measurement has errors

λ_kWhen < beta, there is no error in the measurement

4. Multi-factor robust algorithm

The traditional robust algorithm is to adjust the measurement covariance in real time by introducing a time-varying robust factor

Therefore, the purpose of adjusting the filter gain is achieved, the measurement with the error is used for estimating the state quantity in a smaller way, and the robust self-adaptability of the system to the measurement error is realized. The adjusted measurement covariance may be expressed as:

wherein s is_kIs a robust factor. When there is an error in the measurement, the actual covariance of the innovation is larger than the theoretical covariance, and the relationship between the two can be expressed as:

wherein, Y_kIs the actual covariance of the innovation, and ρ is the forgetting factor, which is usually 0.95. The traditional robust factor obtaining method is to calculate the ratio of the actual covariance and the theoretical covariance of the innovation sequence, and is specifically expressed as follows:

where tr (-) denotes tracing the matrix. As can be seen from the equation, the conventional robust factor, although simple to calculate, has the same adjustment capability for each observed quantity. For the UWB system, because the base stations are distributed around, the system is in a non-line-of-sight environment, usually, the communication between a certain base station and a tag is shielded by an obstacle to generate a ranging error, the error of each observed quantity of the combined system is different in size, and therefore, the traditional scalar robust factor is not suitable for adjusting the observation error of the UWB/MEMS combined system. According to the UWB ranging error characteristics, multiple robust factors are established, different adjusting capabilities are provided for each channel of UWB ranging, and the calculation method of the multiple robust factors is as follows:

wherein S is_kFor robust adjustment of matrix, s_k(i, i) a robust adjustment factor representing the ith observation, which is calculated as follows:

the method firstly provides a UWB/MEMS tight combination strategy aiming at the problem that UWB is easily influenced by a non line-of-sight environment (NLOS) under an indoor environment to cause low positioning precision; secondly, a measurement error criterion is introduced on the basis of a traditional robust algorithm, the problem that the traditional robust algorithm is poor in energy regulation on partial observation interfered is solved, and the actual situation of UWB ranging errors is combined, so that a calculation method of a multiple robust adaptive matrix is provided. Therefore, the invention utilizes MATLAB simulation software to carry out simulation experiments, and compares the MRUKF algorithm with the existing UKF algorithm and RUKF algorithm under the condition that each base station communication is interfered by errors and part of the base station communications are interfered by errors.

Simulation analysis is performed with specific examples as follows:

the sensor parameters are set as: the constant drift and the random noise of the gyroscope are respectively 10 degrees/h and 5 degrees/h, the constant drift and the random noise of the accelerometer are respectively 500ug and 70ug, and the output frequencies of the gyroscope and the accelerometer are both 100 Hz; the random drift of the UWB tag ranging is 0.3m, and the output frequency of the UWB system is 10 Hz. If the UWB base station has an error, the position of each base station is set to have a deviation of 1 m. The carrier motion is set as follows: assuming that the object M performs circular motion on a two-dimensional plane, performs uniform acceleration linear motion in the horizontal direction (x axis), and also performs uniform acceleration linear motion in the vertical direction (y axis), wherein the motion speed is 1M/s, the motion period is two cycles, and the total motion time is 280 s. The 4 base stations are arranged around the motion trail, and the motion trail of the carrier and the positions of the base stations are distributed in the figure 2.

FIG. 3 is a comparison diagram of a carrier track calculated by UWB, MEMS, UWB/MEMS loose combination and UWB/MEMS tight combination algorithms, and it can be seen from the diagram that the MEMS calculated track is smooth and stable, but inertial navigation errors are gradually accumulated over time to cause the positioning result to deviate from the reference track. Compared with MEMS inertial navigation, UWB positioning accuracy is higher, but UWB ranging has noise disturbance, and UWB positioning tracks have larger fluctuation. Compared with MEMS and UWB, the UWB/MEMS combined system inherits the advantages of MEMS inertial navigation and UWB, so that the resolving result is closer to the reference track, and the stability is better.

Fig. 4(a), fig. 4(b), fig. 4(c), fig. 5(a), fig. 5(b), fig. 6(a) and fig. 6(b) are graphs comparing the filter output error of the MRUKF algorithm of the present invention with the DUKF algorithm and the conventional RUKF algorithm under the condition that the communication of each base station is affected by the error, and it can be seen from the error graphs that when the communication of the tag and 4 base stations is affected by the noise, the filtering accuracy is higher than that of the conventional DUKF because the RUKF algorithm adjusts the measurement gain. Although the MRUKF independently adjusts each measurement, the size of noise and field value received by each measurement is the same, so that the size of a robust matrix calculated by the MRUKF is the same as that of a robust factor calculated by the RUKF, and the filtering precision of the two algorithm algorithms is equivalent.

Table 1 shows the root mean square error of the attitude, speed, and position of the MRUKF algorithm, the DUKF algorithm, and the RUKF algorithm in filtering calculation under the condition that each base station is interfered by an error. As can be seen from the table, when the observed quantities all contain errors, both the RUKF and the MRUKF have good regulating effect on the errors.

TABLE 1

Fig. 7(a), fig. 7(b), fig. 7(c), fig. 8(a), fig. 8(b), fig. 9(a) and fig. 9(b) are graphs comparing filtering output errors of the MRUKF algorithm of the present invention with the DUKF algorithm and the conventional RUKF algorithm when a part of the observed quantities contain abrupt noise under the condition that part of the base station communications is interfered by errors. It can be seen from the figure that the filtering accuracy of the MRUKF provided by the invention is obviously higher than that of the RUKF, the main reason is that the RUKF algorithm has the same adjusting capability for each observed quantity, when only 2 UWB ranging measurements are affected by noise, the robust factor obtained by the RUKF algorithm pollutes the other two observed quantities which are not affected by noise, and the original reasonable filtering gain is forced to change.

Table 2 shows the root mean square error of the attitude, speed, and position of the MRUKF algorithm, the DUKF algorithm, and the RUKF algorithm in filtering calculation under the condition that part of the base station communications is interfered by errors, when part of the observed quantities contain mutation noise. It can also be seen from the table that the filtering accuracy of the MRUKF algorithm provided by the invention is obviously higher than that of the RUKF algorithm and the UKF algorithm.

TABLE 2

Fig. 10(a), fig. 10(b), fig. 10(c), fig. 11(a), fig. 11(b), fig. 12(a) and fig. 12(b) are graphs comparing filtering output errors of the MRUKF algorithm of the present invention, the DUKF algorithm and the conventional RUKF algorithm when part of the observed quantities contain outliers, and as can be seen from the above graphs, the RUKF algorithm has a good adjusting capability for the measured outliers, but because the adjusting capability for each measurement is the same, the measurement that is not interfered by outliers is contaminated, and therefore, the filtering output accuracy is lower than that of the MRUKF, and the robustness is poor.

Table 3 shows the root-mean-square error of the attitude, speed and position of the MRUKF algorithm, the DUKF algorithm and the RUKF algorithm in filtering calculation under the condition that part of base station communication is interfered by errors and when part of observed quantities contain wild values. It can also be seen from the table that the adjustment capability of the MRUKF algorithm provided by the invention on partial observed quantity interfered by errors is obviously higher than that of the RUKF algorithm and the UKF algorithm.

TABLE 3

In conclusion, the improved robust unscented Kalman filtering integrated navigation method is high in positioning precision and robustness, has good adjusting capability on errors contained in a part of UWB base stations in practice, and is high in engineering application value.

It should be noted that the detailed description is only for explaining and explaining the technical solution of the present invention, and the scope of protection of the claims is not limited thereby. It is intended that all such modifications and variations be included within the scope of the invention as defined in the following claims and the description.

Claims (5)

1. An improved robust unscented Kalman filter integrated navigation method is characterized by comprising the following steps:

the method comprises the following steps: collecting data output by the UWB system and the MEMS system;

step two: selecting state quantity and observed quantity of a system, and establishing a state space model of the UWB/MEMS combined navigation system;

step three: carrying out filtering initialization on the UWB/MEMS combined navigation system;

step four: time updating and measurement updating are carried out on the UWB/MEMS combined navigation system at the moment k, and error covariance of one-step prediction and cross covariance of one-step prediction of the system state are obtained;

step five: introducing a measurement error criterion, if the measurement has an error, calculating a robust matrix of multiple factors, correcting the error covariance predicted by one step of state, and then entering the step six, and if the measurement has no error, directly entering the step six;

step six: if the measurement has errors, the corrected state one-step prediction error covariance is used for calculating the state estimation value and the state error covariance of the system at the moment k, and if the measurement has no errors, the state one-step prediction error covariance in the step four is used for calculating the state estimation value and the state error covariance of the system at the moment k;

step seven: and acquiring constant drift of the gyroscope and the accelerometer of the MEMS inertial navigation according to the state estimation value obtained in the sixth step, feeding the constant drift back to the combined system, and outputting attitude, speed and position information.

2. The improved robust unscented kalman filter integrated navigation method according to claim 1, characterized in that the system state equation of the UWB/MEMS integrated navigation system state space model is expressed as:

X_k+1＝F_kX_k+w_k

wherein, X_kError of inertial navigation solution information in 15 dimensions, expressed as

Wherein phi is_k＝[φ_k,xφ_k,yφ_k,z]^TAttitude error, Δ V, resolved for moment k inertial navigation_k＝[ΔV_k,xΔV_k,yΔV_k,z]^TVelocity error, Δ P, in a geographic coordinate system resolved for moment k inertial navigation_k＝[ΔL_kΔλ_kΔh_k]^TFor the longitude and latitude height error of the inertial navigation solution at the moment k,_k＝[_{k,x k,y k,z}]^Tfor the moment k the gyro is constantly drifting,

zero offset, w, of the accelerometer at time k_kCombining the systematic process noise for time k, w_kNoise covariance matrix of Q_k，F_kAnd obtaining a state transition matrix of the inertial navigation system at the moment k.

3. The improved robust unscented kalman filter integrated navigation method according to claim 2, characterized in that the system measurement equation of the UWB/MEMS integrated navigation system state space model is expressed as:

Z_k＝h(X′_k)+V_k

wherein Z is_kFor system measurement input, h (X'_k) As a function of the system non-linearity, V_kMeasuring noise, V, for a combined system_kThe noise covariance matrix is R_k，h(X′_k) And V_kRespectively expressed as:

V_k＝-2ρ_Ui,k+²

wherein, X'_kIncluding the position error Deltax of inertial navigation in the geocentric geostationary coordinate system_k、Δy_kWherein Δ x_k、Δy_kAnd the state quantity X_kPosition error delta L of medium inertial navigation in geographic coordinate system_k、Δλ_k、Δh_kThe relationship of (c) is expressed as:

Δx_k＝-(R_n+h_k)cosλ_ksinL_kΔL_k-(R_n+h_k)cosL_ksinλ_kΔλ_k+cosL_kcosλ_kΔh_k

Δy_k＝-(R_n+h_k)sinλ_ksinL_kΔL_k+(R_n+h_k)cosL_kcosλ_kΔλ_k+cosL_ksinλ_kΔh_k

wherein e is the earth's eccentricity, R_nThe radius of the earth meridian.

4. The improved robust unscented kalman filter integrated navigation method according to claim 3, characterized in that the measurement error criterion in the fifth step is specifically expressed as:

λ_kwhen beta is larger than beta, the measurement has errors;

λ_kwhen the beta is less than the beta, no error exists in the measurement;

wherein beta is a threshold value and lambda_kFor the detection function, λ_kObeying x degree of freedom of m²Distribution, i.e. λ_k～χ²(m)，λ_kThe concrete expression is as follows:

wherein v is_kFor the sequence of innovation at time k of the system,

is an innovation sequence v_kThe theoretical covariance of (2).

5. The improved robust unscented kalman filter integrated navigation method according to claim 4, wherein the robust matrix of multiple factors in the step five is represented as:

wherein S is_kFor robust adjustment of the matrix by multiple factors, s_k(i, i) a robust adjustment factor, s, representing the ith observation_k(i, i) the calculation method is as follows:

wherein,

is the value of the innovation theory covariance diagonal, Y_k(i, i) is the value of the innovation real covariance diagonal.

Images