CN104408315B - A kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations - Google Patents

Abstract

The invention discloses a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations, on the basis of the conventional Kalman filter model of SINS/GPS integrated navigations is set up, this method uses matrix in block form technology, the high level matrix computing being related in calculating filtering is deployed to derive in Matlab environment by sub-piecemeal, it is to avoid a large amount of insignificant duplicate keys computings；And by the calculating process of the time-consuming parameter of subdivision, the numerical value decoupling with other renewal processes is obtained in subprocess level, so as to provide a kind of new lossless parallel processing mechanism, drastically increase the real-time that filtering is calculated.Instant invention overcomes conventional decomposition filtering optimization algorithmic derivation complexity it is not easy to the shortcoming of Project Realization, it is to avoid damage precision decoupling in Traditional parallel filtering, realize the high efficiency lossless data filtering of SINS/GPS integrated navigation systems.

Description

A kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations

Technical field

The present invention relates to the technical field of integrated navigation system data filtering, and in particular to one kind is based on SINS/GPS combinations The Kalman filtering numerical optimization of navigation.

Background technology

In the data filtering link of SINS/GPS integrated navigation systems, the processing to " coloured noise " is usually contained, at this moment The applicable elements of classical Kalman filter are not met, in order to remain able to using optimal estimation techniques, it is necessary to the state of progress Expand.But with the increase of state dimension, amount of calculation drastically expands, numerical stability declines, or even " dimension calamity occurs It is difficult ".For this problem, method general at present is that decomposing filtering, SVD using square root information filter SRIF, U-D filters Amount of calculation and enhancing numerical value robustness are reduced Deng filtering optimization algorithm is decomposed, and filtering real-time is improved using parallel algorithm. But, the derivation of decomposition algorithm needs to use the matrix theory knowledge of complexity, there is certain difficulty on engineer applied；And And it is still O (n to decompose filtering algorithm complexity³), with the further increase of state dimension, real-time is also challenged.And Although row Kalman filter greatly improves real-time, but its Uncoupled procedure is based on measuring updating and forces delayed filtering week Phase realizes that this can bring certain precision to damage, and numerical value instability problem may be caused after successive ignition.In addition, making For the optimized algorithm that a class is general, the characteristics of above-mentioned several wave filters are all not based on SINS/GPS integrated navigation systems carries out special Door design, the numerical characteristics for also rarely having research and utilization system both at home and abroad carry out deep optimization to Kalman filter process.

The content of the invention

The technical problem to be solved in the present invention is：Overcome the shortcomings of existing algorithm, from the angle of numerical computations, propose It is simple and easy to apply in a kind of engineering, carry out offline derive using SINS/GPS system digits feature and optimize what is handled with real-time parallel The lossless Kalman filtering optimized algorithm of high efficiency.

The present invention solve the technical scheme that uses of above-mentioned technical problem for：A kind of karr based on SINS/GPS integrated navigations Graceful filtering numerical optimization, it is characterised in that comprise the following steps：

Step (1), set up a kind of SINS/GPS integrated navigation loose coupling closed loops based on the combination of position-tachometric survey Kalman filter model, is deployed to represent the parameter matrix in model, including coefficient of regime A with 3 rank square formations_n, state-transition matrix φ_n, system noise factor G_n, measure vector coefficients H_n, system noise variance Q_n, measuring noise square difference R_n, the error covariance time Updated value P_n(-), error covariance measure updated value P_n(+_e) and Kalman filter gain K_n；

Step (2), usage factor matrix A_nOpenness, special sub-piecemeal and intermediate variableIn Matlab rings Symbolic operation function is used in border, to formulaIt is offline to derive by 3 rank sub-piecemeal unfolding calculations Go out discrete filter transfer matrix φ_nSimplification form of calculation；

Step (3), the φ being derived by using step (2)_nPiecemeal is openness, special sub-piecemeal and noise matrix Γ_nQ_n Symmetry, derived offline in Matlab environmentSimplification form of calculation；

Step (4), utilize φ_nPiecemeal is openness, special sub-piecemeal, intermediate variableAnd covariance square Battle array P_nThe symmetry of (-), to formula in Matlab environmentBased on the expansion of 3 rank sub-piecemeals Calculate, derive P_nThe simplification form of calculation that (-) time updates；

