CN106595705A - GPS-based flight inertial initial reference error estimation method - Google Patents

Abstract

The invention provides a GPS-based flight inertial initial reference error estimation method. According to an inertial navigation error transmission mechanism, an inertial initial reference error model is built, and an inertial initial reference error is estimated through a standard Kalman filter method based on GPS information as observed quantity. Mathematical modelling technique and carrying flight test results show that the method provided by the invention can effectively estimate the inertial reference error so that the accuracy and reliability of the navigation system are improved.

Description

A kind of in-flight inertia initial baseline bias estimation method based on GPS

Technical field

The present invention relates to Guidance ＆ Navigation technical field, more particularly to a kind of in-flight inertia initial baseline based on GPS Bias estimation method.

Background technology

Some guided missiles adopt direct laying of fire or Transfer Alignment, can produce larger initial baseline deviation, can so cause used Property navigation error accelerated accumulation, it is therefore necessary to take measures estimate initial baseline precision with suppress inertial navigation error diverging speed Degree.

The content of the invention

It is an object of the invention to overcome the deficiencies in the prior art, there is provided a kind of in-flight inertia based on GPS is initial Datum drift method of estimation, the method set up inertia initial baseline buggy model according to inertial navigation error propagation mechanism, utilize GPS information estimates inertia initial baseline deviation using standard Kalman filtering method as observed quantity.

The above-mentioned purpose of the present invention is realized by below scheme：

A kind of in-flight inertia initial baseline bias estimation method based on GPS, comprises the following steps：

(1), in moment k, calculate state vector predictive value State vector for moment k-1 is estimated Value, the initial value of the state vector estimated valueIt is set as null vector；Wherein, in Kalman filtering, definition status vector X=[φ_x0 φ_y0 φ_z0 δX₀ δY₀ δZ₀ δV_x0 δV_y0 δV_z0]^T；φ_x0、φ_y0、φ_z0Respectively guided missile is in inertial coodinate system Under X to initial attitude misalignment, Y-direction initial attitude misalignment, Z-direction initial attitude misalignment；δX₀、δY₀、δZ₀Respectively lead X of the bullet under inertial coodinate system is to initial position error, Y-direction initial position error, Z-direction initial position error；δV_x0、δV_y0、δ V_z0Respectively X of the guided missile under inertial coodinate system is to initial velocity error, Y-direction initial velocity error, Z-direction initial velocity error；

(2), in moment k, predict covariance matrix P_k|k-1=P_k-1；P_k-1It is for the estimate covariance matrix of moment k-1, described The initial value of estimate covariance matrix is P₀, P₀For 9 × 9 dimension matrixes of setting；

(3), in moment k, calculate gain matrix K_k=P_k|k-1H^T(H P_k|k-1H^T+R)^-1；Wherein, H is the observation square of setting Battle array, R are the noise variance matrix of setting：

(4), calculate the state vector estimated value of moment kWherein, Z_kFor outside The observation vector of the moment k of offer；In Kalman filtering, observation vector Z=[δ X δ Y δ Z δ V are defined_x δV_y δV_z]^T；δ X, δ Y, δ Z be respectively X that inertial navigation system and GPS system measure to the difference of position, the difference of Y-direction position, Z-direction position difference；δV_x、δ V_y、δV_zThe respectively X of inertial navigation system and GPS system measurement to the difference of speed, the difference of Y-direction speed, Z-direction speed difference；

(5), calculate moment k estimate covariance matrix P_k=(I-K_kH)P_k|k-1, I is unit matrix；

(6), in the initial value datum drift cycle estimator of setting, repeat step (1)～(5), by Kalman filtering meter Calculation obtains initial position error, initial velocity error and initial attitude misalignment.

The above-mentioned in-flight inertia initial baseline bias estimation method based on GPS, in step (3), observing matrix H and Noise variance matrix R is respectively set as：

Wherein：S_x、S_y、S_zRespectively inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent place；W_x、W_y、W_zRespectively Inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent velocity； Respectively set inertial system under X to, Y-direction, Z To position detection noise variance；Under the inertial system for respectively setting, X is to, Y-direction, Z-direction speed observation noise side Difference.

The present invention compared with prior art, with advantages below：

Jing mathematical simulations technology and carrying flight test checking, the in-flight inertia initial baseline estimation of deviation based on GPS Method can effectively estimate inertial reference deviation, reach the purpose of the precision and reliability that improve navigation system.

Description of the drawings

Fig. 1 is the flow chart of the in-flight inertia initial baseline bias estimation method based on GPS of the present invention.

