Cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling

Abstract

The present invention discloses a cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling, including: S1, constructing a high-dimensional GNSS/INS deep coupling filter model; S2, generating an initialization cubature point for the constructed filter model by using standard cubature rules; S3, performing CKF filtering by using novel cubature point update rules. The present invention is suitable for high-dimensional GNSS/INS deep coupling filtering with high precision and high stability.

Claims

1. A method of integrated navigation and target tracking for high-dimensional GNSS/INS deep coupling comprising a cubature Kalman filtering (CKF) method, wherein the cubature Kalman filtering method comprises: S1, constructing a high-dimensional GNSS/INS deep coupling filter model to obtain a constructed filter model; S2, generating an initialization cubature point for the constructed filter model by using standard cubature rules; S3, performing CKF filtering on the constructed filter model by using novel cubature point update rules; wherein the step S3 comprises: S31, calculating a state priori distribution at k moment by using the following formula: $\{\begin{array}{c}{x}_k^{-}N\left({\hat{x}}_{k.Math.k-1},P_{k.Math.k-1}\right)\\ {\hat{x}}_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_{i}f\left(X_{i,k-1.Math.k-1}^{+}\right)\\ P_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i\left(f\left(X_{i,k-1.Math.k-1}^{+}\right)-{\hat{x}}_{k.Math.k-1}\right){\left(f\left(X_{i,k-1.Math.k-1}^{+}\right)-{\hat{x}}_{k.Math.k-1}\right)}^T+Q_{k-1}\end{array},$ wherein in the formula, x.sub.k.sup.represents a state priori estimate at k moment, {circumflex over (x)}.sub.k|k1 is a mean of the x.sub.k.sup., P.sub.k|k1 is a variance of the x.sub.k.sup., {circumflex over (x)}.sub.k|k1 represents a state estimate at k moment speculated from a measurement and a state at k1 moment, P.sub.k|k1 represents a covariance of {circumflex over (x)}.sub.k|k1, w.sub.i=1/2n.sub.x, and Q.sub.k1 is a system noise variance matrix; S32, calculating a cubature point error matrix {tilde over (x)}.sub.i,k|k1.sup.of the prediction process using the following formula, and defining .sub.k.sup.-=P.sub.k|k1Q.sub.k1 as an error variance of a Sigma point statistical linear regression (SLR) in a priori PDF approximation process;
{tilde over (x)}.sub.i,k|k1.sup.=x.sub.i,k|k1{circumflex over (x)}.sub.k|k1, 0i2n.sub.x, wherein x.sub.i,k|k1=f(x.sub.i,k1|k1.sup.+) is a cubature point after x.sub.i,k1|k1.sup.+propagates through a system equation; S33, taking the cubature point after the propagation of the system equation as a cubature point of a measurement update process; S34, using a CKF measurement update to calculate a likelihood distribution function of a measured value; wherein, $\{\begin{array}{c}{z}_k^{-}N\left({\hat{z}}_{k.Math.k-1},P_{\mathrm{zz},k.Math.k-1}\right)\\ {\hat{z}}_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_{i}h\left(X_{i,k.Math.k-1}\right)\\ P_{\mathrm{zz},k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right){\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right)}^T+R_k\end{array},$ wherein in the formula, z.sub.k.sup.represents a measurement likelihood estimate at k moment, {circumflex over (z)}.sub.k|k1 is a mean of the z.sub.k.sup., P.sub.zz,k|k1 is a variance of the z.sub.k.sup., {circumflex over (z)}.sub.k|k1 is a measurement at k moment predicted from the state at k1 moment, h(x) is a measurement equation, w.sub.i=1/2n.sub.x, and R.sub.k is a measurement noise variance matrix; S35, calculating a posterior distribution function of a state variable x; wherein $\{\begin{array}{c}{x}_k^{+}N\left({\hat{x}}_{k.Math.k},P_{k.Math.k}\right)\\ {\hat{x}}_{k.Math.k}={\hat{x}}_{k.Math.k-1}+K_k\left(z_k-{\hat{z}}_{k.Math.k-1}\right)\\ P_{k.Math.k}=P_{k.Math.k-1}-K_k{P}_{\mathrm{zz},k.Math.k-1}{K}_k^T\\ P_{\mathrm{xz},k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i(f(X_{i,k-1.Math.k-1}\left(-{\hat{x}}_{k.Math.k-1}\right){\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right)}^T\end{array},$ wherein in the formula, x.sub.k.sup.+, is a posterior estimate of the state variable at k moment, a mean and a variance of the x.sub.k.sup.+ are {circumflex over (x)}.sub.k|k and P.sub.k|k, respectively, K.sub.k=P.sub.xz,k|k1(P.sub.zz,k|k1).sup.1 is a Kalman gain matrix, P.sub.xz,k|k1 is a cross-covariance between a posterior estimate of the state variable and the measurement likelihood estimate; S36, defining an error caused by a Sigma point approximation to a posterior distribution as .sub.k.sup.+=P.sub.k|k, w=[w.sub.1 . . . w.sub.2n.sub.x]is a weight of a CKF cubature point SLR, a SLR of the priori distribution at k moment accurately captures a mean and a covariance of the state, and consider effects of system uncertainty and noise, then
{tilde over (x)}.sub.i,k|k1.sup.w=0,
{tilde over (x)}.sub.i,k|k1.sup.diag(w){tilde over (x)}.sub.i,k|k1.sup..sup.T=P.sub.k|k1Q.sub.k, wherein in the formula, {tilde over (x)}.sub.i,k|k1.sup.is the cubature point error matrix of the prediction process, =diag(w) represents that the matrix is constructed using w diagonal elements, in a SLR of a similar posterior distribution, the cubature point can at least accurately match the mean and the variance of the state, namely,
{tilde over (x)}.sub.i,k|kw=0,
{tilde over (x)}.sub.i,k|k.sup.+diag(w)({tilde over (x)}.sub.i,k|k.sup.+).sup.T=P.sub.k|k, wherein in the formula, {tilde over (X)}.sub.i,k|k.sup.+is an updated cubature point; S37, assuming both .sub.k.sup.and .sub.k.sup.+are symmetric positive definite matrices, and {tilde over (x)}.sub.i,k|k =B.Math.{tilde over (X)}.sub.i,k|K1.sup., then .sub.k.sup.=L.sub.k(L.sub.k).sup.T, .sub.k.sup.+=L.sub.k+1(L.sub.k+1).sup.T wherein B is a transformation matrix to be solved, {tilde over (x)}.sub.i,k|k.sup.+is an updated cubature point error matrix; further .sub.k.sup.+=BL.sub.k (L.sub.k).sup.TB.sup.T , B=L.sub.k+1(L.sub.k).sup.1, wherein is an arbitrary orthogonal matrix that satisfies .sup.T=I.sub.n.sub.x, when is taken as a unit matrix, B=L.sub.k+1(L.sub.k).sup.1 is obtained; S38, obtaining an updated cubature point based on a posterior state estimate mean {circumflex over (x)}.sub.k|k and an updated cubature point error matrix as x.sup.+.sub.i,k|k={circumflex over (x)}.sub.k|k={tilde over (x)}.sub.i,k|k.sup.+, 0i2n.sub.x.

2. The method according to claim 1, wherein the step S1 comprises: S11, setting an INS state variable of a subsystem of an INS as x.sub.I and a GNSS state variable of a subsystem of a GNSS as x.sub.G, respectively, wherein,
x.sub.I =[P V k.sub.a b.sub.a K.sub.107 b.sub.],
x.sub.G =[b.sub.c d.sub.c .sub.dll .sub.pll f.sub.pll], and wherein in the formula, [P V k.sub.b.sub.k.sub.b.sub.] are respectively and successively a 3D position error, a velocity error, an attitude error, an accelerometer coefficient error, an accelerometer coefficient null bias, a gyroscope coefficient error and a constant value drift; [b.sub.c d.sub.c .sub.dll .sub.pll f.sub.pll ] are respectively and successively a receiver clock bias, a clock drift, a phase-discrimination pseudo-range error of a code tracking loop, a phase error of a carrier tracking loop, and a frequency error of a carrier tracking loop; S12, establishing a system model of the high-dimensional GNSS/INS deep coupling according to the INS state variable x.sub.I and the GNSS state variable x.sub.G as: $\left[\begin{array}{c}{\stackrel{.}{x}}_I\\ {\stackrel{.}{x}}_G\end{array}\right]=\left[\begin{array}{cccc}{F}_I\left(t\right)& O_{212}& O_{214}& O_{218}\\ O_{221}& F_G\left(t\right)& O_{24}& O_{28}\\ F_{\mathrm{ID}}\left(t\right)& O_{42}& F_D\left(t\right)& O_{48}\\ F_{\mathrm{IP}}\left(t\right)& O_{82}& O_{84}& F_P\left(t\right)\end{array}\right]\left[\begin{array}{c}{x}_I\\ x_G\end{array}\right]+G\left[\begin{array}{c}{W}_I\\ W_G\end{array}\right],$ wherein in the formula, $F_I\left(t\right)={\left[\begin{array}{cc}{F}_N\left(t\right)& F_S\left(t\right)\\ O_{99}& F_M\left(t\right)\end{array}\right]}_{2121},$ F.sub.N (t) is a 9-dimensional inertial navigation basic system matrix and O.sub.39, $F_S\left(t\right)={\left[\begin{array}{cccc}{C}_a& O_{33}& O_{33}& O_{33}\\ O_{33}& C_b^n\left(t\right)& O_{33}& O_{33}\\ O_{33}& O_{33}& C_{}& O_{33}\\ O_{33}& O_{33}& O_{33}& C_b^n\left(t\right)\end{array}\right]}_{1212},$ F.sub.M(t)=[O.sub.1212], O.sub.ij is a null matrix with i rows and j columns, C.sub.and C.sub.are an accelerometer coefficient error matrix and a gyroscope coefficient error matrix respectively, C.sub.b.sup.n(t) is an attitude matrix at t moment, $F_G\left(t\right)=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_r}\end{array}\right],F_P\left(t\right)={\left[\begin{array}{cc}{0}_{44}& 2.Math.I_{44}\\ K_{\mathrm{pll}}\text{/}{T}_1.Math.I_{44}& 2K_{\mathrm{pll}}{T}_2\text{/}{T}_1.Math.I_{44}\end{array}\right]}_{88},$ T.sub.1 and T.sub.2 are parameters of a loop filter, $F_{\mathrm{ID}}\left(t\right)={\left[\begin{array}{ccc}-{\stackrel{.fwdarw.}{L}}_1^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_1^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_2^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_2^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_3^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_3^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_4^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_4^T.Math.C_n^e& O_{115}\end{array}\right]}_{421},F_D\left(t\right)=-{\left[\begin{array}{cccc}{K}_{\mathrm{dll}}& 0& 0& 0\\ 0& K_{\mathrm{dll}}& 0& 0\\ 0& 0& K_{\mathrm{dll}}& 0\\ 0& 0& 0& K_{\mathrm{dll}}\end{array}\right]}_{44},F_{\mathrm{IP}}\left(t\right)={\left[\begin{array}{c}{O}_{44}2.Math.I_{44}{O}_{413}\\ K_{\mathrm{pll}}\text{/}{T}_1.Math.I_{44}2K_{\mathrm{pll}}{T}_2\text{/}{T}_1.Math.I_{44}{O}_{413}\end{array}\right]}_{821},$ {dot over (x)}.sub.I and {dot over (x)}.sub.G are differential terms of x.sub.I and x.sub.G respectively, G is a system-noise-driven matrix, W.sub.I and W.sub.G are system noise of the INS, the GNSS and tracking loops, T.sub.r is a relevant time, K.sub.pll is a carrier loop gain, I.sub.ij is a unit matrix with i rows and j columns, {right arrow over (L)}.sup.T=[{right arrow over (L)}.sub.1 .sup.T{right arrow over (L)}.sub.2.sup.T{right arrow over (L)}.sub.3.sup.T{right arrow over (L)}.sub.4.sup.T] is a radial position vector matrix of a satellite and a receiver, W is a radial velocity error of the satellite and the receiver caused by INS position calculation errors, C.sub.n.sup.e is a conversion matrix of navigation coordinate system to earth-centered earth-fixed coordinate system, K.sub.dll is a code loop gain; S13, establishing a measurement model of GNSS/INS deep coupling according to the system model of GNSS/INS deep coupling as:
=.sub.I.sub.G=.sub.I(b.sub.c+v.sub.+.sub.dll),
{dot over ()}=.sub.I{dot over ()}.sub.G={dot over ()}.sub.I(b.sub.c+v.sub.{dot over ()}+.sub.dll), wherein in the formula, and {dot over ()} are observed quantities of a pseudo-range and a pseudo-range rate of a satellite channel respectively, .sub.I and {dot over ()}.sub.I, are a pseudo-range and a pseudo-range rate predicted by the INS respectively, .sub.G and {dot over ()}.sub.G are a pseudo-range and a pseudo-range rate measured by the GNSS respectively, b.sub.c and d.sub.c are a clock bias and a clock drift of the receiver respectively, v.sub.and v.sub.{dot over ()}are observation noise of the pseudo-range and the pseudo-range rate respectively, .sub.dll and {dot over ()}.sub.pll are pseudo-range errors and pseudo-range rate errors of the tracking loops respectively, v is a measurement noise matrix, that is $\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]=H\left(t\right)\left[\begin{array}{c}{x}_I\\ x_G\end{array}\right]+v,H\left(t\right)={\left[\begin{array}{c}{\stackrel{.fwdarw.}{L}}^T{C}_n^e{O}_{418}-I_{41}{O}_{41}-I_{44}{O}_{44}{O}_{44}\\ O_{43}{O}_{418}{O}_{41}-I_{41}{O}_{44}{O}_{44}c\text{/}{f}_{L1}.Math.I_{44}\end{array}\right]}_{835},$ f.sub.L1 is a GPS L1 carrier frequency, and c is speed of light.

3. The method according to claim 2, wherein an equation of the phase-discrimination pseudo-range error .sub.dll of the code tracking loop is
{dot over ()}.sub.dll=K.sub.dll.sub.dll+V.sub.aid +K.sub.dllQ, wherein .sub.dll is an output of a code loop filter, {dot over ()}.sub.dll is a differential of the .sub.dll, K.sub.dll is a code loop gain, V.sub.aid is a speed assistance provided by the INS to the code tracking loop after a filtering output calibration, Q is system noise caused by loop thermal noise and interferences.

4. The method dimensional GNS SANS deep coupling according to claim 2, wherein equations of errors of the carrier tracking loop are ${\stackrel{.}{}}_{\mathrm{pll}}=2\left(f_{\mathrm{pll}}+f_{\mathrm{aid}}\right),{\stackrel{.}{f}}_{\mathrm{pll}}=\left[2\frac{T_2}{T_1}\left(f_{\mathrm{pll}}+f_{\mathrm{aid}}\right)+\frac{{}_{\mathrm{pll}}}{T_1}\right].Math.K_{\mathrm{pll}},$ wherein, .sub.pll is the phase error of the carrier tracking loop, f.sub.pll is the frequency error of the carrier tracking loop, and f.sub.aid is a Doppler frequency error assistance provided by the INS to the carrier tracking loop after a filtering output calibration, T.sub.1 and T.sub.2 are parameters of the loop filter, that is, a transfer function of the loop filter is $F\left(s\right)=\frac{T_{2}s+1}{T_{1}s}.$

5. The method according to claim 1, wherein the step S2 comprises: S21, calculating a state posterior distribution {circumflex over (x)}.sub.k1 at kl moment by using a high-precision nonlinear filtering method; wherein, {circumflex over (x)}.sub.k1 N({circumflex over (x)}.sub.k1|k1, P.sub.k1|k1), {circumflex over (x)}.sub.k1 represents the state posterior distribution at k1 moment, {circumflex over (x)}.sub.k1|k1 represents the state distribution at k1 moment speculated from the measurement at k-1 moment, P.sub.k1|k1 represents a covariance of the {circumflex over (x)}.sub.k1|K1, xN(,.sup.2) represents that x obeys a Gaussian distribution with a mean of and a variance of .sup.2; S22, generating cubature points required during a prediction process of a system state equation f(x) by using the standard cubature rules; wherein. $\{\begin{array}{c}{X}_{i,k-1.Math.k-1}^{+}=S_{k-1.Math.k-1}{}_i+{\hat{x}}_{k-1.Math.k-1}\\ {}_i={\sqrt{n_x}\left[I_{n_x}-I_{n_x}\right]}_i\end{array},$ wherein in the formula, x is a state variable of the GNSS/INS deep coupling filtering model, and a dimension of the x is n.sub.x, I.sub.n.sub.xis a n.sub.x dimensional unit square matrix, x.sub.i,k1|k1 represents the i.sup.th cubature point at k-1 moment speculated from the measurement at k-1 moment, .sub.i is the i.sup.th standard cubature point in the CKF filtering, i=1, . . . , 2n.sub.x, S.sub.k1|k1 satisfies P.sub.k1|k1=S.sub.k1|k1(S.sub.k1|k1).sup.T.

6. The method according to claim 5, wherein the high-precision nonlinear filtering method comprises: setting a measurement equation of an iterated nonlinear filtering as:
x.sub.k|k.sup.j+1=x.sub.k|k.sup.j+.sub.j{K.sub.j(z.sub.kh(x.sub.k|k.sup.j)(p.sub.k|k1 .sup.1p.sub.xz,k|k1).sup.T.sub.j)+.sub.j}, wherein x.sub.k|k.sup.j , is a state estimate value of the j.sup.th measurement iteration at k moment, z.sub.k is a measured value output by a phase discriminator of a multi-satellite channel at k moment, h(.Math.) is a measurement equation of a deep coupling tracking loop, .sub.j=x.sub.k|k .sup.j{circumflex over (x)}.sub.k|k1 is an error between a posterior estimate and a priori estimate in the current iteration, K.sub.j is a Kalman gain of the jt.sup.h iteration, .sub.j is an iterated step-size control coefficient set by accelerating convergence, satisfying 0.sub.j<1, P.sub.k|k1.sup.1 is an inverse of P.sub.k|k1, P.sub.xz,k|k1 is an cross-covariance generated during a prediction process, a covariance matrix of state posterior estimation errors takes a result of the last iteration calculation.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a system block diagram according to one embodiment of the present invention.

(2) FIG. 2 is a schematic diagram of high-dimensional GNSS/INS deep coupling filtering model.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(3) As shown in FIG. 1, the cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling of the present embodiment includes the following steps:

(4) S1, constructing a high-dimensional GNSS/INS deep coupling filter model, as shown in FIG. 2.

(5) Step S1 specifically includes:

(6) S11, setting the INS state variable of the subsystem of the INS as x.sub.I and the GNSS state variable of the subsystem of the GNSS as x.sub.G, respectively, wherein,
x.sub.I=[P V k.sub.b.sub. k.sub. b.sub.],
x.sub.G=[b.sub.c d.sub.c .sub.dll .sub.pll f.sub.pll],

(7) and wherein in the formula, [P V k.sub.a b.sub.a k.sub. b.sub.107 ] are respectively and successively a 3D position error, a velocity error, an attitude error, an accelerometer coefficient error, an accelerometer coefficient null bias, a gyroscope coefficient error and a constant value drift; [b.sub.c d.sub.c .sub.dll .sub.pll f.sub.pll] are respectively and successively a receiver clock bias, a clock drift, a phase-discrimination pseudo-range error of the code tracking loop, a phase error of the carrier tracking loop, and a frequency error of the carrier tracking loop.

(8) Wherein an equation of the GNSS code tracking phase-discrimination pseudo-range error .sub.dll is
{dot over ()}.sub.dll=K.sub.dll.sub.dll+V.sub.aid+K.sub.dllQ,

(9) wherein .sub.dll is an output of the code loop filter, {dot over ()}.sub.dll is a differential of the .sub.dll K.sub.dll is a code loop gain, V.sub.aid is a speed assistance provided by the INS to the code tracking loop after filtering output calibration, Q is the system noise caused by loop thermal noise and interference.

(10) Wherein equations of the carrier loop errors are

(11) ${\stackrel{.}{}}_{\mathrm{pll}}=2\left(f_{\mathrm{pll}}+f_{\mathrm{aid}}\right),{\stackrel{.}{f}}_{\mathrm{pll}}=\left[2\frac{T_2}{T_1}\left(f_{\mathrm{pll}}+f_{\mathrm{aid}}\right)+\frac{{}_{\mathrm{pll}}}{T_1}\right].Math.K_{\mathrm{pll}},$

(12) wherein, .sub.pll is the phase error of the carrier loop, f.sub.pll is the frequency error of the carrier loop, K.sub.dll is a loop gain, and f.sub.aid is a Doppler frequency error assistance provided by the INS to the carrier tracking loop after filtering output calibration, T.sub.1 and T2 are parameters of the loop filter, that is, the transfer function of the loop low-pass filter is

(13) $F\left(s\right)=\frac{T_{2}s+1}{T_{1}s}.$

(14) S12, establishing a system model of GNSS/INS deep coupling according to the state variables x.sub.I, and x.sub.G as:

(15) $\left[\begin{array}{c}{\stackrel{.}{x}}_I\\ {\stackrel{.}{x}}_G\end{array}\right]=\left[\begin{array}{cccc}{F}_I\left(t\right)& O_{212}& O_{214}& O_{218}\\ O_{221}& F_G\left(t\right)& O_{24}& O_{28}\\ F_{\mathrm{ID}}\left(t\right)& O_{42}& F_D\left(t\right)& O_{48}\\ F_{\mathrm{IP}}\left(t\right)& O_{82}& O_{84}& F_p\left(t\right)\end{array}\right]\left[\begin{array}{c}{x}_I\\ x_G\end{array}\right]+G\left[\begin{array}{c}{W}_I\\ W_G\end{array}\right],$

(16) wherein in the formula,

(17) $F_I\left(t\right)={\left[\begin{array}{cc}{F}_N\left(t\right)& F_S\left(t\right)\\ O_{99}& F_M\left(t\right)\end{array}\right]}_{2121},$

(18) F.sub.N (t) is a 9-dimensional inertial navigation basic System matrix and O.sub.39,

(19) $F_S\left(t\right)={\left[\begin{array}{cccc}{C}_a& O_{33}& O_{33}& O_{33}\\ O_{33}& C_b^n\left(t\right)& O_{33}& O_{33}\\ O_{33}& O_{33}& C_{}& O_{33}\\ O_{33}& O_{33}& O_{33}& C_b^n\left(t\right)\end{array}\right]}_{1212},$

(20) F.sub.M(t)=[O.sub.1212], O.sub.ij is a null matrix with i rows and j columns, C.sub. and C.sub. are an accelerometer coefficient error matrix and a gyroscope coefficient error matrix respectively, C.sub.b.sup.n(t) is an attitude matrix at t moment,

(21) $F_G\left(t\right)=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_r}\end{array}\right],F_P\left(t\right)={\left[\begin{array}{cc}{O}_{44}& 2.Math.I_{44}\\ K_{\mathrm{pll}}\text{/}{T}_1.Math.I_{44}& 2K_{\mathrm{pll}}{T}_2\text{/}{T}_1.Math.I_{44}\end{array}\right]}_{88},$