Step (5), utilize system measurements equation coefficient matrix H_nPiecemeal is openness, special sub-piecemeal, in Matlab rings To formula in borderThe offline derivation of equation is carried out, filtering gain K is obtained_nSimplify meter Calculation form；

Step (6), utilize H_nPiecemeal is openness, special sub-piecemeal, covariance matrix P_nThe symmetry of (-), in Matlab To formula P in environment_n(+_e)=(I-K_nH_n)P_n(-) presses 3 rank submatrix unfolding calculations, and P is pushed away offline_n(+_e) measure the simplified meter updated Calculation form；

Step (7), subdivision P_n(+_e) and P_nThe calculating process of (-), and by time-consuming matrix P_n(-) divides " useful ", " useless " Information, by dependence of the subprocess analytic hierarchy process to parameter, obtaining a kind of new numerical value decoupling mechanism, on this basis Realize the lossless parallel processing to filtering.

Further, described step (2) is right3 calculation optimization is specifically comprised the following steps：

Step (21), usage factor matrix A_nOpenness and its special sub-piecemeal, derive offlineSimplify calculating Form is simultaneously preserved as intermediate variable, and derivation result showsIt is openness with piecemeal, and the 3rd row piecemeal S_i3, on diagonal Piecemeal S₅₅、S₆₆For special sub-piecemeal；

Step (22), usage factor matrix A_n、Openness and its special sub-piecemeal, will2 by 3 ranks point Block unfolding calculation, is derived offlineSimplification form of calculation and preserved as intermediate variable, it is indicated thatIt is openness with piecemeal, And the 3rd row piecemeal T_i3, piecemeal T on diagonal₅₅、T₆₆For special sub-piecemeal；

Step (23), in step (21), the calculating process of (22) introduce numerical value be O_3×3Submatrix symbol A₃₁、S₃₄、 S₃₅, to keep the continuity of form of calculation, reduce the programming complexity of real-time calculation procedure.

Further, described step (3) is to Γ_nQ_n Calculation optimization specifically comprise the following steps：

Step (31), using noise drive matrix G (t) it is openness, by 3 rank piecemeal unfolding calculation Q_1n=G (t) Q (t) G (t)^T, Q (t) discrete form is derived offline, it is indicated that Q_1nWith block diagonal form；

Step (32), utilize Q_1nDiagonal form, φ_nOpenness and Γ_nQ_nΓ^T _nSymmetry, pressDerive Γ offline_nQ_n Simplification form of calculation.

Further, described step (5) is to K_nCalculation optimization specifically comprise the following steps：

Step (51), utilize H_nIt is openness, by 3 rank piecemeal unfolding calculationsThe inverse square of its result Battle array saves as intermediate variable D；

Step (52), utilize H_nOpenness and P_nThe symmetry of (-), is pressedDerive K offline_n Simplification form of calculation.

Further, described step (7) parallel optimization processing is specifically comprised the following steps：

Step (71), by P_n(+_e) calculating process press P_n(+_e)=P_n(-)-K_nH_nP_n(-) is segmented, K_nH_nP_n(-) replaces P_n (+_e) set up new measurement renewal；

Step (72), definition P_n2, the 3 piecemeals row of (-) are defined as " useful " information, and remaining is " useless " information, " useless " There is numerical value decoupling relation, the processing of the two process real-time parallels with the new renewal process that measures in the renewal process of information；

Step (73), by P_nThe calculating process of (-) " useful " information is pressedSubdivision, SubprocessUpdate and Γ_nQ_n There is numerical value decoupling relation, real-time parallel processing in renewal.

The advantage of the present invention compared with prior art is：

(1) present invention takes full advantage of SINS/GPS numerical characteristic progress offline optimization, and its real-time computational efficiency is better than Popular general filtering optimization algorithm.

(2) compared with general decomposition filtering optimization algorithm, the present invention is derived without complicated matrix theory, it is easier to work Cheng Shixian.

(3) numerical value Uncoupled procedure of the invention is only derived by the numerical characteristic of system and obtained, without prevalence parallel filtering It will be measured in algorithm and update the processing for forcing a delayed filtering cycle, therefore with higher filtering accuracy.

(4) optimization that the present invention is calculated filtering is that pure values offline optimization and real-time parallel optimize, can be general excellent Double optimization is carried out on the basis of change algorithm, to reach the purpose of more Computationally efficient.

Brief description of the drawings

Fig. 1 is the object of numerical optimization of the present invention --- all period renewal process of closed loop Kalman filter.

