CN103941273B - Adaptive filtering method of onboard inertia/satellite integrated navigation system and filter - Google Patents

Abstract

The invention discloses an adaptive Kalman filtering method of an onboard inertia/satellite integrated navigation system and a filter. A state noise variance array and a measurement noise variance array are updated in real time through a state estimation error and a measurement error in the filtering process, and therefore the adaptive change of internal state information and external measurement information of the system is achieved. The method has the advantages of being moderate in calculation amount, high in estimation accuracy and the like so that adaptive adjustment of Kalman filter parameters can be achieved, and therefore the navigation and measurement accuracy of the system is improved.

Description

The adaptive filter method and wave filter of Airborne Inertial/satellite combined guidance system

Technical field

The present invention relates to field of navigation technology, more particularly to a kind of Airborne Inertial/satellite combined guidance system it is adaptive Answer filtering method and wave filter.

Background technology

INS/GPS (Inertial Navigation Syste/Global Positioning System, inertia/satellite Integrated navigation system) it is a kind of accurate carrier navigation and movement parameter measurement system, with good reliability, high precision, output The advantages of parameter species is more, are widely used in aviation field.Conventional INS is with gps data blending algorithm Kalman (Kalman) wave filter, it can realize that the optimal of system mode is estimated in the case of known system noise statistical property Meter.

But a large amount of engineering practices show that in airborne use environment, the inertial sensor in INS is vulnerable to temperature, electricity The flight environment of vehicle such as magnetic, vibration influence, and cause sensor noise characteristic that unknown real-time change occurs；Additionally, due to GPS With the reason such as the change of antenna measurement condition, the measurement noise of GPS can also occur unknown variations.These problems can cause conventional How effectively Kalman filter estimated accuracy declines, filtering divergence, therefore adaptive estimation system are even resulted in when serious State-noise and measure noise statisticses, it is ensured that wave filter estimated accuracy, with important engineering significance.

ART network is filtered in calculating process is filtered, constantly to unknown model state noise and measurement noise Statistical parameter estimated and corrected, and realizes the inhibitory action to filter divergence.Application number 201010552746.5, invention name " a kind of adaptive filter method based on GPS/INS integrated navigation systems difference measurement characteristicses " is claimed to disclose a kind of using double systems The mutual difference sequence of unified test amount, estimates to measure the adaptive filter method of noise covariance, realizes the real-time tracking to GPS noises. A kind of application number 201110385191.4, denomination of invention " adaptive filter method based on observation noise variance matrix estimation " is open A kind of method that noise characteristic is measured using mutual difference sequence dynamic estimation, while constructing scale factor realizes Automatic adjusument Filtering gain.But it can be seen that these methods do not carry out ART network to system mode noise.It is dry in external environment Disturb in the case of causing system inside sensors performance change or failure, system mode noise can occur significant change, as shadow Ring the principal element of filtering accuracy, it is necessary to which it is processed.

The content of the invention

It is an object of the invention to overcome the shortcomings of existing filtering method, a kind of Airborne Inertial/combinations of satellites navigation is proposed The adaptive filter method and wave filter of system, to improve the navigation accuracy of airborne INS/GPS systems.Therefore, the scheme for using It is as follows.

A kind of method for adaptive kalman filtering of Airborne Inertial/satellite combined guidance system, comprises the following steps：

The error model of inertial navigation system is set up, as the state equation of Kalman filter, by inertial navigation system With the alternate position spike of global position system the measurement equation of Kalman filter, state equation are set up with speed difference as measurement It is as follows respectively with measurement equation：

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively It is the state white noise and measurement white noise of zero-mean, noise variance matrix is respectively Q_k-1And R_k；

According to the initial position of system performance and inertial navigation system, speed and attitude information, init state vector X₀, measure vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error covariance matrix P₀, measuring noise square difference battle array R_kWith State-noise variance matrix Q_k；