Specific embodiment

The present invention is described in further detail with instantiation below in conjunction with the accompanying drawings：

The present invention sets up inertia initial baseline buggy model according to inertial navigation error propagation mechanism, is made using GPS information For observed quantity, inertia initial baseline deviation is estimated using standard Kalman filtering method, flow process is implemented as shown in Figure 1.

First, set up initial baseline estimation of deviation state equation：

Wherein：State vector X=[φ_x0 φ_y0 φ_z0 δX₀ δY₀ δZ₀ δV_x0 δV_y0 δV_z0]^T；φ_x0、φ_y0、φ_z0 Respectively X of the guided missile under inertial coodinate system is to initial attitude misalignment, Y-direction initial attitude misalignment, Z-direction initial attitude misalignment Angle；δX₀、δY₀、δZ₀At the beginning of respectively X of the guided missile under inertial coodinate system to initial position error, Y-direction initial position error, Z-direction Beginning site error；δV_x0、δV_y0、δV_z0Respectively X of the guided missile under inertial coodinate system is to initial velocity error, Y-direction initial velocity Error, Z-direction initial velocity error；Sytem matrix F is 9 × 9 dimension null matrix.

Then, set up the observational equation of inertia initial baseline estimation of deviation：

Z=HX+V；

Wherein：Observation vector Z=[δ X δ Y δ Z δ V_x δV_y δV_z]^T；δ X, δ Y, δ Z are respectively inertial navigation system and GPS systems The X of unified test amount to the difference of position, the difference of Y-direction position, Z-direction position difference；δV_x、δV_y、δV_zRespectively inertial navigation system and GPS system The X of measurement to the difference of speed, the difference of Y-direction speed, Z-direction speed difference；V is observation noise vector；H is observing matrix, at this Specifically it is set as in bright：

Wherein：S_x、S_y、S_zRespectively inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent place；W_x、W_y、W_zRespectively Inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent velocity.

Kalman filter equation based on more than, the in-flight inertia initial baseline estimation of deviation based on GPS of the present invention Method realizes that step is as follows：

(1), in moment k, calculate state vector predictive value State vector for moment k-1 is estimated Value, the initial value of the state vector estimated valueIt is set as null vector；Wherein, in Kalman filtering, definition status vector X=[φ_x0 φ_y0 φ_z0 δX₀ δY₀ δZ₀ δV_x0 δV_y0 δV_z0]^T；φ_x0、φ_y0、φ_z0Respectively guided missile is in inertial coodinate system Under X to initial attitude misalignment, Y-direction initial attitude misalignment, Z-direction initial attitude misalignment；δX₀、δY₀、δZ₀Respectively lead X of the bullet under inertial coodinate system is to initial position error, Y-direction initial position error, Z-direction initial position error；δV_x0、δV_y0、δ V_z0Respectively X of the guided missile under inertial coodinate system is to initial velocity error, Y-direction initial velocity error, Z-direction initial velocity error；

(2), in moment k, predict covariance matrix P_k|k-1=P_k-1；P_k-1It is for the estimate covariance matrix of moment k-1, described The initial value of estimate covariance matrix is P₀, P₀For 9 × 9 dimension matrixes of setting；

(3), in moment k, calculate gain matrix K_k=P_k|k-1H^T(HP_k|k-1H^T+R)^-1；Wherein, H is the observation square of setting Battle array, R are the noise variance matrix of setting：

Wherein：S_x、S_y、S_zRespectively inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent place；W_x、W_y、W_zRespectively Inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent velocity； Respectively set inertial system under X to, Y-direction, Z To position detection noise variance；Under the inertial system for respectively setting, X is to, Y-direction, Z-direction speed observation noise side Difference；

(4), calculate the state vector estimated value of moment kWherein, Z_kFor outside The observation vector of the moment k of offer；In Kalman filtering, observation vector Z=[δ X δ Y δ Z δ V are defined_x δV_y δV_z]^T；δ X, δ Y, δ Z be respectively X that inertial navigation system and GPS system measure to the difference of position, the difference of Y-direction position, Z-direction position difference；δV_x、δ V_y、δV_zThe respectively X of inertial navigation system and GPS system measurement to the difference of speed, the difference of Y-direction speed, Z-direction speed difference；

(5), calculate moment k estimate covariance matrix P_k=(I-K_kH)P_k|k-1, I is unit matrix；

(6), in the initial value datum drift cycle estimator of setting, repeat step (1)～(5), by Kalman filtering meter Calculation obtains initial position error, initial velocity error and initial attitude misalignment.

