CN104408315A - SINS/GPS integrated navigation based Kalman filter numerical optimization method - Google Patents

Abstract

The invention discloses an SINS/GPS integrated navigation based Kalman filter numerical optimization method. According to the method, on the basis of establishing of an SINS/GPS integrated navigation conventional Kalman filter model, a block matrix technology is used, high-order matrix operations involved in filter calculation is deduced in accordance with sub-blocks in the Matlab environment, and a great number of meaningless duplicate operations are prevented; numerical value decoupling with other updating processes is obtained in the sub-process level through a time-consuming parameter segmentation calculation process, and accordingly, a novel lossless parallel processing mechanism is given, and the filter calculation timeliness is improved greatly. By the aid of the method, the defects of complex deduction and the difficulty in project implementation of common filter decomposition optimization algorithms are overcome, traditional lossy accuracy decoupling in parallel filters is prevented, and efficient lossless data filters of the SINS/GPS integrated navigation system is achieved.

Description

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

Technical field

The present invention relates to the technical field of integrated navigation system data filtering, be specifically related to a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation.

Background technology

In the data filtering link of SINS/GPS integrated navigation system, often comprise the process to " coloured noise ", at this moment do not met the applicable elements of classical Kalman filter, in order to still can optimal estimation techniques be used, need to carry out state expand.But along with the increase of state dimension, calculated amount sharply expands, numerical stability declines, and even there will be " dimension disaster ".For this problem, method general is at present, utilizes square root information filter SRIF, U-D to decompose filtering, SVD filtering etc. and decomposes filtering optimization algorithm to reduce calculated amount and to strengthen numerical value robustness, and utilize parallel algorithm to improve filtering real-time.But the derivation of decomposition algorithm needs to use complicated matrix theory knowledge, and engineer applied has certain difficulty; And decomposition filtering algorithm complexity is still O (n ³), along with the further increase of state dimension, real-time also receives challenge.Although parallel Kalman filter significantly improves real-time, its Uncoupled procedure upgrades based on measurement forces a delayed filtering cycle to realize, and this can bring certain precision infringement, may cause numerical value instability problem after successive ignition.In addition, as the optimized algorithm that a class is general, above-mentioned several wave filter does not all have to carry out specialized designs based on the feature of SINS/GPS integrated navigation system, rarely has the numerical characteristics of research and utilization system to carry out deep optimization to Kalman filter process both at home and abroad yet.

Summary of the invention

The technical problem to be solved in the present invention is: the deficiency overcoming existing algorithm, from the angle of numerical evaluation, propose in a kind of engineering simple, the high-level efficiency utilizing SINS/GPS system digits feature to carry out off-line derivation optimization and real-time parallel process can't harm Kalman filtering optimized algorithm.

The technical scheme that the present invention solves the problems of the technologies described above employing is: a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation, is characterized in that comprising the following steps:

Step (1), set up the SINS/GPS integrated navigation loose coupling closed loop Kalman filter model of a kind of position-based-velocity survey combination, represent the parameter matrix in model with 3 rank square formations expansion, comprise coefficient of regime A _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, 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 variable symbolic operation function is used, to formula in Matlab environment by the 3 sub-piecemeal unfolding calculation in rank, off-line derives discrete filter transition matrix φ _nsimplification form of calculation;

Step (3), step (2) is utilized to derive the φ obtained _nopenness, the special sub-piecemeal of piecemeal and noise matrix Γ _nq _n symmetry, in Matlab environment, off-line is derived simplification form of calculation;

Step (4), utilize φ _nopenness, the special sub-piecemeal of piecemeal, intermediate variable and covariance matrix P _nthe symmetry of (-), to formula in Matlab environment by the 3 sub-piecemeal unfolding calculation in rank, derive P _nthe simplification form of calculation that (-) time upgrades;

Step (5), utilize system measurements equation coefficient matrix H _nopenness, the special sub-piecemeal of piecemeal, to formula in Matlab environment carry out the off-line derivation of equation, obtain filter gain K _nsimplification form of calculation;

Step (6), utilize H _nopenness, the special sub-piecemeal of piecemeal, covariance matrix P _nthe symmetry of (-), to formula P in Matlab environment _n(+ _e)=(I-K _nh _n) P _n(-), by 3 rank submatrix unfolding calculation, off-line pushes away P _n(+ _e) measure the simplification form of calculation upgraded;

Step (7), segmentation P _n(+ _e) and P _nthe computation process of (-), and by matrix P consuming time _n(-) divides " useful ", " useless " information, by the dependence of subprocess analytic hierarchy process to parameter, obtains a kind of new numerical value decoupling mechanism, realizes the harmless parallel processing to filtering on this basis.

Further, described step (2) is right the calculation optimization of 3 specifically comprises following steps:

