CN102997921B - Kalman filtering algorithm based on reverse navigation - Google Patents

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 kind of Kalman filter algorithm based on reverse navigation

Technical field

The present invention relates to a kind of oppositely Integrated Navigation Algorithm, particularly reverse inertia/GPS (GlobalPositioning System) Integrated Navigation Algorithm.

Background technology

Common inertia/GPS Integrated Navigation Algorithm carries out by the real-time forward process after forward direction the time series that measurement data is formed, and POS (Position and Orientation System) stores the metric data in whole task process.For the processing procedure afterwards of POS without requirement of real-time and all known feature of all metric data, carry out by backward front process afterwards by reverse navigation to the metric data that POS stores, thus be that the precision improving POS opens up a new route, expand Kalman filter coefficient range of application simultaneously.

Summary of the invention

The object of the present invention is to provide a kind of oppositely inertia/GPS Integrated Navigation Algorithm, to inertia, GPS metric data inverted sequence that POS stores, simultaneously to gyrostatic measured value negate number in inverted sequence process, carry out being resolved by backward front reverse navigation, and utilize Kalman filter technology to realize inertia/GPS integrated navigation.

The present invention is achieved in that a kind of Kalman filter algorithm based on reverse navigation, is that one carries out aftertreatment Kalman filter algorithm to navigation information data, wherein, in filtering, uses equation as described below:

The renewal equation of the attitude matrix of reverse inertial navigation system and speed, position is:

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

In formula:

K represents the current time of calculating, real current time;

K-1 represents the previous moment of calculating, real previous moment;

T be reverse inertial navigation system resolve the cycle, unit: second, for constant after determining;

for n system of navigation system of b system of carrier system, i.e. east, northern sky geographic coordinate system, the attitude transition matrix of conversion, 3 × 3 dimensions;

wherein ω _iefor earth rotation angular speed, unit: radian per second, for the latitude of reverse inertial navigation system, unit: radian;