The angular speed and acceleration information in the relative inertness space measured using Inertial Measurement Unit, carry out strapdown solution Calculate, obtain position, speed and the attitude value at inertial navigation system current time；

Using the navigation data and the measurement data of global position system of inertial navigation system, Kalman filtering meter is carried out Calculate, using filtering estimateThe navigation data of inertial navigation system is corrected, while updating Φ_k,k-1And H_k；

Calculate measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measurement noise variance matrix R_kMade an uproar with state Sound variance matrix Q_k, obtain estimate

Setting computing upper limit M, judges whether filter times reach, if not up to, return strapdown is otherwise tied the step of resolve Beam is filtered.

A kind of adaptive Kalman filter of Airborne Inertial/satellite combined guidance system, including：

Establishing equation unit, the error model for setting up inertial navigation system, as the state side of Kalman filter Journey, using the alternate position spike and speed difference of inertial navigation system and global position system as measurement, sets up Kalman filter Measurement equation, state equation is as follows respectively with measurement equation：

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively It is the state white noise and measurement white noise of zero-mean, noise variance matrix is respectively Q_k-1And R_k；

Parameter initialization unit, for according to the initial position of system performance and inertial navigation system, speed and attitude Information, init state vector X₀, measure vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error covariance matrix P₀, measuring noise square difference battle array R_kWith state-noise variance matrix Q_k；

Inertial data asks for unit, for the angular speed in relative inertness space that is measured using Inertial Measurement Unit and plus Velocity information, carries out strapdown resolving, obtains position, speed and the attitude value at inertial navigation system current time；

Filtering computing unit, for navigation data and the measurement data of global position system using inertial navigation system, Kalman filtering calculating is carried out, using filtering estimateThe navigation data of inertial navigation system is corrected, while updating Φ_k,k-1 And H_k；

Updating block is estimated, for calculating measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measurement noise Variance matrix R_kWith state-noise variance matrix Q_k, obtain estimate

Filter times identifying unit, for setting computing upper limit M, judges whether filter times reach, if not up to, returning The step of strapdown is resolved, otherwise terminates filtering.

Present invention advantage compared with prior art is：

（1）Present invention improves over the scale of flexible IMU (Inertial measurement unit, Inertial Measurement Unit) Factor error calibrating method, has refined the demarcation to constant multiplier under small input angular velocity, according to the flexible gyroscope " hyperbolic for finding Line " rule sets up the equation of linear regression of constant multiplier and input angular velocity.

（2）Present invention improves over the scale factor error compensation method of IMU.Using initial data, by the recurrence set up Equation iterates and obtains accurate constant multiplier, and then, the error compensation precision of IMU is improve on this basis.

Brief description of the drawings

Fig. 1 is the schematic flow sheet of the adaptive filter method of Airborne Inertial/satellite combined guidance system of the present invention；

Fig. 2 is that Airborne Inertial of the present invention defends the/structural representation of the sef-adapting filter of star integrated navigation system.

Specific embodiment

In order to make the purpose , technical scheme and advantage of the present invention be clearer, it is right below in conjunction with drawings and Examples The present invention is described in further detail.It should be appreciated that specific embodiment described herein is only used to explain this hair It is bright, do not limit protection scope of the present invention.

As shown in figure 1, the specific steps of the adaptive filter method of Airborne Inertial/satellite combined guidance system of the present invention are such as Under：

Step s101：Set up inertial navigation system（INS）Error model, as INS/GPS combined systems Kalman filter The state equation of ripple device, using the alternate position spike and speed difference of INS and GPS as measurement, sets up the measurement equation of wave filter, respectively It is as follows

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively It is the state white noise and measurement white noise of zero-mean, noise variance matrix is respectively Q_k-1And R_k。

Selecting system state vector is X=[φ δ v δ p ε ▽]^T, wherein attitude error φ=[φ_e φ_n φ_u]^T, speed Degree error delta v=[δ v_e δv_n δv_u]^T, site error δ p=[δ L δ λ δ h]^T, gyro error ε=[ε_e ε_n ε_u]^T, accelerometer Error ▽=[▽_e ▽_n ▽_u]^T.Then state equation is：