Step (21), usage factor matrix A _nopenness and special sub-piecemeal, off-line is derived simplification form of calculation and as intermediate variable preserve, derivation result shows there is piecemeal openness, and the 3rd row piecemeal S _i3, piecemeal S on diagonal line ₅₅, S ₆₆for special sub-piecemeal;

Step (22), usage factor matrix A _n, openness and special sub-piecemeal, will 2 by 3 rank piecemeal unfolding calculation, and off-line is derived simplification form of calculation and as intermediate variable preserve, point out there is piecemeal openness, and the 3rd row piecemeal T _i3, piecemeal T on diagonal line ₅₅, T ₆₆for special sub-piecemeal;

Step (23), in the computation process of step (21), (22), introduce numerical value be O _{3 × 3}submatrix 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 following steps:

Step (31), utilize noise drive matrix G (t) openness, by 3 rank piecemeal unfolding calculation Q _1n=G (t) Q (t) G (t) ^t, off-line derives the discrete form of Q (t), points out Q _1nthere is block diagonal form;

Step (32), utilize Q _1ndiagonal form, φ _nopenness and Γ _nq _nΓ ^t _nsymmetry, press off-line derives Γ _nq _n simplification form of calculation.

Further, described step (5) is to K _ncalculation optimization specifically comprise following steps:

Step (51), utilize H _nopenness, by 3 rank piecemeal unfolding calculation the inverse matrix of its result saves as intermediate variable D;

Step (52), utilize H _nopenness and P _nthe symmetry of (-), presses off-line derives K _nsimplification form of calculation.

Further, described step (7) parallel optimization process specifically comprises following steps:

Step (71), by P _n(+ _e) computation process press P _n(+ _e)=P _n(-)-K _nh _np _n(-) segments, K _nh _np _n(-) replaces P _n(+ _e) set up new measurement renewal;

Step (72), definition P _n2, the 3 piecemeal row of (-) are defined as " useful " information, and all the other are " useless " information, and the renewal process of " useless " information is with newly measuring renewal process exists numerical value decoupling zero relation, this two process real-time parallels process;

Step (73), by P _nthe computation process of (-) " useful " information is pressed segmentation, subprocess upgrade and Γ _nq _n there is numerical value decoupling zero relation in renewal, real-time parallel process.

The present invention's advantage is compared with prior art:

(1) numerical characteristic that the present invention takes full advantage of SINS/GPS carries out offline optimization, and its real-time counting yield is better than popular general filtering optimization algorithm.

(2) compared with general decomposition filtering optimization algorithm, the present invention does not have complicated matrix theory to derive, and is easier to Project Realization.

(3) numerical value Uncoupled procedure of the present invention obtains by means of only the numerical characteristic derivation of system, does not have, in popular parallel filtering algorithm, measurement is upgraded the process forcing a delayed filtering cycle, therefore has higher filtering accuracy.

(4) the present invention is pure values offline optimization and real-time parallel optimization to the optimization that filtering calculates, and can carry out double optimization, to reach the object of more Computationally efficient on the basis of common optimization algorithm.

Accompanying drawing explanation

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

Fig. 2 is the Kalman filter numerical optimization routines that the present invention proposes.

Fig. 3 is the harmless decoupled Kalman filtering parallel processing mechanism that the present invention proposes.

Embodiment

Implementation process of the present invention and main methods is illustrated below in conjunction with Fig. 1 and Fig. 2.The concrete steps of the method are as follows:

Step 1. adopts indirect method filtering, directly provides system state equation:

$\stackrel{\·}{x}\left(t\right)=A\left(t\right)x\left(t\right)+G\left(t\right)\mathrm{\ω}\left(t\right)$ 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

$A\left(t\right)={\left[\begin{array}{cccccc}{A}_{11}& A_{12}& A_{13}& C_B^N& C_B^N& O_{3\×3}\\ A_{21}& A_{22}& A_{23}& O_{3\×3}& O_{3\×3}& C_B^N\\ O_{3\×3}& A_{32}& A_{33}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& A_{\mathrm{IMU}22}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& A_{\mathrm{IMU}33}\end{array}\right]}_{18\×18}$ Formula 1-4

$G\left(t\right)={\left[\begin{array}{cccc}{C}_B^N& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{9\×3}& O_{9\×3}& O_{9\×3}& O_{9\×3}\\ O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& I_{3\×3}\end{array}\right]}_{18\×9}$ Formula 1-5

In formula, γ _x, γ _y, γ _zbe respectively platform error Jiao Dong, north, sky durection component; δ v _x, δ v _y, δ v _zfor east, north, direction, sky velocity error component; δ L, δ λ, δ h are longitude, latitude, height error; ε _c, ε _r, ▽ _abe respectively Random Constant Drift vector, random Markov process drift vector and accelerometer random drift vector; ω _g, ω _r, ω _abe respectively Gyroscope Random Drift white noise, single order Markov drives white noise and accelerometer random drift white noise; for matrix attitude matrix; O _{3 × 3}for null matrix; I _{3 × 3}for unit matrix; Point out from numerical value angle, A in other piecemeals _i3only the 1st column vector non-zero, A _iMUiifor diagonal matrix.