wherein v _n, v _ebe respectively the north orientation speed of reverse inertial navigation system, east orientation speed, unit: meter per second, R _m, R _nbe respectively earth meridian circle, prime vertical radius, unit: rice, h is the height of reverse inertial navigation system, unit: rice;

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ω}}_{\mathrm{ibx}}^b& -{\mathrm{\ω}}_{\mathrm{iby}}^b& -{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T,$ be respectively carrier system x-axis gyro to measure value, y-axis gyro to measure value, z-axis gyro to measure value, unit: radian per second;

represent antisymmetric matrix;

$v_{\mathrm{enk}-1}^n=v_{\mathrm{enk}}^n-T[C_{\mathrm{bk}}^n{f}_{k-1}^b-(2{\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×v_{\mathrm{enk}}^n+g_k^n]---\left(1b\right)$

In formula:

$v_{\mathrm{en}}^n={\left[\begin{array}{ccc}{v}_N& v_U& v_E\end{array}\right]}^T,$ Wherein v _ufor the vertical velocity of reverse inertial navigation system, unit: meter per second;

When carrying out reverse navigation for the first time and resolving, make speed initial value reverse, namely this speed initial value is true last value of navigation procedure medium velocity;

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ be respectively carrier system x-axis acceleration measuring value, y-axis acceleration measuring value, z-axis acceleration measuring value, unit: meter per second ²;

G ⁿ=[0-g 0] ^t, wherein g is the acceleration of gravity of the earth, unit: meter per second ²;

In formula:

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

The speed of reverse inertial navigation system, position and attitude error equation are:

In formula:

δ λ is respectively latitude, the longitude error of reverse inertial navigation system, unit: radian;

δ h is the height error of reverse inertial navigation system, unit: rice;

F ⁿ=[f _nf _uf _e] ^t, wherein f _n, f _u, f _ebe respectively north orientation acceleration, vertical acceleration, east orientation acceleration, unit: meter per second ²;

$\mathrm{\δ}{f}^n=C_b^n{\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T,$ Wherein be respectively carrier system x-axis accelerometer bias error, y-axis accelerometer bias error, z-axis accelerometer bias error, unit: meter per second ²;

φ=[φ _nφ _uφ _e] ^t, wherein φ _n, φ _u, φ _ebe respectively the north orientation error angle of reverse inertial navigation system, vertical error angle, east orientation error angle, unit: radian;

$\mathrm{\δ}{v}_{\mathrm{en}}^n={\left[\begin{array}{ccc}\mathrm{\δ}{v}_N& \mathrm{\δ}{v}_U& {\mathrm{\δv}}_E\end{array}\right]}^T,$ Wherein δ v _n, δ v _u, δ v _erepresent the north orientation velocity error of reverse inertial navigation system, vertical velocity error and east orientation velocity error respectively, unit: meter per second;

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

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ϵ}}_x^b& -{\mathrm{\ϵ}}_y^b& -{\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ Wherein be respectively the carrier system x-axis gyroscopic drift error, y-axis gyroscopic drift error, the z-axis gyroscopic drift error that are processed into first-order Markov process, unit: radian per second.

Advantage of the present invention is the error equation of attitude matrix by setting up reverse inertial navigation and speed, the renewal equation of position and speed, position and attitude angle, provide the Filtering Model of reverse Kalman filter, and then realize the reverse integrated navigation of inertia/GPS information.This method provide a kind of new post processing mode, for the precision improving POS opens a new route.

The present invention gives the implementation procedure of reverse Kalman filter simultaneously, expand the range of application of Kalman filter.

Embodiment

Below in conjunction with specific embodiment, the present invention is described further:

1) reverse inertial navigation and error equation

The renewal equation of the attitude matrix of reverse inertial navigation system and speed, position is:

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

(being with the difference of positive navigation: calculate from moment k to k-1)

In formula:

K represents the current time of calculating, real current time;

K-1 represents the previous moment of calculating, real current time;

T be reverse inertial navigation system resolve the cycle, unit: second, for constant after determining;

for the attitude transition matrix that n system of navigation system of b system of carrier system (east, northern sky geographic coordinate system) transforms, 3 × 3 dimensions;

wherein ω _iefor earth rotation angular speed, unit: radian per second, for the latitude of reverse inertial navigation system, unit: radian; (being with the difference of positive navigation: sign)

wherein v _n, v _ebe respectively the north orientation speed of reverse inertial navigation system, east orientation speed, unit: meter per second, R _m, R _nbe respectively earth meridian circle, prime vertical radius, unit: rice, h is the height of reverse inertial navigation system, unit: rice;

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ω}}_{\mathrm{ibx}}^b& -{\mathrm{\ω}}_{\mathrm{iby}}^b& -{\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]}^T,$ be respectively carrier system x-axis gyro to measure value, y-axis gyro to measure value, z-axis gyro to measure value, unit: radian per second; (being with the difference of positive navigation: sign);

represent antisymmetric matrix.

In formula:

$v_{\mathrm{en}}^n={\left[\begin{array}{ccc}{v}_N& v_U& v_E\end{array}\right]}^T,$ Wherein v _ufor the vertical velocity of reverse inertial navigation system, unit: meter per second;

When carrying out reverse navigation for the first time and resolving, make speed initial value reverse, namely this speed initial value is true last value of navigation procedure medium velocity;

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ be respectively carrier system x-axis acceleration measuring value, y-axis acceleration measuring value, z-axis acceleration measuring value, unit: meter per second ²;

G ⁿ=[0-g 0] ^t, wherein g is the acceleration of gravity of the earth, unit: meter per second ².

In formula:

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

The speed of reverse inertial navigation system, position and attitude error equation are:

In formula:

δ λ is respectively latitude, the longitude error of reverse inertial navigation system, unit: radian;

δ h is the height error of reverse inertial navigation system, unit: rice;

