CN106885570A - A kind of tight integration air navigation aid based on robust SCKF filtering - Google Patents

Abstract

The invention discloses a kind of tight integration air navigation aid based on robust SCKF filtering.The method step is as follows：Simulate the IMU data and GPS intermediate-freuqncy signals of generation guided missile successively by path generator, satellite signal simulator；The IMU data for simulating generation are carried out into inertial reference calculation, GPS intermediate-freuqncy signals injection software receiver is carried out into navigation calculation；Set up GPS/SINS tight integration Navigation System Models under launch inertial coordinate system；On the basis of normal square root volume Kalman filtering (SCKF), introduce robust M estimation, Automatic adjusument is carried out to systematic observation noise battle array, robust square root volume Kalman filtering (RSCKF) algorithm is constituted, correction is filtered to system mode.The observed anomaly error that the present invention can occur effectively in elimination system, improves the antijamming capability and navigation accuracy of GPS/SINS tight integrations navigation.

Description

A kind of tight integration air navigation aid based on robust SCKF filtering

First, technical field

The present invention relates to GPS/SINS integrated navigation technologies field, particularly a kind of tight integration based on robust SCKF filtering Air navigation aid.

2nd, background technology

GPS/SINS(Global Positioning System/Strap down Inertial Navigation System respective advantage has been merged in) integrated navigation, makes navigation accuracy higher than the precision that each system works independently.Due to navigation ring The polytropy in border and complexity, the system model of GPS/SINS integrated navigations often have nonlinear feature, therefore research should Nonlinear filtering for integrated navigation is particularly important.

Traditional nonlinear filtering includes two kinds of EKF (EKF) and Unscented kalman filtering (UKF).EKF will Nonlinear function is linearized, and can produce truncated error；UKF has filtering accuracy higher to nonlinear system, but without strict Mathematical derivation, in the iterative process of filtering, due to matrix decomposition and inverting, it is difficult to ensure the positive definite of state covariance matrix Property.Square root volume Kalman filtering (SCKF) is a kind of emerging non-linear filtering method, is carried by Arasaratnam et al. Go out.The algorithm pushes away proof by strict mathematics, takes one group of equal volume point of weights to approach based on sphere radial direction rule and is The Posterior distrbutionp of system state, it is ensured that the orthotropicity of covariance matrix.Meanwhile, transmission prediction and evaluated error covariance matrix are used The difference of two squares, it is to avoid matrix decomposition and inversion operation, it is ensured that its Positive.The algorithm be widely used in mixed filtering, The field such as Attitude estimation and navigational guidance.

However, in practice, due to the influence of inside and outside portion's environment uncertain factor, it also occur that observed anomaly etc. Disturbance, so as to cause larger evaluated error.

3rd, the content of the invention

It is an object of the invention to provide a kind of real-time it is good, it is high precision based on robust SCKF filtering tight integration navigation Method, to eliminate influence of the observed anomaly to system and improve the performance of combined filter.

The technical solution for realizing the object of the invention is：A kind of tight integration air navigation aid based on robust SCKF filtering, It is characterised in that it includes following steps：

Step 1, the IMU data and GPS intermediate frequencies of simulating generation guided missile successively by path generator, satellite signal simulator Signal；

Step 2, will simulate generation IMU data carry out inertial reference calculation, by GPS intermediate-freuqncy signals injection receiver navigated Resolve；

Step 3, set up GPS/SINS tight integration Navigation System Models under launch inertial coordinate system；

Step 4, be SCKF in normal square root volume Kalman filtering on the basis of, introduce robust M, systematic observation is made an uproar Acoustic matrix carries out Automatic adjusument, and it is RSCKF algorithms to constitute robust square root volume Kalman filtering, and system mode is filtered Correction.

Compared with prior art, its remarkable advantage is the present invention：(1) standard SCKF filtering algorithms are introduced into integrated navigation In, meet the nonlinear characteristic of system model, compared with nonlinear filtering UKF, CKF, it is to avoid matrix decomposition and inversion operation, Ensure that the Positive of covariance matrix；(2) robust M-estimator principle is introduced into SCKF filtering, further enhancing combination filter The robustness of ripple, improves integrated navigation and location precision, ability of tracking in adverse circumstances.

4th, illustrate

