CN102997915A - POS post-processing method with combination of closed-loop forward filtering and reverse smoothing - Google Patents

Abstract

The invention belongs to a POS post-processing method, and specifically relates to a POS post-processing method with the combination of closed-loop forward filtering and reverse-smoothing. The invention aims at providing a high-precision position and attitude post-processing method. The method comprises a Kalman filtering step and a reverse R-T-S smoothing step. Horizontal velocity error and height error estimated by forward Kalman filtering is fed back to an input end of an inertial navigation solver in real time through feedback controlling parameters; inertial velocity and position divergences are inhibited, and error model linearity is improved; and reverse R-T-S smoothing is carried out on the basis, such that a POS high-precision position and attitude post-processing result is obtained. The method has the advantages that: POS application requirements are satisfied; and high-precision position and attitude information is provided for a related image processing system (such as radar or a camera) and the like.

Description

A kind of closed loop forward filtering is in conjunction with reverse level and smooth POS post-processing approach

Technical field

The present invention relates to the high precision position attitude post-processing approach of a kind of POS of being applicable to (Position and Orientation System).

Background technology

POS has high requirement to the position in the whole task process and attitude accuracy, after particularly working long hours, increase very fast in the inertial navigation error short time, the error model of system is out of true gradually, the filter effect variation that causes system, the forward Kalman filtering of usually adopting can't address this problem with the post-processing approach that reverse R-T-S (Rauch-Tung-Striebel) smoothly combines.

Summary of the invention

The purpose of this invention is to provide a kind of high-precision position and attitude post-processing approach, by adopting the closed loop real-time feedback method, control parameter reasonable in design, the error oscillation amplitude of establishment system, improve the degree of accuracy of error model, thereby improve position and attitude accuracy in the whole task process of POS.

The present invention is achieved in that a kind of closed loop forward filtering in conjunction with reverse level and smooth POS post-processing approach, is the method for the position and attitude data of POS being carried out aftertreatment, comprising: the step of Kalman filtering, and the reverse level and smooth step of R-T-S, wherein,

In the process of Kalman filtering, the process of setting up Kalman filter model is as follows:

Step 1, use second order model are described inertial navigation system;

Design inertial navigation system horizontal channel becomes typical second-order system;

${{\▿}_1}^{\′}\left(s\right)=s^2+K_{1}s+(K_2+\frac{1}{R_e})g_0---\left(1\right)$

In the formula: K ₁, K ₂Two real-time feedback control coefficients of horizontal channel;

Design inertial navigation system vertical passage becomes typical second-order system;

${{\▿}_2}^{\′}\left(s\right)=s^2+C_{1}s+(C_2-\frac{2g_0}{R_e})---\left(2\right)$

In the formula: C ₁, C ₂Two real-time feedback control coefficients of vertical passage;

The secular equation of typical case's second-order system is

$\▿\left(s\right)=s^2+2\mathrm{\ξ}{w}_{n}s+w_n^2---\left(3\right)$

According to POS system afterwards processing procedure ξ, w are determined in the requirement of filtering regulating cycle and overshoot _nThen calculate according to the following procedure feedback control coefficient;

1, determines filtering regulating cycle and overshoot according to the performance requirement of system;

2, according to regulating cycle and overshoot, determine ξ, w _n

3, respectively formula (1), (2) are carried out namely obtaining formula (4), (5) with the coefficient contrast with formula (3), resolve to get feedback control coefficient K according to formula (4), (5) ₁, K ₂, C ₁, C ₂

K ₁＝2ξw _n

$K_2+\frac{1}{R_e}=w_n^2---\left(4\right)$

C ₁＝2ξw _n

$C_2-\frac{{2g}_0}{R_e}=w_n^2---\left(5\right)$

Step 2, set up the error equation of POS:

In the formula:

v _N, v _U, v _EBe respectively north orientation speed, vertical velocity, the east orientation speed of inertial navigation system, unit: meter per second was the amount in a upper moment;

δ v _N, δ v _U, δ v _EBe respectively north orientation velocity error, vertical velocity error, the east orientation velocity error of inertial navigation system, unit: meter per second was the amount in a upper moment;

