Method for improving relative course angle precision of SINS/DR integrated navigation system
阅读说明:本技术 一种提高sins/dr组合导航系统相对航向角精度的方法 (Method for improving relative course angle precision of SINS/DR integrated navigation system ) 是由 王万征 邓亮 裴兴凯 陈静 庄广琛 于 2018-08-14 设计创作,主要内容包括:本发明属于导航系统数据后处理技术,具体为一种提高SINS/DR组合导航系统相对航向角精度的方法,首先建立改进的SINS/DR组合导航滤波模型,其速度误差状态量为捷联惯导系统速度误差减去航位推算系统速度误差,进行导航解算和正向卡尔曼滤波之后,对RTS平滑器进行初始化,从后往前进行逆向平滑计算,通过改进滤波模型,一方面避免了系统矩阵中通过加速度计的值计算加速度的误差,另一方面避免由于惯导速度发散而导致的观测矩阵中相关项引起的误差。另外对于实时性要求不高或可以进行离线处理的应用场合,通过平滑后处理来提高SINS/DR组合导航系统航向角相对精度,从而提高相对位置测量精度。(The invention belongs to the navigation system data post-processing technology, in particular to a method for improving the relative course angle precision of an SINS/DR integrated navigation system. In addition, for the application occasions with low real-time requirement or offline processing, the relative accuracy of the course angle of the SINS/DR integrated navigation system is improved through smoothing post-processing, so that the measurement accuracy of the relative position is improved.)
1. A method for improving the relative course angle accuracy of an SINS/DR combined navigation system is characterized by comprising the following steps:
1) establishing improved SINS/DR combined navigation filtering model
The improved state transition matrix is as follows:
wherein M is1Is a velocity error factor matrix, M, in a velocity error differential equation2Is a matrix of velocity error factors, g, in an attitude error differential equationnIs a gravity vector under a geographic coordinate system,
the improved observation matrix is as follows:
H=[I2×202×9]
the speed error state quantity is the speed error of the strapdown inertial navigation system minus the speed error of the dead reckoning system, that is
2) Performing navigation solution and forward Kalman filtering
N forward direction at k-1, 2.. n.And a posteriori state estimation
3) Initializing RTS smoother
Wherein the content of the first and second substances,
4) RTS smoothing calculation
Starting from k to N-1, the inverse smooth calculation is carried out from back to front, and the calculation formula is as follows
In the formula (I), the compound is shown in the specification,for inverting the intermediate variables of the matrix, FkTransferring matrices for a system(derived from the state matrix A), KkFor smoothing the gain, PkTo smooth the error estimate covariance matrix,
Technical Field
The invention belongs to a navigation system data post-processing technology, and particularly relates to a method for improving the relative course angle precision of a navigation system.
Background
In an SINS/DR integrated navigation system, the position and speed obtained by dead reckoning with a speedometer are generally used as measurement values to perform integrated navigation with a strapdown inertial navigation system, and the attitude error of the strapdown inertial navigation system is estimated through kalman filtering, so that the inertial navigation attitude precision is maintained. By analyzing the observability degree of each error item of the SINS/DR combined navigation system, the horizontal attitude angle error can be observed, the course angle error angle can not be observed under the general condition, and the observability of the course misalignment angle can not be improved through line maneuvering, so that the course angle error is estimated.
Disclosure of Invention
The invention aims to provide a method for improving the relative course angle precision of an SINS/DR combined navigation system, which can improve the relative course precision and reduce the influence of course gyro drift, thereby improving the relative position measurement precision.
The technical scheme of the invention is as follows:
a method for improving the relative course angle accuracy of an SINS/DR combined navigation system comprises the following steps:
1) establishing improved SINS/DR combined navigation filtering model
The improved state transition matrix is as follows:
wherein M is1Is a velocity error factor matrix, M, in a velocity error differential equation2Is a matrix of velocity error factors, g, in an attitude error differential equationnIs a gravity vector under a geographic coordinate system,
is the rotation vector of the geographic coordinate system relative to the inertial coordinate system,is an attitude matrix;the improved observation matrix is as follows:
H=[I2×202×9]
the speed error state quantity is the speed error of the strapdown inertial navigation system minus the speed error of the dead reckoning system, that is
2) Performing navigation solution and forward Kalman filtering
N forward direction at k-1, 2.. n.
And a posteriori state estimationPrior covariance matrixAnd posterior covariance matrix3) Initializing RTS smoother
Wherein the content of the first and second substances,is an initial value of the smoother state quantity, PNFor the initial value of the covariance matrix of the smoother,is the state estimate for the last N moments of the kalman filter,
for Kalman filtering last N timeAn exact covariance matrix;4) RTS smoothing calculation
Starting from k to N-1, the inverse smooth calculation is carried out from back to front, and the calculation formula is as follows
In the formula (I), the compound is shown in the specification,
for inverting the intermediate variables of the matrix, FkFor the system transition matrix (derived from the state matrix A), KkFor smoothing the gain, PkTo smooth the error estimate covariance matrix,the state variable of the smoothing filter, the other quantities are the values saved for the forward filtering in step 2).The invention has the following remarkable effects: by improving the filtering model, on one hand, the error of acceleration calculation through the value of the accelerometer in the system matrix is avoided, and on the other hand, the error caused by related items in the observation matrix due to the divergence of the inertial navigation speed is avoided. In addition, for the application occasions with low real-time requirement or offline processing, the relative accuracy of the course angle of the SINS/DR integrated navigation system is improved through smoothing post-processing, so that the measurement accuracy of the relative position is improved.
Drawings
FIG. 1a is a schematic diagram of an attitude angle error of an original filtering model after Kalman filtering correction;
FIG. 1b is a schematic diagram of an attitude angle error of the improved post-filter model after Kalman filtering correction;
FIG. 2a is a schematic diagram of an attitude angle error of an original filter model after RTS smoothing correction;
FIG. 2b is a schematic diagram illustrating a comparison of the attitude angle error after RTS smoothing correction of the improved filtering model.
Detailed Description
The invention is described in further detail below with reference to the figures and the embodiments.
Step 1) establishing an improved SINS/DR combined navigation filtering model
According to the characteristics of a speed error differential equation in an SINS/DR combined navigation system, a system matrix and an observation matrix of the combined navigation system are improved, and the improved system matrix is set as shown in a formula (1):
wherein M is1Is a velocity error factor matrix, M, in a velocity error differential equation2Is a matrix of velocity error factors, g, in the attitude error differential equationnIs a gravity vector under a geographic coordinate system,
is the rotation vector of the geographic coordinate system relative to the inertial coordinate system,is a matrix of poses.The improved observation matrix is shown as formula (2):
H=[I2×202×9](2)
in the improved filtering model, the speed error state quantity of the system is no longer the speed error of the SINS
But rather thatStep 2) Forward Filtering
Normal navigation calculation and Kalman filtering are carried out, and in the calculation process, the prior state estimation of each moment is calculated and stored according to the k-1, 2
And a posteriori state estimationPrior covariance matrixAnd posterior covariance matrixStep 3) smoothing filter initialization
After the forward Kalman filtering calculation is finished, initializing the RTS smoother, wherein the initialization method is as follows:
wherein the content of the first and second substances,is an initial value of the smoother state quantity, PNFor the initial value of the covariance matrix of the smoother,
is the state estimate for the last N moments of the kalman filter,and (4) a covariance matrix of the last N moments of Kalman filtering.Step 4) RTS smoothing calculation
After initialization is completed, starting from k ═ N-1, reverse smoothing calculation is performed from back to front, and the calculation formula is as shown in formula (4):
in the formula (I), the compound is shown in the specification,
for inverting the intermediate variables of the matrix, FkFor the system transition matrix (derived from the state matrix A), KkFor smoothing the gain, PkTo smooth the error estimate covariance matrix,the state variable of the smoothing filter, the other quantities are the values saved for the forward filtering in step 2).The results of kalman filtering and smoothing with the improved filtering model and the results of filtering and smoothing with the common filtering model are shown in fig. 1a, 1b, 2a, and 2b, for example.
The relative accuracy of the attitude angle after the improved filter model is used for smoothing correction is superior to that of the original filter model, and the relative accuracy of the course angle after RTS smoothing is superior to that of Kalman filtering under the two filter models. After smoothing treatment is carried out under the improved filtering model, the relative error of the course angle in 2000s is not more than 0.002 degrees, and the relative precision of the course angle is greatly improved. By improving the relative accuracy of the course angle, the relative position accuracy of the track measurement can be improved.
- 上一篇:一种医用注射器针头装配设备
- 下一篇:一种空间完全非合作目标相对位姿和惯量估计方法