CN102997921B

CN102997921B - Kalman filtering algorithm based on reverse navigation

Info

Publication number: CN102997921B
Application number: CN201110273048.6A
Authority: CN
Inventors: 尚克军; 李文耀; 周东灵; 张勤拓; 扈光锋; 刘辉; 周祖洋; 邱宏波; 刘峰
Original assignee: Beijing Automation Control Equipment Institute BACEI
Current assignee: Beijing Automation Control Equipment Institute BACEI
Priority date: 2011-09-15
Filing date: 2011-09-15
Publication date: 2015-02-25
Anticipated expiration: 2031-09-15
Also published as: CN102997921A

Abstract

The invention relates to a reverse combination navigation algorithm, and discloses a Kalman filtering algorithm based on reverse navigation. The invention aims at providing a reverse inertial/GPS combined navigation algorithm. According to the invention, inertia and GPS measurement data stored in POS are subjected to order reversing; reverse navigation solution from back to front is carried out; and inertial/GPS combined navigation is realized with a Kalman filtering technology. According to the invention, reverse inertial navigation attitude matrix and update equation of speed and position are established, and an error equation of speed, position and attitude angle is provided. On the basis, state amount and observation amount of reverse Kalman filter are selected, and a reverse Kalman filter filtering model is provided. After a back-to-front reverse processing of inertial/satellite data on a time series, reverse inertial/GPS combined navigation can be carried out by using the method provided by the invention. The method has the advantages that: a novel post treatment method is provided, a novel approach is provided for improving POS precision, and Kalman filter coefficient application range is expanded.

Description

A Kalman Filter Algorithm Based on Reverse Navigation

技术领域 technical field

本发明涉及一种反向组合导航算法，特别是反向惯性/GPS(GlobalPositioning System)组合导航算法。The invention relates to a reverse integrated navigation algorithm, in particular to a reverse inertia/GPS (Global Positioning System) integrated navigation algorithm.

背景技术 Background technique

通常的惯性/GPS组合导航算法是对测量数据构成的时间序列进行由前向后的实时正向处理，而POS(Position and Orientation System)存储了整个任务过程中的量测数据。针对POS的事后处理过程无实时性要求且所有量测数据都已知的特点，可通过反向导航对POS存储的量测数据进行由后向前的事后处理，从而为提高POS的精度开辟一条新路径，同时拓展Kalman滤波系数应用范围。The usual inertial/GPS integrated navigation algorithm is to perform real-time forward processing on the time series composed of measurement data from front to back, while POS (Position and Orientation System) stores the measurement data during the entire mission. In view of the fact that the POS post-processing process has no real-time requirements and all measurement data are known, the measurement data stored in the POS can be post-processed from the back to the front through reverse navigation, thereby opening up a new path for improving the accuracy of the POS. A new path, while expanding the application range of Kalman filter coefficients.

发明内容 Contents of the invention

本发明的目的在于提供一种反向惯性/GPS组合导航算法，对POS存储的惯性、GPS量测数据反序，在反序过程中同时对陀螺仪的测量值取反号，进行由后向前的反向导航解算，并利用Kalman滤波技术实现惯性/GPS组合导航。The purpose of the present invention is to provide a kind of reverse inertial/GPS combined navigation algorithm, to the inertia of POS storage, GPS measurement data reverse sequence, simultaneously to the measured value of gyroscope in the reverse sequence process, take the negative sign, carry out from backward The previous reverse navigation solution, and the use of Kalman filter technology to achieve inertial/GPS integrated navigation.

本发明是这样实现的：一种基于反向导航的Kalman滤波算法，是一种对导航信息数据进行后处理Kalman滤波算法，其中，在滤波过程中，使用如下所述的方程式：The present invention is achieved in that a kind of Kalman filtering algorithm based on reverse navigation is a kind of post-processing Kalman filtering algorithm to navigation information data, wherein, in the filtering process, use the following equation:

反向惯导系统的姿态矩阵和速度、位置的更新方程为：The update equations of the attitude matrix, velocity and position of the reverse inertial navigation system are:

${C C}_{bk bk - - 11}^{n no} = = {C C}_{bk bk}^{n no} - - {TC TC}_{bk bk}^{n no} [[{ω ω}_{ibk ibk}^{b b} - - {(({C C}_{bk bk}^{n no}))}^{T T} (({ω ω}_{iek iek}^{n no} + + {ω ω}_{enk enk}^{n no}))]] \times \times - - - - - - ((11 a a))$

式中：In the formula:

k表示计算的当前时刻，真实的当前时刻；k represents the current moment of calculation, the real current moment;

k-1表示计算的前一时刻，真实的前一时刻；k-1 means the previous moment of calculation, the real previous moment;