Fig. 2 is Kalman filter numerical optimization routines proposed by the present invention.

Fig. 3 is lossless decoupled Kalman filtering parallel processing mechanism proposed by the present invention.

Embodiment

The implementation process and main methods of the present invention is illustrated with reference to Fig. 1 and Fig. 2.The specific step of this method It is rapid as follows：

Step 1. is filtered using indirect method, directly gives system state equation：

Formula 1-1

Wherein state error x (t), system white noise ω (t), coefficient matrices A (t), G (t) are：

Formula 1-2

Formula 1-3

Formula 1-4

Formula 1-5

In formula, γ_x、γ_y、γ_zRespectively platform error Jiao Dong, north, day durection component；δv_x、δv_y、δv_zFor east, north, day Direction velocity error component；δ L, δ λ, δ h are longitude, latitude, height error；ε_c、ε_r、▽_aRespectively Random Constant Drift vector, Random markoff process drift vector and accelerometer random drift vector；ω_g、ω_r、ω_aRespectively gyroscope floats at random Move white noise, single order Markov driving white noise and accelerometer random drift white noise；For matrix attitude matrix；O_3×3 For null matrix；I_3×3For unit matrix；Pointed out from numerical value angle, A in other piecemeals_i3Only the 1st column vector non-zero, A_IMUiiTo be diagonal Matrix.

The white noise square formation Q (t) for directly giving system is：

Formula 1-6

Wherein Q_iiFor diagonal matrix.

Using position, the measurement vector z of velocity composition pattern definition wave filter_obs(t)：

Formula 1-7

Directly give measurement equation：

Formula 1-8

Wherein coefficient matrix H (t), observation white noise n (t) are：

Formula 1-9a

H₁₃=diag { R_n,R_eCosL, 1 } formula 1-9b

N (t)=(N_n,N_e,N_h,M_n,M_e,M_u)^TFormula 1-10

Directly give measurement white noise covariance matrix R (t)：

Formula 1-11

Directly give the discrete form of system state equation (formula 1-1) and observational equation (formula 1-8)：

x_n=φ_nx_n-1+B_nu_cn+Γ_nω_nFormula 1-12

z_n=H_nx_n+n_nFormula 1-13

Wherein u_cnFor control vector, φ_nError transfer matrix, B_nFor control coefrficient matrix, Γ_nSquare is driven for state-noise Battle array.Directly give controlled closed loop Kalman filter iterative process：

Formula 1-14a

Formula 1-14b

P_n(+_e)=(I-K_nH_n)P_n(-) formula 1-14c

u_cn=-K_nz_obsnFormula 1-14d

In formula ,~represent that Kalman filter estimates the parameter value of (or prediction)；(+_e) represent t_nAfter moment estimation resets Parameter value；(-) represents t_nParameter value before moment estimation；(+_c) represent t_nParameter value after the correction of moment control action.

The real-time calculating process of Kalman filter is summarized, as shown in figure 1, main include the discretization of parameter, the time updates With measurement renewal process.It is related to the multiplying of multiple high level matrix, it is computationally intensive, it is necessary to carry out the optimization of computational efficiency.

Step 2. φ_nThe optimization of calculating.φ_nCalculation basis following formula carry out：

Formula 2-1

Calculate mainly around 18 rank square formation A_nCarry out, by A_nDeploy by 3 rank square formation piecemeals, symbol is utilized in Matlab environment Number calculation function, is derived byReduced form, such as formula 2-2, formula 2-3.

Formula 2-2

Formula 2-3

Wherein S_ij、T_ijWrite as and be easy to the form of computer programming and be：

Formula 2-4a

Formula 2-4b

S_i5=S_i4, i, j=1,2 formulas 2-4c

Formula 2-4d

Formula 2-4e

Formula 2-4f

Formula 2-4g

Formula 2-5a

Formula 2-5b

T₅₅=(A_IMU22S₅₅)_SimFormula 2-5c

T₆₆=(A_IMU33S₆₆)_SimFormula 2-5d

It is related to A in formula 2-4, formula 2-5₃₁、S₃₄、S₃₅Computational item be O_3×3, it is related to A_i3、S_i3、A_IMUii、S₅₅、S₆₆Etc. special The computational item of piecemeal is to simplify computational item, with ()_SimRepresent.Wherein A_i3、S_i3、T_i3With following matrix form：

Formula 2-6

For any 3 rank square formationX_i3There is following property：