Fig. 1 is the flow chart of tight integration air navigation aid of the present invention based on robust SCKF filtering.

Fig. 2 is by the ballistic trajectory under the launch inertial coordinate system of path generator simulation generation in embodiment 1.

Fig. 3 is the position error comparing result that robust SCKF and SCKF filters in X-direction in embodiment 1.

Fig. 4 is robust SCKF and SCKF filtering position error comparing result in the Y direction in embodiment 1.

Fig. 5 is the position error comparing result that robust SCKF and SCKF filters in Z-direction in embodiment 1.

5th, specific embodiment

Below in conjunction with the accompanying drawings and specific embodiment is described in further detail to the present invention.

With reference to Fig. 1, GPS/SINS tight integration air navigation aid of the present invention based on strong tracking UKF filtering, step is as follows：

Step 1, the IMU data and GPS intermediate frequencies of simulating generation guided missile successively by path generator, satellite signal simulator Signal；

According to the physical model that ballistic missile flies, the parameter in flight each stage, such as acceleration, angular speed change are set, The trajectory track of guided missile and corresponding IMU data can be generated, trajectory track is imported into satellite simulator and treatment is Can obtain corresponding GPS intermediate-freuqncy signals.

Step 2, the IMU data of simulation generation carry out inertial reference calculation, and GPS intermediate-freuqncy signals injection software receiver is led Boat is resolved；

Inertial reference calculation updates position, speed, the attitude information of body；Software receiver navigation calculation obtains body position, Satellite position, pseudo-range information.

Step 3, GPS/SINS tight integration Navigation System Models under launch inertial coordinate system are set up, it is specific as follows：

(3.1) state equation：

Wherein, X_s15 state error amounts comprising SINS；X_g2 state error amounts comprising GPS, it is specific as follows：

Wherein,It is the attitude misalignment of system；δV_x、δV_y、δV_zIt is three axle sides under launch inertial coordinate system To velocity error；δ X, δ Y, δ Z are the site error of three direction of principal axis under launch inertial coordinate system；ε_x、ε_y、ε_zWithRespectively gyroscope constant value drift, accelerometer bias three direction of principal axis component, Δ l_uIt is equivalent clock Poor range error, Δ l_ruIt is to float equivalent distance rate error with clock；

Wherein, T_ruIt is the correlation time of GNSS clock frequency drift, w_uIt is GNSS clock error white noise；w_ruDuring for GNSS Clock frequency error white noise；F₁It is the antisymmetric matrix of the axle specific force of X, Y, Z tri-；G_eIt is three axle local derviations of observation station to underground vector Matrix；The transition matrix of navigational coordinate system is transformed into for missile coordinate system；I is unit matrix；The noise of system drives matrix G_sT () is：

The noise vector w of system_sT () is：

w_s(t)=[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

Wherein, w_gx、w_gy、w_gzRespectively white Gaussian noise of the gyroscope under the axle of X, Y, Z tri-；w_ax、w_ay、w_azRespectively plus White Gaussian noise of the speedometer under the axle of X, Y, Z tri-.

(3.2) systematic observation equation

Under launch inertial coordinate system, the observational equation of ballistic Missile GPS/SINS deep integrated navigation systems be divided into pseudorange difference and Pseudorange rates differ from two parts, specially：

Wherein：M represents the number of satellite that GPS is received；ρ_IFor the positional information calculation gained of inertial reference calculation is pseudo- Away from ρ G are that GPS measures gained pseudorange；H () is non-linear measurement function；V (t) is that each element is the Gauss of zero-mean White noise.

Step 4, on the basis of normal square root volume Kalman filtering (SCKF), introduce robust M, to systematic observation noise Battle array carries out Automatic adjusument, constitutes robust square root volume Kalman filtering (RSCKF) algorithm, and school is filtered to system mode Just, it is specific as follows：

The system model set up in step 3 is carried out into discretization first, following formula is obtained：

In formula：X_k∈Rⁿ,Z_k∈R^mThe state vector of etching system and measurement vector during respectively k；F_kFor linear condition is shifted Matrix, h () is that mission nonlinear measures function；w_k、v_kIt is orthogonal zero mean Gaussian white noise sequence, statistical property Meet following condition：

In formula：Q_kIt is nonnegative definite matrix, R_kIt is positive definite matrix；δ_kjIt is Kronecker- δ functions；

SCKF algorithms are realized, three rank volume criterions are first according to, the one group 2n volume point for waiting weights distribution is chosen {ω_i, ξ_iNone-linear approximation is realized, wherein：

Wherein：ξ_iIt is volume point vector；ω_iIt is respective weights；N is the dimension of system state variables；[l]∈Rⁿ, it is generation Operator, as n=2, point set expressed as shown below：

[l]_iIt is [l] ∈ R²In the i-th column element；SCKF algorithms are comprised the following steps that：

1) filtering initialization

S₀=chol (P₀)

Wherein, x₀It is system mode initial value；It is x₀Average；P₀It is x₀State error covariance matrix；Subscript T is to this Matrix or vectorial transposition, it is as follows；E () is to seek mathematic expectaion；Chol () represents Cholesky factorization；S₀It is P₀Qiao in This gene polyadenylation signal.

2) time renewal：