F ⁿ=[f _nf _uf _e] ^t, wherein f _n, f _u, f _ebe respectively north orientation acceleration, vertical acceleration, east orientation acceleration, unit: meter per second ²;

$\mathrm{\δ}{f}^n=C_b^n{\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right]}^T,$ Wherein be respectively carrier system x-axis accelerometer bias error, y-axis accelerometer bias error, z-axis accelerometer bias error, unit: meter per second ²;

φ=[φ _nφ _uφ _e] ^t, wherein φ _n, φ _u, φ _ebe respectively the north orientation error angle of reverse inertial navigation system, vertical error angle, east orientation error angle, unit: radian;

$\mathrm{\δ}{v}_{\mathrm{en}}^n={\left[\begin{array}{ccc}\mathrm{\δ}{v}_N& \mathrm{\δ}{v}_U& {\mathrm{\δv}}_E\end{array}\right]}^T,$ Wherein δ v _n, δ v _u, δ v _erepresent the north orientation velocity error of reverse inertial navigation system, vertical velocity error and east orientation velocity error respectively, unit: meter per second;

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

$\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}-{\mathrm{\ϵ}}_x^b& -{\mathrm{\ϵ}}_y^b& -{\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ Wherein be respectively the carrier system x-axis gyroscopic drift error, y-axis gyroscopic drift error, the z-axis gyroscopic drift error that are processed into first-order Markov process, unit: radian per second;

2) reverse Kalman filter

In the present embodiment, according to the System's composition of POS, simultaneously according to the error equation of deriving, chosen position error, velocity error, attitude error, gyroscopic drift error and Jia Biao zero partially error etc. totally 15 error variances as the quantity of state of reverse integrated navigation system, choose position that the position of reverse inertial navigation, speed and GPS provide, speed is poor, as the observed quantity of POS, wherein subscript G represents the information that GPS provides; λ ^gbe respectively latitude, longitude that GPS provides, unit: radian; h ^gfor the height that GPS provides, unit: rice; be respectively north orientation speed, vertical velocity, east orientation speed that GPS provides, unit: meter per second.

According to the error equation of the reverse inertial navigation system provided, determine that its state equation is

In formula:

A ^bt () is 15 × 15 maintain system parameter matrix, calculate according to formula (2);

W ^b(t) be 15 × 1 maintain system excitation noise vector.

Measurement equation is:

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

In formula:

H ^bbe 6 × 15 dimension measurement matrixes;

V ^bt () is measurement noise vector.

By state equation with measurement equation is discrete turns to:

$X_{k-1}^B={\mathrm{\Φ}}_{k-1,k}^B{X}_k^B+W_k^B---\left(5\right)$

$Z_{k-1}^B=H^B{X}_{k-1}^B+V_{k-1}^B---\left(6\right)$

${\mathrm{\Φ}}_{k-1,k}^B=I+T_d\underset{i=N-1}{\overset{0}{\mathrm{\Σ}}}{A}_i^B---\left(7\right)$

Wherein for the state transfer matrix of system; T _dfor the filtering cycle of system, unit: second; N be reverse inertial navigation system resolve frequency.

The Filtering Model of reverse Kalman filter is:

${\hat{X}}_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{\hat{X}}_k^B---\left(8a\right)$

$P_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{P}_k^B{\left({\mathrm{\Φ}}_{k-1,k}^B\right)}^T+Q_k^B---\left(8b\right)$

$K_{k-1}^B=P_{k-1,k}^B{\left(H^B\right)}^T{[H^B{P}_{k-1,k}^B{\left(H^B\right)}^T+R_{k-1}^B]}^{-1}---\left(8c\right)$

${\hat{X}}_{k-1}^B={\hat{X}}_{k-1,k}^B+K_{k-1}^B(Z_{k-1}^B-H^B{\hat{X}}_{k-1,k}^B)---\left(8d\right)$

$P_{k-1}^B=(I-K_{k-1}^B{H}^B)P_{k-1,k}^B{(I-K_{k-1}^B{H}^B)}^T+K_{k-1}^B{R}_{k-1}^B{\left(K_{k-1}^B\right)}^T---\left(8e\right)$