T为反向惯导系统的解算周期，单位：秒，确定后为常量；T is the solution period of the reverse inertial navigation system, unit: second, and it will be a constant after being determined;

为载体系b系向导航系n系，即北天东地理坐标系，转化的姿态转移矩阵，3×3维； It is the attitude transfer matrix transformed from the carrier system b to the navigation system n, that is, the Beitiandong geographical coordinate system, 3×3 dimensions;

其中ω_ie为地球自转角速率，单位：弧度/秒，为反向惯导系统的纬度，单位：弧度； Where ω _ie is the earth's rotation angular rate, unit: radian/second, is the latitude of the reverse inertial navigation system, unit: radian;

其中v_N、v_E分别为反向惯导系统的北向速度、东向速度，单位：米/秒，R_M、R_N分别为地球子午圈、卯酉圈半径，单位：米，h为反向惯导系统的高度，单位：米； Among them, v _N and v _E are the northward velocity and eastward velocity of the reverse inertial navigation system, unit: m/s, R _M , R _N are the radii of the earth's meridian circle and Maoyou circle respectively, unit: meter, and h is the inverse Height of the inertial navigation system, unit: meter;

$ω_{ib}^{b} = {[\begin{matrix} - ω_{ibx}^{b} & - ω_{iby}^{b} & - ω_{ibz}^{b} \end{matrix}]}^{T},$ 分别为载体系x轴陀螺测量值、y轴陀螺测量值、z轴陀螺测量值，单位：弧度/秒； $ω_{ib}^{b} = {[\begin{matrix} - ω_{ibx}^{b} & - ω_{iby}^{b} & - ω_{ibz}^{b} \end{matrix}]}^{T},$ They are the x-axis gyro measurement value, y-axis gyro measurement value, and z-axis gyro measurement value of the carrier system, unit: radian/second;

表示的反对称矩阵； express The antisymmetric matrix of ;

${v v}_{enk enk - - 11}^{n no} = = {v v}_{enk enk}^{n no} - - T T [[{C C}_{bk bk}^{n no} {f f}_{k k - - 11}^{b b} - - ((22 {ω ω}_{iek iek}^{n no} + + {ω ω}_{enk enk}^{n no})) \times \times {v v}_{enk enk}^{n no} + + {g g}_{k k}^{n no}]] - - - - - - ((11 b b))$

式中：In the formula:

$v_{en}^{n} = {[\begin{matrix} v_{N} & v_{U} & v_{E} \end{matrix}]}^{T},$ 其中v_U为反向惯导系统的垂向速度，单位：米/秒； $v_{en}^{no} = {[\begin{matrix} v_{N} & v_{u} & v_{E.} \end{matrix}]}^{T},$ Where v _U is the vertical velocity of the reverse inertial navigation system, unit: m/s;

在第一次进行反向导航解算的时候，使速度初值反向，即此速度初值为真实导航过程中速度最后一个值；When the reverse navigation solution is performed for the first time, the initial value of the velocity is reversed, that is, The initial value of this speed is the last value of the speed in the real navigation process;

$f^{b} = {[\begin{matrix} f_{x}^{b} & f_{y}^{b} & f_{z}^{b} \end{matrix}]}^{T},$ 分别为载体系x轴加速度计测量值、y轴加速度计测量值、z轴加速度计测量值，单位：米/秒²； $f^{b} = {[\begin{matrix} f_{x}^{b} & f_{the y}^{b} & f_{z}^{b} \end{matrix}]}^{T},$ They are the measured values of the x-axis accelerometer, the y-axis accelerometer, and the z-axis accelerometer of the carrier system, unit: m/ ^s2 ;

gⁿ＝[0-g 0]^T，其中g为地球的重力加速度，单位：米/秒²；g ⁿ =[0-g 0] ^T , where g is the gravitational acceleration of the earth, unit: m/s ² ;

式中：In the formula:

λ为反向惯导系统的经度，单位：弧度；λ is the longitude of the reverse inertial navigation system, unit: radian;

反向惯导系统的速度、位置和姿态角误差方程为：The speed, position and attitude angle error equations of the reverse inertial navigation system are:

式中：In the formula:

δλ分别为反向惯导系统的纬度、经度误差，单位：弧度； δλ are the latitude and longitude errors of the reverse inertial navigation system, unit: radian;

δh为反向惯导系统的高度误差，单位：米；δh is the altitude error of the reverse inertial navigation system, unit: meter;

fⁿ＝[f_N f_U f_E]^T，其中f_N、f_U、f_E分别为北向加速度、垂向加速度、东向加速度，单位：米/秒²；f ⁿ ＝[f _N f _U f _E ] ^T , where f _N , f _U , and f _E are northward acceleration, vertical acceleration, and eastward acceleration respectively, unit: m/ ^s2 ;