(22) T.sub.1 and T.sub.2 are parameters of the loop filter,

(23) $F_{\mathrm{ID}}\left(t\right)={\left[\begin{array}{ccc}-{\stackrel{.fwdarw.}{L}}_1^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_1^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_2^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_2^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_3^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_3^T.Math.C_n^e& O_{115}\\ -{\stackrel{.fwdarw.}{L}}_4^T.Math.C_n^e.Math.W& {\stackrel{.fwdarw.}{L}}_4^T.Math.C_n^e& O_{115}\end{array}\right]}_{412},F_D\left(t\right)=-{\left[\begin{array}{cccc}{K}_{\mathrm{dll}}& 0& 0& 0\\ 0& K_{\mathrm{dll}}& 0& 0\\ 0& 0& K_{\mathrm{dll}}& 0\\ 0& 0& 0& K_{\mathrm{dll}}\end{array}\right]}_{44},F_{\mathrm{IP}}\left(t\right)={\left[\begin{array}{ccc}{O}_{44}& 2.Math.I_{44}& O_{413}\\ K_{\mathrm{pll}}\text{/}{T}_1.Math.I_{44}& 2K_{\mathrm{pll}}{T}_2\text{/}{T}_1.Math.I_{44}& O_{413}\end{array}\right]}_{821},$

