CN102997921A - 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 filtering algorithm based on reverse navigation

Technical field

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

Background technology

Common inertia/GPS Integrated Navigation Algorithm is that the time series that measurement data consists of is carried out being processed by the real-time forward behind the forward direction, and POS (Position and Orientation System) has stored the metric data in the whole task process.For the afterwards processing procedure of POS without requirement of real-time and all metric data known characteristics all, can be undertaken by the afterwards processing before backward by the metric data of reverse navigation to the POS storage, thereby open up a new route for the precision that improves POS, expand simultaneously Kalman filter factor range of application.

Summary of the invention

The object of the present invention is to provide a kind of reverse inertia/GPS Integrated Navigation Algorithm, inertia, GPS metric data inverted sequence to the POS storage, in the inverted sequence process, simultaneously gyrostatic measured value is got opposite sign, carry out being resolved by the reverse navigation before backward, and utilize the Kalman filtering technique to realize inertia/GPS integrated navigation.

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

Oppositely the renewal equation of the attitude matrix of 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 the formula:

K represents the current time that calculates, real current time;

K-1 represents the previous moment calculated, real previous moment;

T is resolving the cycle of reverse inertial navigation system, unit: second, be constant after determining;

For carrier is that b is that navigation is n system, i.e. north day eastern geographic coordinate system, the attitude transition matrix of conversion, 3 * 3 dimensions;

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

V wherein _N, v _EBe respectively north orientation speed, the east orientation speed of reverse inertial navigation system, 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,$

Being respectively carrier is x axle gyro to measure value, y axle gyro to measure value, z axle gyro to measure value, unit: radian per second;

Expression

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 the formula:

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

Carrying out for the first time making the speed initial value reverse, namely when reverse navigation resolves 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,$

Being respectively carrier is x axis accelerometer measured value, y axis accelerometer measured value, z axis accelerometer measured 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 the formula:

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

Oppositely speed, position and the attitude error equation of inertial navigation system are:

In the 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, f wherein _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 Being respectively carrier is partially error, y axis accelerometer zero inclined to one side error, the zero partially error of z axis accelerometer of x axis accelerometer zero, 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,$ δ v wherein _N, δ v _U, δ v _EThe north orientation velocity error, vertical velocity error and the east orientation velocity error that represent respectively reverse inertial navigation system, 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

Being respectively the carrier that is processed into first-order Markov process is x axle gyroscopic drift error, y axle gyroscopic drift error, z axle gyroscopic drift error, unit: radian per second.

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

Simultaneously the present invention has provided the implementation procedure of reverse Kalman filtering, has expanded the range of application of Kalman filtering.

Embodiment

The present invention is described further below in conjunction with specific embodiment:

1) reverse inertial navigation and error equation

Oppositely the renewal equation of the attitude matrix of 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)$

(be with the difference of positive navigation: k calculates to k-1 from the moment)

In the formula:

K represents the current time that calculates, real current time;

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

T is resolving the cycle of reverse inertial navigation system, unit: second, be constant after determining;

For carrier is that b is that navigation is the attitude transition matrix that n system (north day eastern geographic coordinate system) transforms, 3 * 3 dimensions;

ω wherein _IeBe the earth rotation angular speed, unit: radian per second,

Be the latitude of reverse inertial navigation system, unit: radian; (be with the difference of positive navigation: sign)

V wherein _N, v _EBe respectively north orientation speed, the east orientation speed of reverse inertial navigation system, 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,$

Being respectively carrier is x axle gyro to measure value, y axle gyro to measure value, z axle gyro to measure value, unit: radian per second; (be with the difference of positive navigation: sign);

Expression

Antisymmetric matrix.

In the formula:

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

Carrying out for the first time making the speed initial value reverse, namely when reverse navigation resolves

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,$ Being respectively carrier is x axis accelerometer measured value, y axis accelerometer measured value, z axis accelerometer measured 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 the formula:

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

Oppositely speed, position and the attitude error equation of inertial navigation system are:

In the 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, f wherein _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 Being respectively carrier is partially error, y axis accelerometer zero inclined to one side error, the zero partially error of z axis accelerometer of x axis accelerometer zero, 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,$ δ v wherein _N, δ v _U, δ v _EThe north orientation velocity error, vertical velocity error and the east orientation velocity error that represent respectively reverse inertial navigation system, 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

Being respectively the carrier that is processed into first-order Markov process is x axle gyroscopic drift error, y axle gyroscopic drift error, z axle gyroscopic drift error, unit: radian per second;