Φ (4,2)=- f_u, Φ (4,3)=f_n

Φ (5,1)=f_u, Φ (5,3)=- f_e

Φ (6,1)=- f_n, Φ (6,2)=f_e

Wherein,It is pitching, roll, course attitude error, v_e、v_n、v_uIt is east, north, sky orientation speed, R_m、R_nIt is meridian plane, prime plane earth radius, p represents position, and L is local latitude, and h is to work as ground level, f_e、f_n、f_uFor east, north, It is to specific force, ω_ieIt is rotational-angular velocity of the earth.

Choosing measurement vector isWherein

Wherein,East orientation, north orientation, sky orientation speed that inertial navigation system measurement is obtained are represented respectively,East orientation, north orientation, sky orientation speed that global position system measurement is obtained, L are represented respectively_INS、λ_INS、h_INS Latitude, longitude, height that inertial navigation system measurement is obtained, L are represented respectively_GPS、λ_GPS、h_GPSGlobal position system is represented respectively Latitude, longitude, height that measurement is obtained；

Then measurement equation is：

H (1,4)=1, H (2,5)=1, H (3,6)=1

H (4,7)=R_m, H (5,8)=R_nCosL, H (6,9)=1

Step s102：According to the initial latitude L of system performance and INS, longitude λ, height h, northeast sky orientation speed v_e、v_n、 v_uAnd attitudeInit state vector X₀, measure vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error Variance matrix P₀, measuring noise square difference battle array R and state-noise variance matrix Q；

Step s103：Using Inertial Measurement Unit（IMU）Angular speed and the acceleration letter in the relative inertness space for measuring Breath, carries out strapdown resolving, obtains position, speed and the attitude value at INS current times；

Step s104：Using the INS navigation datas and gps measurement data at k moment, Kalman filter calculating is carried out, filtered Device equation is

Wherein,It is state one-step prediction estimate vector, K_kIt is filtering gain matrix, P_kFor state estimation error is assisted Variance matrix, P_{K, k-1}It is state estimation error one-step prediction covariance matrix.

Using filtering estimateCorrection INS navigation datas p=(L λ h)^T, v=(v_e v_n v_u)^TWithI.e.Wherein

Φ is updated simultaneously_k,k-1And H_k；

Step s105：Calculate measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measurement noise variance matrix R_k With state-noise variance matrix Q_k；

Step a：Setting data sampling window length N, calculates the measurement residuals v at current time_k=Z_k-H_kΦ_k,k-1X_k-1, enter And calculating measurement residual covariance battle array is

Step b：Calculate the state estimation residual error at current timeAnd then it is residual to calculate state estimation Differing from covariance matrix is

Step c：Calculate respectivelyIt is achieved thereby that R_k、Q_kRenewal；

Step s106：Setting computing upper limit M, judges whether filter times reach, if not up to, return to step s103 continues Calculating is filtered, otherwise terminates filtering.

The invention allows for one kind wave filter corresponding with above-mentioned adaptive filter method, its structure as shown in Fig. 2 each The step of function of unit is with above-mentioned filtering method is corresponding, specifically includes：

Establishing equation unit, the error model for setting up inertial navigation system, as the state side of Kalman filter Journey, using the alternate position spike and speed difference of inertial navigation system and global position system as measurement, sets up Kalman filter Measurement equation, state equation is as follows respectively with measurement equation：

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively It is the state white noise and measurement white noise of zero-mean, noise variance matrix is respectively Q_k-1And R_k；

Parameter initialization unit, for according to the initial position of system performance and inertial navigation system, speed and attitude Information, init state vector X₀, measure vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error covariance matrix P₀, measuring noise square difference battle array R_kWith state-noise variance matrix Q_k；

