CN113916225B - A Gross Error Robust Estimation Method for Combined Navigation Based on Robust Weight Factor Coefficients - Google Patents

Abstract

The invention discloses a combined navigation coarse-difference robust estimation method based on a robust weight coefficient. Firstly, based on the traditional least square method criterion, different weight factors are given to different observation items, and the observation items are converted into an robust least square method estimation form; then, through the equivalent weight principle, different equivalent weight functions are established, and the observation value is optimized by adopting a type III weight factor function of the institute of measurement and geophysical study (Institute of Geodesy and Geophysics, IGG) of the Chinese academy of sciences; then, modifying a Kalman filtering equation based on a robust weight factor coefficient method, and selecting a proper weight function to replace an observed noise covariance matrix through analyzing a gain matrix so as to reduce or eliminate the influence of a coarse difference on an estimated structure; and then, based on the inertial navigation/satellite navigation combined navigation space-time reference matching model, establishing an optimal estimation model based on the combined navigation. The invention can solve the influence of the observed quantity rough difference on the navigation result in the integrated navigation system, thereby ensuring the application range and the positioning precision of the integrated navigation system.

Description

A Gross Error Robust Estimation Method for Combined Navigation Based on Robust Weight Factor Coefficients

technical field

The present invention relates to the field of enhanced robustness of integrated navigation systems, in particular to systems based on Global Navigation Satellite System (Global Navigation Satellite System, GNSS), Doppler Velocity Log (Doppler Velocity Log, DVL), etc. In the field of sensor-integrated navigation, a robust estimation method is proposed.

Background technique

As a hot research field, navigation has received extensive attention from various countries. Among them, factors such as simple structure, wide engineering application, and complementary sensor characteristics are the advantages of satellite navigation system/inertial navigation system (Inertial Navigation System, INS) integrated navigation, so it is widely used in national defense, civil and other fields. The advantage of the inertial navigation system is that it does not need to receive any external information and does not radiate any information. It can realize navigation in any environment based on the motion state of the carrier, and can obtain information such as attitude, speed, and position. However, the disadvantages Due to the limitation of inertial devices such as gyroscopes and accelerometers, long-term work is likely to cause the accumulation of positioning errors. Under a single navigation condition, this accumulation cannot be eliminated. The GNSS navigation system has high navigation accuracy, and the civilian positioning accuracy can reach within five meters, and does not accumulate positioning errors over time, but it is easily affected by obstructions, and the output frequency of GNSS information is lower than that of INS.

In order to realize combined navigation, the optimal estimation method is usually used. The optimal estimation method is aimed at the optimal estimation of different sensor noises, estimating and eliminating the systematic error from the perspective of the optimal probability and statistics. Kalman filtering is generally used in the design of integrated navigation systems. The characteristics of inertial navigation and GNSS are just complementary in principle and application, so using INS/GNSS as the subsystem in the integrated navigation design for navigation is the optimal solution.

Although Kalman filtering is a very effective method for processing dynamic data in satellite integrated navigation and positioning systems, the standard Kalman filtering algorithm cannot effectively deal with gross errors in the data. Methods such as M estimation and robust estimation are used in least squares estimation, and these methods can detect and compensate for errors. The robust Kalman filter can well resist the influence of gross errors, and can process data quickly, accurately, and effectively, and can also make forecasts for future data development trends. Gross errors are unavoidable in the measurement and calculation of satellite signals. Statistics show that gross errors account for about 1% to 10% of the total number of observations. The standard Kalman filter is applied to the classical estimation model, which is aimed at accidental errors. The existence of gross errors will inevitably affect the estimation results, and even a few large gross errors will cause major deviations in the results.

Therefore, in order to solve the combined navigation positioning problem affected by the gross error of observations, the present invention proposes a combined navigation gross error robust estimation method based on robust weight factor coefficients. This method is firstly based on the traditional least square method criterion, assigning different weight factors to different observation items, and transforming it into the form of robust least square method estimation; then, through the principle of equivalent weight, different equivalent weight functions are established. In the invention, the Institute of Geodesy and Geophysics (Institute of Geodesy and Geophysics, IGG) III method function is used to optimize the observed value; finally, the Kalman filter equation is transformed based on the robust weight factor coefficient method, and then by analyzing the Kalman filter gain matrix, it can be An appropriate weight function is selected to replace the observation noise covariance matrix, and the measurement noise matrix R is modified to reduce the impact on the optimal estimation of position positioning. The invention can solve the influence of gross errors of observations on navigation results in the integrated navigation system, thereby ensuring the application range and positioning accuracy of the integrated navigation system.