White noise square formation Q (t) directly providing system is:

$Q\left(t\right)={\left[\begin{array}{ccc}{Q}_{11}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& Q_{22}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& Q_{33}\end{array}\right]}_{9\×9}$ Formula 1-6

Wherein Q _iifor diagonal matrix.

Adopt the measurement vector z of position, velocity composition pattern definition wave filter _obs(t):

$z_{\mathrm{obs}}\left(t\right)=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{GPS}})R_n\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{GPS}})R_e\cos L\\ h_{\mathrm{INS}}-h_{\mathrm{GPS}}\\ v_{\mathrm{nINS}}-v_{\mathrm{nGPS}}\\ v_{\mathrm{eINS}}-v_{\mathrm{eGPS}}\\ v_{\mathrm{uINS}}-v_{\mathrm{uGPS}}\end{array}\right]=\left[\begin{array}{c}{R}_n\mathrm{\δL}+N_n\\ R_e\cos \mathrm{L\δ\λ}+N_e\\ \mathrm{\δh}+N_h\\ {\mathrm{\δv}}_n+M_n\\ {\mathrm{\δv}}_e+M_e\\ {\mathrm{\δv}}_u+M_u\end{array}\right]$ Formula 1-7

Directly to going out measurement equation:

$z\left(t\right)\stackrel{\mathrm{\Δ}}{=}H\left(t\right)x\left(t\right)+n\left(t\right)$ Formula 1-8

Wherein matrix of coefficients H (t), observation white noise n (t) are:

$H\left(t\right)={\left[\begin{array}{cccc}{O}_{3\×3}& O_{3\×3}& H_{13}& O_{3\×9}\\ O_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×9}\end{array}\right]}_{18\×6}$ 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 provide and measure white noise covariance matrix R (t):

$R\left(t\right)=\mathrm{diag}\{{\mathrm{\σ}}_{\mathrm{pn}}^2,{\mathrm{\σ}}_{\mathrm{pe}}^2,{\mathrm{\σ}}_{\mathrm{pu}}^2,{\mathrm{\σ}}_{\mathrm{vn}}^2,{\mathrm{\σ}}_{\mathrm{ve}}^2,{\mathrm{\σ}}_{\mathrm{vu}}^2\}=\left[\begin{array}{cc}{R}_{11}& O_{3\×3}\\ O_{3\×3}& R_{22}\end{array}\right]$ Formula 1-11

Directly provide the discrete form of system state equation (formula 1-1) and observation 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 controlling vector, φ _nerror transfer matrix, B _nfor control coefrficient matrix, Γ _nfor state-noise drives matrix.Directly provide the closed loop Kalman filter iterative process of control:

$P_n(-)={\mathrm{\φ}}_n{P}_{n-1}\left({+}_e\right){\mathrm{\φ}}_n^T+{\mathrm{\Γ}}_n{Q}_n{\mathrm{\Γ}}_n^T$ Formula 1-14a

$K_n=P_n(-)H_n^T{(H_n{P}_n(-)H_n^T+R_n)}^{-1}$ 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 the parameter value that Kalman filter is estimated (or prediction); (+ _e) represent t _nmoment estimates the parameter value after resetting; (-) represents t _nparameter value before moment estimation; (+ _c) represent t _nparameter value after moment control action corrects.

Sum up the real-time computation process of Kalman filter, as shown in Figure 1, mainly comprise the discretize of parameter, the time upgrades and measures renewal process.Relate to repeatedly the multiplying of high level matrix, calculated amount is large, needs the optimization carrying out counting yield.

Step 2. φ _nthe optimization calculated.φ _nbasis following formula carry out:

${\mathrm{\φ}}_n=e^{{\∫}_{t_{n-1}}^{t_n}A\left(t\right)\mathrm{dt}}\≈I+T_n{A}_n+\frac{T_n^2}{2!}{A}_n^2+\frac{T_n^3}{3!}{A}_n^3$ Formula 2-1

Calculate main around 18 rank square formation A _ncarry out, by A _nlaunch by 3 rank square formation piecemeals, in Matlab environment, utilize symbolic operation function, derivation obtains reduced form, such as formula 2-2, formula 2-3.

$A_n^2={\left[\begin{array}{cccccc}{S}_{11}& S_{12}& S_{13}& S_{14}& S_{15}& S_{16}\\ S_{21}& S_{22}& S_{23}& S_{24}& S_{25}& S_{26}\\ S_{31}& S_{32}& S_{33}& O_{3\×1}& O_{3\×3}& S_{36}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& S_{55}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& S_{66}\end{array}\right]}_{18\×18}$ Formula 2-2