Inertial data asks for unit, for the angular speed in relative inertness space that is measured using Inertial Measurement Unit and plus Velocity information, carries out strapdown resolving, obtains position, speed and the attitude value at inertial navigation system current time；

Filtering computing unit, for navigation data and the measurement data of global position system using inertial navigation system, Kalman filtering calculating is carried out, using filtering estimateThe navigation data of inertial navigation system is corrected, while updating Φ_k,k-1 And H_k；

Updating block is estimated, for calculating measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measurement noise Variance matrix R_kWith state-noise variance matrix Q_k, obtain estimate

Filter times identifying unit, for setting computing upper limit M, judges whether filter times reach, if not up to, returning The step of strapdown is resolved, otherwise terminates filtering.

Used as a preferred embodiment, the state equation that the establishing equation unit is set up is specific with measurement equation For：

Selecting system state vector is X=[φ δ v δ p ε ▽]^T, wherein attitude error φ=[φ_e φ_n φ_u]^T, speed Degree error delta v=[δ v_e δv_n δv_u]^T, site error δ p=[δ L δ λ δ h]^T, gyro error ε=[ε_e ε_n ε_u]^T, accelerometer Error ▽=[▽_e ▽_n ▽_u]^T,

Then state equation is：

Φ (4,2)=- f_u, Φ (4,3)=f_n

Φ (5,1)=f_u, Φ (5,3)=- f_e

Φ (6,1)=- f_n, Φ (6,2)=f_e

Wherein,It is pitching, roll, course attitude error, v_e、v_n、v_uIt is east, north, sky orientation speed, R_m、R_nIt is meridian plane, prime plane earth radius, p represents position, and L is local latitude, and λ is local longitude, and h is to work as ground level, f_e、 f_n、f_uIt is east, north, day to specific force, ω_ieIt is rotational-angular velocity of the earth；

Choosing measurement vector isWherein

Wherein,East orientation, north orientation, sky orientation speed that inertial navigation system measurement is obtained are represented respectively,East orientation, north orientation, sky orientation speed that global position system measurement is obtained, L are represented respectively_INS、λ_INS、h_INS Latitude, longitude, height that inertial navigation system measurement is obtained, L are represented respectively_GPS、λ_GPS、h_GPSGlobal position system is represented respectively Latitude, longitude, height that measurement is obtained；

Then measurement equation is：

H (1,4)=1, H (2,5)=1, H (3,6)=1

H (4,7)=R_m, H (5,8)=R_nCosL, H (6,9)=1.

Used as a preferred embodiment, the filtering computing unit is carrying out wave filter side when Kalman filtering is calculated Cheng Wei：

Wherein,It is state one-step prediction estimate vector, K_kIt is filtering gain matrix, P_kFor state estimation error is assisted Variance matrix, P_{K, k-1}It is state estimation error one-step prediction covariance matrix.

Used as a preferred embodiment, the estimation updating block is to measuring noise square difference battle array R_kWith state-noise variance Battle array Q_kEstimateComputational methods be：

Setting data sampling window length N, calculates the measurement residuals v at current time_k=Z_k-H_kΦ_k,k-1X_k-1, and then calculate Measurement residuals covariance matrix is

Calculate the state estimation residual error at current timeAnd then calculate state estimation residual error association side Differing from battle array is

Calculate respectivelyAnd then realize that wave filter is joined Several renewals.

To sum up, adaptive filter method of the invention and wave filter are characterized in filtering, to be utilized respectively state Evaluated error and error in measurement, real-time update state-noise variance matrix and measuring noise square difference battle array, so as to realize to internal system The adaptive change of status information and external measurement information.This method has the advantages of amount of calculation is moderate, and estimated accuracy is high, can be with The self-adaptative adjustment to Kalman filter parameter is realized, system navigation and certainty of measurement is improved.