It is consistent that the time of robust square root volume Kalman filtering updates normal square root Kalman filtering；

Wherein, S_k-1It is the square-root factor at k-1 moment；It is the state vector updated value at k-1 moment；It is i-th State volume sampled point；u_k-1It is system input quantity；F () is nonlinear state transfer function；It is i-th one-step prediction State sampled value；For the system mode one-step prediction value that weighted average is obtained；Tria () is represented QR is decomposed；S_k|k-1It is the k moment square-root factors estimated；MatrixIt is expressed as：

3) measure and update：

It is consistent with standard SCKF measurements renewal process, change only is made to noise matrix R：

Wherein,To measure the i-th volume sampled point for updating；H () is non-linear measurement function；It is i-th Individual measuring value sampled point；Measurement estimate obtained by measuring value sampled point weighted average；According to Robust M estimation algorithm carries out the observation noise matrix after Automatic adjusument to noise R；Matrix ζ_k|k-1It is expressed as：

Estimate that Cross-covariance is：

Wherein, matrix χ_k|k-1It is expressed as：

S_k=Tria ([χ_k|k-1-K_k·ζ_k|k-1,K_k·S_R,k])

Wherein, K_kIt is the Kalman filtering gain at tried to achieve k moment；S_kIt is the square-root factor decomposed by QR；Other Parameter is as defined above.

4) robust modified R, comprises the following steps that：

Based on standard SCKF equations, the Filtering Model of robust SCKF is set up；The amount in model is only influenceed due to measurement information Renewal process is surveyed, so relative to standard SCKF algorithms, robust SCKF algorithms are only to measuring the associated expression in renewal equation Adjustment amendment, robust noise are carried outBattle array be and R_kMeasuring noise square difference battle array of equal value, by the equivalence in robust M-estimator method Weight matrixInvert acquisition, i.e.,

Equivalent weight matrix is asked for using Huber methods herein；IfMatrix element beI, j=1,2 ..., n then have Following method determines equivalent weight matrix：

Wherein,The respectively diagonal element and off-diagonal element of equivalent weight matrix；σ_ii,σ_ijIt is former R_kBattle array Diagonal element and off-diagonal element；K is constant, generally takes 1.2~1.5；v_iIt is observed quantity z_iResidual component,For standard is residual Difference component, byDraw, wherein：

Wherein, (P_yy,k|k-1)_iiTo take P_yy,k|k-1The i row i column elements of matrix；z_kIt is substantial amount measured value；To ask above The measurement estimate for obtaining；v_iIt is i-th element of observation residual vector v.

Robust amendment is obtainedSubstitute into step 3) measure update in, that is, obtain robust square root volume volume Kalman Filtering.According to above step, after being filtered, position, speed, the margin of error of attitude quantity of state in system are obtained, then to combination The SINS states of navigation are corrected, and export final navigation results.

The present invention is described in further detail with reference to specific embodiment.

Embodiment 1

In order to verify the validity and superiority of robust SCKF filtering methods proposed by the present invention, this filtering method is used Emulated in GPS fixed on shell/SINS tight integration navigation system, and will emulation navigation results and standard SCKF filter result ratios Compared with.Emulation is as follows：

1) simulated conditions