Be the latitude of inertial navigation system, unit: radian was the amount in a upper moment;

Be the latitude error of inertial navigation system, unit: radian was the amount in a upper moment;

H is the height of inertial navigation system, unit: rice was the amount in a upper moment;

δ h is the height error of inertial navigation system, unit: rice was the amount in a upper moment;

φ _U, φ _EBe respectively vertical error angle, the east orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

f _U, f _EBe respectively vertical acceleration, east orientation acceleration, unit: meter per second ², be the amount of current time;

Being respectively carrier is partially error, y axis accelerometer zero inclined to one side error, Z axis accelerometer bias error of x axis accelerometer zero, unit: meter per second ², be the amount of current time;

C _1j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

R _M, R _NBe respectively earth meridian circle, prime vertical radius, unit: rice was the amount in a upper moment;

ω _IeBe the earth rotation angular speed, unit: radian per second is constant;

Be system's fast error in north that Kalman filtering is estimated, unit: meter per second is the amount of current time;

K ₁Real-time feedback control coefficient for the horizontal channel is calculated by said process;

In the formula:

f _NBe the north orientation acceleration, unit: meter per second ², be the amount of current time;

φ _NBe the north orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

C _2j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be the system height error that Kalman filtering is estimated, unit: rice is the amount of current time;

C ₂Real-time feedback control coefficient for vertical passage is calculated by said process;

In the formula:

C _3j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be system's fast error in east that Kalman filtering is estimated, unit: meter per second is the amount of current time;

In the formula:

C ₁Real-time feedback control coefficient for vertical passage is calculated by said process;

(6g)

In the formula:

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;

(6h)

(6i)

Step 3, set up Kalman filter model;

Get state variable

Observed quantity 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; λ is the longitude of inertial navigation system, unit: radian;

According to the error equation that provides, determine that the state equation of system is

In the formula:

A (t) 15 * 15 maintains the system parameter matrix, calculates according to formula (6);

L (t) is 15 * 3 dimension control coefrficient matrixes, L (t)=[0 _{3 * 2}L _{3 * 7}0 _{3 * 6}] ^T, wherein $L_{3\×7}=\left[\begin{array}{ccccccc}-C_1& 0& -C_2& 0& 0& 0& 0\\ 0& -K_1& 0& 0& 0& 0& -K_2\\ 0& 0& 0& -K_1& K_2& 0& 0\end{array}\right];$

$U\left(t\right)={\left[\begin{array}{ccc}\mathrm{\δ}\hat{h}\left(t\right)& \mathrm{\δ}{\hat{v}}_N\left(t\right)& \mathrm{\δ}{\hat{v}}_E\left(t\right)\end{array}\right]}^T$ Be system's control vector;

W (t) 15 * 1 maintains system excitation noise vector;

Measurement equation is:

Z＝HX(t)+V(t)

(8)

In the formula:

H is 6 * 15 dimension measurement matrixes;

V (t) is the measurement noise vector;

Turn to state equation and measurement equation are discrete:

X _k＝Φ _k，k-1X _k-1+L _kU _k-1+W _k-1 (9)

Z _k＝HX _k+V _k (10)

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

Φ wherein _{K, k-1}State transitions battle array for system; T _dBe the filtering cycle of system, unit: second; N is the frequency of resolving of inertial navigation system;

Kalman filter Filtering Model with controlled quentity controlled variable is:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+L_{k-1}{U}_{k-1}$

(12a)

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

(12b)

K _k＝P _k，k-1H ^T(HP _k，k-1H ^T+R _k) ^-1

(12c)

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H{\hat{X}}_{k,k-1})$

(12d)

P _k＝(I-K _kH)P _k，k-1(I-K _kH) ^T+K _kR _k(K _k) ^T

(12e)

In the formula:

P _{K, k-1}Prediction square error battle array for wave filter;

P _kEstimation square error battle array for wave filter;

Q _kSystem noise variance battle array for wave filter;

K _kGain battle array for wave filter;

R _kMeasuring noise square difference battle array for wave filter.