(24) {dot over (x)}.sub.I and {dot over (x)}.sub.G are differential terms of x.sub.I and x.sub.G respectively, G is a system-noise-driven matrix, W.sub.I and W.sub.G are system noise of INS, GNSS and tracking loops, T.sub.r is a relevant time, K.sub.pll is a carrier loop gain, I.sub.ij is a unit matrix with i rows and j columns, {right arrow over (L)}.sup.T=[{right arrow over (L)}.sub.1.sup.T {right arrow over (L)}.sub.2.sup.T {right arrow over (L)}.sub.3.sup.T {right arrow over (L)}.sub.4.sup.T] is a radial position vector matrix of satellite and receiver, W is a radial velocity error of satellite and receiver caused by INS position calculation errors, C.sub.n.sup.e is a conversion matrix of navigation coordinate system to earth-centered, earth-fixed coordinate system, K.sub.dll is a code loop gain.

(25) S13, establishing a measurement model of GNSS/INS deep coupling according to the system model of GNSS/INS deep coupling as:
=.sub.I.sub.G=.sub.I(b.sub.c+v.sub.+.sub.dll),
{dot over ()}={dot over ()}.sub.I{dot over ()}.sub.G={dot over ()}.sub.I(d.sub.c+v.sub.{dot over ()}+{dot over ()}.sub.pll),