It is 90 ° to set the initial pitching of body, and initial roll angle and yaw angle are 0 °；Initial position is：30.03 ° of latitude, 109.6 ° of longitude, is highly 10m；Initial velocity：Forward direction be 394.8917m/s (earth rotation speed), day to be laterally 0m/s；Azimuth firing angle is 90 °；Gyro zero is set to 10 °/h partially, and white noise is set to 1 °/h；Accelerometer bias are set to 1mg, in vain Noise is set to 0.5mg；Receive star number m=4.GPS sample frequencys 1HZ, INS sample frequency 200HZ, filtering cycle 1s, simulation time 360s.Within the 200s-210s time periods, GPS pseudoranges are made to add white noise to meet average 0, the Gaussian Profile of standard deviation 100；GPS It is 0 that pseudorange rates add white noise to meet average, and standard deviation is 1 Gaussian Profile.

Ballistic trajectory such as Fig. 2 of guided missile is set；Fig. 3-5 be respectively standard SCKF and robust SCKF tight integrations navigation X, Y, Tri- position errors in direction of Z.

2) interpretation of result

Fig. 2-Fig. 4 is the integrated navigation site error comparison diagram for respectively being filtered by SCKF, robust SCKF.Can see Go out, 10m is up in initial position error, but can Fast Convergent.In 20s~200s periods, GPS/SINS integrated navigations In the case that the measurement noise of system is in normal condition, two kinds of filtering algorithms can realize being accurately positioned for guided missile；In 200s ~210s the periods, due to the pseudorange by adding, pseudorange rates observation rough error influenceed, standard SCKF exist X-direction error [- 9.5m, 4.1m], Y-direction position error [- 9.6m, 3.8m], Z-direction position error [- 3.6m, 3.8m], and convergence time is more long；And resist Three direction of principal axis errors of difference SCKF filtering are held in [- 1.5m, 2.1m] interval, and positioning result is substantially better than standard SCKF calculations Method；In 250s~360s periods, standard SCKF filtering convergences reach the effect suitable with robust SCKF.Therefore, knot can be drawn By：A kind of tight integration air navigation aid based on robust SCKF filtering proposed by the present invention, is meeting combination under normal circumstances While navigation work, the influence of observed quantity anomalous differences Chang Sheng is reduced, effectively improve filter filtering performance, improve navigation Positioning precision.

Claims (5)

1. it is a kind of based on robust SCKF filtering tight integration air navigation aid, it is characterised in that comprise the following steps：

Step 1, the IMU data for simulating generation guided missile successively by path generator, satellite signal simulator and GPS intermediate frequencies letter Number；

Step 2, will simulate generation IMU data carry out inertial reference calculation, by GPS intermediate-freuqncy signals injection receiver carry out navigational solution Calculate；

Step 3, set up GPS/SINS tight integration Navigation System Models under launch inertial coordinate system；

Step 4, be SCKF in normal square root volume Kalman filtering on the basis of, introduce robust M, to systematic observation noise battle array Automatic adjusument is carried out, it is RSCKF algorithms to constitute robust square root volume Kalman filtering, and school is filtered to system mode Just.

2. according to claim 1 based on robust SCKF filtering tight integration air navigation aid, it is characterised in that step 1 Described in by path generator, satellite signal simulator simulate successively generation guided missile IMU data and GPS intermediate-freuqncy signals, tool Body is：

According to the physical model that ballistic missile flies, the parameter in flight each stage is set, generate guided missile trajectory track and Corresponding IMU data, satellite simulator treatment is imported by trajectory track, obtains corresponding GPS intermediate-freuqncy signals.

3. according to claim 1 based on robust SCKF filtering tight integration air navigation aid, it is characterised in that step 2 Described in will simulate generation IMU data carry out inertial reference calculation, by GPS intermediate-freuqncy signals injection receiver carry out navigation calculation, have Body is：

Inertial reference calculation updates position, speed, the attitude information of body；Receiver navigation calculation obtain body position, satellite position, Pseudo-range information.

4. according to claim 1 based on robust SCKF filtering tight integration air navigation aid, it is characterised in that step 3 Described in set up GPS/SINS tight integration Navigation System Models under launch inertial coordinate system, comprise the following steps that：