$δ f^{n} = C_{b}^{n} {[\begin{matrix} {&dtri;}_{x}^{b} & {&dtri;}_{y}^{b} & {&dtri;}_{z}^{b} \end{matrix}]}^{T},$ 其中分别为载体系x轴加速度计零偏误差、y轴加速度计零偏误差、z轴加速度计零偏误差，单位：米/秒²； $δ f^{no} = C_{b}^{no} {[\begin{matrix} {&dtri;}_{x}^{b} & {&dtri;}_{the y}^{b} & {&dtri;}_{z}^{b} \end{matrix}]}^{T},$ in Respectively, the zero-bias error of the x-axis accelerometer, the zero-bias error of the y-axis accelerometer, and the zero-bias error of the z-axis accelerometer of the carrier system, unit: m/ ^s2 ;

φ＝[φ_N φ_U φ_E]^T，其中φ_N、φ_U、φ_E分别为反向惯导系统的北向失调角、垂向失调角、东向失调角，单位：弧度；φ＝[φ _N φ _U φ _E ] ^T , where φ _N , φ _U , and φ _E are the northward misalignment angle, vertical misalignment angle, and eastward misalignment angle of the reverse inertial navigation system, unit: radian;

$δ v_{en}^{n} = {[\begin{matrix} δ v_{N} & δ v_{U} & {δv}_{E} \end{matrix}]}^{T},$ 其中δv_N、δv_U、δv_E分别表示反向惯导系统的北向速度误差、垂向速度误差和东向速度误差，单位：米/秒； $δ v_{en}^{no} = {[\begin{matrix} δ v_{N} & δ v_{u} & {δv}_{E.} \end{matrix}]}^{T},$ Among them, δv _N , δv _U , and δv _E respectively represent the northward speed error, vertical speed error and eastward speed error of the reverse inertial navigation system, unit: m/s;

δgⁿ＝[0 -δg 0]^T；δg ⁿ = [0 - δg 0] ^T ;

$δ ω_{ib}^{b} = {[\begin{matrix} - ϵ_{x}^{b} & - ϵ_{y}^{b} & - ϵ_{z}^{b} \end{matrix}]}^{T},$ 其中分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒。 $δ ω_{ib}^{b} = {[\begin{matrix} - ϵ_{x}^{b} & - ϵ_{the y}^{b} & - ϵ_{z}^{b} \end{matrix}]}^{T},$ in They are the x-axis gyro drift error, y-axis gyro drift error, and z-axis gyro drift error of the carrier system processed into a first-order Markov process, unit: radian/second.

本发明的优点是通过建立反向惯性导航的姿态矩阵和速度、位置的更新方程及速度、位置和姿态角的误差方程，给出反向Kalman滤波器的滤波模型，进而实现惯性/GPS信息的反向组合导航。此方法提供了一种新的后处理方式，为提高POS的精度开辟了一条新路径。The advantage of the present invention is that by setting up the attitude matrix of reverse inertial navigation and the update equation of speed, position and the error equation of speed, position and attitude angle, the filter model of reverse Kalman filter is given, and then realize the inertial/GPS information Reverse combined navigation. This method provides a new post-processing method and opens up a new path for improving the accuracy of POS.

同时本发明给出了反向Kalman滤波的实现过程，拓展了Kalman滤波的应用范围。At the same time, the invention provides the realization process of the reverse Kalman filter, which expands the application range of the Kalman filter.

具体实施方式 Detailed ways

下面结合具体实施例对本发明做进一步的说明：The present invention will be further described below in conjunction with specific embodiment:

1)反向惯性导航及误差方程1) Reverse inertial navigation and error equation

(与正向导航的区别在于：从时刻k到k-1计算)(The difference from forward navigation is: calculation from time k to k-1)

式中：In the formula:

k-1表示计算的前一时刻，真实的当前时刻；k-1 means the previous moment of calculation and the real current moment;

为载体系b系向导航系n系(北天东地理坐标系)转化的姿态转移矩阵，3×3维； is the attitude transfer matrix for transforming the carrier system b into the navigation system n (North Tiandong geographic coordinate system), 3×3 dimensions;

其中ω_ie为地球自转角速率，单位：弧度/秒，为反向惯导系统的纬度，单位：弧度；(与正向导航的区别在于：正负号) Where ω _ie is the earth's rotation angular rate, unit: radian/second, It is the latitude of reverse inertial navigation system, unit: radian; (the difference with forward navigation is: sign)