When concrete engineering is realized, the in-flight inertia initial baseline bias estimation method based on GPS of the present invention is by solid The software changed in missile-borne computer realizes that during missile flight, Flight Control Software calls the method, initial to guided missile Inertial reference deviation estimated, improves the navigation accuracy and accuracy at target of guided missile.

The above, only one specific embodiment of the present invention, but protection scope of the present invention is not limited thereto, and appoints What those familiar with the art the invention discloses technical scope in, the change or replacement that can be readily occurred in, all Should be included within the scope of the present invention.

The content not being described in detail in description of the invention belongs to the known technology of professional and technical personnel in the field.

Claims (2)

1. a kind of in-flight inertia initial baseline bias estimation method based on GPS, it is characterised in that comprise the following steps：

(1), in moment k, calculate state vector predictive value For the state vector estimated value of moment k-1, institute State the initial value of state vector estimated valueIt is set as null vector；Wherein, in Kalman filtering, definition status vector X= [φ_x0 φ_y0 φ_z0 δX₀ δY₀ δZ₀ δV_x0 δV_y0 δV_z0]^T；φ_x0、φ_y0、φ_z0Respectively guided missile is under inertial coodinate system X to initial attitude misalignment, Y-direction initial attitude misalignment, Z-direction initial attitude misalignment；δX₀、δY₀、δZ₀Respectively guided missile X under inertial coodinate system is to initial position error, Y-direction initial position error, Z-direction initial position error；δV_x0、δV_y0、δV_z0 Respectively X of the guided missile under inertial coodinate system is to initial velocity error, Y-direction initial velocity error, Z-direction initial velocity error；

(2), in moment k, predict covariance matrix P_k|k-1=P_k-1；P_k-1For the estimate covariance matrix of moment k-1, the estimation The initial value of covariance matrix is P₀, P₀For 9 × 9 dimension matrixes of setting；

(3), in moment k, calculate gain matrix K_k=P_k|k-1H^T(H P_k|k-1H^T+R)^-1；Wherein, H be setting observing matrix, R For the noise variance matrix of setting：

(4), calculate the state vector estimated value of moment kWherein, Z_kThere is provided for outside Moment k observation vector；In Kalman filtering, observation vector Z=[δ X δ Y δ Z δ V are defined_x δV_y δV_z]^T；δX、δY、 δ Z are respectively the difference of the X of inertial navigation system and GPS system measurement to position, the difference of Y-direction position, the difference of Z-direction position；δV_x、δV_y、δV_z The respectively X of inertial navigation system and GPS system measurement to the difference of speed, the difference of Y-direction speed, Z-direction speed difference；

(5), calculate moment k estimate covariance matrix P_k=(I-K_kH)P_k|k-1, I is unit matrix；

(6), in the initial value datum drift cycle estimator of setting, repeat step (1)～(5) are calculated by Kalman filtering To initial position error, initial velocity error and initial attitude misalignment.

2. a kind of in-flight inertia initial baseline bias estimation method based on GPS according to claim 1, its feature exist In：In step (3), observing matrix H and noise variance matrix R are respectively set as：

$H=\left[\begin{array}{ccccccccc}0& -S_z& S_y& 1& 0& 0& 0& 0& 0\\ S_z& 0& -S_x& 0& 1& 0& 0& 0& 0\\ -S_y& S_x& 0& 0& 0& 1& 0& 0& 0\\ 0& -W_z& W_y& 0& 0& 0& 1& 0& 0\\ W_z& 0& -W_x& 0& 0& 0& 0& 1& 0\\ -W_y& W_x& 0& 0& 0& 0& 0& 0& 1\end{array}\right];$

$R=\left[\begin{array}{cccccc}{\mathrm{\σ}}_x^2& & & & & \\ & {\mathrm{\σ}}_y^2& & & & \\ & & {\mathrm{\σ}}_z^2& & & \\ & & & {\mathrm{\σ}}_{v_x}^2& & \\ & & & & {\mathrm{\σ}}_{v_y}^2& \\ & & & & & {\mathrm{\σ}}_{v_z}^2\end{array}\right];$

Wherein：S_x、S_y、S_zRespectively inertial navigation system provide inertial system X to, Y-direction, Z-direction apparent place；W_x、W_y、W_zRespectively inertial navigation System provide inertial system X to, Y-direction, Z-direction apparent velocity； X is set under inertial system respectively to, Y-direction, Z-direction position Put observation noise variance；Under the inertial system for respectively setting X to, Y-direction, Z-direction speed observation noise variance.