(26) wherein in the formula, and {dot over ()} are observed quantities of the pseudo-range and pseudo-range rate of the satellite channel respectively, .sub.I and {dot over ()}.sub.I are the pseudo-range and the pseudo-range rate predicted by INS respectively, .sub.G and {dot over ()}.sub.G are the pseudo-range and the pseudo-range rate measured by GNSS respectively, b.sub.c and d.sub.c are the clock bias and clock drift of the receiver respectively, v.sub. and v.sub. are observation noise of the pseudo-range and pseudo-range rate respectively, .sub.dll and {dot over ()}.sub.pll are the pseudo-range errors and pseudo-range rate errors of the tracking loop respectively, v is a measurement noise matrix, that is

(27) 0$\left[\begin{array}{c}\\ \stackrel{.}{}\end{array}\right]=H\left(t\right)\left[\begin{array}{c}{x}_I\\ x_G\end{array}\right]+v,H\left(t\right)={\left[\begin{array}{ccccccc}{\stackrel{.fwdarw.}{L}}^T{C}_n^e& O_{418}& -I_{41}& O_{41}& -I_{44}& O_{44}& O_{44}\\ O_{43}& O_{418}& O_{41}& -I_{41}& O_{44}& O_{44}& c/f_{L1}.Math.I_{44}\end{array}\right]}_{835},$