$ω_{ib}^{b} = {[\begin{matrix} - ω_{ibx}^{b} & - ω_{iby}^{b} & - ω_{ibz}^{b} \end{matrix}]}^{T},$ 分别为载体系x轴陀螺测量值、y轴陀螺测量值、z轴陀螺测量值，单位：弧度/秒；(与正向导航的区别在于：正负号)； $ω_{ib}^{b} = {[\begin{matrix} - ω_{ibx}^{b} & - ω_{iby}^{b} & - ω_{ibz}^{b} \end{matrix}]}^{T},$ They are the x-axis gyro measurement value, y-axis gyro measurement value, and z-axis gyro measurement value of the carrier system, unit: radian/second; (the difference from positive navigation is: sign);

表示的反对称矩阵。 express The antisymmetric matrix of .

式中：In the formula:

gⁿ＝[0-g 0]^T，其中g为地球的重力加速度，单位：米/秒²。g ⁿ =[0-g 0] ^T , where g is the gravitational acceleration of the earth, unit: m/s ² .

式中：In the formula:

λ为反向惯导系统的经度，单位：弧度。λ is the longitude of the reverse inertial navigation system, unit: radian.

式中：In the formula:

δgⁿ＝[0-δg 0]^T；δg ⁿ = [0-δg 0] ^T ;

$δ ω_{ib}^{b} = {[\begin{matrix} - ϵ_{x}^{b} & - ϵ_{y}^{b} & - ϵ_{z}^{b} \end{matrix}]}^{T},$ 其中分别为处理成一阶马尔可夫过程的载体系x轴陀螺漂移误差、y轴陀螺漂移误差、z轴陀螺漂移误差，单位：弧度/秒； $δ ω_{ib}^{b} = {[\begin{matrix} - ϵ_{x}^{b} & - ϵ_{the y}^{b} & - ϵ_{z}^{b} \end{matrix}]}^{T},$ in They are the x-axis gyro drift error, y-axis gyro drift error, and z-axis gyro drift error of the carrier system processed into a first-order Markov process, unit: radian/second;

2)反向Kalman滤波2) Inverse Kalman filtering

本实施例中，根据POS的系统构成，同时依据推导的误差方程，选取位置误差、速度误差、姿态角误差、陀螺漂移误差和加表零偏误差等共15个误差变量作为反向组合导航系统的状态量，选取反向惯性导航的位置、速度与GPS提供的位置、速度作差，作为POS的观测量，其中上标G表示GPS提供的信息；λ^G分别为GPS提供的纬度、经度，单位：弧度；h^G为GPS提供的高度，单位：米；分别为GPS提供的北向速度、垂向速度、东向速度，单位：米/秒。In this embodiment, according to the system configuration of POS and the derived error equation, a total of 15 error variables such as position error, velocity error, attitude angle error, gyroscope drift error, and table zero offset error are selected as the reverse integrated navigation system state quantity, Select the difference between the position and speed of reverse inertial navigation and the position and speed provided by GPS as the observation quantity of POS, The superscript G indicates the information provided by GPS; λ ^G is the latitude and longitude provided by GPS, unit: radian; h ^G is the altitude provided by GPS, unit: meter; Northward velocity, vertical velocity and eastward velocity respectively provided by GPS, unit: m/s.

根据给出的反向惯导系统的误差方程，确定其状态方程为According to the given error equation of the reverse inertial navigation system, its state equation is determined as

式中：In the formula:

A^B(t)为15×15维系统参数矩阵，根据式(2)计算；A ^B (t) is a 15×15-dimensional system parameter matrix, calculated according to formula (2);

W^B(t)为15×1维系统激励噪声向量。W ^B (t) is a 15×1-dimensional system excitation noise vector.

量测方程为：The measurement equation is:

Z^B＝H^BX^B(t)+V^B(t) (4)Z ^B ＝H ^B X ^B (t)+V ^B (t) (4)

式中：In the formula:

H^B为6×15维量测矩阵；H ^B is a 6×15-dimensional measurement matrix;

V^B(t)为量测噪声向量。V ^B (t) is the measurement noise vector.

将状态方程和量测方程离散化为：The state equation and measurement equation are discretized as:

${X x}_{k k - - 11}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {X x}_{k k}^{B B} + + {W W}_{k k}^{B B} - - - - - - ((55))$

${Z Z}_{k k - - 11}^{B B} = = {H h}^{B B} {X x}_{k k - - 11}^{B B} + + {V V}_{k k - - 11}^{B B} - - - - - - ((66))$

${Φ Φ}_{k k - - 11,, k k}^{B B} = = I I + + {T T}_{d d} {Σ Σ}_{i i = = N N - - 11}^{00} {A A}_{i i}^{B B} - - - - - - ((77))$