Contents of the invention

The purpose of the present invention is to provide a gross error robust estimation method that can be applied to an integrated navigation system when the positioning accuracy decreases under gross error interference. The technical solution for realizing the object of the present invention is: a method for estimating gross errors and robustness of integrated navigation based on robust weight factor coefficients, comprising the following steps:

Step 1: Construct a robust least squares estimation model using the principle of equivalent weights;

Step 2: Based on IGGIII, the observation value optimization of anti-difference filtering is realized;

Step 3: According to the principle of robust estimation equivalent weight, by analyzing the Kalman filter gain matrix, an appropriate weight function can be selected to replace the observation noise covariance matrix, and the measurement noise matrix R can be corrected to derive the robust generalized estimation of the Kalman filter Recursion equation.

Step 4: Based on the mechanical arrangement of the SINS, construct an error equation based on the SINS error.

Step 5: Build the Kalman filter state equation and measurement equation based on the strapdown inertial navigation system, where the error equation is the state quantity, and realize the robustness system design for gross errors.

In step 1, the principle of equivalent weight is used to construct the robust least squares estimation model, and the specific method is as follows:

Assuming a group of independent observations {z _i }, the weight of the observations is {P _i }, the error equation can be obtained as:

是自变量，V是残差。若按照抗差最小二乘准则∑ρ(v_i)＝min，v_i为观测量对应的残差向量，其解为：A is the linear parameter matrix, Z is the observation,

is the independent variable and V is the residual. If according to the robust least squares criterion ∑ρ(v _i )=min, and v _i is the residual vector corresponding to the observation, the solution is:

为等价权，P_i为原观测值的权，w_i称为权因子。In the formula,

is the equivalent weight, P _i is the weight of the original observation value, and w _i is called the weight factor.

In the second step, based on IGGIII, the observation value optimization of the anti-difference filter is realized, and the specific method is as follows:

为v_i的均方差，实际情况下，

为v_i的权倒数；方差因子σ₀可以根据/>

求得；k₀与k_i根据需要，可以取1.5～5.0。Among them, v _i is the residual vector corresponding to the observation;

is the mean square error of v _i , in actual cases,

is the reciprocal of the weight of v _i ; the variance factor σ ₀ can be calculated according to

Obtained; k ₀ and _ki can be 1.5 to 5.0 according to needs.

In the third step, according to the principle of equivalent weight of robust estimation, by analyzing the gain matrix, an appropriate weight function is selected to replace the observation noise covariance matrix, and the recursion equation of the robust generalized estimation of Kalman filtering is derived. The specific method is:

Calculate the information variance P _zz :

是量测一步预测值，/>

为观测噪声方差阵R_k的等价协方差阵，可以由等价权阵/>

求逆获得。Z is the actual measured value,

is the measure-one-step forecast value, />

is the equivalent covariance matrix of the observation noise variance matrix R _k , which can be obtained by the equivalent weight matrix />

Get the inverse.

Calculate filter gain

P _xz and P _zz are the prediction mean square error matrix and the covariance matrix between the state one-step prediction and the measurement one-step prediction respectively.

update error covariance

P(k+1/k) is the state one-step forecast mean square error covariance matrix.

update state estimate

是状态一步预测。

is the state-one-step forecast.

为等价权：

For equivalent rights:

In step five, build the error equation of the strapdown inertial navigation system based on the INS/GPS lever-arm error and time asynchronous error, and further build the Kalman filter state equation and measurement equation to realize the robust system design for gross errors. The specific method for:

和/>

分别为陀螺仪测量白噪声和加速度计测量白噪声，V_v和V_p分别为卫星接收机速度测量白噪声和位置测量白噪声，M为捷联导航参数矩阵，/>

为姿态转换矩阵。X is the state vector, F and G are the known system structure parameters, H is the measurement matrix, Z is the measurement vector, V is the measurement noise vector, W is the system noise vector; Accurate angle error, velocity error, position error, gyroscope zero bias, accelerometer zero bias, space lever arm error, time asynchronous error;

and />

are the white noise measured by the gyroscope and the white noise measured by the accelerometer, respectively, V _v and V _p are the white noise measured by the satellite receiver velocity and the white noise measured by the position, M is the strapdown navigation parameter matrix, />

is the attitude transformation matrix.