Embodiment described above only expresses several embodiments of the invention, and its description is more specific and detailed, but simultaneously Therefore the limitation to the scope of the claims of the present invention can not be interpreted as.It should be pointed out that for one of ordinary skill in the art For, without departing from the inventive concept of the premise, various modifications and improvements can be made, these belong to guarantor of the invention Shield scope.Therefore, the protection domain of patent of the present invention should be determined by the appended claims.

Claims (8)

1. a kind of method for adaptive kalman filtering of Airborne Inertial/satellite combined guidance system, it is characterised in that including following Step：

Set up the error model of inertial navigation system, as the state equation of Kalman filter, by inertial navigation system with defend The alternate position spike of star alignment system, as measurement, sets up the measurement equation of Kalman filter, state equation and amount with speed difference Survey equation as follows respectively：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively zero The state white noise and measurement white noise of average, noise variance matrix are respectively Q_k-1And R_k, k represent kth walk filtering operation；

Wherein, choosing measurement vector isWherein

$Z_k^v=\left(\begin{array}{c}{v}_e^{INS}-v_e^{GPS}\\ v_n^{INS}-v_n^{GPS}\\ v_u^{INS}-v_u^{GPS}\end{array}\right),Z_k^p=\left(\begin{array}{c}{L}_{INS}-L_{GPS}\\ {\mathrm{\λ}}_{INS}-{\mathrm{\λ}}_{GPS}\\ h_{INS}-h_{GPS}\end{array}\right)$

Wherein,East orientation, north orientation, sky orientation speed that inertial navigation system measurement is obtained are represented respectively,East orientation, north orientation, sky orientation speed that global position system measurement is obtained, L are represented respectively_INS、λ_INS、h_INS Latitude, longitude, height that inertial navigation system measurement is obtained, L are represented respectively_GPS、λ_GPS、h_GPSGlobal position system is represented respectively Latitude, longitude, height that measurement is obtained；

Then measurement equation is：

H (1,4)=1, H (2,5)=1, H (3,6)=1

H (4,7)=R_m, H (5,8)=R_nCosL, H (6,9)=1；

Wherein, R_m、R_nIt is meridian plane, prime plane earth radius, L is local latitude；

According to the initial position of system performance and inertial navigation system, speed and attitude information, init state vector X₀, amount Survey vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error covariance matrix P₀, measuring noise square difference battle array R_kWith state Noise variance matrix Q_k；

The angular speed and acceleration information in the relative inertness space measured using Inertial Measurement Unit, carry out strapdown resolving, obtain To the position at inertial navigation system current time, speed and attitude value；

Using the navigation data and the measurement data of global position system of inertial navigation system, Kalman filtering calculating is carried out, profit With filtering estimateThe navigation data of inertial navigation system is corrected, while updating Φ_k,k-1And H_k；

Calculate measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measurement noise variance matrix R_kWith state-noise side Difference battle array Q_k, obtain estimate

Setting computing upper limit M, judges whether filter times reach, if not up to, returning to strapdown the step of resolve, otherwise terminates to filter Ripple.

2. the method for adaptive kalman filtering of Airborne Inertial/satellite combined guidance system according to claim 1, it is special Levy and be, state equation is specially with measurement equation：

Selecting system state vector isWherein attitude error φ=[φ_e φ_n φ_u ]^T, velocity error δ v=[δ v_e δv_n δv_u]^T, site error δ p=[δ L δ λ δ h]^T, gyro error ε=[ε_e ε_n ε_u]^T, plus Speedometer error

Then state equation is：

$\mathrm{\Φ}(1,2)={\mathrm{\ω}}_{ie}\sin L+\frac{v_e}{R_n+h}\tan L$

$\mathrm{\Φ}(1,3)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_e}{R_n+h},\mathrm{\Φ}(1,5)=-\frac{1}{R_m+h}$

$\mathrm{\Φ}(2,1)=-{\mathrm{\ω}}_{ie}\sin L-\frac{v_e}{R_n+h}\tan L$