其中为系统的状态转移阵；T_d为系统的滤波周期，单位：秒；N为反向惯导系统的解算频率。in is the state transition matrix of the system; T _d is the filtering period of the system, unit: second; N is the calculation frequency of the reverse inertial navigation system.

反向Kalman滤波器的滤波模型为：The filtering model of the reverse Kalman filter is:

${\overset{^^}{X x}}_{k k - - 11,, k k}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {\overset{^^}{X x}}_{k k}^{B B} - - - - - - ((88 a a))$

${P P}_{k k - - 11,, k k}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {P P}_{k k}^{B B} {(({Φ Φ}_{k k - - 11,, k k}^{B B}))}^{T T} + + {Q Q}_{k k}^{B B} - - - - - - ((88 b b))$

${K K}_{k k - - 11}^{B B} = = {P P}_{k k - - 11,, k k}^{B B} {(({H h}^{B B}))}^{T T} {[[{H h}^{B B} {P P}_{k k - - 11,, k k}^{B B} {(({H h}^{B B}))}^{T T} + + {R R}_{k k - - 11}^{B B}]]}^{- - 11} - - - - - - ((88 c c))$

${\overset{^^}{X x}}_{k k - - 11}^{B B} = = {\overset{^^}{X x}}_{k k - - 11,, k k}^{B B} + + {K K}_{k k - - 11}^{B B} (({Z Z}_{k k - - 11}^{B B} - - {H h}^{B B} {\overset{^^}{X x}}_{k k - - 11,, k k}^{B B})) - - - - - - ((88 d d))$

${P P}_{k k - - 11}^{B B} = = ((I I - - {K K}_{k k - - 11}^{B B} {H h}^{B B})) {P P}_{k k - - 11,, k k}^{B B} {((I I - - {K K}_{k k - - 11}^{B B} {H h}^{B B}))}^{T T} + + {K K}_{k k - - 11}^{B B} {R R}_{k k - - 11}^{B B} {(({K K}_{k k - - 11}^{B B}))}^{T T} - - - - - - ((88 e e))$

式中：In the formula:

为反向滤波器的预测均方误差阵； is the predicted mean square error matrix of the inverse filter;

为反向滤波器的估计均方误差阵； is the estimated mean square error matrix of the inverse filter;

为反向滤波器的系统噪声方差阵； is the system noise variance matrix of the inverse filter;

为反向滤波器的增益阵； is the gain matrix of the inverse filter;

为反向滤波器的量测噪声方差阵。 is the measurement noise variance matrix of the inverse filter.

选取状态初始值估计均方误差阵初始值系统噪声方差阵初始值及量测噪声方差阵初始值后，采用公式(8)进行反向Kalman滤波。Select state initial value Estimate the initial value of the mean square error matrix The initial value of the system noise variance matrix and the initial value of the measurement noise variance matrix Afterwards, reverse Kalman filtering is performed using formula (8).

3)输出校正3) Output correction

采用反向Kalman滤波的误差估计量对反向惯性导航结果进行输出校正。Error Estimator Using Inverse Kalman Filtering Output correction for reverse inertial navigation results.

位置的输出校正方程为The output correction equation for position is

${λ λ}_{k k}^{B B} = = {λ λ}_{k k} - - {δ δ \overset{^^}{λ λ}}_{k k / / N N}^{B B} - - - - - - ((99 b b))$

${h h}_{k k}^{B B} = = {h h}_{k k} - - δ δ {\overset{^^}{h h}}_{k k / / N N}^{B B} - - - - - - ((99 c c))$

式中：In the formula:

分别为反向组合导航的纬度、经度，单位：弧度； Respectively, the latitude and longitude of reverse combined navigation, unit: radian;

为反向组合导航的高度，单位：米； It is the height of reverse combined navigation, unit: meter;

分别为纬度、经度误差的反向Kalman滤波估计值，单位：弧度； Respectively, the estimated value of reverse Kalman filter of latitude and longitude error, unit: radian;

为高度误差的反向Kalman滤波估计值，单位：米； is the reverse Kalman filter estimated value of height error, unit: meter;

姿态矩阵的输出校正方程为The output correction equation of the attitude matrix is

${C C}_{bk bk}^{\overset{^^}{n no}} = = {C C}_{nk nk}^{\overset{^^}{n no}} {C C}_{bk bk}^{n no} - - - - - - ((1010))$