The present invention uses in POS system work: closed loop real-time feedback control parameter model; Horizontal velocity error and height error that forward Kalman filtering is estimated, the input end that resolves to inertial navigation by the feedback control parameters Real-time Feedback, and then suppress dispersing of velocity inertial and position, improve the linearity of error model, and it is level and smooth on this basis the filtering estimator to be carried out reverse R-T-S, thereby obtains the high precision position attitude aftertreatment result of POS.

Advantage is: introduce closed loop Real-time Feedback (damping is arranged) in filtering, can suppress dispersing of velocity inertial and position, improve the degree of accuracy of error model.After the closed loop Real-Time Filtering, it is level and smooth that the filtering estimator is carried out reverse R-T-S, adopt the level and smooth estimate of error of reverse R-T-S that the inertial navigation result is carried out output calibration, not only can improve in the whole Filtering and smoothing process estimated accuracy to error, and adopt high-precision smoothing error estimator that the inertial navigation result is carried out output calibration, can obtain high-precision position and attitude aftertreatment result, satisfy the application demand of POS, provide high-precision position and attitude information to associated picture disposal system (for example comprising radar, perhaps camera) etc.

Embodiment

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

Inertia, the gps measurement data of certain model POS are used said method carry out aftertreatment.

The closed loop Real-time Feedback is divided into level and vertical two passages, and the specific implementation process was divided into for two steps:

1) determines feedback control parameters.

The secular equation of conventional inertial navigation system horizontal channel is

(vibration can occur), wherein R _e=6378137 meters is the earth radius under the WGS84 coordinate system, g ₀=9.780318 meter per seconds ²Gravitational constant for the earth.It is designed to typical second-order system

${{\▿}_1}^{\′}\left(s\right)=s^2+K_{1}s+(K_2+\frac{1}{R_e})g_0$ (this is the non-oscillatory system)

(1)

In the formula: K ₁, K ₂Two real-time feedback control coefficients of horizontal channel.

The secular equation of inertial navigation system vertical passage is

It is designed to typical second-order system

${{\▿}_2}^{\′}\left(s\right)=s^2+C_{1}s+(C_2-\frac{2g_0}{R_e})---\left(2\right)$

In the formula: C ₁, C ₂Two real-time feedback control coefficients of vertical passage.

The secular equation of typical case's second-order system is

$\▿\left(s\right)=s^2+2\mathrm{\ξ}{w}_{n}s+w_n^2$

(3)

According to POS system afterwards processing procedure ξ, w are determined in the requirement of filtering regulating cycle and overshoot _n

Then, resolve in accordance with the following steps feedback control coefficient:

1, determines filtering regulating cycle and overshoot, with System Dependent, set respectively;

2, according to regulating cycle and overshoot, determine ξ, w _n, (common practise);

3, respectively formula (1), (2) are carried out namely obtaining formula (4), (5) with the coefficient contrast with formula (3), resolve to get feedback control coefficient K according to formula (4), (5) ₁, K ₂, C ₁, C ₂

Calculating K ₁, K ₂

K ₁＝2ξw _n

$K_2+\frac{1}{R_e}=w_n^2---\left(4\right)$

C ₁＝2ξw _n

$C_2-\frac{{2g}_0}{R_e}=w_n^2---\left(5\right)$

2) realize close-loop feedback

It is poor that position, the speed that inertia and GPS (Global Positioning System) are provided is done, as observed quantity.

Adopt Kalman filtering to realize inertia/GPS combination, the margin of error of system is estimated.

System's fast error in north with the filtering estimation The fast error in east

Pass through K ₁Feed back to horizontal velocity resolve in, pass through K ₂Feed back to the horizontal attitude angle resolve in (detailed process is as described below);

System height error with Kalman filtering estimation

Pass through C ₁And C ₂Feed back to respectively resolving of inertia vertical velocity, height, realize closed loop Real-time Feedback (detailed process is as described below);

Above-mentioned thinking is specifically by following process implementation:

The closed loop forward filtering is as follows in conjunction with oppositely level and smooth POS post-processing approach implementation procedure:

According to the above-mentioned close-loop feedback scheme that provides, the error equation that can set up POS is as follows:

In the formula:

v _N, v _U, v _EBe respectively north orientation speed, vertical velocity, the east orientation speed of inertial navigation system, unit: meter per second was the amount in a upper moment;

