US 10,890,667 B2 |
||

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

Xiyuan Chen, Nanjing (CN); Bingbo Cui, Nanjing (CN); Wei Wang, Nanjing (CN); Zhengyang Zhao, Nanjing (CN); and Lin Fang, Nanjing (CN) |
||

Assigned to SOUTHEAST UNIVERSITY, Nanjing (CN) |
||

Appl. No. 16/94,474 |
||

Filed by SOUTHEAST UNIVERSITY, Nanjing (CN) |
||

PCT Filed Apr. 13, 2017, PCT No. PCT/CN2017/080383§ 371(c)(1), (2) Date Oct. 18, 2018, PCT Pub. No. WO2018/014602, PCT Pub. Date Jan. 25, 2018. |
||

Claims priority of application No. 2016 1 0575270 (CN), filed on Jul. 19, 2016. |
||

Prior Publication US 2019/0129044 A1, May 2, 2019 |
||

Int. Cl. (2010.01); G01S 19/47 (2006.01); G01C 21/16 (2010.01); G01S 5/02 (2006.01)G01C 21/20 |

CPC (2013.01) [G01S 19/47 (2013.01); G01C 21/16 (2013.01); G01S 5/0294G01C 21/165 (2013.01); G01C 21/20 (2013.01)] |
6 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: S
, constructing a high-dimensional GNSS/INS deep coupling filter model to obtain a constructed filter model;1S
, generating an initialization cubature point for the constructed filter model by using standard cubature rules;2S
, performing CKF filtering on the constructed filter model by using novel cubature point update rules;3wherein the step S
comprises:3S
, calculating a state priori distribution at k moment by using the following formula:31wherein in the formula, x
_{k}^{−}represents a state priori estimate at k moment, x_{k|k−1 }is a mean of the x_{k}^{−}, P_{k|k−1 }is a variance of the x_{k}^{−}, x_{k|k−1 }represents a state estimate at k moment speculated from a measurement and a state at k−1 moment, P_{k|k−1 }represents a covariance of x_{k|k−1}, w_{i}=/1n2_{x}, and Q_{k−1 }is a system noise variance matrix;S
, calculating a cubature point error matrix x32_{i,k|k−1}^{−}of the prediction process using the following formula, and defining Ξ_{k}^{-}=P_{k|k−1}−Q_{k−1 }as an error variance of a Sigma point statistical linear regression (SLR) in a priori PDF approximation process;x
_{i,k|k−1}^{−}=x_{i,k|k−1}−x_{k|k−1}, 0≤i≤2n_{x},wherein x
_{i,k|k−1}=f(x_{i,k−1|k−1}^{+}) is a cubature point after x_{i,k−1|k−1}^{+}propagates through a system equation;S
, taking the cubature point after the propagation of the system equation as a cubature point of a measurement update process;33S
, using a CKF measurement update to calculate a likelihood distribution function of a measured value;34wherein,
wherein in the formula, z
_{k}^{−}represents a measurement likelihood estimate at k moment, z_{k|k−1 }is a mean of the z_{k}^{−}, P_{zz,k|k−1 }is a variance of the z_{k}^{−}, z_{k|k−1 }is a measurement at k moment predicted from the state at k−1 moment, h(x) is a measurement equation, w_{i}=1/2n_{x}, and R_{k }is a measurement noise variance matrix;S
, calculating a posterior distribution function of a state variable x;35wherein
wherein in the formula, x
_{k}^{+}, is a posterior estimate of the state variable at k moment, a mean and a variance of the x_{k}^{+ }are x_{k|k }and P_{k|k}, respectively, K_{k}=P_{xz,k|k−1}(P_{zz,k|k−1})^{−1 }is a Kalman gain matrix, P_{xz,k|k−1 }is a cross-covariance between a posterior estimate of the state variable and the measurement likelihood estimate;S
, defining an error caused by a Sigma point approximation to a posterior distribution as Ξ36_{k}^{+}=P_{k|k}, w=[w_{1 }. . . w_{2n}_{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, thenx
_{i,k|k−1}^{−}w=0,x
_{i,k|k−1}^{−}diag(w)x_{i,k|k−1}^{−}^{T}=P_{k|k−1}−Q_{k},wherein in the formula, x
_{i,k|k−1}^{−}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,x
_{i,k|k}w=0,x
_{i,k|k}^{+}diag(w)(x_{i,k|k}^{+})^{T}=P_{k|k},wherein in the formula, X
_{i,k|k}^{+}is an updated cubature point;S
, assuming both Ξ37_{k}^{−}and Ξ_{k}^{+}are symmetric positive definite matrices, and x_{i,k|k }=B·X_{i,k|K−1}^{−}, then Ξ_{k}^{−}=L_{k}(L_{k})^{T}, Ξ_{k}^{+}=L_{k+1}(L_{k+1})^{T }wherein B is a transformation matrix to be solved, x_{i,k|k}^{+}is an updated cubature point error matrix; further Ξ_{k}^{+}=BL_{k }(L_{k})^{T}B^{T }, B=L_{k+1}Γ(L_{k})^{−1}, wherein Γ is an arbitrary orthogonal matrix that satisfies ΓΓ^{T}=I_{n}_{x}, when Γ is taken as a unit matrix, B=L_{k+1}(L_{k})^{−1 }is obtained;S
, obtaining an updated cubature point based on a posterior state estimate mean x38_{k|k }and an updated cubature point error matrix as x^{+}_{i,k|k}=x_{k|k}=x_{i,k|k}^{+}, 0≤i≤2n_{x}. |