$A_n^3={\left[\begin{array}{cccccc}{T}_{11}& T_{12}& T_{13}& T_{14}& T_{15}& T_{16}\\ T_{21}& T_{22}& T_{23}& T_{24}& T_{25}& T_{26}\\ T_{31}& S_{32}& S_{33}& T_{34}& T_{35}& T_{36}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& T_{55}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& T_{66}\end{array}\right]}_{18\×18}$ Formula 2-3

Wherein S _ij, T _ijbeing write as the form being easy to computer programming is:

$S_{\mathrm{ij}}=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}{A}_{\mathrm{kj}},i,j=\mathrm{1,2,3}$ Formula 2-4a

$S_{\mathrm{ij}}=A_{i(j/2-1)}{C}_B^N,i=\mathrm{1,2,3};j=\mathrm{4,6}$ Formula 2-4b

S _i5=S _i4, i, j=1,2 formula 2-4c

$S_{15}=S_{15}+{\left(C_B^N{A}_{\mathrm{IMC}22}\right)}_{\mathrm{Sim}}$ Formula 2-4d

$S_{26}=S_{26}+{\left(C_B^N{A}_{\mathrm{IMU}33}\right)}_{\mathrm{Sim}}$ Formula 2-4e

$S_{55}={\left(A_{\mathrm{IMU}22}^2\right)}_{\mathrm{Sim}}$ Formula 2-4f

$S_{66}={\left(A_{\mathrm{IMU}33}^2\right)}_{\mathrm{Sim}}$ Formula 2-4g

$T_{\mathrm{ij}}=\underset{k=1}{\overset{3}{\mathrm{\Σ}}}{A}_{\mathrm{ik}}{S}_{\mathrm{kj}},i=\mathrm{1,2,3};j=\mathrm{1,2},...,6$ Formula 2-5a

$T_{26}=T_{26}+{\left(C_B^N{A}_{\mathrm{IMU}33}^2\right)}_{\mathrm{Sim}}$ Formula 2-5b

T ₅₅=(A _iMU22s ₅₅) _simformula 2-5c

T ₆₆=(A _iMU33s ₆₆) _simformula 2-5d

A is related in formula 2-4, formula 2-5 ₃₁, S ₃₄, S ₃₅computational item be O _{3 × 3}, relate to A _i3, S _i3, A _iMUii, S ₅₅, S ₆₆etc. the computational item of special piecemeal for simplifying computational item, with () _simrepresent.Wherein A _i3, S _i3, T _i3there is following matrix form:

$X_{i3}=\left[\begin{array}{ccc}*& 0& 0\\ *& 0& 0\\ *& 0& 0\end{array}\right],i=\mathrm{1,2,3}$ Formula 2-6

For any 3 rank square formations $M=\left[\begin{array}{ccc}*& *& *\\ *& *& *\\ *& *& *\end{array}\right],$ X _i3there is following character:

${\mathrm{MX}}_{i3}=\left[\begin{array}{ccc}*& 0& 0\\ *& 0& 0\\ *& 0& 0\end{array}\right]$ Formula 2-7

Formula 2-7 can be used for the simplification of formula 2-4, formula 2-5 and subsequent calculations.By A _n, substitution formula 2-1 can obtain:

${\mathrm{\φ}}_n={\left[\begin{array}{cccccc}{\mathrm{\φ}}_{11}& {\mathrm{\φ}}_{12}& {\mathrm{\φ}}_{13}& {\mathrm{\φ}}_{14}& {\mathrm{\φ}}_{15}& {\mathrm{\φ}}_{16}\\ {\mathrm{\φ}}_{21}& {\mathrm{\φ}}_{22}& {\mathrm{\φ}}_{23}& {\mathrm{\φ}}_{24}& {\mathrm{\φ}}_{25}& {\mathrm{\φ}}_{26}\\ {\mathrm{\φ}}_{31}& {\mathrm{\φ}}_{32}& {\mathrm{\φ}}_{33}& {\mathrm{\φ}}_{34}& {\mathrm{\φ}}_{35}& {\mathrm{\φ}}_{36}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& I_{3\×3}& O_{3\×3}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& {\mathrm{\φ}}_{55}& O_{3\×3}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& {\mathrm{\φ}}_{66}\end{array}\right]}_{18\×18}$ Formula 2-8

Wherein:

${\mathrm{\φ}}_{\mathrm{ij}}=T_n{A}_{\mathrm{ij}}+\frac{T_n^2}{2!}{S}_{\mathrm{ij}}+\frac{T_n^3}{3!}{T}_{\mathrm{ij}},i\∈\left[\mathrm{1,3}\right],j\∈\left[\mathrm{1,6}\right],i\≠j$ Formula 2-9a

