CN105988129A - Scalar-estimation-algorithm-based INS/GNSS combined navigation method - Google Patents

Abstract

The invention discloses a scalar-estimation-algorithm-based INS/GNSS combined navigation method. According to different error dynamic characteristics of a dead reckoning technology and a satellite navigation positioning technology, the two kinds of navigation methods are combined based on a scalar estimation algorithm to construct an INS/GNSS combined navigation system. The method comprises: a discretized INS/GNSS combined navigation system error model is established; on the basis of a kalman filtering algorithm, a scalar estimation algorithm is constructed; on the basis of the scalar estimation algorithm, a new measurement vector Z* is introduced; a state variable of the combined navigation system error model is expressed into a scalar estimation mode by using the scalar estimation algorithm; and then on the basis of the scalar estimation mode, an INS/GNSS combined navigation system is constructed and thus a navigation body navigation system error is compensated. According to the invention, a filter divergence problem caused by inaccurate noise statistic characteristic determination of the traditional kalman filtering algorithm can be solved; reliability of the combined navigation system is improved; and the combined navigation system cost is reduced. A novel combined navigation method is provided.

Description

A kind of INS/GNSS Combinated navigation method based on scalar algorithm for estimating

Technical field

The present invention relates to field of navigation technology, be specifically related to a kind of INS/GNSS Combinated navigation method based on scalar algorithm for estimating.

Background technology

Inertial navigation system (INS) is to utilize inertia sensitive element (accelerometer and gyroscope) to measure sail body relative to inertial space Line motion and angular movement parameter, under conditions of given initial motion parameter, extrapolate sail body position, speed and attitude etc. Navigational parameter, and guide sail body to complete the dead reckoning navigation system of predetermined navigational duty.Owing to inertial navigation method is set up On the basis of Newton mechanics law, it is independent of any extraneous metrical information, is not disturbed by natural or anthropic factor, because of This, inertial navigation system has that autonomy is strong, navigational parameter is complete, precision is high in short-term and the advantage such as good concealment.But, by There is drift in inertance element, position error is accumulated in time, can produce the most permissible accumulated error after working long hours.

Satellite navigation and location system is the navigator fix signal utilizing satellite to send, and determines ground, ocean, aerial and space navigation Body position and kinestate, and guide sail body to complete the navigator fix technology of predetermined navigational duty.Satellite navigation and location system Typically it is made up of space segment, ground control segment and User Part, there is the advantage such as Global coverage, round-the-clock, high accuracy, But its signal power is low, intensity is weak, is easily trapped and disturbs.At present, running or the Global Satellite developed are led in-orbit Boat system (GNSS) mainly has following four big core systems: the global positioning system (GPS) developed by U.S. Department of Defense and safeguard； The GLONASS (GLONASS) safeguarded it is responsible for by former Soviet Union's exploitation, Russia；The Big Dipper developed by China and build Satellite navigation system (BDS)；Supported by European Union and European Space Agency, the GALILEO positioning system (Galileo) built.

In engineering practice, according to different applications and application background, need the precision of reasonable design navigation system, letter Breath renewal frequency, quality and size, reliability and price etc..Specifically, for civil aircraft, merchant ship and vehicle etc., There is no need to require that navigation system has the highest precision excessively, it is only necessary to Navigation system error is maintained at a certain permission Within range of error；For ordnance missile, rocket and operational aircraft etc., do not require nothing more than navigation system and there is higher navigation Positioning precision, and requirement can play due effect in the environment of electromagnetism operation；The miniature self-service being had for individual Machine and vehicle etc., need to be placed on design focal point the aspects such as the weight of navigation system, size, price and power consumption.In order to Adapt to different application field and the requirement of application background, according to dead reckoning (such as, INS) and Technique of Satellite Navigation and Positioning (example Such as, GNSS) different error dynamics, application message algorithm for estimating, two kinds of airmanships are combined, build INS/GNSS integrated navigation system (as shown in Figure 1).

In terms of integrated navigation system information processing and fusion, most widely used algorithm for estimating is Kalman filtering algorithm, such as: Adaptive Kalman filter algorithm, non-linear Kalman filtering algorithm and modified model non-linear Kalman filtering algorithm etc..Above-mentioned biography System algorithm for estimating requires that the Complete mathematic model of institute's Study system is known.This is objectively requiring the calculating facility entrained by sail body Have and calculate speed and enough calculating spaces faster.For complex high-order dynamic system, entrained computer System is difficult to meet the requirement of information processing real-time in Practical Project.

Summary of the invention

It is an object of the invention to provide a kind of INS/GNSS Combinated navigation method based on scalar algorithm for estimating, overcome traditional Integrated navigation system low, the poor reliability of the precision when Kalman filter dissipates etc. combined based on inertial navigation and satellite fix Shortcoming, improves the estimated accuracy of navigation system attitude orientation equal error, it is provided that a kind of simple in construction, cheap, reliability is high INS/GNSS Combinated navigation method based on scalar algorithm for estimating.

The technical solution realizing the object of the invention is: a kind of INS/GNSS Combinated navigation method based on scalar algorithm for estimating, Method step is as follows:

Step 1. builds INS/GNSS integrated navigation system error model:

In order to obtain relative position, speed and the attitude information of sail body, choose sky, northeast geographic coordinate system as integrated navigation system The fundamental coordinate system of system clearing, wherein, sky, northeast coordinate origin is sail body barycenter, and x-axis is along reference ellipsoid prime vertical direction Pointing to east, y-axis points to north along reference ellipsoid meridian circle direction, and z-axis is determined by the right-hand rule.

Northeastward in sky coordinate system, INS/GNSS integrated navigation system error model is reduced to following form:

In formula: δ V_xRefer to that east is to movement velocity error for sail body；δV_yFor sail body north pointer direction movement velocity error；ΔV_x For accelerometer instrumental error along the x-axis direction；ΔV_yFor accelerometer instrumental error along the y-axis direction；a_xFor sail body along x Axial component of acceleration；a_yFor sail body component of acceleration along the y-axis direction；Ф_xFor gyroscope platform around x-axis direction Misalignment；Ф_yFor the gyroscope platform misalignment around y-axis direction；Ф_zFor the gyroscope platform misalignment around z-axis direction；ε_xFor The drift in gyroscope x-axis direction；ε_yDrift for gyroscope y-axis direction；ε_zDrift for gyroscope z-axis direction；ω_xFor top Spiral shell platform angular velocity component along the x-axis direction；ω_yFor gyropanel angular velocity component along the y-axis direction；ω_zFor gyropanel edge The angular velocity component in z-axis direction；w_xFor external disturbance along the x-axis direction；w_yFor external disturbance along the y-axis direction；w_zFor edge The external disturbance in z-axis direction；μ is the average frequency of gyroscopic drift change at random；G is acceleration of gravity；R is earth radius；Latitude residing for gyroscope platform；Represent variable × time differential.

Expression formula (1) is expressed as matrix form, i.e. integrated navigation system error model state equation is:

$\stackrel{\·}{X}=\mathrm{FX}+W---\left(2\right)$

In formula:

System state vector X=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

System input noise $W={\left[\begin{array}{cccccccc}\mathrm{\Δ}{\stackrel{\·}{V}}_x& \mathrm{\Δ}{\stackrel{\·}{V}}_y& 0& 0& 0& w_x\left(t\right)& w_y\left(t\right)& w_z\left(t\right)\end{array}\right]}^T,$

Systematic state transfer matrix

Step 2. discretization INS/GNSS integrated navigation system error model:

The state equation of INS/GNSS integrated navigation system error model is expressed as following discrete matrix form:

X_k=Φ X_k-1+W_k-1 (3)

In formula:

State vector X of integrated navigation system error model_k=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

Wherein, δ V_xRefer to that east is to movement velocity error for sail body；δV_yFor sail body north pointer direction movement velocity error；Ф_xFor Gyroscope platform is around the misalignment in x-axis direction；Ф_yFor the gyroscope platform misalignment around y-axis direction；Ф_zFor gyroscope platform around The misalignment in z-axis direction；ε_xDrift for gyroscope x-axis direction；ε_yDrift for gyroscope y-axis direction；ε_zFor gyroscope The drift in z-axis direction；K is time parameter, represents current time.

Input noise W of integrated navigation system error model_k-1=[B_x B_y 0 0 0 w_x w_y w_z]^T,

Wherein, B_xFor accelerometer drift error along the x-axis direction；B_yFor accelerometer drift error along the y-axis direction；w_x For external interfering noise along the x-axis direction；w_yFor external interfering noise along the y-axis direction；w_zDo for outside along the z-axis direction Disturb noise.

The systematic state transfer matrix of integrated navigation system error model

Wherein, T is the sampling time；μ is the average frequency of gyroscopic drift change at random；G is acceleration of gravity；R is the earth Radius；Latitude residing for gyroscope platform.

Step 3. based on Kalman filtering algorithm, builds scalar algorithm for estimating:

Shown in integrated navigation system error model such as expression formula (3) after discretization, now, integrated navigation system error measure equation It is expressed as:

Z_k=HX_k+V_k (4)

In formula: X_kFor system state vector；Z_kFor systematic survey vector；V_kFor measuring noise；K is time parameter, represents and works as The front moment；H is systematic survey matrix.

Now, classical Kalman filtering algorithm has a following form:

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(5\right)$

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1}---\left(6\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(7\right)$

P_k/k-1=Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k (8)

P_k=(I-K_kH_k)P_k/k-1 (9)

Wherein, K_kKalman gain matrix；P_k/k-1The variance matrix of prior estimate error；P_kThe variance matrix of Posterior estimator error；R_k Measure the variance matrix of noise；Q_kThe variance matrix of input noise；K is time parameter, represents current time.

Based on initial time state variable X₁, it is considered to expression formula (3) and (4), state variable X in n+1 moment_n+1Recursively represent For:

$X_{n+1}={\mathrm{\Φ}}^n{X}_1+{\mathrm{\Φ}}^{n-1}{W}_1+{\mathrm{\Φ}}^{n-2}{W}_2+...+W_n={\mathrm{\Φ}}^n{X}_1+\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j}---\left(10\right)$

Wherein, j is the natural number between 1-n.

Expression formula (10) is launched, is expressed as following form:

$\left[\begin{array}{c}{x}_{n+1}^1\\ x_{n+1}^2\\ ...\\ x_{n+1}^n\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]\left[\begin{array}{c}{x}_1^1\\ x_1^2\\ ...\\ x_1^n\end{array}\right]+\begin{array}{c}\\ \\ \\ \end{array}\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]---\left(11\right)$

Wherein: $\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]={\mathrm{\Φ}}^n,$ φ is matrix ΦⁿIn each element； $\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j}.$

Known by expression formula (11), the i-th variation per minute in state vectorIt is expressed as:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\φ}}_{i1}{x}_1^1+{\mathrm{\φ}}_{i2}{x}_1^2+...+{\mathrm{\φ}}_{\mathrm{in}}{x}_1^n+w_1^i---\left(12\right)$

In conjunction with expression formula (4), expression formula (12) is expressed as form:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+({\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n})+({\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n})+w_1^i---\left(13\right)$

Wherein: $Z_1^{*}=\left[\begin{array}{c}{z}_1^{*1}\\ z_1^{*2}\\ ...\\ z_1^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_1\\ z_2\\ ...\\ z_n\end{array}\right];V_1^{*}=\left[\begin{array}{c}{v}_1^{*1}\\ v_1^{*2}\\ ...\\ v_1^{*n}\end{array}\right]=-{\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ {\mathrm{Hw}}_1+v_2\\ ...\\ {\mathrm{HA}}^{n-2}{w}_1+...+{\mathrm{Hw}}_1+v_n\end{array}\right].$

Vector shown in expression formula (13)It is based on initial time state variable X₁Introduced newly measures vector.Similar Ground, state variable X based on k the most in the same time_k, when introducing k the most in the same time respectively, newly measure vector

$Z_k^{*}=\left[\begin{array}{c}{z}_k^{*1}\\ z_k^{*2}\\ ...\\ z_k^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ ...\\ z_{k+n-1}\end{array}\right]---\left(14\right)$