Compared with prior art, the beneficial effect of the present invention is:

The present invention further improves the robustness of the system when disturbed by gross errors or large errors based on the principle of structural equivalent weight and the optimal estimation principle of the IGGIII judgment function when the gross errors of combined navigation observations are obvious, The robust weight factor coefficient method is used to modify the Kalman filter equation, further analyze the gain matrix, adjust the measurement noise matrix R, reduce or eliminate the influence of gross errors on the estimation structure, and improve the positioning accuracy of the system in a disturbed environment.

Description of drawings

Fig. 1 carrier trajectory diagram;

Figure 2 shows the optimal estimated positioning results under different estimated sample sizes.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings.

In order to verify the effectiveness of the present invention, the method of the present invention is simulated using Matlab.

First, in the optimal estimation of integrated navigation, there is a group of independent observations {z _i }, and the weight of the observations is {P _i }, the error equation can be obtained as:

是自变量，V是残差。若按照抗差最小二乘准则∑ρ(v_i)＝min，v_i为观测量对应的残差向量，其解为：A is the linear parameter matrix, Z is the observation,

is the independent variable and V is the residual. If according to the robust least squares criterion ∑ρ(v _i )=min, and v _i is the residual vector corresponding to the observation, the solution is:

为等价权，P_i为原观测值的权，w_i称为权因子。In the formula,

is the equivalent weight, P _i is the weight of the original observation value, and w _i is called the weight factor.

The most critical part of the robust filtering is to select and use the most suitable ρ(v) or w _i . At present, the widely used methods include the Huber method, the IGGIII method, etc., among which the IGGIII method is a robust estimation method, which uses In the form of the multi-segment segmentation method, by adding a new bad data threshold, the measurement is divided into normal measurement, suspicious measurement, and bad data according to the residual, and the measurement is divided into three types through the two-segment threshold. To a certain extent, the residual submersion and pollution are alleviated. The specific method is as follows

为v_i的均方差，实际情况下，

为v_i的权倒数；方差因子σ₀可以根据/>

求得；k₀与k_i根据需要，可以取1.5～2.5、3.0～5.0。Among them, v _i is the residual vector corresponding to the observation;

is the mean square error of v _i , in actual cases,

is the reciprocal of the weight of v _i ; the variance factor σ ₀ can be calculated according to

Obtained; k ₀ and _ki can be 1.5-2.5, 3.0-5.0 according to needs.

都会受到粗差的影响。通过分析增益矩阵K_k，可以选取适当的权函数代替观测噪声协方差阵，进一步对R值进行修正，以减小或消除粗差对估计结构的影响。等价权选定之后，重新利用广义最小二乘原理，可导出滤波的稳健推广估计的递推方程，即抗差滤波递推方程如下：It can be obtained from the analysis that in the Kalman filter, when there is a gross error in the observed value Z,

will be affected by gross errors. By analyzing the gain matrix K _k , an appropriate weight function can be selected to replace the observation noise covariance matrix, and the R value can be further corrected to reduce or eliminate the influence of gross errors on the estimation structure. After the equivalent weight is selected, the generalized least squares principle can be used again to derive the recursive equation of the robust generalized estimation of the filter, that is, the recursive equation of the robust filter is as follows:

Calculate the information variance:

Calculate the filter gain:

Update the error covariance:

update status:

为等价权：

For equivalent rights:

为R_k的等价协方差阵，可以由等价权阵/>

求逆获得。In the formula,

is the equivalent covariance matrix of R _k , which can be obtained by the equivalent weight matrix />

Get the inverse.

Constructing the error equation of the strapdown inertial navigation system is an important step in establishing the Kalman filter equation. Usually, the error of the scale coefficient matrix is ignored, and the space lever arm error and time asynchronous error of GNSS should be considered at the same time. The specific steps are as follows:

是失准角误差，δv＝[δv_E δv_N δv_U]^T是速度误差，δp＝[δLδλ δh]^T是位置误差，δL是纬度误差，δλ是经度误差，δh是高度误差，R_Nh是卯酉圈主曲率半径，R_Mh是子午圈主曲率半径，ε是陀螺零偏，/>

是加速度计零偏，/>

是地球自转角速度在东北天方向上的分量，/>

是加速度计输出值，β是考虑了Heiskanen垂线偏差之后的修正系数。in,