${\mathrm{\φ}}_{\mathrm{ii}}={I_{3\×3}+T}_n{A}_{\mathrm{ii}}+\frac{T_n^2}{2!}{S}_{\mathrm{ii}}+\frac{T_n^3}{3!}{T}_{\mathrm{ii}},i=\mathrm{1,2,3,5,6}$ Formula 2-9b

Point out φ ₁₃, φ ₂₃also there is the form of formula 3.1-3, φ ₃₃can be analyzed to the formal matrices and I with formula 3.1-3 _{3 × 3}and; Similarly, φ is known ₅₅, φ ₆₆for diagonal matrix.

In sum, by using 3 rank piecemeals to launch to derive, at φ _noff-line derivation result in directly obtain the O accounting for half quantity _{3 × 3}, I _{3 × 3}piecemeal; For the calculating of other piecemeals, special piecemeal A can be utilized _i3, S _i3, T _i3and A _jj, S _jj, T _jjcharacter simplify; Introduce intermediate variable reduce a large amount of double countings.Statistics shows that offline optimization decreases φ _nupgrade the floating-point multiplication of 90% and the additive operation of 94%, internal memory cost reduces 69% simultaneously.

Step 3.Q _nthe optimization calculated.Directly calculate Γ _nq _n , have as Two-order approximation:

${\mathrm{\Γ}}_n{Q}_n{\mathrm{\Γ}}_n^T=\frac{T_n}{2}(Q_{1n}+{\mathrm{\φ}}_n{Q}_{1n}{\mathrm{\φ}}_n^T)$ Formula 3-1

Wherein:

$Q_{1n}=G\left(t\right)Q\left(t\right)G{\left(t\right)}^T={\left[\begin{array}{cccc}{Q}_{11}^{\′}& O_{3\×9}& O_{3\×3}& O_{3\×3}\\ O_{9\×3}& O_{9\×9}& O_{9\×3}& O_{9\×3}\\ O_{3\×3}& O_{3\×9}& Q_{22}& O_{3\×3}\\ O_{3\×3}& O_{3\×9}& O_{3\×3}& Q_{33}\end{array}\right]}_{18\×18}$ Formula 3-2

In formula:

$Q_{11}^{\′}=C_B^N{Q}_{11}{\left(C_B^N\right)}^T$ Formula 3-3

Obviously, Q _1nand Γ _nq _n be symmetric matrix, to Γ _nq _n only need to calculate triangular portions, Q _1nsubstitute into Γ _nq _n can obtain:

${\mathrm{\Γ}}_n{Q}_n{\mathrm{\Γ}}_n^T=\frac{T_n}{2}{\left[\begin{array}{cccccc}{Q}_{n11}& Q_{n12}& Q_{n13}& O_{3\×3}& Q_{n15}& Q_{n16}\\ Q_{n12}^T& Q_{n22}& Q_{n23}& O_{3\×3}& Q_{n25}& Q_{26}\\ Q_{n13}^T& Q_{n23}^T& Q_{n23}& O_{3\×3}& Q_{n35}& Q_{n36}\\ O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}& O_{3\×3}\\ Q_{n15}^T& Q_{n25}^T& Q_{n35}^T& O_{3\×3}& Q_{n55}& O_{3\×3}\\ Q_{n16}^T& Q_{n26}^T& Q_{n36}^T& O_{3\×3}& O_{3\×3}& Q_{n66}\end{array}\right]}_{18\×18}$ Formula 3-4

Wherein:

$Q_{\mathrm{nij}}=\frac{T_n}{2}[{\mathrm{\φ}}_{\mathrm{ij}}{Q}_{11}^{\′}{\mathrm{\φ}}_{j1}^T+{\mathrm{\φ}}_{i5}{\left(Q_{22}{\mathrm{\φ}}_{j5}^T\right)}_{\mathrm{Sim}}+{\mathrm{\φ}}_{i6}{\left(Q_{33}{\mathrm{\φ}}_{j6}^T\right)}_{\mathrm{Sim}}],i,j=\mathrm{1,2,3};i\≤j$ Formula 3-5a

$Q_{n11}=Q_{n11}+\frac{T_n}{2}{Q}_{11}^{\′}$ Formula 3-5b

$Q_{\mathrm{nij}}=\frac{T_n}{2}{\left({\mathrm{\φ}}_{\mathrm{ij}}{Q}_{j-3,j-3}{\mathrm{\φ}}_{\mathrm{jj}}\right)}_{\mathrm{Sim}},i=\mathrm{1,2,3};j=\mathrm{5,6}$ Formula 3-5c

$Q_{n55}=\frac{T_n}{2}{(Q_{22}+{\mathrm{\φ}}_{55}{Q}_{22}{\mathrm{\φ}}_{55})}_{\mathrm{Sim}}$ Formula 3-5d

$Q_{n66}=\frac{T_n}{2}{(Q_{33}+{\mathrm{\φ}}_{66}{Q}_{33}{\mathrm{\φ}}_{66})}_{\mathrm{Sim}}$ Formula 3-5e