Formula 2-7

Formula 2-7 can be used for formula 2-4, formula 2-5 and the simplification subsequently calculated.By A_n、Substitution formula 2-1 can be obtained：

Formula 2-8

Wherein：

Formula 2-9a

Formula 2-9b

Point out φ₁₃、φ₂₃Also there is formula 3.1-3 form, φ₃₃It can be analyzed to formal matrices and I with formula 3.1-3_3×3 Sum；Similarly, φ is known₅₅、φ₆₆For diagonal matrix.

In summary, deploy to derive by using 3 rank piecemeals, in φ_nOffline derivation result in directly obtain and account for one The O of half quantity_3×3、I_3×3Piecemeal；For the calculating of other piecemeals, it is possible to use special piecemeal A_i3、S_i3、T_i3And A_jj、S_jj、T_jj Property simplified；Introduce intermediate variableReduce substantial amounts of compute repeatedly.Statistical result shows that offline optimization is reduced φ_nThe floating-point multiplication of renewal 90% and 94% add operation, while internal memory cost reduces 69%.

Step 3.Q_nThe optimization of calculating.Directly calculate Γ_nQ_n , have as Two-order approximation：

Formula 3-1

Wherein：

Formula 3-2

In formula：

Formula 3-3

Obviously, Q_1nAnd Γ_nQ_n It is symmetrical matrix, to Γ_nQ_n Only need to calculate triangular portions, Q_1nSubstitute into Γ_nQ_n It can obtain：

Formula 3-4

Wherein：

Formula 3-5a

Formula 3-5b

Formula 3-5c

Formula 3-5d

Formula 3-5e

To Γ_nQ_n The optimization of calculating mainly uses sparse matrix G (t), diagonal matrix piecemeal Q₂₂、Q₃₃φ₅₅、φ₆₆Spy Different property and Γ_nQ_n Symmetry derived offline.Especially, Q in formula 3-5c_j-3,j-3φ_jj, Q in formula 3-5d_n55, formula 3- Q in 5e_n66Still it is diagonal matrix, is calculated available for simplifying.Statistical result shows that The present invention reduces Γ_nQ_n Update 94% (96%) multiplication (addition) computing and 88% internal memory cost.

Step 4.P_nThe optimization that (-) calculates.It is convenient to derive, by P_n(-)、P_n(+_e) uniformly use P_nRepresent (in actual program, P in same filtering cycle_n(-)、P_n(+_e) also only use P_nDifferent conditions represent), P_nBy 3 × 3 rank partitionings of matrix, write as：

Formula 4-1

P_n(-) is symmetrical matrix, then has：

Formula 4-2

First do not consider Γ_nQ_n , formula 1-14a write as：

Formula 4-3

Substitution formula 2-8, formula 4-1 are obtained：

Formula 4-4a

Formula 4-4b

P_n,66=(φ₆₆P_n-1,66φ₆₆)_SimFormula 4-4c

In formula ()_TempIt is to compute repeatedly the intermediate quantity preserved in a computer, () to reduce_RepRepresent ()_TempIn The duplicate keys calculated, be substantiallyResult of calculation.To ()_TempCalculating it is availableParticularity carry out it is excellent Change.

Consider Γ_nQ_n , wushu 3-4, formula 4-1 substitute into formula 1-14a and obtained：

P_n,ij=P_n,ij+Q_n,ij, i, j=1,2,3,5,6；I >=j formulas 4-5

In summary, P_nThe calculating of (-) is divided into two steps of formula 3-4 and formula 4-3, and optimization process mainly utilizes φ_nPiecemeal Discreteness, φ_i3And φ_{Ii (i=5,6)}Particularity and introduce intermediate quantity avoid computing repeatedly item.Statistical result shows, to P_n(-) Optimization reduce 69% amount of calculation and 44% memory cost.

The optimization that step 5.Kn is calculated.The part of inverting that wushu 1-9, formula 1-11 and formula 4-1 are updated to formula 1-14b can be obtained：

Formula 5-1

Order

Formula 5-2

Easily prove D^T=D, is write as：

Formula 5-3

Then have：

Formula 5-4

It is designated as：

Formula 5-5

Have：

Formula 5-6

It was found from above-mentioned derivation, carried out using matrix in block form after derivation optimization, K_nCalculating only need to execution formula 5- 1st, formula 5-2, tri- relatively simple processes of formula 5-6, utilize H_nIt is openness, it is to avoid 5 high level matrixs multiply in formula 1-14b Method.Statistics to optimum results shows that The present invention reduces K_nThe amount of calculation and 61% internal memory cost of renewal 89%.