(3.1) state equation：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_s\left(t\right)\\ {\stackrel{\·}{X}}_g\left(t\right)\end{array}\right]=\left[\begin{array}{cc}{F}_s\left(t\right)& 0\\ 0& F_g\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_s\left(t\right)\\ X_g\left(t\right)\end{array}\right]+\left[\begin{array}{cc}{G}_s\left(t\right)& 0\\ 0& G_g\left(t\right)\end{array}\right]\left[\begin{array}{c}{w}_s\left(t\right)\\ w_g\left(t\right)\end{array}\right]$

Wherein, X_s15 state error amounts comprising SINS；X_g2 state error amounts comprising GPS, it is specific as follows：

Wherein,It is the attitude misalignment of system；δV_x、δV_y、δV_zIt is three direction of principal axis under launch inertial coordinate system Velocity error；δ X, δ Y, δ Z are the site error of three direction of principal axis under launch inertial coordinate system；ε_x、ε_y、ε_zAnd ▽_x、▽_y、▽_zRespectively For gyroscope constant value drift, accelerometer bias three direction of principal axis component, Δ l_uIt is the range error of equivalent clock correction, Δ l_ru It is to float equivalent distance rate error with clock；

w_g(t)=[w_u w_ru]^T

$F_s\left(t\right)={\left[\begin{array}{ccccc}{0}_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_b^n& 0_{3\×3}\\ F_1& 0_{3\×3}& G_e& 0_{3\×3}& C_b^n\\ 0_{3\×3}& I& & 0_{3\×9}& \\ & & 0_{6\×15}& & \end{array}\right]}_{15\×15}$

Wherein, T_ruIt is the correlation time of GNSS clock frequency drift, w_uIt is GNSS clock error white noise；w_ruFor GNSS clock frequently Rate error white noise；F₁It is the antisymmetric matrix of the axle specific force of X, Y, Z tri-；G_eIt is three axle local derviation matrixes of observation station to underground vector；The transition matrix of navigational coordinate system is transformed into for missile coordinate system；I is unit matrix；The noise of system drives matrix G_s(t) For：

$G_s\left(t\right)={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$

The noise vector w of system_sT () is：

w_s(t)=[ω_gx ω_gy ω_gz ω_ax ω_ay ω_az]^T

Wherein, w_gx、w_gy、w_gzRespectively white Gaussian noise of the gyroscope under the axle of X, Y, Z tri-；w_ax、w_ay、w_azRespectively acceleration Count the white Gaussian noise under the axle of X, Y, Z tri-；

(3.2) systematic observation equation

Under launch inertial coordinate system, the observational equation of ballistic Missile GPS/SINS deep integrated navigation systems is divided into pseudorange difference and pseudorange Rate differs from two parts, specially：

$Z={\left[\begin{array}{c}\mathrm{\δ}\mathrm{\ρ}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\ρ}}\end{array}\right]}_{2m\×1}={\left[\begin{array}{c}{\mathrm{\ρ}}_I-{\mathrm{\ρ}}_G\\ {\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G\end{array}\right]}_{2m\×1}=H\left(X\right(t\left)\right)+v\left(t\right)$

Wherein：M represents the number of satellite that GPS is received；ρ_IIt is the positional information calculation gained pseudorange of inertial reference calculation, ρ_G For GPS measures gained pseudorange；H () is non-linear measurement function；V (t) is that each element is the Gauss white noise of zero-mean Sound.

5. according to claim 1 based on robust SCKF filtering tight integration air navigation aid, it is characterised in that step 4 Described in be SCKF in normal square root volume Kalman filtering on the basis of, introduce robust M estimation, to systematic observation noise battle array Automatic adjusument is carried out, it is RSCKF algorithms to constitute robust square root volume Kalman filtering, and school is filtered to system mode Just, comprise the following steps that：

The system model set up in step 3 is carried out into discretization first, following formula is obtained：

$\left\{\begin{array}{c}{X}_k=F_k{X}_{k-1}+w_k\\ Z_k=h\left(X_k\right)+v_k\end{array}\right.$

In formula：X_k∈Rⁿ,Z_k∈R^mThe state vector of etching system and measurement vector during respectively k；F_kIt is linear condition transfer matrix, H () is that mission nonlinear measures function；w_k、v_kIt is orthogonal zero mean Gaussian white noise sequence, statistical property meets such as Lower condition：

$\left\{\begin{array}{cc}e\[w_k\]=0& \cos (w_k,w_j)=Q_k{\mathrm{\δ}}_{kj}\\ e\[v_k\]=0& \cos (v_k,v_j)=R_k{\mathrm{\δ}}_{kj}\\ \cos (w_k,v_k)=0& \end{array}\right.$

In formula：Q_kIt is nonnegative definite matrix, R_kIt is positive definite matrix；δ_kjIt is Kronecker- δ functions；

SCKF algorithms are realized, three rank volume criterions are first according to, the one group 2n volume point { ω for waiting weights distribution is chosen_i, ξ_i} None-linear approximation is realized, wherein：

${\mathrm{\ω}}_i=\frac{1}{m},i=1,2,...,m;m=2n$

${\mathrm{\ξ}}_i=\sqrt{\frac{m}{2}{\[l\]}_i}$

Wherein：ξ_iIt is volume point vector；ω_iIt is respective weights；N is the dimension of system state variables；[l]∈Rⁿ, preconceived plan of making a living Son, as n=2, point set expressed as shown below：

$\left\{\left(\begin{array}{c}1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\end{array}\right)\right\}$

[l]_iIt is [l] ∈ R²In the i-th column element；SCKF algorithms are comprised the following steps that：

1) filtering initialization

${\hat{x}}_0={\mathrm{Ex}}_0$

$P_0=E\[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\]$

S₀=chol (P₀)

Wherein, x₀It is system mode initial value；It is x₀Average；P₀It is x₀State error covariance matrix；Subscript T is to the matrix Or vectorial transposition, it is as follows；E () is to seek mathematic expectaion；Chol () represents Cholesky factorization；S₀It is P₀Cholesky The factor；

2) time renewal：

It is consistent that the time of robust square root volume Kalman filtering updates normal square root Kalman filtering；

${\widetilde{\mathrm{\χ}}}_{k-1}^{\left(i\right)}={\hat{x}}_{k-1}+S_{k-1}{\mathrm{\ξ}}_i$

${\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}=f({\widetilde{\mathrm{\χ}}}_{k-1}^{\left(i\right)},u_{k-1})$

${\hat{x}}_{k|k-1}=\underset{1}{\overset{m}{\Σ}}\frac{1}{m}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(i\right)}$