In formula:

for the prediction mean squared error matrix of inverse filter;

for the estimation mean squared error matrix of inverse filter;

for the system noise variance matrix of inverse filter;

for the gain battle array of inverse filter;

for the measuring noise square difference battle array of inverse filter.

Choose state initial value estimate mean squared error matrix initial value system noise variance matrix initial value and measuring noise square difference battle array initial value after, adopt formula (8) to carry out reverse Kalman filter.

3) output calibration

Adopt the estimate of error of reverse Kalman filter output calibration is carried out to reverse inertial navigation result.

The output calibration equation of position is

${\mathrm{\λ}}_k^B={\mathrm{\λ}}_k-{\mathrm{\δ}\widehat{\mathrm{\λ}}}_{k/N}^B---\left(9b\right)$

$h_k^B=h_k-\mathrm{\δ}{\hat{h}}_{k/N}^B---\left(9c\right)$

In formula:

be respectively the latitude of reverse integrated navigation, longitude, unit: radian;

for the height of reverse integrated navigation, unit: rice;

be respectively the reverse Kalman filter estimated value of latitude, longitude error, unit: radian;

for the reverse Kalman filter estimated value of height error, unit: rice;

The output calibration equation of attitude matrix is

$C_{\mathrm{bk}}^{\hat{n}}=C_{\mathrm{nk}}^{\hat{n}}{C}_{\mathrm{bk}}^n---\left(10\right)$