$C_{nk}^{\hat{n}} = [\begin{matrix} 1 & - {\hat{φ}}_{Ek / N}^{B} & {\hat{φ}}_{Uk / N}^{B} \\ {\hat{φ}}_{Ek / N}^{B} & 1 & - {\hat{φ}}_{Nk / N}^{B} \\ - {\hat{φ}}_{Uk / N}^{B} & {\hat{φ}}_{Nk / N}^{B} & 1 \end{matrix}]$ 为姿态误差矩阵，分别为反向Kalman滤波估计的北向失调角、垂向失调角、东向失调角，单位：弧度； $C_{nk}^{\hat{no}} = [\begin{matrix} 1 & - {\hat{φ}}_{Ek / N}^{B} & {\hat{φ}}_{Uk / N}^{B} \\ {\hat{φ}}_{Ek / N}^{B} & 1 & - {\hat{φ}}_{Nk / N}^{B} \\ - {\hat{φ}}_{Uk / N}^{B} & {\hat{φ}}_{Nk / N}^{B} & 1 \end{matrix}]$ is the attitude error matrix, They are the northward misalignment angle, vertical misalignment angle, and eastward misalignment angle estimated by reverse Kalman filter, unit: radian;

为输出校正后的姿态矩阵，用于计算反向组合导航的姿态角。 To output the corrected attitude matrix, it is used to calculate the attitude angle of reverse integrated navigation.

用滤波的误差估计量对反向惯性导航结果进行校正，解算出反向组合导航结果。The reverse inertial navigation result is corrected with the filtered error estimate, and the reverse integrated navigation result is solved.

以某型号POS的机载惯性(陀螺零偏稳定性0.03°/h，加速度计零偏稳定性40μg)、GPS(定位精度米级)试验数据为例，进一步说明本发明的具体实施过程。Taking the airborne inertial (gyro bias stability 0.03°/h, accelerometer bias stability 40 μg) and GPS (positioning accuracy meter level) test data of a certain type of POS as an example, the specific implementation process of the present invention is further described.

确定反向滤波的估计均方误差初始值系统噪声的初始方差阵和量测噪声方差阵初始值后，根据公式(8)进行反向Kalman滤波。当GPS周秒为20000.000秒时，估计的状态向量为根据公式(8)、(9)，采用反向滤波的误差估计量对反向惯性导航结果进行输出校正，得反向组合导航的纬度为经度为λ^B＝75.41156148度，高度为h＝11170.922米，滚动角为γ^B＝0.0209度，航向角为ψ^B＝19.2987度，俯仰角为θ^B＝-1.0891度。Determining the initial value of estimated mean square error for inverse filtering The initial variance matrix of the system noise and the initial value of the measurement noise variance matrix Afterwards, inverse Kalman filtering is performed according to formula (8). When the GPS cycle second is 20000.000 seconds, the estimated state vector is According to the formulas (8) and (9), the reverse inertial navigation results are output and corrected by using the error estimator of the reverse filter, and the latitude of the reverse integrated navigation is obtained as The longitude is λ ^B =75.41156148 degrees, the height is h=11170.922 meters, the roll angle is γ ^B =0.0209 degrees, the heading angle is ψ ^B =19.2987 degrees, and the pitch angle is θ ^B =-1.0891 degrees.

Claims

1. A kind of Kalman filtering algorithm based on reverse navigation, is a kind of post-processing Kalman filtering algorithm to navigation information data, it is characterized in that: in filtering process, use following equation:

The update equations of the attitude matrix, velocity and position of the reverse inertial navigation system are:

{C C}_{bk bk - - 11}^{n no} = = {C C}_{bk bk}^{n no} - - {TC TC}_{bk bk}^{n no} [[{ω ω}_{ibk ibk}^{b b} - - {(({C C}_{bk bk}^{n no}))}^{T T} (({ω ω}_{iek iek}^{n no} + + {ω ω}_{enk enk}^{n no}))]] \times \times - - - - - - ((11 a a))

In the formula:

k represents the current moment of calculation, the real current moment;

k-1 means the previous moment of calculation and the real current moment;

T is the solution period of the reverse inertial navigation system, unit: second, and it will be a constant after being determined;

It is the attitude transfer matrix transformed from the carrier system b to the navigation system n, that is, the Beitiandong geographical coordinate system, 3×3 dimensions;

Where ω _ie is the earth's rotation angular rate, unit: radian/second, is the latitude of the reverse inertial navigation system, unit: radian;

Among them, v _N and v _E are the northward velocity and eastward velocity of the reverse inertial navigation system, unit: m/s, R _M , R _N are the radii of the earth's meridian circle and Maoyou circle respectively, unit: meter, and h is the inverse Height of the inertial navigation system, unit: meter;

They are the x-axis gyro measurement value, y-axis gyro measurement value, and z-axis gyro measurement value of the carrier system, unit: radian/second;

[ω_{ibk}^{b} - {(C_{bk}^{no})}^{T} (ω_{iek}^{no} + ω_{enk}^{no})] \times