δ v _N, δ v _U, δ v _EBe respectively north orientation velocity error, vertical velocity error, the east orientation velocity error of inertial navigation system, unit: meter per second was the amount in a upper moment;

Be the latitude of inertial navigation system, unit: radian was the amount in a upper moment;

Be the latitude error of inertial navigation system, unit: radian was the amount in a upper moment;

H is the height of inertial navigation system, unit: rice was the amount in a upper moment;

δ h is the height error of inertial navigation system, unit: rice was the amount in a upper moment;

φ _U, φ _EBe respectively vertical error angle, the east orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

f _U, f _EBe respectively vertical acceleration, east orientation acceleration, unit: meter per second ², be the amount of current time;

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 ², be the amount of current time;

C _1j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

R _M, R _NBe respectively earth meridian circle, prime vertical radius, unit: rice was the amount in a upper moment;

ω _IeBe the earth rotation angular speed, unit: radian per second is constant;

Be system's fast error in north that Kalman filtering is estimated, unit: meter per second is the amount of current time;

K ₁Being the real-time feedback control coefficient of horizontal channel, is constant after determining.Compared with prior art, added K ₁Item.

In the formula:

f _NBe the north orientation acceleration, unit: meter per second ², be the amount of current time;

φ _NBe the north orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

C _2j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be the system height error that Kalman filtering is estimated, unit: rice is the amount of current time;

C ₂Being the real-time feedback control coefficient of vertical passage, is constant after determining.

In the formula:

C _3j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be system's fast error in east that Kalman filtering is estimated, unit: meter per second is the amount of current time;

In the formula:

C ₁Being the real-time feedback control coefficient of vertical passage, is constant after determining.

(6g)

In the formula:

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.

(6h)

(6i)

According to error equation, set up Kalman filter equation:

Get state variable

Observed quantity 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; λ is the longitude of inertial navigation system, unit: radian.

According to the error equation that provides, determine that the state equation of system is

(with respect to the undamped state, many item of L (t) U (t)) (7)

In the formula:

A (t) 15 * 15 maintains the system parameter matrix, calculates according to formula (6);

L (t) is 15 * 3 dimension control coefrficient matrixes, L (t)=[0 _{3 * 2}L _{3 * 7}0 _{3 * 6}] ^T, wherein $L_{3\×7}=\left[\begin{array}{ccccccc}-C_1& 0& -C_2& 0& 0& 0& 0\\ 0& -K_1& 0& 0& 0& 0& -K_2\\ 0& 0& 0& -K_1& K_2& 0& 0\end{array}\right];$

$U\left(t\right)={\left[\begin{array}{ccc}\mathrm{\δ}\hat{h}\left(t\right)& \mathrm{\δ}{\hat{v}}_N\left(t\right)& \mathrm{\δ}{\hat{v}}_E\left(t\right)\end{array}\right]}^T$ Be system's control vector;

W (t) 15 * 1 maintains system excitation noise vector.

Measurement equation is:

Z＝HX(t)+V(t) (8)

In the formula:

H is 6 * 15 dimension measurement matrixes;

V (t) is the measurement noise vector.

Turn to state equation and measurement equation are discrete:

X _k＝Φ _k，k-1X _k-1+L _kU _k-1+W _k-1 (9)

Z _k＝HX _k+V _k (10)

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

Φ wherein _{K, k-1}State transitions battle array for system; T _dBe the filtering cycle of system, unit: second; N is the frequency of resolving of inertial navigation system.

Kalman filter Filtering Model with controlled quentity controlled variable is:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+L_{k-1}{U}_{k-1}---\left(12a\right)$

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}---\left(12b\right)$

K _k＝P _k，k-1H ^T(HP _k，k-1H ^T+R _k) ^-1 (12c)

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H{\hat{X}}_{k,k-1})---\left(12d\right)$

P _k＝(I-K _kH)P _k，k-1(I-K _kH) ^T+K _kR _k(K _k) ^T (12e)

In the formula:

P _{K, k-1}Prediction square error battle array for wave filter;

P _kEstimation square error battle array for wave filter;

Q _kSystem noise variance battle array for wave filter;

