CN111896008A - An Improved Robust Unscented Kalman Filtering Integrated Navigation Method - Google Patents

Abstract

An improved robust unscented Kalman filter integrated navigation method relates to the technical field of integrated navigation. In view of the problems in the prior art that the filtering accuracy is low, which leads to poor positioning accuracy, and its ability to handle outliers is poor, the present The invention establishes a UWB/MEMS tight combination mathematical model based on the UWB and MEMS inertial navigation positioning principles, which has higher positioning accuracy and stronger adaptability to the environment than the conventional UWB/MEMS loose combination model. Aiming at measuring sudden change noise and outliers, the invention introduces measurement error criterion on the basis of traditional robust algorithm, and enhances the processing capability of outliers. Aiming at the problem that the traditional robust algorithm has poor ability to adjust the interference to some observations, and combined with the actual situation of the UWB ranging error, the invention proposes a calculation method of multiple robust adaptive matrices. The algorithm is aimed at different errors of each measurement situation, adjust the filter gain matrix in real time to make the filter more adaptive.

Description

An 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 filter integrated navigation method.

Background technique

In recent years, with the development of technology and changes in people's lives, location-based services (LBS) have a wide range of application backgrounds in industry and markets. Nowadays, with the rapid development of Global Navigation Satellite System (GNSS), it can provide users with high-precision location services in an outdoor environment. However, in an indoor environment, GNSS signals are easily blocked, making it impossible for users to achieve normal navigation and positioning through GNSS. At the same time, most human activities take place in the indoor environment, and high-precision indoor positioning services can provide users with networked services such as route navigation, commodity shopping guidance, and resource guidance. Therefore, high-precision indoor positioning technology has broad application prospects.

As one of the core contents of LBS, the design of indoor positioning system has become a hot spot in this field. Nowadays, wireless positioning is generally used in indoor positioning technology. The disadvantage of wireless positioning system is that its positioning effect is easily affected by non-line-of-sight (NLOS, non-line of sight) and other environments. The widely used indoor positioning technologies mainly include Wireless Local Area Networks (WLAN), Radio Frequency Identification (RFID), Ultra-wide Band (UWB), Bluetooth, etc. The advantages of bandwidth and good anti-multipath interference ability are more suitable for providing indoor high-precision positioning services. However, UWB positioning accuracy is susceptible to non-line-of-sight environments. Therefore, many scholars propose to fuse two or more sensors to make up for the shortcomings of a single-sensor navigation system. With the rise and development of Micro Electro Mechanical System (MEMS) technology, the volume and cost of traditional inertial navigation equipment have been effectively reduced. The integrated navigation system based on UWB/MEMS can make full use of their respective advantages to improve the positioning accuracy and stability of the system, and has broad development prospects.

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 UWB fails by studying the combination of UWB/MEMS. Although these multi-sensor combinations all improve the combined positioning accuracy to a certain extent, the introduction of other sensors also makes their engineering implementation more difficult. The other is to solve the noise and outlier problems of UWB from the perspective of filtering. Although some scholars have proposed to use strong tracking Sage adaptive filtering to solve the problem of UWB positioning noise from the perspective of filtering, if the fading factor in strong tracking is not selected properly, the filtering accuracy will be reduced, and its ability to handle outliers is poor. .

SUMMARY OF THE INVENTION

The purpose of the present invention is to propose an improved robust unscented Kalman filter combined navigation method for the problems of low filtering accuracy in the prior art, which leads to poor positioning accuracy and poor processing capability for outliers.

The technical scheme that the present invention takes in order to solve the above-mentioned technical problems is:

An improved robust unscented Kalman filter integrated navigation method includes the following steps:

Step 1: Collect the data output by the UWB system and the MEMS system;

Step 2: Select the state quantity and observation quantity of the system, and establish the state space model of the UWB/MEMS integrated navigation system;

Step 3: Perform filter initialization on the UWB/MEMS integrated navigation system;