is the misalignment angle error, δv=[δv _E δv _N δv _U ] ^T is the speed error, δp=[δLδλ δh] ^T is the position error, δL is the latitude error, δλ is the longitude error, δh is the height error, R _Nh is The main radius of curvature of the Maoyou circle, R _Mh is the main radius of curvature of the meridian circle, ε is the zero bias of the gyro, />

is the accelerometer zero bias, />

is the component of the earth's rotation angular velocity in the northeast direction, />

is the output value of the accelerometer, and β is the correction factor after considering the deviation of the Heiskanen vertical line.

It should be noted that in INS/GNSS navigation, the installation positions of inertial navigation components and GPS receivers will be different, which will cause errors when the integrated navigation computer calculates the information of inertial navigation; similarly, INS When calculating the current attitude, velocity and position information with the GPS sensor, they will be subject to different hysteresis effects. Therefore, the model proposed by this method takes such errors into account. The lever-arm error vector is:

δl ^b is the projection of the lever arm under the carrier system, and M _pv is the error transformation matrix, defined below. The time out-of-sync error vector is:

是INS解算的速度在导航系下的投影。δt is the time asynchronous error,

It is the projection of the speed calculated by INS in the navigation system.

Then the Kalman filter can be established

和/>

分别为陀螺仪测量白噪声和加速度计测量白噪声，V_v和V_p分别为卫星接收机速度测量白噪声和位置测量白噪声，M为捷联导航参数矩阵，下标表示的是前文所述误差方程中的不同量之间的联系，与误差方程中的量一一对应，此不赘述，/>

为姿态转换矩阵。The selection of the state quantity is the misalignment angle error, velocity error, position error of the carrier, the zero bias of the gyroscope, the zero bias of the accelerometer, the space lever arm error, and the time asynchronous error;

and />

are the white noise measured by the gyroscope and the white noise measured by the accelerometer, respectively, V _v and V _p are the white noise measured by the satellite receiver velocity and the white noise measured by the position, respectively, M is the strapdown navigation parameter matrix, and the subscripts represent the above-mentioned The relationship between different quantities in the error equation corresponds to the quantities in the error equation one by one, so I won’t go into details here, />

is the attitude transformation matrix.

In order to verify the effectiveness of the combined navigation gross error robust estimation method based on the robust weight factor coefficient proposed by the present invention, a simulation experiment is specially set up, and a comparative analysis is carried out with the navigation method without the robust error estimation method.

The simulation conditions are set as follows:

加速度计零偏为100μg，加速度计测量噪声为/>

东北天方向的初始姿态误差分别为[0.5′，-0.5′，20′]；东北天方向的初始速度误差分别为[0.1m/s，0.1m/s，0.1m/s]；东北天方向的初始位置误差分别为[1m，1m，0.1m]；卡尔曼滤波的初始值设置分别为，P、Q、R均为对角阵，P的对角元素为[davp；eb；db]²，分别为位置误差、陀螺零偏误差、加速度计零偏误差；Q的对角元素为[web；wdb；zeros(9,1)]²，分别为陀螺随机游走误差和加速度计随机游走误差；R的对角元素为初始位置误差。仿真实验设置了采样频率为10HZ，定位精度为10m的GNSS信号作为观测量，粗差出现的频率为2％利用GNSS相邻的3、4、5个采样周期作为估计样本，进行稳健估计。The latitude, longitude and altitude of the simulation experiment are 45.246048°N; 128.909664°E; 20m, the simulation time is 965s; the zero bias of the gyroscope is 0.03°/h, and the random walk is

The accelerometer zero bias is 100μg, and the accelerometer measurement noise is />

The initial attitude errors in the northeast direction are [0.5′, -0.5′, 20′]; the initial velocity errors in the northeast direction are [0.1m/s, 0.1m/s, 0.1m/s]; The initial position errors of are [1m, 1m, 0.1m]; the initial value settings of Kalman filter are respectively, P, Q, R are all diagonal arrays, and the diagonal elements of P are [davp; eb; db] ² , are position error, gyro zero bias error, and accelerometer zero bias error; the diagonal elements of Q are [web; wdb; zeros(9,1)] ² , which are gyro random walk error and accelerometer random walk Error; the diagonal element of R is the initial position error. In the simulation experiment, the GNSS signal with a sampling frequency of 10HZ and a positioning accuracy of 10m is set as the observation quantity, and the frequency of the gross error is 2%. The 3, 4, and 5 adjacent sampling periods of the GNSS are used as the estimation samples for robust estimation.