$\mathrm{\Φ}(2,3)=-\frac{v_n}{R_m+h},\mathrm{\Φ}(2,4)=\frac{1}{R_n+h}$

Φ (2,7)=- ω_ieSinL,

$\mathrm{\Φ}(3,2)=\frac{v_n}{R_m+h},\mathrm{\Φ}(3,4)=\frac{\tan L}{R_m+h}$

$\mathrm{\Φ}(3,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_e{\sec }^{2}L}{R_n+h}$

Φ (4,2)=- f_u, Φ (4,3)=f_n

$\mathrm{\Φ}(4,4)=\frac{v_n\tan L}{R_n+h}-\frac{v_u}{R_n+h}$

$\mathrm{\Φ}(4,5)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_e\tan L}{R_n+h}$

$\mathrm{\Φ}(4,6)=-2{\mathrm{\ω}}_{ie}\cos L-\frac{v_e}{R_n+h}$

$\mathrm{\Φ}(4,7)=2{\mathrm{\ω}}_{ie}{v}_n\cos L+\frac{v_e{v}_n{\sec }^{2}L}{R_n+h}+2{\mathrm{\ω}}_{ie}{v}_u\sin L$

Φ (5,1)=f_u, Φ (5,3)=- f_e

$\mathrm{\Φ}(5,4)=-2({\mathrm{\ω}}_{ie}\sin L+\frac{v_e\tan L}{R_n+h})$

$\mathrm{\Φ}(5,5)=-\frac{v_u}{R_m+h},\mathrm{\Φ}(5,6)=-\frac{v_n}{R_m+h}$

$\mathrm{\Φ}(5,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_e{\sec }^{2}L}{R_n+h})v_e$

Φ (6,1)=- f_n, Φ (6,2)=f_e

$\mathrm{\Φ}(6,4)=2({\mathrm{\ω}}_{ie}\cos L+\frac{v_e}{R_n+h})$

Φ (6,7)=- 2 ω_iev_esinL

$\mathrm{\Φ}(7,5)=\frac{1}{R_m+h},\mathrm{\Φ}(8,4)=\frac{\sec L}{R_n+h}$

$\mathrm{\Φ}(8,7)=\frac{v_e\tan L}{(R_n+h)\cos L},\mathrm{\Φ}(9,6)=1$

Wherein, φ_e、φ_n、φ_uIt is pitching, roll, course attitude error, v_e、v_n、v_uIt is east, north, sky orientation speed, p represents position Put, λ is local longitude, and h is to work as ground level, f_e、f_n、f_uIt is east, north, day to specific force, ω_ieIt is rotational-angular velocity of the earth.

3. the method for adaptive kalman filtering of Airborne Inertial/satellite combined guidance system according to claim 1 and 2, Characterized in that, being filter equation when Kalman filtering is calculated is carried out：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})\\ K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+{\hat{R}}_k)}^{-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\hat{Q}}_{k-1}\\ P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{\hat{R}}_k{K}_k^T\end{array}\right.$

Wherein,It is state one-step prediction estimate vector, K_kIt is filtering gain matrix, P_kIt is state estimation error covariance square Battle array, P_k,k-1It is state estimation error one-step prediction covariance matrix, I is unit matrix.

4. the method for adaptive kalman filtering of Airborne Inertial/satellite combined guidance system according to claim 3, it is special Levy and be, measuring noise square difference battle array R_kWith state-noise variance matrix Q_kEstimate Computational methods be：

Setting data sampling window length N, calculates the measurement residuals v at current time_k=Z_k-H_kΦ_k,k-1X_k-1, and then calculate measurement Residual covariance battle array be

Calculate the state estimation residual error at current timeAnd then calculate state estimation residual covariance battle array For

Calculate respectivelyAnd then realize filter parameter Update.

5. a kind of adaptive Kalman filter of Airborne Inertial/satellite combined guidance system, it is characterised in that including：