Step 6.P_n(+_e) calculate optimization.Formula 1-14c is rewritten into：

P_n(+_e)=P_n(-)-K_nH_nP_n(-) formula 6-1

Main amount of calculation is K_nH_nP_n(-), because P_n(+_e) and P_n(-) is all symmetrical matrix, then K_nH_nP_n(-) is also pair Claim matrix, it is only necessary to triangular portions in calculating.

Remember K_nH_nP_n(-) is Δ P_n：

Formula 6-2

Substitution formula 1-9, formula 4-1 and formula 5-5 are obtained：

ΔP_ij=K_i1H₁₃P_n,3j+K_i2P_n,2j, i, j=1,2 ..., 6；I≤j formulas 6-3

Then have：

P_n,ij=P_n,ij-ΔP_ij, i, j=1,2 ..., 6；I≤j formulas 6-4

From derivation it can be seen that, utilize P_n(+_e) symmetry and H_nPiecemeal is openness, derives P_n(+_e) succinct Form of calculation (formula 6-3, formula 6-4), only need K in the formula 6-3 for accounting for main amount of calculation_nWith P_n(+_e) the 2nd, 3 piecemeal rows work it is a small amount of 3 rank matrix multiplications, reduce a large amount of unnecessary calculating.Made comparisons with conventional algorithm, reduce 85% amount of calculation and 61% it is interior Deposit expense.

The lossless decoupling parallel optimization of step 7..In terms of optimum results, P_nThe amount of calculation of (-) still accounts for whole filtering Half, as the further bottleneck for improving computational efficiency.Consider to P_nParallel processing is implemented in the renewal of (-), to improve real-time. It was found from above-mentioned Kalman filter iterative calculation derivation, except P_n(+_e) outside, the renewal of other parameters or and P_n(-) it is unrelated or Only pass is shown with the 2nd, 3 piecemeal rows and 2,3 piecemeals.Therefore P is defined_n(-) the 2nd, 3 piecemeal row (column) is " useful " information, remaining Piecemeal is " without having " information.Further, because P_n(-) is symmetrical matrix, therefore " useful " piecemeal actually there was only the 2nd, 3 points Block is arranged." useless " piecemeal represents with "×", then P_n(-) can be write as：

Formula 3.6-1

In renewal is measured, only P_n(+_e) coupled with " useless " data.By P_n(+_e) renewal be subdivided into Δ P_ij(formula 6-3) Renewal and P_n(-) adds up (formula 6-4) two processes, and uses Δ P_ijInstead of P_n(+_e), constituted newly with formula 1-14b, formula 1-14d Measure renewal process.Therefore, P is obtained_n(-) " useless " information updating is decoupled with the new numerical value updated that measures, can be by the two Concurrent process processing, to solve P_n(-) calculates time-consuming bottleneck problem.Similarly, to P_nThe renewal subdivision of (-) " useful " piecemeal Two calculation procedures are updated into by formula 4-3 renewals and by formula 4-5, it is clear that in first stepCalculate and when Between update in Γ_nQ_n Calculating be that can carry out parallel processing.

In summary, the Kalman filter implementation procedure after being optimized using parallel processing is as shown in Figure 3.With without parallel optimization Processing is compared, and the parallel optimization efficiency shown in Fig. 3 improves about 25%.More importantly Uncoupled procedure of the invention is to utilize The digital characteristic of SINS/GPS integrated navigations, is derived by strict numeral and obtained, different from popular method for parallel processing, no Need to make " forcing delayed " processing, be a kind of lossless method for parallel processing.

What the present invention was not elaborated partly belongs to techniques well known.

Claims (3)

1. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations, it is characterised in that including following step Suddenly：

Step (1), set up a kind of SINS/GPS integrated navigation loose coupling closed loops Kalman filters based on the combination of position-tachometric survey Wave pattern, is deployed to represent the parameter matrix in model, including coefficient of regime matrix A with 3 rank square formations_n, state-transition matrix φ_n, be Unite noise coefficient G_n, measure coefficient matrix H_n, system noise variance Q_n, measuring noise square difference R_n, error covariance time updated value P_n(-), error covariance measure updated value P_n(+_e) and Kalman filter gain K_n；