$S_{k|k-1}=Tria(\[{\mathrm{\χ}}_{k|k-1}^{*},S_{Q,k-1}\])$

Wherein, S_k-1It is the square-root factor at k-1 moment；It is the state vector updated value at k-1 moment；It is i-th state Volume sampled point；u_k-1It is system input quantity；F () is nonlinear state transfer function；It is i-th one-step prediction state Sampled value；For the system mode one-step prediction value that weighted average is obtained；Tria () represents QR points Solution；S_k|k-1It is the k moment square-root factors estimated；MatrixIt is expressed as：

${\mathrm{\χ}}_{k|k-1}^{*}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(1\right)}-{\hat{x}}_{k|k-1}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(2\right)}-{\hat{x}}_{k|k-1}& \mathrm{...}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{*\left(m\right)}-{\hat{x}}_{k|k-1}\end{array}\right]$

3) measure and update：

It is consistent with standard SCKF measurements renewal process, change only is made to noise matrix R：

${\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(i\right)}={\hat{x}}_{k|k-1}+S_{k|k-1}{\mathrm{\ξ}}_i$

$Z_{k|k-1}^{\left(i\right)}=h\left({\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(i\right)}\right)$

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}\frac{1}{m}{Z}_{k|k-1}^{\left(i\right)}$

$S_{zz,k|k-1}=Tria\left(\left[\begin{array}{cc}{\mathrm{\ζ}}_{k|k-1}& {\stackrel{\‾}{S}}_{R,k}\end{array}\right]\right)$

Wherein,To measure the i-th volume sampled point for updating；H () is non-linear measurement function；It is i-th amount Measured value sampled point；Measurement estimate obtained by measuring value sampled point weighted average；It is according to robust M Algorithm for estimating carries out the observation noise matrix after Automatic adjusument to noise R；Matrix ζ_k|k-1It is expressed as：

${\mathrm{\ζ}}_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{Z}_{k|k-1}^{\left(1\right)}-{\hat{z}}_{k|k-1}& Z_{k|k-1}^{\left(2\right)}-{\hat{z}}_{k|k-1}& \mathrm{...}& Z_{k|k-1}^{\left(m\right)}-{\hat{z}}_{k|k-1}\end{array}\right]$