The carrier motion curve corresponding to the simulation experiment is shown in Figure 1, and the starting point is 45.246048°N; 128.909664°E. Figure 2 shows the position error curve obtained based on the simulated driving trajectory in Figure 1. It can be seen from Figure 1 that in order to ensure the universality of motion, we chose a trajectory that combines curved motion and linear motion. It can be seen from Figure 2 that the dot-dash line, realization, dotted line, and thick solid line respectively represent the unrobust estimation, the robust estimation with 3 estimated samples, the robust estimation with 4 estimated samples, and the robust estimation with 5 estimated samples. From the positioning results, it can be seen that the first 200s is the process of estimation convergence, so the position error is relatively large, but the positioning error is obviously suppressed after the robust estimation, which can better suppress the positioning error during the convergence process of the Kalman filter. After becoming stable, it can be seen that the positioning accuracy has been greatly improved. When affected by the gross error and there is no robust estimation, the standard deviation of the position error is 21.875m. When there are 3 estimated samples for robust estimation, The standard deviation of position error is 5.3563m. When there are 4 estimated samples for robust estimation, the standard deviation of position error is 3.3585m. When there are 5 estimated samples for robust estimation, the standard deviation of position error is 2.8555m , it can be seen that when more estimation samples are selected for robust estimation, better robustness effect can be produced. Therefore, compared with the Kalman filter integrated navigation method without robust estimation, the method based on the robust weight factor coefficient proposed by the present invention has faster convergence speed and better gross error suppression ability.

Claims (2)

1. The integrated navigation coarse difference tolerance estimation method based on the robust weight factor coefficients is characterized by comprising the following steps of:

step one: constructing robust least square estimation by adopting an equivalent weight principle;

step two: based on the measurement of the Chinese academy and the III method of the geophysical institute (Institute of Geodesy and Geophysics, IGG), the observation value optimization of the contrast filtering is realized;

step three: according to the principle of robust estimation of equivalent weights, a proper weight function can be selected to replace an observed noise covariance matrix by analyzing a Kalman filtering gain matrix, and a measured noise matrix R is corrected to derive a recurrence equation of robust popularization estimation of Kalman filtering;

step four: based on mechanical arrangement of the strapdown inertial navigation system, constructing an error equation based on the error of the strapdown inertial navigation system;

step five: constructing a Kalman filtering state equation and a measurement equation which are based on the strapdown inertial navigation system and take an error equation as a state quantity, and realizing the design of an robust system aiming at the gross error;

the second step of optimizing the observation value of the contrast filtering based on IGGIII comprises the following specific steps:

wherein v is _i A residual vector corresponding to the observed quantity;

v is _i Mean square error of (1), in practical cases, +.>

q _vi V is _i The inverse of the weights of (2); variance factor sigma ₀ Can be according to->

Obtaining; k (k) ₀ And k is equal to _i 1.5 to 2.5 and 3.0 to 5.0 can be taken according to the requirements;

according to the principle of robust estimation equivalent weight, the appropriate weight function can be selected to replace the observed noise covariance matrix by analyzing the Kalman filtering gain matrix, and the measured noise matrix R is corrected to derive a recursive equation of robust popularization estimation of Kalman filtering, and the specific method is as follows:

calculating information variance P _zz ：

Z is a true measurement value of the measurement,

is to measure one-step predictive value,/->

For observing noise variance matrix R _k Can be defined by an equivalent matrix of covariates>

Inversion is carried out to obtain;

calculating a filter gain

P _xz And P _zz Respectively predicting a mean square error matrix and a covariance matrix between one-step prediction of states and one-step prediction of measurement;

updating error covariance

P (k+1/k) is a state one-step predictive mean square error covariance matrix;

updating state estimates

Is one-step prediction of state;

is equivalent weight:

2. the method for estimating the gross error and the gross error of the integrated navigation based on the steady weight factor according to claim 1, wherein the method for constructing the robust least square estimation by adopting the equivalent weight principle in the step 1 is characterized in that the specific method comprises the following steps:

is provided with a group of mutually independent observation values { z } _i Weight of observed value { P } _i The error equation is found as:

a is a linear parameter matrix, Z is an observed quantity,

is an argument, V is the residual; if the sum of the values is based on the robust least squares criterion Σρ (v _i )＝min，v _i The solution for the residual vector corresponding to the observed quantity is:

in the method, in the process of the invention,

as equivalent weight, P _i Weight of original observed value, w _i Called weighting factors.

Images