(28) f.sub.L1 is a GPS L1 carrier frequency, and c is speed of light.

(29) S2, generating an initialization cubature point for the constructed filter model by using standard cubature rules.

(30) Step S2 specifically includes:

(31) S21, calculating a state posterior distribution {circumflex over (x)}.sub.k1 at k1 moment by using a high-precision nonlinear filtering method;

(32) wherein, {circumflex over (x)}.sub.k1N({circumflex over (x)}.sub.k1|k1, P.sub.k1|k1 {circumflex over (x)}.sub.k1 represents the state posterior distribution at k1 moment, {circumflex over (x)}.sub.k1|k1 represents the state distribution at k1 moment speculated from the measurement at k1 moment, P.sub.k1|k1 represents a covariance of the {circumflex over (x)}.sub.k1|k1, xN(, .sup.2 ) represents that x obeys a Gaussian distribution with a mean of and a variance of .sup.2.

(33) Wherein, the high-precision nonlinear filtering method includes: setting the measurement equation of an iterated nonlinear filtering as:
x.sub.k|k.sup.j+1=x.sub.k|k.sup.j+.sub.j{K.sub.j(z.sub.kh(x.sub.k|k.sup.j)(p.sub.k|k1 .sup.1p.sub.xz,k|k1).sup.T.sub.j)+.sub.j},

(34) where x.sub.k|k.sup.j is a state estimate value of the j.sup.th measurement iteration at k moment, z.sub.k is a measured value output by a phase discriminator of the multi-satellite channel at k moment, h(.Math.) is a measurement equation of the deep coupling tracking loop, .sub.j=x.sub.k|k.sup.j{circumflex over (x)}.sub.k|k1 is an error between a posterior estimate and a priori estimate in the current iteration, K.sub.j is a Kalman gain of the j.sup.th iteration, .sub.j is an iterated step-size control coefficient set by accelerating convergence, satisfying 0<.sub.j1, P.sub.k|k1.sup.1 is an inverse of P.sub.k|k1, P.sub.xz, k|k1 is an cross-covariance generated during the prediction process, the covariance matrix of the state posterior estimation errors takes the result of the last iteration calculation.

(35) S22, generating cubature points required during the prediction process of the system state equation f(x) by using the standard cubature rules;

(36) wherein,

(37) $\{\begin{array}{c}{X}_{i,k-1.Math.k-1}^{+}=S_{k-1.Math.k-1}{}_i+{\hat{x}}_{k-1.Math.k-1}\\ {}_i={\sqrt{n_x}\left[I_{n_x}-I_{n_x}\right]}_i\end{array},$

(38) wherein in the formula, x is a state variable of the GNSS/INS deep coupling filtering model, and its dimension is n.sub.x, I.sub.n.sub.x, is a n.sub.x dimensional unit square matrix, X.sub.i,k 1|k.sup.+ represents the i.sup.th cubature point at k1 moment speculated from the measurement at k1 moment, .sub.i is the i.sup.th standard cubature point in CKF filtering, i=1, . . . , 2n.sub.x, S.sub.k1|k1satisfies the P.sub.k1|k1=S.sub.k1|k1 (S.sub.k1|k1).sup.T.

(39) S3, performing CKF filtering by using novel cubature point update rules.

(40) Step S3 includes:

(41) S31, calculating the state priori distribution at k moment by using the following formula:

(42) $\{\begin{array}{c}{x}_k^{-}N\left({\hat{x}}_{k.Math.k-1},P_{k.Math.k-1}\right)\\ {\hat{x}}_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_{i}f\left(X_{i,k-1.Math.k-1}^{+}\right)\\ P_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i\left(f\left(X_{i,k-1.Math.k-1}^{+}\right)-{\hat{x}}_{k.Math.k-1}\right){\left(f\left(X_{i,k-1.Math.k-1}^{+}\right)-{\hat{x}}_{k.Math.k-1}\right)}^T+Q_{k-1}\end{array},$

(43) wherein in the formula, x.sub.k.sup.represents a state priori estimate at k moment, {circumflex over (x)}.sub.k|k1 is its mean, P.sub.k|k1 is its variance, {circumflex over (x)}.sub.k|k1 represents the state estimate at k moment speculated from the measurement and state at k1 moment, P.sub.k|k1 represents the covariance of {circumflex over (x)}.sub.k|k1, w.sub.i=1/2n.sub.x, and Q.sub.k1 is the system noise variance matrix;

(44) S32, calculating the cubature point error matrix X.sub.i,k|k1.sup.of the prediction process using the following formula, and defining .sub.k.sup.=P.sub.k|k1 Q.sub.k1 as an error variance of the Sigma point statistical linear regression (SLR) in a priori PDF approximation process;
{tilde over (X)}.sub.i,k|k1.sup.=X.sub.i,k|k1{tilde over (x)}.sub.k|k1, 0i2n.sub.x,

(45) wherein X.sub.i,k|k1=f(X.sub.i,k 1k 1 .sup.+) is the cubature point after X.sub.i,k1|k1.sup.+ propagates through the system equation;

(46) S33, taking the cubature point after the propagation of the system equation as a cubature point of the measurement update process;

(47) S34, using CKF measurement update to calculate a likelihood distribution function of the measured value;

(48) wherein,

(49) $\{\begin{array}{c}{z}_k^{-}N\left({\hat{z}}_{k.Math.k-1},P_{\mathrm{zz},k.Math.k-1}\right)\\ {\hat{z}}_{k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_{i}h\left(X_{i,k.Math.k-1}\right)\\ P_{\mathrm{zz},k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right){\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right)}^T+R_k\end{array},$

(50) wherein in the formula, z.sub.k.sup. represents a measurement likelihood estimate at k moment, {tilde over (z)}.sub.k|k1 is its mean, P.sub.zz,k|k1 is its variance, {tilde over (z)}.sub.k|k1 is a measurement at k moment predicted from the state at k1 moment, h(x) is the measurement equation, w.sub.i=1/2n.sub.x, and R.sub.k is the measurement noise variance matrix;

(51) S35, calculating a posterior distribution function of a state quantity x;

(52) wherein,

(53) $\{\begin{array}{c}{x}_k^{+}N\left({\hat{x}}_{k.Math.k},P_{k.Math.k}\right)\\ {\hat{x}}_{k.Math.k}={\hat{x}}_{k.Math.k-1}+K_k\left(z_k-{\hat{z}}_{k.Math.k-1}\right)\\ P_{k.Math.k}=P_{k.Math.k-1}-K_k{P}_{\mathrm{zz},k.Math.k-1}{K}_k^T\\ P_{\mathrm{xz},k.Math.k-1}=\underset{i=1}{\overset{2n_x}{.Math.}}{w}_i(f(X_{i,k-1.Math.k-1}\left(-{\hat{x}}_{k.Math.k-1}\right){\left(h\left(X_{i,k.Math.k-1}\right)-{\hat{z}}_{k.Math.k-1}\right)}^T\end{array},$

(54) wherein in the formula, x.sub.k.sup.+ is a posterior estimate of the state variable at k moment, its mean and variance are {circumflex over (x)}.sub.k|k, and P.sub.k|k, respectively, K.sub.k=P.sub.xz,k|k1(P.sub.zz,k|k1).sup.1 is a Kalman gain matrix, P.sub.xz,k|k1 is a cross-covariance between a posterior estimate of the state variable and the measured likelihood estimate;

(55) S36, defining an error caused by the Sigma point approximation to the posterior distribution as .sub.k.sup.+=P.sub.k|k, w=[w.sub.1 . . . w.sub.2n.sub.x ] is a weight of a CKF cubature point SLR, the SLR of the priori distribution at k moment should accurately capture the mean and covariance of the state, and consider the effects of system uncertainty and noise, then
{tilde over (X)}.sub.i,k|k1.sup.W=0,
{tilde over (X)}.sub.i,k|k1.sup.diag(w)X.sub.i,k|k1.sup..sup.T=P.sub.k|k1Q.sub.k,

(56) wherein in the formula, {tilde over (X)}.sub.i,k|k1 is a cubature point error matrix of the prediction process, =diag(W) represents that the matrix is constructed using W diagonal elements, in an SLR of a similar posterior distribution, the cubature point can at least accurately match the mean and variance of the state, namely,
{tilde over (X)}.sub.i,k|k.sup.+w=0,
{tilde over (X)}.sub.i,k|k.sup.+diag(W)({tilde over (X)}i,k|k.sup.+).sup.Ti=P.sub.k|k,

(57) wherein in the formula, {tilde over (X)}.sub.i,k|k.sup.+ is an updated cubature point;

(58) S37, assuming both .sub.k.sup. and .sub.k.sup. are symmetric positive definite matrices, and {tilde over (X)}.sub.i,k|k=B.Math.{tilde over (X)}.sub.i,k|k1.sup., then .sub.k.sup.=L.sub.k(L.sub.k).sup.T, .sub.k.sup.+=L.sub.k+1(L.sub.k+1).sup.T, wherein B is a transformation matrix to be solved, {tilde over (X)}.sub.i,k|k.sup.+is an updated cubature point error matrix; further .sub.k.sup.+=BL.sub.k(L.sub.k).sup.TB.sup.T, B=L.sub.k+1(L.sub.k).sup.1, wherein is an arbitrary orthogonal matrix that satisfies .sup.T=I.sub.n.sub.x, when is taken as unit matrix, B=L.sub.k+1(L.sub.k).sup.1 is obtained;

(59) S38, obtaining an updated cubature point based on the posterior state estimate mean {circumflex over (x)}.sub.k|k and updated cubature point error matrix as X.sub.i,k|k.sup.+={circumflex over (x)}.sub.k|k+{tilde over (X)}.sub.i,k|k.sup.+0 i2n.sub.x.

(60) The above are only preferred embodiments of the present invention, and the scope of the present invention is not limited thereto. Therefore, equivalent changes made according to the claims of the present invention are still within the scope of the present invention.