$C_{\mathrm{nk}}^{\hat{n}}=\left[\begin{array}{ccc}1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B\\ {\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& 1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B\\ -{\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B& 1\end{array}\right]$ For attitude error matrix, be respectively the north orientation error angle of reverse Kalman filter estimation, vertical error angle, east orientation error angle, unit: radian;

for the attitude matrix after output calibration, for calculating the attitude angle of reverse integrated navigation.

With the estimate of error of filtering, reverse inertial navigation result is corrected, calculate reverse integrated navigation result.

Be example with the Airborne Inertial of certain model POS (gyro bias instaility 0.03 °/h, accelerometer bias stability 40 μ g), GPS (positioning precision meter level) test figure, further illustrate specific embodiment of the invention process.

Determine the estimation square error initial value of inverse filtering the initial variance battle array of system noise with measuring noise square difference battle array initial value after, carry out reverse Kalman filter according to formula (8).When GPS week, second was 20000.000 seconds, the state vector of estimation is according to formula (8), (9), adopt the estimate of error of inverse filtering to carry out output calibration to reverse inertial navigation result, the latitude obtaining reverse integrated navigation is longitude is λ ^b=75.41156148 degree, be highly h=11170.922 rice, roll angle is γ ^b=0.0209 degree, course angle is ψ ^b=19.2987 degree, the angle of pitch is θ ^b=-1.0891 degree.

Claims (1)

1., based on a Kalman filter algorithm for reverse navigation, be that one carries out aftertreatment Kalman filter algorithm to navigation information data, it is characterized in that: in filtering, use following equation:

The renewal equation of the attitude matrix of reverse inertial navigation system and speed, position is:

$C_{\mathrm{bk}-1}^n=C_{\mathrm{bk}}^n-{\mathrm{TC}}_{\mathrm{bk}}^n[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×---\left(1a\right)$

In formula:

K represents the current time of calculating, real current time;

K-1 represents the previous moment of calculating, real current time;

T be reverse inertial navigation system resolve the cycle, unit: second, for constant after determining;

for n system of navigation system of b system of carrier system, i.e. east, northern sky geographic coordinate system, the attitude transition matrix of conversion, 3 × 3 dimensions;

wherein ω _iefor earth rotation angular speed, unit: radian per second, for the latitude of reverse inertial navigation system, unit: radian;

wherein v _n, v _ebe respectively the north orientation speed of reverse inertial navigation system, east orientation speed, unit: meter per second, R _m, R _nbe respectively earth meridian circle, prime vertical radius, unit: rice, h is the height of reverse inertial navigation system, unit: rice;

be respectively carrier system x-axis gyro to measure value, y-axis gyro to measure value, z-axis gyro to measure value, unit: radian per second;

$[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]\×$ Represent $[{\mathrm{\ω}}_{\mathrm{ibk}}^b-{\left(C_{\mathrm{bk}}^n\right)}^T({\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)]$ Antisymmetric matrix;

$v_{\mathrm{enk}-1}^n=v_{\mathrm{enk}}^n-T[C_{\mathrm{bk}}^n{f}_{k-1}^b-({2\mathrm{\ω}}_{\mathrm{iek}}^n+{\mathrm{\ω}}_{\mathrm{enk}}^n)\×v_{\mathrm{enk}}^n+g_k^n]---|\left(1b\right)$

In formula:

wherein v _ufor the vertical velocity of reverse inertial navigation system, unit: meter per second;

When carrying out reverse navigation for the first time and resolving, make speed initial value reverse, namely this speed initial value is true last value of navigation procedure medium velocity;

be respectively carrier system x-axis acceleration measuring value, y-axis acceleration measuring value, z-axis acceleration measuring value, unit: meter per second ²;

G ⁿ=[0-g 0] ^t, wherein g is the acceleration of gravity of the earth, unit: meter per second ²;

In formula:

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

The speed of reverse inertial navigation system, position and attitude error equation are:

$\mathrm{\δ}{\stackrel{\·}{v}}_{\mathrm{en}}^n=f^n\×\mathrm{\φ}+{\mathrm{\δf}}^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δv}}_{\mathrm{en}}^n-({2\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×v_{\mathrm{en}}^n+{\mathrm{\δg}}^n---\left(2a\right)$

$\stackrel{\·}{\mathrm{\φ}}=({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\φ}-({\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)+C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(2c\right)$

δ λ is respectively latitude, the longitude error of reverse inertial navigation system, unit: radian;

δ h is the height error of reverse inertial navigation system, unit: rice;

F ⁿ=[f _nf _uf _e] ^t, wherein f _n, f _u, f _ebe respectively north orientation acceleration, vertical acceleration, east orientation acceleration, unit: meter per second ²;

wherein be respectively carrier system x-axis accelerometer bias error, y-axis accelerometer bias error, z-axis accelerometer bias error, unit: meter per second ²;

φ=[φ _nφ _uφ _e] ^t, wherein φ _n, φ _u, φ _ebe respectively the north orientation error angle of reverse inertial navigation system, vertical error angle, east orientation error angle, unit: radian;

wherein δ v _n, δ v _u, δ v _erepresent the north orientation velocity error of reverse inertial navigation system, vertical velocity error and east orientation velocity error respectively, unit: meter per second;

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

wherein be respectively the carrier system x-axis gyroscopic drift error, y-axis gyroscopic drift error, the z-axis gyroscopic drift error that are processed into first-order Markov process, unit: radian per second;

Then, reverse Kalman filter is carried out:

In this method, according to the System's composition of POS, simultaneously according to the error equation of deriving, choose comprise site error, velocity error, attitude error, gyroscopic drift error and Jia Biao zero error partially totally 15 error variances as the quantity of state of reverse integrated navigation system choose position that the position of reverse inertial navigation, speed and GPS provide, speed is poor, as the observed quantity of POS, wherein subscript G represents the information that GPS provides; λ ^gbe respectively latitude, longitude that GPS provides, unit: radian; h ^gfor the height that GPS provides, unit: rice; be respectively north orientation speed, vertical velocity, east orientation speed that GPS provides, unit: meter per second;

According to the error equation of the reverse inertial navigation system provided, determine that its state equation is

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

In formula:

A ^bt () is 15 × 15 maintain system parameter matrix, calculate according to formula (2);

W ^b(t) be 15 × 1 maintain system excitation noise vector;

Measurement equation is:

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

In formula:

H ^bbe 6 × 15 dimension measurement matrixes;

V ^bt () is measurement noise vector;

By state equation with measurement equation is discrete turns to:

$X_{k-1}^B={\mathrm{\Φ}}_{k-1,k}^B{X}_k^B+W_k^B---\left(5\right)$

$Z_{k-1}^B=H^B{X}_{k-1}^B+V_{k-1}^B---\left(6\right)$

${\mathrm{\Φ}}_{k-1,k}^B=I+T_d\underset{i-N1}{\overset{0}{\mathrm{\Σ}}}{A}_i^B---\left(7\right)$

Wherein for the state transfer matrix of system; T _dfor the filtering cycle of system, unit: second; N be reverse inertial navigation system resolve frequency;

The Filtering Model of reverse Kalman filter is:

${\hat{X}}_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{\hat{X}}_k^B---\left(8a\right)$

$P_{k-1,k}^B={\mathrm{\Φ}}_{k-1,k}^B{P}_k^B{\left({\mathrm{\Φ}}_{k-1,k}^B\right)}^T+Q_k^B---\left(8b\right)$

$K_{k-1}^B=P_{k-1,k}^B{\left(H^B\right)}^T{[H^B{P}_{k-1,k}^B{\left(H^B\right)}^T+R_{k-1}^B]}^{-1}---\left(8c\right)$

${\hat{X}}_{k-1}^B={\hat{X}}_{k-1,k}^B+K_{k-1}^B(Z_{k-1}^B-H^B{\hat{X}}_{k-1,k}^B)---\left(8d\right)$

$P_{k-1}^B=(I-K_{k-1}^B{H}^B)P_{k-1,k}^B{(I-K_{k-1}^B{H}^B)}^T+K_{k-1}^B{R}_{k-1}^B{\left(K_{k-1}^B\right)}^T---\left(8e\right)$

In formula:

for the prediction mean squared error matrix of inverse filter;

for the estimation mean squared error matrix of inverse filter;

for the system noise variance matrix of inverse filter;

for the gain battle array of inverse filter;

for the measuring noise square difference battle array of inverse filter;

Choose state initial value estimate mean squared error matrix initial value system noise variance matrix initial value and measuring noise square difference battle array initial value after, adopt formula (8) to carry out reverse Kalman filter;

Finally, output calibration is carried out:

Adopt the estimate of error of reverse Kalman filter output calibration is carried out to reverse inertial navigation result;

The output calibration equation of position is

${\mathrm{\λ}}_k^B={\mathrm{\λ}}_k-\mathrm{\δ}{\widehat{\mathrm{\λ}}}_{k/N}^B---\left(9b\right)$

$h_k^B=h_k-\mathrm{\δ}{\hat{h}}_{k/N}^B---\left(9c\right)$

In formula:

be respectively the latitude of reverse integrated navigation, longitude, unit: radian;

for the height of reverse integrated navigation, unit: rice;

be respectively the reverse Kalman filter estimated value of latitude, longitude error, unit: radian;

for the reverse Kalman filter estimated value of height error, unit: rice;

The output calibration equation of attitude matrix is

$C_{\mathrm{bk}}^{\hat{n}}=C_{\mathrm{nk}}^{\hat{n}}{C}_{\mathrm{bk}}^n---\left(10\right)$

$C_{\mathrm{nk}}^{\hat{n}}=\left[\begin{array}{ccc}1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B\\ {\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^B& 1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B\\ -{\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^B& {\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^B& 1\end{array}\right]$ For attitude error matrix, be respectively the north orientation error angle of reverse Kalman filter estimation, vertical error angle, east orientation error angle, unit: radian;

for the attitude matrix after output calibration, for calculating the attitude angle of reverse integrated navigation;

With the estimate of error of filtering, reverse inertial navigation result is corrected, calculate reverse integrated navigation result.