To Γ _nq _n the optimization calculated mainly utilizes sparse matrix G (t), diagonal matrix piecemeal Q ₂₂, Q ₃₃φ ₅₅, φ ₆₆singularity and Γ _nq _n symmetry carry out off-line derivation.Especially, Q in formula 3-5c _{j-3, j-3}φ _jj, Q in formula 3-5d _n55, Q in formula 3-5e _n66be still diagonal matrix, can be used for simplifying calculating.Statistics shows, The present invention reduces Γ _nq _n upgrade multiplication (addition) computing of 94% (96%) and the internal memory cost of 88%.

Step 4.P _nthe optimization that (-) calculates.For deriving conveniently, by P _n(-), P _n(+ _e) unification P _nrepresent (in actual program, the P in the 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 symmetric matrix, then have:

$P_{n,\mathrm{ij}}=P_{n,\mathrm{ji}}^T,i,j=\mathrm{1,2}...6$ Formula 4-2

First do not consider Γ _nq _n , formula 1-14a is write as:

$P_n(-)={\mathrm{\φ}}_n{P}_{n-1}\left({+}_e\right){\mathrm{\φ}}_n^T$ Formula 4-3

Substitution formula 2-8, formula 4-1 obtain:

$P_{n,11}=\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\left\{{\mathrm{\φ}}_{1i}{\left[\underset{j=1}{\overset{6}{\mathrm{\Σ}}}\left(P_{n-1,\mathrm{ij}}{\mathrm{\φ}}_{1j}^T\right)\right]}_{\mathrm{Temp}}\right\}$ Formula 4-4a

$P_{n,21}=\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\left\{{\mathrm{\φ}}_{2i}{\left[\underset{j=1}{\overset{6}{\mathrm{\Σ}}}\left(P_{n-1,\mathrm{ij}}{\mathrm{\φ}}_{1j}^T\right)\right]}_{\mathrm{Temp}}\right\}$ Formula 4-4b

P _{n, 66}=(φ ₆₆p _n-1,66φ ₆₆) _simformula 4-4c

In formula () _tempbe for reduce double counting preserve in a computer intermediate quantity, () _reprepresent () _tempin calculated duplicate keys, essence is result of calculation.To () _tempcalculating can utilize singularity be optimized.

Consider Γ _nq _n , wushu 3-4, formula 4-1 substitute into formula 1-14a and obtain:

P _{n, ij}=P _{n, ij}+ Q _{n, ij}, i, j=1,2,3,5,6; I>=j formula 4-5

In sum, P _nthe calculating of (-) is divided into formula 3-4 and formula 4-3 two steps, and optimizing process mainly utilizes φ _npiecemeal discreteness, φ _i3and φ _{ii (i=5,6)}singularity and introduce intermediate quantity avoid double counting item.Statistics shows, to P _nthe optimization of (-) decreases the calculated amount of 69% and the memory cost of 44%.

The optimization that step 5.Kn calculates.The part of inverting that wushu 1-9, formula 1-11 and formula 4-1 are updated to formula 1-14b can obtain:

$H_n{P}_n(-)H_n^T+R_n={\left[\begin{array}{cc}{H}_{13}{P}_{m,33}{H}_{13}+R_{11}& H_{13}{T}_{n,23}^T\\ P_{n,23}{H}_{13}& P_{n,22}+R_{22}\end{array}\right]}_{6\×6}$ Formula 5-1

Order

$D={[H_n{P}_n(-)H_n^T+R_n]}^{-1}$ Formula 5-2

Easy proof D ^t=D, is write as:

$D={\left[\begin{array}{cc}{D}_{11}& D_{12}\\ D_{12}^T& D_{22}\end{array}\right]}_{6\×6}$ Formula 5-3

Then have:

$K_n={\left[\begin{array}{cc}{P}_{n,12}{D}_{12}^T+P_{n,13}{H}_{13}{D}_{11}& P_{n,12}{D}_{22}+P_{n,13}{H}_{13}{D}_{12}\\ P_{n,22}{D}_{12}^T+P_{n,23}{H}_{13}{D}_{11}& P_{n,22}{D}_{22}+P_{n,23}{H}_{13}{D}_{12}\\ .& .\\ .& .\\ .& .\\ P_{n,62}{D}_{12}^T+P_{n,63}{H}_{13}{D}_{11}& P_{n,62}{D}_{22}+P_{n,63}{H}_{13}{D}_{12}\end{array}\right]}_{18\×6}$ Formula 5-4

Be designated as:

$K_n={\left[\begin{array}{cc}{K}_{11}& K_{12}\\ K_{21}& K_{22}\\ .& .\\ .& .\\ .& .\\ K_{61}& K_{62}\end{array}\right]}_{18\×6}$ Formula 5-5

Have:

$K_{\mathrm{ij}}=P_{n,i2}{D}_{j2}^T+P_{n,i3}{H}_{13}{D}_{1j},i=\mathrm{1,2},...,6;j=\mathrm{1,2}$ Formula 5-6

From above-mentioned derivation, after adopting partitioned matrix to carry out derivation optimization, K _ncalculating only need the relatively simple process of execution formula 5-1, formula 5-2, formula 5-6 tri-, utilize H _nopenness, avoid 5 high level matrix multiplication in formula 1-14b.The statistics of optimum results is shown, The present invention reduces K _nupgrade the calculated amount of 89% and the internal memory cost of 61%.

Step 6.P _n(+ _e) optimization that calculates.Formula 1-14c is rewritten into:

P _n(+ _e)=P _n(-)-K _nh _np _n(-) formula 6-1

Main calculated amount is K _nh _np _n(-), because P _n(+ _e) and P _n(-) is all symmetric matrix, then K _nh _np _n(-) is also symmetric matrix, only needs to calculate triangular portions.

Note K _nh _np _n(-) is Δ P _n:

formula 6-2

Substitution formula 1-9, formula 4-1 and formula 5-5 obtain:

Δ P _ij=K _i1h ₁₃p _{n, 3j}+ K _i2p _{n, 2j}, i, j=1,2 ..., 6; I≤j formula 6-3

Then have:

P _{n, ij}=P _{n, ij}-Δ P _ij, i, j=1,2 ..., 6; I≤j formula 6-4

Can see from derivation, utilize P _n(+ _e) symmetry and H _npiecemeal is openness, derives P _n(+ _e) succinct form of calculation (formula 6-3, formula 6-4), account in the formula 6-3 of main calculated amount and only need K _nwith P _n(+ _e) the 2nd, a small amount of 3 rank matrix multiplications of work that 3 piecemeals are capable, reduce a large amount of unnecessary computations.Make comparisons with conventional algorithm, decrease the calculated amount of 85% and the memory cost of 61%.

Step 7. can't harm decoupling parallel optimization.From optimum results, P _nthe calculated amount of (-) still accounts for the half of whole filtering, becomes the bottleneck improving counting yield further.Consider P _nthe renewal of (-) implements parallel processing, to improve real-time.From above-mentioned Kalman filter iterative computation derivation, except P _n(+ _e) outward, the renewal of other parameter or and P _n(-) irrelevant or only with the 2nd, capable and 2,3 piecemeals of 3 piecemeals show pass.Therefore P is defined _n(-) the 2nd, 3 piecemeal row (column) be " useful " information, all the other piecemeals are " without have " information.Further, because P _n(-) is symmetric matrix, and therefore in fact " useful " piecemeal only has the 2nd, 3 piecemeal row." useless " piecemeal "×" represents, then P _n(-) can be write as:

$P_n={\left[\begin{array}{cccccc}\×& P_{n,12}& P_{n,31}& \×& \×& \×\\ \×& P_{n,22}& P_{n,32}& \×& \×& \×\\ \×& P_{n,32}& P_{n,33}& \×& \×& \×\\ \×& P_{n,42}& P_{n,34}& \×& \×& \×\\ \×& P_{n,52}& P_{n,35}& \×& \×& \×\\ \×& P_{n,62}& P_{n,36}& \×& \×& \×\end{array}\right]}_{18\×18}$ Formula 3.6-1

In measurement upgrades, only P _n(+ _e) and " useless " data coupling.By P _n(+ _e) renewal be subdivided into Δ P _ijthe renewal of (formula 6-3) and P _n(-) adds up (formula 6-4) two processes, and uses Δ P _ijreplace P _n(+ _e), form new measurement renewal process with formula 1-14b, formula 1-14d.Therefore, P is obtained _n(-) " useless " information updating with newly measure the numerical value decoupling zero upgraded, can by these two concurrent process process, to solve P _n(-) calculates bottleneck problem consuming time.Similarly, to P _nthe renewal of (-) " useful " piecemeal is subdivided into by formula 4-3 renewal and upgrades two calculation procedures by formula 4-5, obviously, in first step calculate Γ in upgrading with the time _nq _n calculating be to carry out parallel processing.

In sum, the Kalman filter implementation after using parallel processing to optimize as shown in Figure 3.With compared with parallel optimization process, the parallel optimization efficiency shown in Fig. 3 improves about 25%.The more important thing is that Uncoupled procedure of the present invention is the digital characteristic utilizing SINS/GPS integrated navigation, being derived by strict numeral is obtained, different from popular method for parallel processing, and not needing to do " forcing delayed " process, is a kind of harmless method for parallel processing.

The part that the present invention does not elaborate belongs to techniques well known.

Claims (5)

1., based on a Kalman filtering numerical optimization for SINS/GPS integrated navigation, it is characterized in that comprising the following steps:

Step (1), set up the SINS/GPS integrated navigation loose coupling closed loop Kalman filter model of a kind of position-based-velocity survey combination, represent the parameter matrix in model with 3 rank square formations expansion, comprise coefficient of regime A _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, 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 variable symbolic operation function is used, to formula in Matlab environment by the 3 sub-piecemeal unfolding calculation in rank, off-line derives discrete filter transition matrix φ _nsimplification form of calculation;

Step (3), step (2) is utilized to derive the φ obtained _nopenness, the special sub-piecemeal of piecemeal and noise matrix symmetry, in Matlab environment, off-line is derived simplification form of calculation;

Step (4), utilize φ _nopenness, the special sub-piecemeal of piecemeal, intermediate variable and covariance matrix P _nthe symmetry of (-), to formula in Matlab environment by the 3 sub-piecemeal unfolding calculation in rank, derive P _nthe simplification form of calculation that (-) time upgrades;

Step (5), utilize system measurements equation coefficient matrix H _nopenness, the special sub-piecemeal of piecemeal, to formula in Matlab environment carry out the off-line derivation of equation, obtain filter gain K _nsimplification form of calculation;

Step (6), utilize H _nopenness, the special sub-piecemeal of piecemeal, covariance matrix P _nthe symmetry of (-), to formula P in Matlab environment _n(+ _e)=(I-K _nh _n) P _n(-), by 3 rank submatrix unfolding calculation, off-line pushes away P _n(+ _e) measure the simplification form of calculation upgraded;

Step (7), segmentation P _n(+ _e) and P _nthe computation process of (-), and by matrix P consuming time _n(-) divides " useful ", " useless " information, by the dependence of subprocess analytic hierarchy process to parameter, obtains a kind of new numerical value decoupling mechanism, realizes the harmless parallel processing to filtering on this basis.

2. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation according to claim 1, is characterized in that: described step (2) is right calculation optimization specifically comprise following steps:

Step (21), usage factor matrix A _nopenness and special sub-piecemeal, off-line is derived simplification form of calculation and as intermediate variable preserve, derivation result shows there is piecemeal openness, and the 3rd row piecemeal S _i3, piecemeal S on diagonal line ₅₅, S ₆₆for special sub-piecemeal;

Step (22), usage factor matrix A _n, openness and special sub-piecemeal, will by 3 rank piecemeal unfolding calculation, off-line is derived simplification form of calculation and as intermediate variable preserve, point out there is piecemeal openness, and the 3rd row piecemeal T _i3, piecemeal T on diagonal line ₅₅, T ₆₆for special sub-piecemeal;

Step (23), in the computation process of step (21), (22), introduce numerical value be O _{3 × 3}submatrix symbol A ₃₁, S ₃₄, S ₃₅, to keep the continuity of form of calculation, reduce the programming complexity of real-time calculation procedure.

3. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation according to claim 1, is characterized in that: described step (3) is right calculation optimization specifically comprise following steps:

Step (31), utilize noise drive matrix G (t) openness, by 3 rank piecemeal unfolding calculation Q _1n=G (t) Q (t) G (t) ^t, off-line derives the discrete form of Q (t), points out Q _1nthere is block diagonal form;

Step (32), utilize Q _1ndiagonal form, φ _nopenness and Γ _nq _nΓ ^t _nsymmetry, press ${\mathrm{\Γ}}_n{Q}_n{\mathrm{\Γ}}_n^T=\frac{T_n}{2}(Q_{1n}+{\mathrm{\φ}}_n{Q}_{1n}{\mathrm{\φ}}_n^T)$ Off-line is derived simplification form of calculation.

4. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation according to claim 1, is characterized in that: described step (5) is to K _ncalculation optimization specifically comprise following steps:

Step (51), utilize H _nopenness, by 3 rank piecemeal unfolding calculation the inverse matrix of its result saves as intermediate variable D;

Step (52), utilize H _nopenness and P _nthe symmetry of (-), presses off-line derives K _nsimplification form of calculation.

5. a kind of Kalman filtering numerical optimization based on SINS/GPS integrated navigation according to claim 1, is characterized in that: described step (7) parallel optimization process specifically comprises following steps:

Step (71), by P _n(+ _e) computation process press P _n(+ _e)=P _n(-)-K _nh _np _n(-) segments, K _nh _np _n(-) replaces P _n(+ _e) set up new measurement renewal;

Step (72), definition P _n2, the 3 piecemeal row of (-) are defined as " useful " information, and all the other are " useless " information, and the renewal process of " useless " information is with newly measuring renewal process exists numerical value decoupling zero relation, this two process real-time parallels process;

Step (73), by P _nthe computation process of (-) " useful " information is pressed segmentation, subprocess upgrade with there is numerical value decoupling zero relation in renewal, real-time parallel process.