Establishing equation unit, the error model for setting up inertial navigation system, as the state equation of Kalman filter, will The alternate position spike of inertial navigation system and global position system, as measurement, sets up the measurement side of Kalman filter with speed difference Journey, state equation is as follows respectively with measurement equation：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right.$

Wherein, state vector is X_k, it is Z to measure vector_k, state matrix is Φ_k,k-1, measurement matrix is H_k, W_k-1、V_kRespectively zero The state white noise and measurement white noise of average, noise variance matrix are respectively Q_k-1And R_k；

Wherein, choosing measurement vector isWherein

$Z_k^v=\left(\begin{array}{c}{v}_e^{INS}-v_e^{GPS}\\ v_n^{INS}-v_n^{GPS}\\ v_u^{INS}-v_u^{GPS}\end{array}\right),Z_k^p=\left(\begin{array}{c}{L}_{INS}-L_{GPS}\\ {\mathrm{\λ}}_{INS}-{\mathrm{\λ}}_{GPS}\\ h_{INS}-h_{GPS}\end{array}\right)$

Wherein,East orientation, north orientation, sky orientation speed that inertial navigation system measurement is obtained are represented respectively,East orientation, north orientation, sky orientation speed that global position system measurement is obtained, L are represented respectively_INS、λ_INS、h_INS Latitude, longitude, height that inertial navigation system measurement is obtained, L are represented respectively_GPS、λ_GPS、h_GPSGlobal position system is represented respectively Latitude, longitude, height that measurement is obtained；

Then measurement equation is：

H (1,4)=1, H (2,5)=1, H (3,6)=1

H (4,7)=R_m, H (5,8)=R_nCosL, H (6,9)=1；

Wherein, R_m、R_nIt is meridian plane, prime plane earth radius, L is local latitude；

Parameter initialization unit, for according to the initial position of system performance and inertial navigation system, speed and attitude information, Init state vector X₀, measure vector Z₀, state matrix Φ_k,k-1, measurement matrix H_k, state estimation error covariance matrix P₀, measure Noise variance matrix R_kWith state-noise variance matrix Q_k；

Inertial data asks for unit, the angular speed and acceleration in the relative inertness space for being measured using Inertial Measurement Unit Information, carries out strapdown resolving, obtains position, speed and the attitude value at inertial navigation system current time；

Filtering computing unit, for the navigation data using inertial navigation system and the measurement data of global position system, is carried out Kalman filtering is calculated, using filtering estimateThe navigation data of inertial navigation system is corrected, while updating Φ_k,k-1And H_k；

Updating block is estimated, for calculating measurement residuals v_kWith state estimation residual delta X_k, and then estimate to update measuring noise square difference Battle array R_kWith state-noise variance matrix Q_k, obtain estimate

Filter times identifying unit, for setting computing upper limit M, judges whether filter times reach, if not up to, returning to strapdown The step of resolving, otherwise terminate filtering.

6. the adaptive Kalman filter of Airborne Inertial/satellite combined guidance system according to claim 5, its feature It is that the state equation that the establishing equation unit is set up is specially with measurement equation：

Selecting system state vector isWherein attitude error φ=[φ_e φ_n φ_u ]^T, velocity error δ v=[δ v_e δv_n δv_u]^T, site error δ p=[δ L δ λ δ h]^T, gyro error ε=[ε_e ε_n ε_u]^T, plus Speedometer error

Then state equation is：

$\mathrm{\Φ}(1,2)={\mathrm{\ω}}_{ie}\sin L+\frac{v_e}{R_n+h}\tan L$

$\mathrm{\Φ}(1,3)=-{\mathrm{\ω}}_{ie}\cos L-\frac{v_e}{R_n+h},\mathrm{\Φ}(1,5)=-\frac{1}{R_m+h}$

$\mathrm{\Φ}(2,1)=-{\mathrm{\ω}}_{ie}\sin L-\frac{v_e}{R_n+h}\tan L$