express

[ω_{ibk}^{b} - {(C_{bk}^{no})}^{T} (ω_{iek}^{no} + ω_{enk}^{no})]

The antisymmetric matrix of ;

{v v}_{enk enk - - 11}^{n no} = = {v v}_{enk enk}^{n no} - - T T [[{C C}_{bk bk}^{n no} {f f}_{k k - - 11}^{b b} - - (({22 ω ω}_{iek iek}^{n no} + + {ω ω}_{enk enk}^{n no})) \times \times {v v}_{enk enk}^{n no} + + {g g}_{k k}^{n no}]] - - - - - - | | ((11 b b))

In the formula:

Where v _U is the vertical velocity of the reverse inertial navigation system, unit: m/s;

When the reverse navigation solution is performed for the first time, the initial value of the velocity is reversed, that is, The initial value of this speed is the last value of the speed in the real navigation process;

They are the measured values of the x-axis accelerometer, the y-axis accelerometer, and the z-axis accelerometer of the carrier system, unit: m/ ^s2 ;

g ⁿ =[0-g 0] ^T , where g is the gravitational acceleration of the earth, unit: m/s ² ;

In the formula:

λ is the longitude of the reverse inertial navigation system, unit: radian;

The speed, position and attitude angle error equations of the reverse inertial navigation system are:

δ δ {\overset{\cdot \cdot}{v v}}_{en en}^{n no} = = {f f}^{n no} \times \times φ φ + + {δf δ f}^{n no} - - (({22 ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times {δv δ v}_{en en}^{n no} - - (({22 δω δω}_{ie ie}^{n no} + + {δω δω}_{en en}^{n no})) \times \times {v v}_{en en}^{n no} + + {δg δg}^{n no} - - - - - - ((22 a a))

\overset{\cdot \cdot}{φ φ} = = (({ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times φ φ - - (({δω δω}_{ie ie}^{n no} + + {δω δω}_{en en}^{n no})) + + {C C}_{b b}^{n no} {δω δω}_{ib ib}^{b b} - - - - - - ((22 c c))

δλ are the latitude and longitude errors of the reverse inertial navigation system, unit: radian;

δh is the altitude error of the reverse inertial navigation system, unit: meter;

f ⁿ ＝[f _N f _U f _E ] ^T , where f _N , f _U , and f _E are northward acceleration, vertical acceleration, and eastward acceleration respectively, unit: m/ ^s2 ;

in Respectively, the zero-bias error of the x-axis accelerometer, the zero-bias error of the y-axis accelerometer, and the zero-bias error of the z-axis accelerometer of the carrier system, unit: m/ ^s2 ;

φ＝[φ _N φ _U φ _E ] ^T , where φ _N , φ _U , and φ _E are the northward misalignment angle, vertical misalignment angle, and eastward misalignment angle of the reverse inertial navigation system, unit: radian;

Among them, δv _N , δv _U , and δv _E respectively represent the northward speed error, vertical speed error and eastward speed error of the reverse inertial navigation system, unit: m/s;

δg ⁿ = [0 - δg 0] ^T ;

in They are the x-axis gyro drift error, y-axis gyro drift error, and z-axis gyro drift error of the carrier system processed into a first-order Markov process, unit: radian/second;

Then, reverse Kalman filtering:

In this method, according to the system composition of POS and the derived error equation, a total of 15 error variables including position error, velocity error, attitude angle error, gyro drift error, and table zero offset error are selected as the reverse integrated navigation system state quantity, Select the difference between the position and speed of reverse inertial navigation and the position and speed provided by GPS as the observation quantity of POS, The superscript G indicates the information provided by GPS; λ ^G is the latitude and longitude provided by GPS, unit: radian; h ^G is the altitude provided by GPS, unit: meter; Northward velocity, vertical velocity, and eastward velocity respectively provided by GPS, unit: m/s;

According to the given error equation of the reverse inertial navigation system, its state equation is determined as

X ^B (t) = A ^B (t) X ^B (t) + W ^B (t) (3)

In the formula:

A ^B (t) is a 15×15-dimensional system parameter matrix, calculated according to formula (2);

W ^B (t) is a 15×1-dimensional system excitation noise vector;

The measurement equation is:

Z ^B ＝H ^B X ^B (t)+V ^B (t) (4)

In the formula:

H ^B is a 6×15-dimensional measurement matrix;

V ^B (t) is the measurement noise vector;

The state equation and measurement equation are discretized as:

{X x}_{k k - - 11}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {X x}_{k k}^{B B} + + {W W}_{k k}^{B B} - - - - - - ((55))

{Z Z}_{k k - - 11}^{B B} = = {H h}^{B B} {X x}_{k k - - 11}^{B B} + + {V V}_{k k - - 11}^{B B} - - - - - - ((66))

{Φ Φ}_{k k - - 11,, k k}^{B B} = = I I + + {T T}_{d d} {Σ Σ}_{i i - - N N 11}^{00} {A A}_{i i}^{B B} - - - - - - ((77))

in is the state transition matrix of the system; T _d is the filtering period of the system, unit: second; N is the calculation frequency of the reverse inertial navigation system;

The filtering model of the reverse Kalman filter is:

{\overset{^^}{X x}}_{k k - - 11,, k k}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {\overset{^^}{X x}}_{k k}^{B B} - - - - - - ((88 a a))

{P P}_{k k - - 11,, k k}^{B B} = = {Φ Φ}_{k k - - 11,, k k}^{B B} {P P}_{k k}^{B B} {(({Φ Φ}_{k k - - 11,, k k}^{B B}))}^{T T} + + {Q Q}_{k k}^{B B} - - - - - - ((88 b b))

{K K}_{k k - - 11}^{B B} = = {P P}_{k k - - 11,, k k}^{B B} {(({H h}^{B B}))}^{T T} {[[{H h}^{B B} {P P}_{k k - - 11,, k k}^{B B} {(({H h}^{B B}))}^{T T} + + {R R}_{k k - - 11}^{B B}]]}^{- - 11} - - - - - - ((88 c c))

{\overset{^^}{X x}}_{k k - - 11}^{B B} = = {\overset{^^}{X x}}_{k k - - 11,, k k}^{B B} + + {K K}_{k k - - 11}^{B B} (({Z Z}_{k k - - 11}^{B B} - - {H h}^{B B} {\overset{^^}{X x}}_{k k - - 11,, k k}^{B B})) - - - - - - ((88 d d))

{P P}_{k k - - 11}^{B B} = = ((I I - - {K K}_{k k - - 11}^{B B} {H h}^{B B})) {P P}_{k k - - 11,, k k}^{B B} {((I I - - {K K}_{k k - - 11}^{B B} {H h}^{B B}))}^{T T} + + {K K}_{k k - - 11}^{B B} {R R}_{k k - - 11}^{B B} {(({K K}_{k k - - 11}^{B B}))}^{T T} - - - - - - ((88 e e))

In the formula:

is the predicted mean square error matrix of the inverse filter;

is the estimated mean square error matrix of the inverse filter;

is the system noise variance matrix of the inverse filter;

is the gain matrix of the inverse filter;

is the measurement noise variance matrix of the inverse filter;

Select state initial value Estimate the initial value of the mean square error matrix The initial value of the system noise variance matrix and the initial value of the measurement noise variance matrix Afterwards, adopt formula (8) to carry out inverse Kalman filtering;

Finally, output correction is performed:

Error Estimator Using Inverse Kalman Filtering Output correction for reverse inertial navigation results;

The output correction equation for position is

{λ λ}_{k k}^{B B} = = {λ λ}_{k k} - - δ δ {\overset{^^}{λ λ}}_{k k / / N N}^{B B} - - - - - - ((99 b b))

{h h}_{k k}^{B B} = = {h h}_{k k} - - δ δ {\overset{^^}{h h}}_{k k / / N N}^{B B} - - - - - - ((99 c c))

In the formula:

Respectively, the latitude and longitude of reverse combined navigation, unit: radian;

It is the height of reverse combined navigation, unit: meter;

Respectively, the estimated value of reverse Kalman filter of latitude and longitude error, unit: radian;

is the reverse Kalman filter estimated value of height error, unit: meter;

The output correction equation of the attitude matrix is

{C C}_{bk bk}^{\overset{^^}{n no}} = = {C C}_{nk nk}^{\overset{^^}{n no}} {C C}_{bk bk}^{n no} - - - - - - ((1010))

C_{nk}^{\hat{no}} = [\begin{matrix} 1 & - {\hat{φ}}_{Ek / N}^{B} & {\hat{φ}}_{Uk / N}^{B} \\ {\hat{φ}}_{Ek / N}^{B} & 1 & - {\hat{φ}}_{Nk / N}^{B} \\ - {\hat{φ}}_{Uk / N}^{B} & {\hat{φ}}_{Nk / N}^{B} & 1 \end{matrix}]

is the attitude error matrix, They are the northward misalignment angle, vertical misalignment angle, and eastward misalignment angle estimated by reverse Kalman filter, unit: radian;

To output the corrected attitude matrix, it is used to calculate the attitude angle of reverse integrated navigation;

The reverse inertial navigation result is corrected with the filtered error estimate, and the reverse integrated navigation result is solved.