Order ${\mathrm{\ξ}}_1^i={\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n}---\left(15\right)$

$w_1^{*i}={\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n}+w_1^i---\left(16\right)$

Now, expression formula (13) is expressed as:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\ξ}}_1^i+w_1^{*i}---\left(17\right)$

Wherein,For based on initial time state variable X₁Introduced measurement correction value；For based on initial time state variable X₁Introduced noise correction value.

Based on classical Kalman filtering algorithm, in conjunction with expression formula (5), i-th variation per minuteAt k=1,2 ... the optimum in moment is estimated MeterThere is following form:

${\hat{x}}_{\mathrm{nk}+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i+{\mathrm{\ξ}}_k^i+k_k^i(z_{k+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i-{\mathrm{\ξ}}_k^i)---\left(18\right)$

Correspondingly, in conjunction with expression formula (7), gain coefficientFor:

$k_k^i=\frac{p_{k/k-1}^i}{p_{k/k-1}^i+r_k^i}---\left(19\right)$

Measure noise varianceFor:

$r_k^i=M\left[{\left(v_k^i\right)}^2\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{\left(v_j^i\right)}^2---\left(20\right)$

$v_j^i=z_{j+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(j-1)+1}^i---\left(21\right)$

Wherein,For measuring noise.

In conjunction with expression formula (8), the variance of prior estimate errorFor:

$p_{k/k-1}^i={\mathrm{\φ}}_{\mathrm{ii}}^2{p}_{k-1}^i+M\left[{\left(v_k^i\right)}^2\right]\×{\left(k_{k-1}^i\right)}^2---\left(22\right)$

In conjunction with expression formula (9), the variance of Posterior estimator errorFor:

$p_k^i=(1-k_k^i)p_{k/k-1}^i---\left(23\right)$

Step 4. applies scalar algorithm for estimating, and the state variable of integrated navigation system error model is expressed as scalar estimated form:

For integrated navigation system error model, its state variable includes: sail body movement velocity error delta V, and gyroscope is put down Platform misalignmentGyroscopic drift ε.Based on scalar algorithm for estimating, in conjunction with expression formula (14), integrated navigation system error measuring value It is expressed as:

$Z_k^{*}=S^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ z_{k+2}\end{array}\right]---\left(24\right)$

Wherein: $S=\left[\begin{array}{ccc}& & \\ 1& 0& 0\\ 1& -\mathrm{gT}& 0\\ 1-\frac{{\mathrm{gT}}^2}{R}& -2\mathrm{gT}& -g{T}^2\end{array}\right]$ For System Observability matrix.

Specifically, the state variable of integrated navigation system error model is written as scalar form:

Z (δ V)=z_k

$z\left(\mathrm{\Φ}\right)=\frac{1}{\mathrm{gT}}{z}_k-\frac{1}{\mathrm{gT}}{z}_{k+1}---\left(25\right)$

$z\left(\mathrm{\ϵ}\right)=-\frac{1}{{\mathrm{gT}}^2}{z}_k-\frac{1}{R}{z}_k+\frac{2}{{\mathrm{gT}}^2}{z}_{k+1}-\frac{1}{{\mathrm{gT}}^2}{z}_{k+2}$

In conjunction with expression formula (25), when measuring gyropanel misalignment Ф, the conversion variance measuring noise is:

$r^{*}=\frac{2}{g^2{T}^2}r---\left(26\right)$

Wherein: the variance of noise when r is measuring speed error delta V.

In practical engineering application, choose integrated navigation system east orientation and north orientation velocity error as system measurement, application scalar Algorithm for estimating, in conjunction with expression formula (2) and (25), obtains following integrated navigation system error full dimension scalar and estimates expression formula:

Wherein: z_x,kFor integrated navigation system east orientation velocity error measured value；z_y,kMeasure for integrated navigation system north orientation velocity error Value.

The sail body east orientation and north orientation velocity error obtained is measured in application, in conjunction with expression formula (27), calculates and estimates integrated navigation The velocity error of system all directions, misalignment and gyroscopic drift, realize integrated navigation system error compensation and correction with this.

The present invention compared with prior art, its remarkable advantage: according to the error dynamics that INS with GNSS is different, build knot The integrated navigation system that structure is simple, cheap and reliability is high, on the one hand ensure that integrated navigation system velocity error compensates Effect, on the other hand improve the precision that gyropanel misalignment compensates, solve Kalman filtering and determine not because of noise statistics The filter divergence difficult problem accurately caused, improves the reliability of integrated navigation system, reduces the cost of integrated navigation system, for The integrated navigation of small aircraft, civilian vehicle and other sail bodies provides a kind of novel Combinated navigation method.

Accompanying drawing explanation

Fig. 1 is integrated navigation system schematic diagram of the present invention.

Fig. 2 is the technology of the present invention flowchart.

Fig. 3 is INS/GNSS Combinated navigation method schematic diagram of the present invention.

Fig. 4 is gyroscope the misaligned angle of the platform output calibration loop diagram.

East orientation attitude error figure before Fig. 5 output calibration.

East orientation attitude error figure after Fig. 6 output calibration.

East orientation attitude error comparison figure before and after Fig. 7 output calibration.

North orientation attitude error figure before Fig. 8 output calibration.

North orientation attitude error figure after Fig. 9 output calibration.

North orientation attitude error comparison figure before and after Figure 10 output calibration.

Detailed description of the invention

Below in conjunction with the accompanying drawings the present invention is described in further detail.

A kind of INS/GNSS Combinated navigation method based on scalar algorithm for estimating, its detailed description of the invention is: set up INS/GNSS Integrated navigation system error model, discretization INS/GNSS integrated navigation system error model, carry based on Kalman filtering algorithm Go out a kind of compact scalar algorithm for estimating, and propose a kind of constructing system measurement vector z^*New method, application scalar estimate calculate Each state variable in integrated navigation system error model is expressed as the scalar of other state variable linear combinations and estimates shape by method Formula, builds INS/GNSS integrated navigation system in conjunction with Fig. 1, according to scalar estimate expression formula correction integrated navigation system position, Speed and attitude orientation equal error, it is achieved integrated navigation system error compensation.

In conjunction with Fig. 2 and Fig. 3, it is embodied as step as follows:

Step 1. sets up INS/GNSS integrated navigation system error model:

In order to realize the integrated navigation of sail body, need to sail body provide relative to a certain space or the position of jobbie, The movable information such as speed and attitude.Due to motion composition, in order to describe the kinematic parameter of sail body, need to set up accordingly Coordinate system.Typically choose the fundamental coordinate system that sky, northeast geographic coordinate system is settled accounts as integrated navigation system.Wherein, sky, northeast is sat Mark system initial point is sail body barycenter, and x-axis points to east along reference ellipsoid prime vertical direction, and y-axis refers to along reference ellipsoid meridian circle direction Northwards, z-axis is determined by the right-hand rule.For inertial navigation system, set up error equation accurately and be by integrated navigation filtering Basis.

Choose the fundamental coordinate system that sky, northeast geographic coordinate system is settled accounts as navigation system, based on INS/GNSS integrated navigation system Error locator equation and accelerometer error equation, establish Navigation system error model as follows:

In formula: δ V_xRefer to that east is to movement velocity error for sail body；δV_yFor sail body north pointer direction movement velocity error；V_xFor Sail body refers to that east is to movement velocity component；V_yFor sail body north pointer direction movement velocity component；ΔV_xFor accelerometer along x-axis The instrumental error in direction；ΔV_yFor accelerometer instrumental error along the y-axis direction；a_xFor sail body acceleration along the x-axis direction Component；a_yFor sail body component of acceleration along the y-axis direction；Ф_xFor the gyroscope platform misalignment around x-axis direction；Ф_yFor Gyroscope platform is around the misalignment in y-axis direction；Ф_zFor the gyroscope platform misalignment around z-axis direction；ε_xFor gyroscope x-axis side To drift；ε_yDrift for gyroscope y-axis direction；ε_zDrift for gyroscope z-axis direction；Latitude residing for platform；λ Longitude residing for platform；For latitude error；δ λ is trueness error；U is earth rotation speed；G is acceleration of gravity；R For earth radius；Represent variable × time differential.

In practical engineering application, for Integrated Navigation Algorithm design and the convenience of application, INS/GNSS integrated navigation system is by mistake Differential mode type is reduced to the form as shown in expression formula (1):

And it is possible to be expressed as the matrix form as shown in expression formula (2):

$\stackrel{\·}{X}=\mathrm{FX}+W---\left(2\right)$

In formula:

System state vector X=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

System input noise $W={\left[\begin{array}{cccccccc}\mathrm{\Δ}{\stackrel{\·}{V}}_x& \mathrm{\Δ}{\stackrel{\·}{V}}_y& 0& 0& 0& w_x\left(t\right)& w_y\left(t\right)& w_z\left(t\right)\end{array}\right]}^T,$

Systematic state transfer matrix

Step 2. discretization INS/GNSS integrated navigation system error model:

For the LINEAR CONTINUOUS integrated navigation system error model shown in expression formula (2), it is assumed that original state is X (t₀)=X₀, this Time, the solution of the state equation of expression formula (2) is:

$X\left(t\right)=\mathrm{\Φ}(t,t_0)X\left(t_0\right)+\underset{t_0}{\overset{t}{\∫}}\mathrm{\Φ}(t,\mathrm{\τ})W\left(\mathrm{\τ}\right)\mathrm{d\τ}---\left(29\right)$

In formula, Φ is state-transition matrix.

It is assumed that constant duration sampling, use interval of delta t=t_k+1-t_k(k=0,1,2 ...) it is constant value.In sampling instant t_k＜ t ＜ t_k+1(k=0,1,2 ...), in conjunction with expression formula (29), obtain:

$X\left(t_{k+1}\right)=\mathrm{\Φ}(t_{k+1},t_k)X\left(t_k\right)+\underset{t_k}{\overset{t_{k+1}}{\∫}}\mathrm{\Φ}(t_{k+1},\mathrm{\τ})W\left(\mathrm{\τ}\right)\mathrm{d\τ}---\left(30\right)$

At sampling interval t_kWith t_k+1Between, it is believed that W (τ) keeps constant value constant, obtains in conjunction with expression formula (30):

$X\left(t_{k+1}\right)=\mathrm{\Φ}(t_{k+1},t_k)X\left(t_k\right)+\left[\underset{t_k}{\overset{t_{k+1}}{\∫}}\mathrm{\Φ}(t_{k+1},\mathrm{\τ})\mathrm{d\τ}\right]W\left(t_k\right)---\left(31\right)$

And then, in conjunction with expression formula (31), obtain the discrete matrix form of integrated navigation system error model as shown in expression formula (3):

X_k=Φ X_k-1+W_k-1 (3)

In formula:

State vector X of integrated navigation system error model_k=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

Input noise W of integrated navigation system error model_k-1=[B_x B_y 0 0 0 w_x w_y w_z]^T,

The systematic state transfer matrix of integrated navigation system error model

When only considering the Navigation system error in space a direction (such as: refer to east to), the integrated navigation system after simplification is by mistake Differential mode type has a following form:

X_(3×1)k=Φ_(3×3)X_(3×1)k-1+W_(3×1)k-1 (32)

In formula:

System state vector $X_{(3\×1)k}=\left[\begin{array}{c}{\mathrm{\δV}}_k\\ {\mathrm{\Φ}}_k\\ {\mathrm{\ϵ}}_k\end{array}\right];$

System input noise $W_{(3\×1)k-1}=\left[\begin{array}{c}0\\ 0\\ w_{k-1}\end{array}\right],$ Wherein, w_k-1For white Gaussian noise；

Systematic state transfer matrix ${\mathrm{\Φ}}_{(3\×3)}=\left[\begin{array}{ccc}1& -\mathrm{gT}& 0\\ \frac{T}{R}& 1& T\\ 0& 0& 1-\mathrm{\μT}\end{array}\right].$

Step 3. based on Kalman filtering algorithm, builds scalar algorithm for estimating:

Classical state estimation algorithm, such as Kalman filtering algorithm, it is desirable to institute's Study system has complete state space and expresses shape The Complete mathematic model of formula, i.e. Study system is known.For high order system, the amount of calculation of this type of classic algorithm is relatively big, very Difficult realization on airborne computer.Estimate more closely to calculate to solve this problem it is necessary to design certain simplification, structure Method.Scalar algorithm for estimating be a kind of low to input noise sensitivity, have and be prone to the mathematical expression form of theory analysis and calculate Measure less adaptive information algorithm for estimating.Application scalar algorithm for estimating, structure scalar estimates expression formula, in institute's Study system Each quantity of state estimate.

Based on classical Kalman filtering algorithm, at moment k=1,2,3 ... time, scalar algorithm for estimating has expression formula (18) to (23) institute The form shown:

${\hat{x}}_{\mathrm{nk}+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i+{\mathrm{\ξ}}_k^i+k_k^i(z_{k+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i-{\mathrm{\ξ}}_k^i)---\left(18\right)$

$k_k^i=\frac{p_{k/k-1}^i}{p_{k/k-1}^i+r_k^i}---\left(19\right)$

$r_k^i=M\left[{\left(v_k^i\right)}^2\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{\left(v_j^i\right)}^2---\left(20\right)$

$v_j^i=z_{j+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(j-1)+1}^i---\left(21\right)$

$p_{k/k-1}^i={\mathrm{\φ}}_{\mathrm{ii}}^2{p}_{k-1}^i+M\left[{\left(v_k^i\right)}^2\right]\×{\left(k_{k-1}^i\right)}^2---\left(22\right)$

$p_k^i=(1-k_k^i)p_{k/k-1}^i---\left(23\right)$

In general, integrated navigation system error equation is expressed as the state space variable form shown in such as expression formula (3) and (4). Now, it is assumed that input noise is uncorrelated with measurement noise.Further, assume without loss of generality only to measure a certain system state variables, I.e. H=[1...0].

Based on initial time state variable X₁, at moment k=2, when 3,4..., n, obtain in conjunction with expression formula (3):

X₂=Φ X₁+W₁

X₃=Φ²X₁+ΦW₁+W₂ (33)

...........................

X_n=Φ^n-1X₁+Φ^n-2W₁+…+W_n-1

Similarly, theoretical based on Kalman's System Observability, in conjunction with expression formula (4), integrated navigation system measured value is expressed as Form:

z₁=HX₁+v₁

z₂=H Φ X₁+HW₁+v₂ (34)

...........................

z_n=H Φ^n-1X₁+HΦ^n-2W₁+....+HW_n-1+v_n

Expression formula (34) is expressed as matrix form:

Z=SX₁+V¹ (35)

Wherein: $Z=\left[\begin{array}{c}{z}_1\\ z_2\\ ...\\ z_n\end{array}\right];S=\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right];V^1=\left[\begin{array}{c}{v}_1\\ {\mathrm{HW}}_1+v_2\\ .......................\\ {\mathrm{H\Φ}}^{n-2}{W}_1+....+{\mathrm{HW}}_{n-1}+v_n\end{array}\right].$

From expression formula (35):

X₁=S^-1Z-S^-1V¹ (36)

In order to estimate the value of each system state variables respectively, in conjunction with expression formula (14), introduce following vector of newly measuring:

Z^*=S^-1Z (37)

Expression formula (37) is expressed as scalar form, i.e. the linear combination of measured value:

$z^{*i}=s_1^i{z}_1+s_2^i{z}_2+...+s_n^i{z}_n---\left(38\right)$

Wherein: z^*iFor vector z^*The i-th row element, sⁱFor S^-1I-th row element of matrix.

In conjunction with expression formula (38), the scalar expression measuring noise is:

$v^{1i}=s_1^i{v}_1+s_2^i{v}_2+...+s_n^i{v}_n---\left(39\right)$

Correspondingly, the conversion variance measuring noise is expressed as:

$r^{1i}=M\left[{\left(v^{1i}\right)}^2\right]=(S_1^{i2}+s_2^{i2}+...+s_n^{i2})r---\left(40\right)$

Wherein: r is the variance of original measurement noise.

Application scalar algorithm for estimating, based on state variable X during moment k_k, introduced newly measures vectorHave such as expression formula (14) form shown in:

$Z_k^{*}=\left[\begin{array}{c}{z}_k^{*1}\\ z_k^{*2}\\ ...\\ z_k^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ ...\\ z_{k+n-1}\end{array}\right]---\left(14\right)$

Correspondingly, in conjunction with expression formula (39) and (40), measuring noise and measuring the conversion variance of noise when trying to achieve moment k:

$v^{\mathrm{ki}}=s_k^i{v}_k+s_{k+1}^i{v}_{k+1}+...+s_{k+n-1}^i{v}_{k+n-1}---\left(41\right)$

$r^{\mathrm{ki}}=M\left[{\left(v^{\mathrm{ki}}\right)}^2\right]=(s_k^{i2}+s_{k+1}^{i2}+...+s_{k+n-1}^{i2})r---\left(42\right)$

Wherein: sⁱFor S^-1I-th row element of matrix, r is the variance of original measurement noise.

Step 4. applies scalar algorithm for estimating, and the state variable of integrated navigation system error model is expressed as scalar estimated form:

In conjunction with expression formula (32), the integrated navigation system error equation in a direction of space is approximately represented as:

δV_k=δ V_k-1-gTФ_k-1

${\mathrm{\Φ}}_k={\mathrm{\Φ}}_{k-1}+\frac{T}{R}\mathrm{\δ}{V}_{k-1}+T{\mathrm{\ϵ}}_{k-1}---\left(43\right)$

ε_k=T ε_k-1

Wherein: δ Vk is velocity error；Ф_kFor gyroscope the misaligned angle of the platform；ε k is gyroscopic drift；R is earth radius；G attaches most importance to Power acceleration；T is the discrete sampling cycle.

In conjunction with expression formula (24) and (38), the state variable of integrated navigation system error model is expressed as the mark as shown in expression formula (25) Amount estimated form:

Z (δ V)=z_k

$z\left(\mathrm{\Φ}\right)=\frac{1}{\mathrm{gT}}{z}_k-\frac{1}{\mathrm{gT}}{z}_{k+1}---\left(25\right)$

$z\left(\mathrm{\ϵ}\right)=-\frac{1}{{\mathrm{gT}}^2}{z}_k-\frac{1}{R}{z}_k+\frac{2}{{\mathrm{gT}}^2}{z}_{k+1}-\frac{1}{{\mathrm{gT}}^2}{z}_{k+2}$

In practical engineering application, when choosing integrated navigation system east orientation and north orientation velocity error as system measurement, application mark Amount algorithm for estimating, obtain integrated navigation system error full dimension scalar estimate expression formula (27):

Wherein: z_x,kFor integrated navigation system east orientation velocity error measured value；z_y,kMeasure for integrated navigation system north orientation velocity error Value.

Step 5. estimates expression formula according to the scalar of system state variables, the output calibration loop of design system state variable:

Based on scalar algorithm for estimating, on the basis of existing system measured value, estimate integrated navigation system velocity error, top respectively The value of the system state variableses such as spiral shell instrument the misaligned angle of the platform, gyroscopic drift, then in conjunction with expression formula (24), (25) and (27) etc., point The state variable output calibration loops such as other design navigating system velocity error, gyroscope the misaligned angle of the platform, thus build INS/GNSS Integrated navigation system, compensates sail body Navigation system error.

In conjunction with expression formula (25) and (27), by refer to east to gyroscope the misaligned angle of the platform output calibration as a example by, devise such as Fig. 4 institute The INS/GNSS integrated navigation system shown.

Understanding in conjunction with Fig. 4, the composition of INS/GNSS integrated navigation system is as follows:

Inertial navigation system (INS), satellite navigation and location system (GNSS), three adders, time delay process, two amplifiers, Depositor and line.

Understanding in conjunction with Fig. 4, the connected mode of INS/GNSS integrated navigation system is as follows:

Inertial navigation system is connected with first adder and second adder respectively；

Satellite navigation and location system is connected with first adder；

First adder is connected with time delay process and the second amplifier；

Time delay process and the first amplifier are connected；

First amplifier and the 3rd adder are connected；

Second amplifier and the 3rd adder are connected；

3rd adder is connected with depositor；

Depositor is connected with second adder；

The outfan of second adder is the outfan of integrated navigation system.

Understanding additionally, combine expression formula (25), the amplification coefficient of the first amplifier and the second amplifier is

INS/GNSS integrated navigation system work process is:

The signal that inertial navigation system (INS) is exported is the true navigation information of sail body navigation system, therefore, believes in this output In breath, inevitably it is mingled with inertial navigation system velocity error.Subsequently, the navigation information being mingled with INS error is input to In first adder；

The signal that satellite navigation and location system (GNSS) is exported is the true navigation information of navigation system being mingled with GNSS error. Subsequently, the navigation information being mingled with GNSS error has been input in first adder；

From the difference that signal is INS and GNSS error of first adder output, i.e. measure signal z_k+1=z_ins,k+1-z_gnss,k+1；

Measure signal z_k+1It is input in time delay process, exports the time delay measurement signal z of one beat at its outfan_k；

Measure signal z_k+1It is input in the second amplifier, is exaggerated from the output of its outfanSignal again；

Measure signal z_kIt is input in the first amplifier, is exaggerated from the output of its outfanSignal again；

It is exaggeratedZ again_kAnd z_k+1Signal is separately input in the 3rd adder, and by its outfan output shape such asGyroscope the misaligned angle of the platform signal；

Gyroscope the misaligned angle of the platform $z\left(\mathrm{\Φ}\right)=\frac{1}{\mathrm{gT}}{z}_k-\frac{1}{\mathrm{gT}}{z}_{k+1}$ Signal is temporary in a register；

According to formulaN=20...40, based on N number of gyroscope the misaligned angle of the platform measured value, calculates Gyroscope the misaligned angle of the platform estimated value after being smoothed and the variance of its estimated value；

Gyroscope the misaligned angle of the platform estimated value after depositor output smoothing, and be input in second adder；

The signal being mingled with INS errors is input in second adder；

Finally the outfan at second adder exports the integrated navigation system information after compensating.

In conjunction with integrated navigation system error correction and the compensation method of the present invention, separately design sail body navigation system position, speed With attitude error output calibration loop, application navigation test system CompaNav-2 has carried out verification experimental verification research, before and after correction Sail body navigation information error as shown in Fig. 5 to Figure 10.

In conjunction with Fig. 5 and Fig. 6, being not difficult to find out, the east orientation attitude error before output calibration is about-1.5 × 10^-6Rad/s arrives 0.5×10^-6Between rad/s, correspondingly, the east orientation attitude error after output calibration is about-7.5 × 10^-8Rad/s arrives -5×10^-8Between rad/s.Similarly, in conjunction with Fig. 8 and Fig. 9, it can be seen that the north orientation attitude error before output calibration is about -1 × 10^-6Rad/s to 6 × 10^-6Between rad/s, the north orientation attitude error after output calibration is about-1.5 × 10^-8Rad/s arrives 4×10^-8Between rad/s.In sum, in conjunction with Fig. 7 and Figure 10, it can be deduced that as drawn a conclusion: based on scalar algorithm for estimating INS/GNSS Combinated navigation method, improves the precision of INS/GNSS integrated navigation system, is that a kind of simple in construction, price are low The INS/GNSS Combinated navigation method that honest and clean, reliability is high.

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

Claims (1)

1. an INS/GNSS Combinated navigation method based on scalar algorithm for estimating, it is characterised in that method step is as follows:

Step 1. builds INS/GNSS integrated navigation system error model:

In order to obtain relative position, speed and the attitude information of sail body, choose sky, northeast geographic coordinate system as integrated navigation system The fundamental coordinate system of system clearing, wherein, sky, northeast coordinate origin is sail body barycenter, and x-axis is along reference ellipsoid prime vertical direction Pointing to east, y-axis points to north along reference ellipsoid meridian circle direction, and z-axis is determined by the right-hand rule；

Northeastward in sky coordinate system, INS/GNSS integrated navigation system error model is reduced to following form:

In formula: δ V_xRefer to that east is to movement velocity error for sail body；δV_yFor sail body north pointer direction movement velocity error；ΔV_x For accelerometer instrumental error along the x-axis direction；ΔV_yFor accelerometer instrumental error along the y-axis direction；a_xFor sail body along x Axial component of acceleration；a_yFor sail body component of acceleration along the y-axis direction；Ф_xFor gyroscope platform around x-axis direction Misalignment；Ф_yFor the gyroscope platform misalignment around y-axis direction；Ф_zFor the gyroscope platform misalignment around z-axis direction；ε_xFor The drift in gyroscope x-axis direction；ε_yDrift for gyroscope y-axis direction；ε_zDrift for gyroscope z-axis direction；ω_xFor top Spiral shell platform angular velocity component along the x-axis direction；ω_yFor gyropanel angular velocity component along the y-axis direction；ω_zFor gyropanel edge The angular velocity component in z-axis direction；w_xFor external disturbance along the x-axis direction；w_yFor external disturbance along the y-axis direction；w_zFor edge The external disturbance in z-axis direction；μ is the average frequency of gyroscopic drift change at random；G is acceleration of gravity；R is earth radius；Latitude residing for gyroscope platform；Represent variable × time differential；

Expression formula (1) is expressed as matrix form, i.e. integrated navigation system error model state equation is:

$\stackrel{.}{X}=\mathrm{FX}+W---\left(2\right)$

In formula:

System state vector X=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

System input noise $W={\left[\begin{array}{cccccccc}\mathrm{\Δ}{\stackrel{.}{V}}_x& \mathrm{\Δ}{\stackrel{.}{V}}_y& 0& 0& 0& w_x\left(t\right)& w_y\left(t\right)& w_z\left(t\right)\end{array}\right]}^T$

Systematic state transfer matrix

Step 2. discretization INS/GNSS integrated navigation system error model:

The state equation of INS/GNSS integrated navigation system error model is expressed as following discrete matrix form:

X_k=Φ X_k-1+W_k-1 (3)

In formula:

State vector X of integrated navigation system error model_k=[δ V_x δV_y Ф_x Ф_y Ф_z ε_x ε_y ε_z]^T,

Wherein, δ V_xRefer to that east is to movement velocity error for sail body；δV_yFor sail body north pointer direction movement velocity error；Ф_xFor Gyroscope platform is around the misalignment in x-axis direction；Ф_yFor the gyroscope platform misalignment around y-axis direction；Ф_zFor gyroscope platform around The misalignment in z-axis direction；ε_xDrift for gyroscope x-axis direction；ε_yDrift for gyroscope y-axis direction；ε_zFor gyroscope The drift in z-axis direction；K is time parameter, represents current time；

Input noise W of integrated navigation system error model_k-1=[B_x B_y 0 0 0 w_x w_y w_z]^T,

Wherein, B_xFor accelerometer drift error along the x-axis direction；B_yFor accelerometer drift error along the y-axis direction；w_x For external interfering noise along the x-axis direction；w_yFor external interfering noise along the y-axis direction；w_zDo for outside along the z-axis direction Disturb noise；

The systematic state transfer matrix of integrated navigation system error model

Wherein, T is the sampling time；μ is the average frequency of gyroscopic drift change at random；G is acceleration of gravity；R is the earth Radius；Latitude residing for gyroscope platform；

Step 3. based on Kalman filtering algorithm, builds scalar algorithm for estimating:

Shown in integrated navigation system error model such as expression formula (3) after discretization, now, integrated navigation system error measure equation It is expressed as:

Z_k=HX_k+V_k (4)

In formula: X_kFor system state vector；Z_kFor systematic survey vector；V_kFor measuring noise；K is time parameter, represents and works as The front moment；H is systematic survey matrix；

Now, classical Kalman filtering algorithm has a following form:

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(5\right)$

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1}---\left(6\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(7\right)$

P_k/k-1=Φ_k/k-1P_k-1Φ_k/k-1 ^T+Q_k (8)

P_k=(I-K_kH_k)P_k/k-1 (9)

Wherein, K_kKalman gain matrix；P_k/k-1The variance matrix of prior estimate error；P_kThe variance matrix of Posterior estimator error；R_k Measure the variance matrix of noise；Q_kThe variance matrix of input noise；K is time parameter, represents current time；

Based on initial time state variable X₁, in conjunction with expression formula (3) and (4), state variable X in n+1 moment_n+1Recursively represent For:

$X_{n+1}={\mathrm{\Φ}}^n{X}_1+{\mathrm{\Φ}}^{n-1}{W}_1+{\mathrm{\Φ}}^{n-2}{W}_2+...+W_n={\mathrm{\Φ}}^n{X}_1+\underset{j-=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j}---\left(10\right)$

Wherein, j is the natural number between 1-n；

Expression formula (10) is launched, is expressed as following form:

$\left[\begin{array}{c}{x}_{n+1}^1\\ x_{n+1}^2\\ ...\\ x_{n+1}^n\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]\left[\begin{array}{c}{x}_1^1\\ x_1^2\\ ...\\ x_1^n\end{array}\right]+\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]---\left(11\right)$

Wherein: $\left[\begin{array}{cccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& ...& {\mathrm{\φ}}_{1n}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& ...& {\mathrm{\φ}}_{2n}\\ ...& ...& ...& ...\\ {\mathrm{\φ}}_{n1}& {\mathrm{\φ}}_{n2}& ...& {\mathrm{\φ}}_{\mathrm{nn}}\end{array}\right]={\mathrm{\Φ}}^n$ φ is matrix ΦⁿIn each element； $\left[\begin{array}{c}{w}_1^1\\ w_1^2\\ ...\\ w_1^n\end{array}\right]=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{\mathrm{\Φ}}^{j-1}{W}_{n+1-j};$

Known by expression formula (11), the i-th variation per minute in state vectorIt is expressed as:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\φ}}_{i1}{x}_1^1+{\mathrm{\φ}}_{i2}{x}_1^2+...+{\mathrm{\φ}}_{\mathrm{in}}{x}_1^n+w_1^i---\left(12\right)$

With reference to expression formula (4), expression formula (12) is expressed as form:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+({\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n})+({\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n})+w_1^i---\left(13\right)$

Wherein: $Z_1^{*}=\left[\begin{array}{c}{z}_1^{*1}\\ z_1^{*2}\\ ...\\ z_1^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_1\\ z_2\\ ...\\ z_n\end{array}\right];$ $V_1^{*}=\left[\begin{array}{c}{v}_1^{*1}\\ v_1^{*2}\\ ...\\ v_1^{*n}\end{array}\right]={-\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{v}_1\\ {\mathrm{Hw}}_1+v_2\\ ...\\ {\mathrm{HA}}^{n-2}{w}_1+...+{\mathrm{Hw}}_1+v_n\end{array}\right];$

Vector shown in expression formula (13)It is based on initial time state variable X₁Introduced newly measures vector, similar Ground, state variable X based on k the most in the same time_k, when introducing k the most in the same time respectively, newly measure vector

$Z_k^{*}=\left[\begin{array}{c}{z}_k^{*1}\\ z_k^{*2}\\ ...\\ z_k^{*n}\end{array}\right]={\left[\begin{array}{c}H\\ \mathrm{H\Φ}\\ ...\\ {\mathrm{H\Φ}}^{n-1}\end{array}\right]}^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ ...\\ z_{k+n-1}\end{array}\right]---\left(14\right)$

Order ${\mathrm{\ξ}}_1^i={\mathrm{\φ}}_{i1}{z}_1^{*1}+{\mathrm{\φ}}_{i2}{z}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{z}_1^{*n}---\left(15\right)$

$w_1^{*i}={\mathrm{\φ}}_{i1}{v}_1^{*1}+{\mathrm{\φ}}_{i2}{v}_1^{*2}+...+{\mathrm{\φ}}_{\mathrm{in}}{v}_1^{*n}+w_1^i---\left(16\right)$

Now, expression formula (13) is expressed as:

$x_{n+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{x}_1^i+{\mathrm{\ξ}}_1^i+w_1^{*i}---\left(17\right)$

Wherein,For based on initial time state variable X₁Introduced measurement correction value；For based on initial time state variable X₁Introduced noise correction value；

Based on classical Kalman filtering algorithm, with reference to expression formula (5), i-th variation per minuteAt k=1,2 ... the optimum in moment is estimated MeterThere is following form:

${\hat{x}}_{\mathrm{nk}+1}^i={\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i+{\mathrm{\ξ}}_k^i+k_k^i(z_{k+1}^{*}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(k-1)+1}^i-{\mathrm{\ξ}}_k^i)---\left(18\right)$

Correspondingly, with reference to expression formula (7), gain coefficientFor:

$k_k^i=\frac{p_{k/k-1}^i}{p_{k/k-1}^i+r_k^i}---\left(19\right)$

Measure noise varianceFor:

$r_k^i=M\left[{\left(v_k^i\right)}^2\right]=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{\left(v_j^i\right)}^2---\left(20\right)$

$v_j^i=z_{j+1}^{*i}-{\mathrm{\φ}}_{\mathrm{ii}}{\hat{x}}_{n(j-1)+1}^i---\left(21\right)$

Wherein,For measuring noise；

With reference to expression formula (8), the variance of prior estimate errorFor:

$p_{k/k-1}^i={\mathrm{\φ}}_{\mathrm{ii}}^2{p}_{k-1}^i+M\left[{\left(v_k^i\right)}^2\right]\×{\left(k_{k-1}^i\right)}^2---\left(22\right)$

With reference to expression formula (9), the variance of Posterior estimator errorFor:

$p_k^i=(1-k_k^i)p_{k/k-1}^i---\left(23\right)$

Step 4. applies scalar algorithm for estimating, and the state variable of integrated navigation system error model is expressed as scalar estimated form

For integrated navigation system error model, its state variable includes: sail body movement velocity error delta V, gyroscope are flat Platform misalignmentGyroscopic drift ε, based on scalar algorithm for estimating, with reference to expression formula (14), integrated navigation system error measuring value It is expressed as:

$Z_k^{*}=S^{-1}\left[\begin{array}{c}{z}_k\\ z_{k+1}\\ z_{k+2}\end{array}\right]---\left(24\right)$

Wherein: $S=\left[\begin{array}{ccc}1& 0& 0\\ 1& -\mathrm{gT}& 0\\ 1-\frac{{\mathrm{gT}}^2}{R}& -2\mathrm{gT}& -{\mathrm{gT}}^2\end{array}\right]$ For System Observability matrix；

Specifically, the state variable of integrated navigation system error model is written as scalar form:

$\begin{array}{c}z\left(\mathrm{\δV}\right)=z_k\\ z\left(\mathrm{\Φ}\right)=\frac{1}{\mathrm{gT}}{z}_k-\frac{1}{\mathrm{gT}}{z}_{k+1}\\ z\left(\mathrm{\ϵ}\right)=-\frac{1}{{\mathrm{gT}}^2}{z}_k-\frac{1}{R}{z}_k+\frac{2}{{\mathrm{gT}}^2}{z}_{k+1}-\frac{1}{{\mathrm{gT}}^2}{z}_{k+2}\end{array}---\left(25\right)$

With reference to expression formula (25), when measuring gyropanel misalignment Ф, the conversion variance measuring noise is:

$r^{*}=\frac{2}{g^2{T}^2}r---\left(6\right)$

Wherein: the variance of noise when r is measuring speed error delta V；

In practical engineering application, choose integrated navigation system east orientation and north orientation velocity error as system measurement, application scalar Algorithm for estimating, with reference to expression formula (2) and (25), obtains following integrated navigation system error full dimension scalar estimation expression formula:

Wherein: z_x,kFor integrated navigation system east orientation velocity error measured value；z_y,kMeasure for integrated navigation system north orientation velocity error Value；

The sail body east orientation and north orientation velocity error obtained is measured in application, with reference to expression formula (27), calculates and estimates integrated navigation The velocity error of system all directions, misalignment and gyroscopic drift, realize integrated navigation system error compensation and correction with this.