$\mathrm{\Φ}(2,3)=-\frac{v_n}{R_m+h},\mathrm{\Φ}(2,4)=\frac{1}{R_n+h}$

Φ (2,7)=- ω_ieSinL,

$\mathrm{\Φ}(3,2)=\frac{v_n}{R_m+h},\mathrm{\Φ}(3,4)=\frac{\tan L}{R_m+h}$

$\mathrm{\Φ}(3,7)={\mathrm{\ω}}_{ie}\cos L+\frac{v_e{\sec }^{2}L}{R_n+h}$

Φ (4,2)=- f_u, Φ (4,3)=f_n

$\mathrm{\Φ}(4,4)=\frac{v_n\tan L}{R_n+h}-\frac{v_u}{R_n+h}$

$\mathrm{\Φ}(4,5)=2{\mathrm{\ω}}_{ie}\sin L+\frac{v_e\tan L}{R_n+h}$

$\mathrm{\Φ}(4,6)=-2{\mathrm{\ω}}_{ie}\cos L-\frac{v_e}{R_n+h}$

$\mathrm{\Φ}(4,7)=2{\mathrm{\ω}}_{ie}{v}_n\cos L+\frac{v_e{v}_n{\sec }^{2}L}{R_n+h}+2{\mathrm{\ω}}_{ie}{v}_u\sin L$

Φ (5,1)=f_u, Φ (5,3)=- f_e

$\mathrm{\Φ}(5,4)=-2({\mathrm{\ω}}_{ie}\sin L+\frac{v_e\tan L}{R_n+h})$

$\mathrm{\Φ}(5,5)=-\frac{v_u}{R_m+h},\mathrm{\Φ}(5,6)=-\frac{v_n}{R_m+h}$

$\mathrm{\Φ}(5,7)=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{v_e{\sec }^{2}L}{R_n+h})v_e$

Φ (6,1)=- f_n, Φ (6,2)=f_e

$\mathrm{\Φ}(6,4)=2({\mathrm{\ω}}_{ie}\cos L+\frac{v_e}{R_n+h})$

Φ (6,7)=- 2 ω_iev_esinL

$\mathrm{\Φ}(7,5)=\frac{1}{R_m+h},\mathrm{\Φ}(8,4)=\frac{\sec L}{R_n+h}$

Φ (9,6)=1

Wherein, φ_e、φ_n、φ_uIt is pitching, roll, course attitude error, v_e、v_n、v_uIt is east, north, sky orientation speed, p represents position Put, λ is local longitude, and h is to work as ground level, f_e、f_n、f_uIt is east, north, day to specific force, ω_ieIt is rotational-angular velocity of the earth.

7. the adaptive Kalman filter of the Airborne Inertial/satellite combined guidance system according to claim 5 or 6, its It is characterised by, filter equation of the filtering computing unit when Kalman filtering calculating is carried out is：

$\left\{\begin{array}{c}{\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}\\ {\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})\\ K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+{\hat{R}}_k)}^{-1}\\ P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\hat{Q}}_{k-1}\\ P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{\hat{R}}_k{K}_k^T\end{array}\right.$

Wherein,It is state one-step prediction estimate vector, K_kIt is filtering gain matrix, P_kIt is state estimation error covariance square Battle array, P_k,k-1It is state estimation error one-step prediction covariance matrix, I is unit matrix.

8. the adaptive Kalman filter of Airborne Inertial/satellite combined guidance system according to claim 7, its feature It is that the estimation updating block is to measuring noise square difference battle array R_kWith state-noise variance matrix Q_kEstimateMeter Calculation method is：

Setting data sampling window length N, calculates the measurement residuals v at current time_k=Z_k-H_kΦ_k,k-1X_k-1, and then calculate measurement Residual covariance battle array be

Calculate the state estimation residual error at current timeAnd then calculate state estimation residual covariance battle array For

Calculate respectivelyAnd then realize filter parameter Update.