K _kGain battle array for wave filter;

R _kMeasuring noise square difference battle array for wave filter.

2) oppositely R-T-S is level and smooth

Oppositely the level and smooth model of R-T-S is

$K_{k/N}^S=P_k{\mathrm{\Φ}}_{k+1,k}^T{\left(P_{k+1,k}\right)}^{-1}---\left(13a\right)$

${\hat{X}}_{k/N}^S={\hat{X}}_k+K_{k/N}^S({\hat{X}}_{k+1/N}^S-{\hat{X}}_{k+1,k})---\left(13b\right)$

$P_{k/N}^S=P_k+K_{k/N}^S(P_{k+1/N}^S-P_{k+1,k}){\left(K_{k/N}^S\right)}^T---\left(13c\right)$

In the formula:

Subscript S represents the R-T-S smoothing process;

Be level and smooth gain battle array;

Be level and smooth state vector;

Be level and smooth estimation variance battle array.

In the forward processing procedure of 0 → n-hour, determine the state initial value

Estimate square error battle array initial value P ₀, system noise variance battle array initial value Q ₀And measuring noise square difference battle array initial value R ₀After, adopt formula (12) to carry out forward filtering, store simultaneously the Φ in the filtering _{K, k-1},

P _{K, k-1},

P _kInitial value is set just according to known method commonly used, does not have particular determination.

In the inverse process constantly of N → 0, make k=N,

To oppositely smoothly carrying out initialization, it is level and smooth to adopt formula (13) to carry out reverse R-T-S.

3) output calibration

Adopt the level and smooth estimate of error of reverse R-T-S

The inertial navigation result is carried out output calibration.The output calibration equation of position is

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

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

In the formula:

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

Be the height of integrated navigation, unit: rice;

Anti-Wei latitude, the level and smooth estimated value of reverse R-T-S of longitude error, unit: radian;

Be the level and smooth estimated value of reverse R-T-S 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(15\right)$