Step 4: Perform time update and measurement update on the UWB/MEMS integrated navigation system at time k to obtain the error covariance of the one-step prediction of the system state and the cross-covariance of the one-step prediction;

Step 5: Introduce the measurement error criterion. If there is an error in the measurement, calculate the robust matrix of multiple factors, and correct the error covariance of the state one-step prediction, and then go to Step 6. If there is no error in the measurement, then Go directly to step six;

Step 6: If there is an error in the measurement, use the corrected state to predict the error covariance in one step, and calculate the state estimate and state error covariance of the system at time k. If there is no error in the measurement, use the state step in step 4. Prediction error covariance, calculate the state estimate and state error covariance of the system at time k;

Step 7: Acquire the constant drift of the gyroscope and accelerometer of the MEMS inertial navigation according to the state estimation value obtained in the step 6, and feed it back to the combined system to output attitude, velocity and position information.

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

X _k+1 =F _k X _k +w _k

其中，φ_k＝[φ_k,x φ_k,y φ_k,z]^T为k时刻惯导解算的姿态误差，ΔV_k＝[ΔV_k,x ΔV_k,y ΔV_k,z]^T为k时刻惯导解算的地理坐标系下的速度误差，ΔP_k＝[ΔL_k Δλ_k Δh_k]^T为k时刻惯导解算的经纬高误差，ε_k＝[ε_k,x ε_k,y ε_k,z]^T为k时刻陀螺常值漂移，

为k时刻加速度计零偏，w_k为k时刻组合系统过程噪声，w_k噪声协方差矩阵为Q_k，F_k为k时刻惯导系统状态转移矩阵。Among them, X _k is the information error of the 15-dimensional inertial navigation solution, which is expressed as

Among them, φ _k =[φ _k,x φ _k,y φ _k,z ] ^T is the attitude error of inertial navigation solution at time k, ΔV _k =[ΔV _k,x ΔV _k,y ΔV _k,z ] ^T is Velocity error in geographic coordinate system calculated by inertial navigation at time k, ΔP _k =[ΔL _k Δλ _k Δh _k ] ^T is the latitude and longitude error calculated by inertial navigation at time k, ε _k =[ε _k,x ε _{k, y} ε _k,z ] ^T is the constant gyro drift at time k,

is the zero bias of the accelerometer at time k, w _k is the combined system process noise at time k, the covariance matrix of w _k noise is Q _k , and F _k is the state transition matrix of the inertial navigation system at time k.

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