2) oppositely Kalman filtering

In the present embodiment, according to system's formation of POS, the error equation that the while foundation is derived, the quantity of state of totally 15 reverse integrated navigation systems of error variance conduct such as chosen position error, velocity error, attitude error, gyroscopic drift error and Jia Biao zero inclined to one side errors etc.,

Choose position, speed that position, speed and the GPS of reverse inertial navigation provide 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 ^GBe 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 that provides, determine that its state equation is

In the formula:

A ^B(t) be 15 * 15 to maintain the system parameter matrix, calculate according to formula (2);

W ^B(t) be 15 * 1 to maintain system excitation noise vector.

Measurement equation is:

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

In the formula:

H ^BBe 6 * 15 dimension measurement matrixes;

V ^B(t) be the measurement noise vector.

Turn to state equation and measurement equation are discrete:

$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 State transitions battle array for system; T _dBe the filtering cycle of system, unit: second; N is the frequency of resolving of reverse inertial navigation system.

Oppositely the Filtering Model of Kalman wave 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 the formula:

Prediction square error battle array for inverse filter;

Estimation square error battle array for inverse filter;

System noise variance battle array for inverse filter;

Gain battle array for inverse filter;

Measuring noise square difference battle array for inverse filter.

Choose the state initial value

Estimate square error battle array initial value

System noise variance battle array initial value

And measuring noise square difference battle array initial value

After, adopt formula (8) to carry out reverse Kalman filtering.

3) output calibration

Adopt the estimate of error of reverse Kalman filtering

Reverse inertial navigation result is carried out output calibration.

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 the formula:

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

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

Be respectively the reverse Kalman filtering estimated value of latitude, longitude error, unit: radian;

Be the reverse Kalman filtering 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]$ Be the attitude error matrix,

Be respectively north orientation error angle, vertical error angle, east orientation error angle that reverse Kalman filtering is estimated, unit: radian;

Be the attitude matrix behind the output calibration, be used for calculating the attitude angle of reverse integrated navigation.

Estimate of error with filtering is proofreaied and correct reverse inertial navigation result, calculates reverse integrated navigation result.

Airborne Inertial (gyro zero is 0.03 °/h of stability partially, accelerometer bias stability 40 μ g), GPS (bearing accuracy meter level) test figure with certain model POS are example, further specify specific implementation process of the present invention.

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 filtering according to formula (8).When GPS week, be 20000.000 seconds second, the state vector of estimation was

According to formula (8), (9), adopt the estimate of error of inverse filtering that reverse inertial navigation result is carried out output calibration, the latitude that gets reverse integrated navigation is

Longitude is λ ^B=75.41156148 degree highly are h=11170.922 rice, and 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. Kalman filtering algorithm based on reverse navigation is a kind of the navigation information data to be carried out aftertreatment Kalman filtering algorithm, it is characterized in that: in filtering, use equation as described below:

Oppositely the renewal equation of the attitude matrix of 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 the formula:

K represents the current time that calculates, real current time;

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

T is resolving the cycle of reverse inertial navigation system, unit: second, be constant after determining;

For carrier is that b is that navigation is n system, i.e. north day eastern geographic coordinate system, the attitude transition matrix of conversion, 3 * 3 dimensions;

ω wherein _IeBe the earth rotation angular speed, unit: radian per second,

Be the latitude of reverse inertial navigation system, unit: radian;

V wherein _N, v _EBe respectively north orientation speed, the east orientation speed of reverse inertial navigation system, 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,$

Being respectively carrier is x axle gyro to measure value, y axle gyro to measure value, z axle gyro to measure value, unit: radian per second;

Expression

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 the formula:

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

Carrying out for the first time making the speed initial value reverse, namely when reverse navigation resolves 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,$ Being respectively carrier is x axis accelerometer measured value, y axis accelerometer measured value, z axis accelerometer measured 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 the formula:

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

Oppositely speed, position and the attitude error equation of inertial navigation system are:

In the 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, f wherein _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 Being respectively carrier is partially error, y axis accelerometer zero inclined to one side error, the zero partially error of z axis accelerometer of x axis accelerometer zero, 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,$ δ v wherein _N, δ v _U, δ v _EThe north orientation velocity error, vertical velocity error and the east orientation velocity error that represent respectively reverse inertial navigation system, 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

Being respectively the carrier that is processed into first-order Markov process is x axle gyroscopic drift error, y axle gyroscopic drift error, z axle gyroscopic drift error, unit: radian per second.