$C_{\mathrm{nk}}^{\hat{n}}=\left[\begin{array}{ccc}1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^S& {\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^S\\ {\widehat{\mathrm{\φ}}}_{\mathrm{Ek}/N}^S& 1& -{\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^S\\ -{\widehat{\mathrm{\φ}}}_{\mathrm{Uk}/N}^S& {\widehat{\mathrm{\φ}}}_{\mathrm{Nk}/N}^S& 1\end{array}\right]$ Be the attitude error matrix,

Be respectively level and smooth north orientation error angle, vertical error angle, the east orientation error angle of estimating of reverse R-T-S, unit: radian;

Be the attitude matrix behind the output calibration, be used for the attitude angle of calculation combination navigation.

More concrete, in the said process, wherein the implementation procedure of closed loop Real-time Feedback is:

Get ratio of damping Oscillation frequency

According to formula (4), (5) respectively with formula (1), (2) and modular system $\▿\left(s\right)=s^2+\frac{\sqrt{2}\mathrm{\π}}{30}s+{\left(\frac{\mathrm{\π}}{30}\right)}^2$ Carry out with the coefficient contrast, namely

$K_1=\frac{\sqrt{2}\mathrm{\π}}{30}$ (4)

$K_2+\frac{1}{R_e}={\left(\frac{\mathrm{\π}}{30}\right)}^2$

C ₁＝2ξw _n

(5)

$C_2-\frac{{2g}_0}{R_e}=w_n^2$

Calculate the control parameter K ₁=0.1481, K ₂=0.0011, C ₁=0.1481, C ₂=0.0109.The Kalman filtering with controlled quentity controlled variable according to formula (12) provides carries out forward filtering, stores simultaneously the Φ that filtering estimates _{K, k-1},

P _{K, k-1},

P _kWhen GPS week, be 19500.000 seconds second, the inertia measurement value under the navigation coordinate system was: north gyro measured value ω _N=0.00565658 radian per second, vertical gyro to measure value ω _U=-0.00171431 radian per second, east orientation gyro to measure value ω _E=-0.00000911 radian per second, north orientation acceleration measuring value f _N=-0.243338 meter per second ², vertical acceleration instrumentation value f _U=9.872432 meter per seconds ², east orientation acceleration measuring value f _E=-0.095566 meter per second ²The partial status estimator that filtering is calculated is: (illustrate:

The system state that expression Kalman filtering is estimated, 1st element that be listed as capable to flow control i),

Then the filtering estimator be multiply by control coefrficient K ₁, K ₂, C ₁, C ₂After feed back to the input end that resolves of inertia measurement value and elevation, namely

ω _U=-0.00171431 radian per second,

Carry out navigation calculation again, namely realized with the filtering estimator inertial navigation result being carried out the process of real-time feedback control, the latitude that this moment, inertial navigation resolved is

Longitude is λ=75.09655065 degree, highly is h=11174.5415 rice, and roll angle is γ=0.1334 degree, and course angle is ψ=19.2063 degree, and the angle of pitch is θ=1.4596 degree.

After whole filtering finishes, adopt the final estimator of filtering to after oppositely smoothly carrying out initialization, it is level and smooth to carry out reverse R-T-S.When calculating to such an extent that GPS week, be 19200.000 seconds second, the latitude state estimator is X ^S(1,1)=0.48282718 degree (illustrates: X ^SThe level and smooth system state of estimating of (i, 1) expression R-T-S, 1st element that be listed as capable to flow control i), longitude state estimator X ^S(2,1)=-0.57648990 degree, height state estimator X ^S(3,1)=-3.4812 meter, roll angle state estimator X ^S(7,1)=0.0133 degree, course angle state estimator X ^S(8,1)=-0.0117 degree, angle of pitch state estimator X ^S(9,1)=0.0066 degree.Adopt these estimate of errors that the inertial navigation result is carried out output calibration, solve the integrated navigation latitude and be

Longitude is λ ^F=75.09671079 degree highly are h ^F=11178.023 meters, roll angle is γ ^F=0.1440 degree, course angle is ψ ^F=19.1945 degree, the angle of pitch is θ ^F=1.4700 degree.

Claims (1)

1. a closed loop forward filtering is the method for the position and attitude data of POS being carried out aftertreatment in conjunction with reverse level and smooth POS post-processing approach, comprising: the step of Kalman filtering, and the reverse level and smooth step of R-T-S is characterized in that:

In the process of Kalman filtering, the process of setting up Kalman filter model is as follows:

Step 1, use second order model are described inertial navigation system;

Design inertial navigation system horizontal channel becomes typical second-order system;

${{\▿}_1}^{\′}\left(s\right)=s^2+K_{1}s+(K_2+\frac{1}{R_e})g_0---\left(1\right)$

In the formula: K ₁, K ₂Two real-time feedback control coefficients of horizontal channel;

Design inertial navigation system vertical passage becomes typical second-order system;

${{\▿}_2}^{\′}\left(s\right)=s^2+C_{1}s+(C_2-\frac{2g_0}{R_e})---\left(2\right)$

In the formula: C ₁, C ₂Two real-time feedback control coefficients of vertical passage;

The secular equation of typical case's second-order system is

$\▿\left(s\right)=s^2+2\mathrm{\ξ}{w}_{n}s+w_n^2---\left(3\right)$

According to POS system afterwards processing procedure ξ, w are determined in the requirement of filtering regulating cycle and overshoot _nThen calculate according to the following procedure feedback control coefficient;

1, determines filtering regulating cycle and overshoot according to the performance requirement of system;

2, according to regulating cycle and overshoot, determine ξ, w _n

3, respectively formula (1), (2) are carried out namely obtaining formula (4), (5) with the coefficient contrast with formula (3), resolve to get feedback control coefficient K according to formula (4), (5) ₁, K ₂, C ₁, C ₂

K ₁＝2ξw _n

(4)

$K_2+\frac{1}{R_e}=w_n^2$

C ₁＝2ξw _n

(5)

$C_2-\frac{{2g}_0}{R_e}=w_n^2$

Step 2, set up the error equation of POS:

In the formula:

v _N, v _U, v _EBe respectively north orientation speed, vertical velocity, the east orientation speed of inertial navigation system, unit: meter per second was the amount in a upper moment;

δ v _N, δ v _U, δ v _EBe respectively north orientation velocity error, vertical velocity error, the east orientation velocity error of inertial navigation system, unit: meter per second was the amount in a upper moment;

Be the latitude of inertial navigation system, unit: radian was the amount in a upper moment;

Be the latitude error of inertial navigation system, unit: radian was the amount in a upper moment;

H is the height of inertial navigation system, unit: rice was the amount in a upper moment;

δ h is the height error of inertial navigation system, unit: rice was the amount in a upper moment;

φ _U, φ _EBe respectively vertical error angle, the east orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

f _U, f _EBe respectively vertical acceleration, east orientation acceleration, unit: meter per second ², be the amount of current time;

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 ², be the amount of current time;

C _1j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

R _M, R _NBe respectively earth meridian circle, prime vertical radius, unit: rice was the amount in a upper moment;

ω _IeBe the earth rotation angular speed, unit: radian per second is constant;

Be system's fast error in north that Kalman filtering is estimated, unit: meter per second is the amount of current time;

K ₁Real-time feedback control coefficient for the horizontal channel is calculated by said process;

In the formula:

f _NBe the north orientation acceleration, unit: meter per second ², be the amount of current time;

φ _NBe the north orientation error angle of inertial navigation system, unit: radian was the amount in a upper moment;

C _2j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be the system height error that Kalman filtering is estimated, unit: rice is the amount of current time;

C ₂Real-time feedback control coefficient for vertical passage is calculated by said process;

In the formula:

C _3j(j=1,2,3) are respectively the element of attitude matrix, are the amount in a upper moment;

Be system's fast error in east that Kalman filtering is estimated, unit: meter per second is the amount of current time;

In the formula:

C ₁Real-time feedback control coefficient for vertical passage is calculated by said process;

(6g)

In the formula:

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;

(6h)

(6i)

Step 3, set up Kalman filter model;

Get state variable

Observed quantity

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; λ is the longitude of inertial navigation system, unit: radian;

According to the error equation that provides, determine that the state equation of system is

In the formula:

A (t) 15 * 15 maintains the system parameter matrix, calculates according to formula (6);

L (t) is 15 * 3 dimension control coefrficient matrixes, L (t)=[0 _{3 * 2}L _{3 * 7}0 _{3 * 6}] ^T, wherein $L_{3\×7}=\left[\begin{array}{ccccccc}-C_1& 0& -C_2& 0& 0& 0& 0\\ 0& -K_1& 0& 0& 0& 0& -K_2\\ 0& 0& 0& -K_1& K_2& 0& 0\end{array}\right];$

$U\left(t\right)={\left[\begin{array}{ccc}\mathrm{\δ}\hat{h}\left(t\right)& \mathrm{\δ}{\hat{v}}_N\left(t\right)& \mathrm{\δ}{\hat{v}}_E\left(t\right)\end{array}\right]}^T$ Be system's control vector;

W (t) 15 * 1 maintains system excitation noise vector;

Measurement equation is:

Z＝HX(t)+V(t)

(8)

In the formula:

H is 6 * 15 dimension measurement matrixes;

V (t) is the measurement noise vector;

Turn to state equation and measurement equation are discrete:

X _k＝Φ _k，k-1X _k-1+L _kU _k-1+W _k-1 (9)

Z _k＝HX _k+V _k (10)

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

Φ wherein _{K, k-1}State transitions battle array for system; T _dBe the filtering cycle of system, unit: second; N is the frequency of resolving of inertial navigation system;

Kalman filter Filtering Model with controlled quentity controlled variable is:

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+L_{k-1}{U}_{k-1}$

(12a)

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

(12b)

K _k＝P _k，k-1H ^T(HP _k，k-1H ^T+R _k) ^-1

(12c)

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H{\hat{X}}_{k,k-1})$

(12d)

P _k＝(I-K _kH)P _k，k-1(I-K _kH) ^T+K _kR _k(K _k) ^T

(12e)

In the formula:

P _{K, k-1}Prediction square error battle array for wave filter;

P _kEstimation square error battle array for wave filter;

Q _kSystem noise variance battle array for wave filter;

K _kGain battle array for wave filter;

R _kMeasuring noise square difference battle array for wave filter.