Z _k =h(X' _k )+V _k

Among them, Z _k is the system measurement input, h(X' _k ) is the system nonlinear measurement function, V _k is the combined system measurement noise, and the V _k noise covariance matrix is R _k , h(X' _k ) and _Vk are respectively expressed as:

V _k =-2ρ _Ui,k ε+ε ²

Among them, X' _k includes the position errors Δx _k , Δy _k of the inertial navigation in the geocentric fixed coordinate system, where Δx _k , Δy _k and the state quantity X _k of the inertial navigation in the geographic coordinate system The position errors ΔL _k , Δλ The relationship between _k and Δh _k is expressed as:

Δx _k =-(R _n +h _k )cosλ _k sinL _k ΔL _k -(R _n +h _k )cosL _k sinλ _k Δλ _k +cosL _k cosλ _k Δh _k

Δy _k =-(R _n +h _k )sinλ _k sinL _k ΔL _k +(R _n +h _k )cosL _k cosλ _k Δλ _k +cosL _k sinλ _k Δh _k

Among them, e is the eccentricity of the earth, and R _n is the radius of the earth's meridian.

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

When λ _k > β, there is an error in the measurement;

When λ _k < β, there is no measurement error;

Among them, β is the threshold value, λ _k is the detection function, and λ _k obeys the χ ² distribution with m degrees of freedom, that is, λ _k ～χ ² (m), and λ _k is specifically expressed as:

为新息序列v_k的理论协方差。Among them, v _k is the innovation sequence at time k of the system,

is the theoretical covariance of the innovation sequence v _k .

Further, the robust matrix of multiple factors in the step 5 is expressed as:

Among them, _Sk is the robust adjustment matrix of multiple factors, and s _k (i, i) represents the robust adjustment factor of the i-th observation. The calculation method of s _k (i, i) is as follows:

为新息理论协方差对角线的值，Y_k(i,i)为新息实际协方差对角线的值。in,

is the value of the innovation theoretical covariance diagonal, and Y _k (i,i) is the innovation actual covariance diagonal value.

The beneficial effects of the present invention are:

(1) According to the UWB and MEMS inertial navigation positioning principles, the present invention establishes a UWB/MEMS tight combination mathematical model, which has higher positioning accuracy and stronger adaptability to the environment than the conventional UWB/MEMS loose combination model.

(2) Aiming at measuring sudden change noise and outliers, the present invention introduces measurement error criteria on the basis of traditional robust algorithms, thereby enhancing the processing capability of outliers.

(3) The present invention proposes a method for calculating multiple robust adaptive matrices in view of the problem that the traditional robust algorithm has poor ability to adjust the interference to some observations, and combined with the actual situation of the UWB ranging error. Under different error conditions, the filter gain matrix is adjusted in real time to make the filter more adaptive.

Description of drawings

Fig. 1 is the principle diagram of the present invention;

Fig. 2 is the target movement trajectory diagram of the present invention;

Figure 3 is a comparison diagram of carrier trajectories calculated by UWB, MEMS, UWB/MEMS loose combination and UWB/MEMS tight combination algorithm;

Figure 4(a) is the attitude angle error comparison Figure 1;

Figure 4(b) is the attitude angle error comparison Figure 2;

Figure 4(c) is the attitude angle error comparison Figure 3;

Figure 5(a) is the speed error comparison Figure 1;

Figure 5(b) is the speed error comparison Figure 2;

Figure 6(a) is the speed error comparison Figure 1;

Figure 6(b) is the speed error comparison Figure 2;

Figure 7(a) is the attitude angle error comparison Figure 1;

Figure 7(b) is the attitude angle error comparison Figure 2;

Figure 7(c) is the attitude angle error comparison Figure 3;

Figure 8(a) is the speed error comparison Figure 1;

Figure 8(b) is the speed error comparison Figure 2;

Fig. 9(a) is the position error comparison Fig. 1;

Fig. 9(b) is the position error comparison Fig. 2;

Figure 10(a) is the attitude angle error comparison Figure 1;

Figure 10(b) is the attitude angle error comparison Figure 2;

Figure 10(c) is the attitude angle error comparison Figure 3;

Figure 11(a) is the speed error comparison Figure 1;

Figure 11(b) is the speed error comparison Figure 2;

Fig. 12(a) is the position error comparison Fig. 1;

Fig. 12(b) is the position error comparison Fig. 2.

Detailed ways

In order to improve the accuracy and robustness of the indoor positioning system, the present invention deduces a UWB/MEMS tight combination algorithm according to the UWB and MEMS positioning models, and at the same time introduces the measurement error criterion on the basis of the traditional robust algorithm. Aiming at the problem that the traditional robust algorithm has poor ability to adjust some observations by error interference, a multiple robust adaptive filtering algorithm is proposed, which avoids the adjustment of the correct quantity measurement, thereby improving the accuracy and robustness of the combined system.

Embodiment 1: This embodiment is described in detail with reference to FIG. 1. An improved robust unscented Kalman filter combined navigation method described in this embodiment includes the following steps:

(1) Collect data output by UWB system and MEMS system;

(2) Select the state quantity and the observation quantity, and establish the state space model of the UWB/MEMS integrated navigation system;

(3) UWB/MEMS combined system filter initialization;

(4) Time update. Calculate the predicted value and covariance of the system state at time k from the estimated state value and covariance at time k-1;

(5) Sigma point selection. Select a set of weighted Sigma points based on state predicted values and their covariances;

(6) Measurement update. Using the Sigma point transformed by the nonlinear measurement equation to perform measurement update, the error covariance of one-step state prediction and the cross-covariance of one-step prediction are obtained;

(7) The measurement error criterion is introduced. If there is an error in the measurement, the robust matrix is calculated and the error covariance of the one-step state prediction is corrected;

(8) Using the corrected one-step state error covariance, calculate the state estimate and state error covariance at time k;

(9) Feedback and compensate the error information output by the filter to the UWB system and the MEMS system, and output high-precision attitude, speed, and position information.

An improved robust unscented Kalman filter combined navigation method of the present invention further comprises:

Building a UWB/MEMS Compact System Model

(1) The system state equation:

X _k+1 =F _k X _k +w _k

其中，φ_k＝[φ_k,x φ_k,y φ_k,z]^T为k时刻惯导解算的姿态误差，ΔV_k＝[ΔV_k,x ΔV_k,y ΔV_k,z]^T为k时刻惯导解算的地理坐标系下的速度误差，ΔP_k＝[ΔL_k Δλ_k Δh_k]^T为k时刻惯导解算的经纬高误差，ε_k＝[ε_k,x ε_k,y ε_k,z]^T为k时刻陀螺常值漂移，

为k时刻加速度计零偏。w_I,k为k时刻惯导系统过程噪声，且满足E[w_I,k]＝0，F_I,k为k时刻惯导系统状态转移矩阵。Among them, X _k is the 15-dimensional inertial navigation solution information error, which can be written as

Among them, φ _k =[φ _k,x φ _k,y φ _k,z ] ^T is the attitude error of inertial navigation solution at time k, ΔV _k =[ΔV _k,x ΔV _k,y ΔV _k,z ] ^T is Velocity error in geographic coordinate system calculated by inertial navigation at time k, ΔP _k =[ΔL _k Δλ _k Δh _k ] ^T is the latitude and longitude error calculated by inertial navigation at time k, ε _k =[ε _k,x ε _{k, y} ε _k,z ] ^T is the constant gyro drift at time k,

is the zero offset of the accelerometer at time k. w _I,k is the inertial navigation system process noise at time k, and satisfies E[w _I,k ]=0, and F _I,k is the state transition matrix of the inertial navigation system at time k.

(2) Derivation of system measurement mathematical model:

因此由MEMS惯导解算所得到的载体到基站S_i之间的伪距ρ_Ii,k为：In the geocentric fixed coordinate system, assuming that the real position of the carrier at time k is (x _k , y _k ), the position of the carrier obtained by the inertial navigation solution is (x _{I, k} , y _{I, k} ), the i-th base station The true position of _Si is

Therefore, the pseudorange ρ _{Ii,k between} the carrier and the base station Si obtained from the MEMS inertial navigation solution is:

The real distance ρ _i from the carrier to the base station S _i is

Subtracting the two equations, we get

Due to the error of the position (x _I,k ,y _I,k ) calculated by the inertial navigation, the relationship between it and the real value (x _k , y _k ) is:

x _k =x _I,k -Δx _k

y _k =y _I,k -Δy _k

Among them, Δx _k and Δy _k are the carrier position errors calculated by MEMS inertial navigation at time k.

Arrange to get:

Assuming that the distance from the carrier to the base station Si measured by UWB at time k is ρ _Ui,k , the relationship between it and the real distance ρ _{i ,k from} the carrier to the base station Si can be expressed as:

ρ _Ui,k =ρ _i,k +ε

Among them, ε is the UWB measurement noise. Arrange the available system measurement model:

为量测输入，-2ρ_Ui,kε+ε²为系统观测噪声，其协方差矩阵为R_k。In the formula,

is the measurement input, -2ρ _Ui,k ε+ε ² is the system observation noise, and its covariance matrix is R _k .

(3) System measurement equation:

Z _k =h(X' _k )+V _k

Among them, Z _k is the system measurement input, h(X' _k ) is the system nonlinear measurement function, V _k is the system measurement noise, and its noise covariance matrix is R _k , h(X' _k ) and V _k can be expressed as:

V _k =-2ρ _Ui,k ε+ε ²

Among them, X' _k includes the position errors Δx _k , Δy _k of the inertial navigation in the geocentric fixed coordinate system, where Δx _k , Δy _k and the state quantity X _k of the inertial navigation in the geographic coordinate system The position errors ΔL _k , Δλ The relationship between _k and Δh _k can be expressed as:

Δx _k =-(R _n +h _k )cosλ _k sinL _k ΔL _k -(R _n +h _k )cosL _k sinλ _k Δλ _k +cosL _k cosλ _k Δh _k

Δy _k =-(R _n +h _k )sinλ _k sinL _k ΔL _k +(R _n +h _k )cosL _k cosλ _k Δλ _k +cosL _k sinλ _k Δh _k

Among them, e is the eccentricity of the earth, and R _n is the radius of the earth's meridian.

2. Derivative Unscented Kalman Filter (DUKF) algorithm

Suppose the nonlinear discrete-time system is:

Among them, x _k ∈ R ⁿ and z _k ∈ R ^m are the state vector and measurement vector of the k time system, respectively, Φ _k/k-1 is the discrete state transition matrix, and h( ) is the non-state vector describing the measurement model. Linear function, w _k and v _k are uncorrelated zero-mean Gaussian white noise, and their variance is:

According to the DUKF filtering algorithm, the system status update and measurement update are performed to obtain the navigation information of the carrier. The specific steps include:

及其协方差

(1) Given a state estimate

and its covariance

及其协方差

的可表示为：(2) Time update. Since the system state equation is linear, the state predicted value

and its covariance

can be expressed as:

(3) Sigma point selection. A set of weighted Sigma points are selected according to the state predicted value and its covariance. The selected Sigma points are:

(4) Measurement update. The Sigma point transformed by the nonlinear measurement equation h(·) is:

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

Compute the measurement predictions and their covariances:

Compute the cross-covariance between state predictions and measurement predictions:

Among them, ω _i is the weight, which can be expressed as

(5) Determine the Kalman filter gain matrix:

及其协方差

(6) Update the state estimate

and its covariance

It can be seen from the algorithm steps of DUKF that in the time update process, it has the same form as the linear KF, which can calculate the state prediction value and its covariance concisely and effectively, avoiding the extra calculation caused by the UT transformation of the standard UKF; During the measurement update process, DUKF selects a set of weighted Sigma points to capture the information of the state prediction value and its variance, and then updates the state estimate value according to the selected Sigma point and the statistical information of the Sigma point transformed by the nonlinear measurement equation and its covariance, inheriting the excellent characteristics of the standard UKF in dealing with nonlinear filtering problems.

3. Measurement noise and outlier criteria

In the Kalman filter framework, the influence of the measurement error on the filtering result is introduced by the innovation, so the measurement error can be detected online from the innovation. The innovation sequence of the system can be expressed as:

In the case of no noise and outliers, the innovation v _{k at time k} is zero-mean Gaussian white noise, and its covariance is:

When there is an error in the measurement, the mean value of the innovation v _k is no longer zero. Therefore, the detection of the innovation sequence can determine whether there is noise and outliers, and make a binary assumption for the innovation v _k :

H ₀ : no error E[v _k ]=0,

H ₁ : error E[v _k ]=μ _k , E[(v _k −μ _k )(v _k −μ _k ) ^T ]=A _k

The detection function can be obtained through the above hypothesis test on the innovation v _k :

Among them, λ _k is a χ ² distribution with m degrees of freedom, that is, λ _k ~χ ² (m). When there is an error in the measurement, λ _k will change. If the probability that λ _k is greater than a certain threshold value β is taken as α, that is, p{λ _k > β}=α, then the judgment criterion for the measurement error of the combined system is: :

When λ _k > β, there is an error in the measurement

When λ _k < β, there is no measurement error

4. Robust algorithm for multiple factors

从而达到调节滤波增益的目的，使有误差的量测量被较小的用于状态量的估计中，实现系统对量测误差的鲁棒自适应性。调整后的量测协方差可以表示为：The traditional robust algorithm is to adjust the measurement covariance in real time by introducing a time-varying robustness factor.

Therefore, the purpose of adjusting the filter gain is achieved, so that the quantity measurement with error is used in the estimation of the state quantity in a smaller amount, and the robust adaptability of the system to the measurement error is realized. The adjusted measurement covariance can be expressed as:

where _sk is the robustness factor. When there is an error in the measurement, the actual covariance of the innovation is greater than the theoretical covariance, and the relationship between the two can be expressed as:

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

Among them, tr( ) represents the trace of the matrix. It can be seen from the formula that although the traditional robust factor is simple to calculate, it has the same ability to adjust each observation. For the UWB system, since the base stations are distributed around, and the system is in a non-line-of-sight environment, the communication between a base station and a tag is usually blocked by an obstacle, resulting in a ranging error. Therefore, the error of each observation in the combined system is different. Therefore, , the traditional scalar robust factor is not suitable for adjusting the observation error of UWB/MEMS combined system. The present invention establishes multiple robust factors according to the error characteristics of UWB ranging, and gives different adjustment capabilities to each channel of UWB ranging. The calculation method of the multiple robust factors is as follows:

Among them, _Sk is the robust adjustment matrix, and _sk (i,i) represents the robust adjustment factor of the i-th observation. The calculation method is as follows:

The invention firstly proposes a UWB/MEMS tight combination strategy to solve the problem that UWB is easily affected by non-line-of-sight environment (NLOS) in indoor environment, which leads to low positioning accuracy; secondly, it introduces measurement error judgment on the basis of traditional robust algorithm. According to the data, and in view of the problem that the traditional robust algorithm has poor ability to adjust the interference of some observations, and combined with the actual situation of the UWB ranging error, a calculation method of multiple robust adaptive matrices is proposed. Error conditions, adjust the filter gain matrix in real time to make the filter more adaptive. Therefore, the present invention uses MATLAB simulation software to carry out simulation experiments, and compares the MRUKF algorithm of the present invention with the existing UKF algorithm and RUKF algorithm under the condition that each base station communication is interfered by errors and some base station communications are interfered by errors.

The following simulation analysis is carried out in combination with specific cases:

The sensor parameters are set as: the constant drift and random noise of the gyroscope are 10°/h and 5°/h, respectively, the constant drift and random noise of the accelerometer are 500ug and 70ug, respectively, and the output frequencies of the gyroscope and accelerometer are both. is 100Hz; the random drift of UWB tag ranging is 0.3m, and the output frequency of the UWB system is 10Hz. When the UWB base station contains errors, the position of each base station is set to have a deviation of 1m. The carrier motion setting is: Suppose the object M performs circular motion on a two-dimensional plane, uniformly accelerated linear motion in the horizontal direction (x-axis), and uniformly accelerated linear motion in the vertical direction (y-axis), and the motion speed is 1m/ s, exercise for two weeks, and the total exercise time is 280s. 4 base stations are arranged around the motion track, Figure 2 bit carrier motion track and base station location distribution.

Figure 3 is a comparison chart of carrier trajectories solved by UWB, MEMS, UWB/MEMS loose combination, and UWB/MEMS tight combination algorithm. It can be seen from the figure that the MEMS solution trajectory is relatively smooth and stable, but the inertial navigation error over time The gradual accumulation causes the positioning results to deviate from the reference trajectory. Compared with MEMS inertial navigation, UWB positioning accuracy is higher, but there is noise disturbance in UWB ranging, which leads to large fluctuations in UWB positioning trajectory. Compared with MEMS and UWB, UWB/MEMS combined system inherits the advantages of MEMS inertial navigation and UWB, so the solution results are closer to the reference trajectory and have better stability.

Figure 4(a), Figure 4(b), Figure 4(c), Figure 5(a), Figure 5(b), Figure 6(a) and Figure 6(b) are the cases where the communication of each base station is interfered by errors The following is a comparison chart of the filtering output errors of the MRUKF algorithm of the present invention, the DUKF algorithm and the traditional RUKF algorithm. It can be seen from the error chart that when the communication between the tag and the four base stations is affected by noise, because the RUKF algorithm adjusts the measurement gain, Therefore, its filtering accuracy is higher than that of the traditional DUKF. Although MRUKF adjusts each measurement independently, since the noise and outliers of each measurement are the same, the robust matrix calculated by MRUKF and the robust factor calculated by RUKF are the same, so the filtering accuracy of the two algorithms is the same. quite.

Table 1 shows the root mean square errors of attitude, velocity, and position calculated by the MRUKF algorithm, the DUKF algorithm, and the RUKF algorithm of the present invention when the communication of each base station is interfered by errors. It can be seen from the table that when the observations all contain errors, both RUKF and MRUKF have a good adjustment 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 the case where the communication of some base stations is interfered by errors , when some of the observations contain mutation noise, the comparison chart of the filtering output error of the MRUKF algorithm of the present invention, the DUKF algorithm and the traditional RUKF algorithm. It can be seen from the figure that the filtering accuracy of the MRUKF proposed by the present invention is significantly higher than that of the RUKF. The main reason is that the RUKF algorithm has the same ability to adjust each observation. When only two UWB ranging are affected by noise, the The robust factor obtained by the RUKF algorithm will contaminate the other two observations that are not affected by noise, forcing the original reasonable filter gain to change. The MRUKF algorithm proposed in the present invention is based on the error conditions of each measurement. , respectively adjust its filter gain independently, therefore, its filter output accuracy is significantly higher than that of the RUKF algorithm.

Table 2 shows the root mean square errors of attitude, velocity and position calculated by the MRUKF algorithm of the present invention, the DUKF algorithm and the RUKF algorithm when the communication of some base stations is interfered by errors and when some observations contain sudden change noise. It can also be seen from the table that the filtering accuracy of the MRUKF algorithm proposed by the present 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 the cases where the communication of some base stations is interfered by errors , when some of the observations contain outliers, the comparison chart of the filtering output error between the MRUKF algorithm of the present invention, the DUKF algorithm and the traditional RUKF algorithm, it can be seen from the above figure that compared with the UKF algorithm, the RUKF algorithm has a good performance on the measurement outliers However, since it has the same adjustment ability for each measurement, it will contaminate the measurement that is not disturbed by outliers. Therefore, its filtering output accuracy is lower than that of MRUKF, and its robustness is poor.

Table 3 shows the root mean square errors of attitude, velocity and position calculated by the MRUKF algorithm of the present invention, the DUKF algorithm and the RUKF algorithm filtering and solving under the condition that some base station communication is interfered by errors, and when some observed values contain outliers. It can also be seen from the table that the MRUKF algorithm proposed in the present invention has a significantly higher adjustment capability to some observations disturbed by errors than the RUKF algorithm and the UKF algorithm.

table 3

In conclusion, an improved robust unscented Kalman filter combined navigation method of the present invention has high positioning accuracy and strong robustness, and has good adjustment capability for errors in some UWB base stations in practice, and has high engineering application value.

It should be noted that the specific embodiments are only explanations and descriptions of the technical solutions of the present invention, and cannot be used to limit the protection scope of the rights. Any changes made according to the claims and description of the present invention are only partial changes, which should still fall within the protection scope of the present invention.

Claims (5)

1. an improved robust unscented Kalman filter combined navigation method is characterized in that comprising the following steps: Step 1: Collect the data output by the UWB system and the MEMS system; Step 2: Select the state quantity and observation quantity of the system, and establish the state space model of the UWB/MEMS integrated navigation system; Step 3: Perform filter initialization on the UWB/MEMS integrated navigation system; Step 4: Perform time update and measurement update on the UWB/MEMS integrated navigation system at time k to obtain the error covariance of the one-step prediction of the system state and the cross-covariance of the one-step prediction; Step 5: Introduce the measurement error criterion. If there is an error in the measurement, calculate the robust matrix of multiple factors, and correct the error covariance of the state one-step prediction, and then go to Step 6. If there is no error in the measurement, then Go directly to step six; Step 6: If there is an error in the measurement, use the corrected state to predict the error covariance in one step, and calculate the state estimate and state error covariance of the system at time k. If there is no error in the measurement, use the state step in step 4. Prediction error covariance, calculate the state estimate and state error covariance of the system at time k; Step 7: Acquire the constant drift of the gyroscope and accelerometer of the MEMS inertial navigation according to the state estimation value obtained in the step 6, and feed it back to the combined system to output attitude, velocity and position information. 2. a kind of improved robust unscented Kalman filter integrated navigation method according to claim 1 is characterized in that the system state equation of described UWB/MEMS integrated navigation system state space model is expressed as: X _k+1 =F _k X _k +w _k Among them, X _k is the information error of the 15-dimensional inertial navigation solution, which is expressed as

Among them, φ _k =[φ _k,x φ _k,y φ _k,z ] ^T is the attitude error of inertial navigation solution at time k, ΔV _k =[ΔV _k,x ΔV _k,y ΔV _k,z ] ^T is Velocity error in geographic coordinate system calculated by inertial navigation at time k, ΔP _k =[ΔL _k Δλ _k Δh _k ] ^T is the latitude and longitude error calculated by inertial navigation at time k, ε _k =[ε _k,x ε _{k, y} ε _k,z ] ^T is the constant gyro drift at time k,

is the zero bias of the accelerometer at time k, w _k is the combined system process noise at time k, the covariance matrix of w _k noise is Q _k , and F _k is the state transition matrix of the inertial navigation system at time k. 3. a kind of improved robust unscented Kalman filter integrated navigation method according to claim 2 is characterized in that the system measurement equation of described UWB/MEMS integrated navigation system state space model is expressed as: Z _k =h(X′ _k )+V _k Among them, Z _k is the system measurement input, h(X′ _k ) is the system nonlinear measurement function, V _k is the combined system measurement noise, and the V _k noise covariance matrix is R _k , h(X′ _k ) and _Vk are respectively expressed as:

V _k =-2ρ _Ui,k ε+ε ² Among them, X′ _k includes the position errors Δx _k , Δy _k of the inertial navigation in the geocentric fixed coordinate system, where Δx _k , Δy _k and the position errors ΔL _k , Δλ of the inertial navigation in the geographic coordinate system in the state quantity X _k The relationship between _k and Δh _k is expressed as: Δx _k =-(R _n +h _k )cosλ _k sinL _k ΔL _k -(R _n +h _k )cosL _k sinλ _k Δλ _k +cosL _k cosλ _k Δh _k Δy _k =-(R _n +h _k )sinλ _k sinL _k ΔL _k +(R _n +h _k )cosL _k cosλ _k Δλ _k +cosL _k sinλ _k Δh _k Among them, e is the eccentricity of the earth, and R _n is the radius of the earth's meridian. 4. a kind of improved robust unscented Kalman filter combined navigation method according to claim 3 is characterized in that in described step 5, measurement error criterion is specifically expressed as: When λ _k > β, there is an error in the measurement; When λ _k < β, there is no measurement error; Among them, β is the threshold value, λ _k is the detection function, and λ _k obeys the χ ² distribution with m degrees of freedom, that is, λ _k ～χ ² (m), and λ _k is specifically expressed as:

Among them, v _k is the innovation sequence at time k of the system,

is the theoretical covariance of the innovation sequence v _k . 5. a kind of improved robust unscented Kalman filter combined navigation method according to claim 4 is characterized in that the robust matrix of multiple factors in the described step 5 is expressed as:

Among them, _Sk is the robust adjustment matrix of multiple factors, and s _k (i, i) represents the robust adjustment factor of the i-th observation. The calculation method of s _k (i, i) is as follows:

in,

is the value of the innovation theoretical covariance diagonal, and Y _k (i,i) is the innovation actual covariance diagonal value.

Images