Step (2), utilization state coefficient matrices A_nOpenness, special sub-piecemeal matrix and intermediate variable Symbolic operation function is used in Matlab environment, to formulaBy 3 rank sub-piecemeal unfolding calculations, Derive discrete filter transfer matrix φ offline_nSimplification form of calculation；

Described step (2) is rightCalculation optimization specifically comprise the following steps：

Step (21), utilization state coefficient matrices A_nOpenness and its special sub-piecemeal, derive offlineSimplify calculating Form is simultaneously preserved as intermediate variable, and derivation result showsIt is openness with piecemeal, and the 3rd row piecemeal S_i3, on diagonal Piecemeal S₅₅、S₆₆For special sub-piecemeal；

Step (22), utilization state coefficient matrices A_n、Openness and its special sub-piecemeal, willBy 3 rank piecemeals Unfolding calculation, is derived offlineSimplification form of calculation and preserved as intermediate variable, it is indicated thatIt is openness with piecemeal, And the 3rd row piecemeal T_i3, piecemeal T on diagonal₅₅、T₆₆For special sub-piecemeal；

Step (23), in step (21), the calculating process of (22) introduce numerical value be O_3×3Submatrix symbol A₃₁、S₃₄、S₃₅, with Keep the continuity of form of calculation；

Step (3), the φ being derived by using step (2)_nPiecemeal is openness, special sub-piecemeal matrix and noise matrixSymmetry, derived offline in Matlab environmentSimplify calculate shape Formula, wherein Γ_nMatrix is driven for state-noise；

Step (4), utilize φ_nPiecemeal is openness, special sub-piecemeal matrix, intermediate variableAnd covariance matrix P_nThe symmetry of (-), to formula in Matlab environmentBased on the expansion of 3 rank sub-piecemeals Calculate, derive P_nThe simplification form of calculation that (-) time updates；

Step (5), using measuring coefficient matrix H_nPiecemeal is openness, special sub-piecemeal matrix, to formula in Matlab environmentThe offline derivation of equation is carried out, filtering gain K is obtained_nSimplification form of calculation；

Step (6), utilize H_nPiecemeal is openness, special sub-piecemeal matrix, covariance matrix P_nThe symmetry of (-), in Matlab To formula P in environment_n(+_e)=(I-K_nH_n)P_n(-) presses 3 rank submatrix unfolding calculations, and P is derived offline_n(+_e) measure the simplification updated Form of calculation；

Step (7), subdivision P_n(+_e) and P_nThe calculating process of (-), and by time-consuming matrix P_n(-) divides " useful ", " useless " information, By in dependence of the subprocess analytic hierarchy process to parameter, obtaining a kind of new numerical value decoupling mechanism, realizing on this basis Lossless parallel processing to filtering；

Described step (7) parallel optimization processing is specifically comprised the following steps：

Step (71), by P_n(+_e) calculating process press P_n(+_e)=P_n(-)-K_nH_nP_n(-) is segmented, K_nH_nP_n(-) replaces P_n(+_e) build Vertical new measurement updates；

Step (72), P_n2, the 3 piecemeals row of (-) are defined as " useful " information, and remaining is defined as " useless " information, " useless " There is numerical value decoupling relation, the processing of the two process real-time parallels with the new renewal process that measures in the renewal process of information；

Step (73), by P_nThe calculating process of (-) " useful " information is pressedSubdivision, sub- mistake JourneyUpdate withThere is numerical value decoupling relation, real-time parallel processing in renewal.

2. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations according to claim 1, its It is characterised by：Described step (3) is rightCalculation optimization specifically comprise the following steps：

Step (31), utilize system noise factor G_nIt is openness, by 3 rank piecemeal unfolding calculation Q_1n=G_nQ(t)G_n ^T, push away offline Export Q (t) discrete form, it is indicated that Q_1nWith block diagonal form；

Step (32), utilize Q_1nDiagonal form, φ_nIt is openness andSymmetry, pressDerive offlineSimplification form of calculation.

3. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigations according to claim 1, its It is characterised by：Described step (5) is to K_nCalculation optimization specifically comprise the following steps：

Step (51), utilize H_nIt is openness, by 3 rank piecemeal unfolding calculationsThe inverse matrix of its result is protected Save as intermediate variable D；

Step (52), utilize H_nOpenness and P_nThe symmetry of (-), is pressedDerive K offline_nLetter Change form of calculation.