Estimate that Cross-covariance is：

$P_{xz,k|k-1}={\mathrm{\χ}}_{k|k-1}\·{\mathrm{\ζ}}_{k|k-1}^T$

Wherein, matrix χ_k|k-1It is expressed as：

${\mathrm{\χ}}_{k|k-1}=\frac{1}{\sqrt{m}}\left[\begin{array}{cccc}{\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(1\right)}-{\hat{x}}_{k|k-1}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(2\right)}-{\hat{x}}_{k|k-1}& \mathrm{...}& {\widetilde{\mathrm{\χ}}}_{k|k-1}^{\left(m\right)}-{\hat{x}}_{k|k-1}\end{array}\right]$

$K_k=(P_{xz,|k-1}/S_{zz,k|k-1}^T)/S_{zz,k|k-1}$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k\[z_k-{\hat{z}}_{k|k-1}\]$

$S_k=Tria(\[{\mathrm{\χ}}_{k|k-1}-K_k\·{\mathrm{\ζ}}_{k|k-1},K_k\·S_{R,k}\])$

Wherein, K_kIt is the Kalman filtering gain at tried to achieve k moment；S_kIt is the square-root factor decomposed by QR；

4) robust modified R, comprises the following steps that：

Based on standard SCKF equations, the Filtering Model of robust SCKF is set up；Measurement in model is only influenceed due to measurement information more New process, so robust SCKF algorithms have only carried out adjustment amendment, robust noise to measuring renewal equationBattle array be and R_kIt is of equal value Measuring noise square difference battle array, by the equivalent weight matrix in robust M-estimator methodInvert acquisition, i.e.,

${\stackrel{\‾}{R}}_k={\stackrel{\‾}{P}}^{-1}$

Equivalent weight matrix is asked for using Huber methods herein；IfMatrix element beI, j=1,2 ..., n then have as follows Method determines equivalent weight matrix：

$\begin{array}{cc}{\stackrel{\‾}{p}}_{t\left(ii\right)}=\left\{\begin{array}{cc}\frac{1}{{\mathrm{\σ}}_{ii}},& \left|\frac{v_i}{{\mathrm{\σ}}_{v_i}}\right|=\left|{\stackrel{\‾}{v}}_i\right|\≤k\\ \frac{k}{{\mathrm{\σ}}_{ii}\left|{\stackrel{\‾}{v}}_i\right|},& \left|{\stackrel{\‾}{v}}_i\right|>k\end{array}\right.& {\stackrel{\‾}{p}}_{t\left(ij\right)}=\left\{\begin{array}{cc}\frac{1}{{\mathrm{\σ}}_{ij}},& \left|{\stackrel{\‾}{v}}_i\right|\≤k,and\left|{\stackrel{\‾}{v}}_j\right|\≤k\\ \frac{k}{{\mathrm{\σ}}_{ij}\·\max \{\left|{\stackrel{\‾}{v}}_i\right|,\left|{\stackrel{\‾}{v}}_j\right|\}},& \left|{\stackrel{\‾}{v}}_i\right|>k,or\left|{\stackrel{\‾}{v}}_j\right|>k\end{array}\right.\end{array}$

Wherein,The respectively diagonal element and off-diagonal element of equivalent weight matrix；σ_ii,σ_ijIt is former R_kThe diagonal element of battle array Element and off-diagonal element；K is constant, takes 1.2~1.5；v_iIt is observed quantity z_iResidual component,It is residual component, byDraw, wherein：

${\mathrm{\σ}}_{v_i}={\left(P_{yy,k|k-1}\right)}_{ii}$

$v_i={(z_k-{\hat{z}}_{k|k-1})}_i$

Wherein, (P_yy,k|k-1)_iiTo take P_yy,k|k-1The i row i column elements of matrix；z_kIt is substantial amount measured value；It is the measurement tried to achieve Estimate；v_iIt is i-th element of observation residual vector v；

Robust amendment is obtainedSubstitute into step 3) middle measurement renewal, that is, obtain robust square root volume Kalman filtering；According to Above step, after being filtered, obtains position, speed, the margin of error of attitude quantity of state in system, then to the SINS of integrated navigation State is corrected, and exports final navigation results.