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. G01S 19/47 (2010.01); G01C 21/16 (2006.01); G01S 5/02 (2010.01); G01C 21/20 (2006.01)
CPC G01S 19/47 (2013.01) [G01C 21/16 (2013.01); G01S 5/0294 (2013.01); G01C 21/165 (2013.01); G01C 21/20 (2013.01)] 6 Claims
OG exemplary drawing
 
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:

OG Complex Work Unit Drawing
wherein in the formula, xkrepresents a state priori estimate at k moment, xk|k−1 is a mean of the xk, Pk|k−1 is a variance of the xk, xk|k−1 represents a state estimate at k moment speculated from a measurement and a state at k−1 moment, Pk|k−1 represents a covariance of xk|k−1, wi=1/2nx, and Qk−1 is a system noise variance matrix;
S32, calculating a cubature point error matrix xi,k|k−1of the prediction process using the following formula, and defining Ξk-=Pk|k−1−Qk−1 as an error variance of a Sigma point statistical linear regression (SLR) in a priori PDF approximation process;
xi,k|k−1=xi,k|k−1−xk|k−1, 0≤i≤2nx,
wherein xi,k|k−1=f(xi,k−1|k−1+) is a cubature point after xi,k−1|k−1+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,

OG Complex Work Unit Drawing
wherein in the formula, zkrepresents a measurement likelihood estimate at k moment, zk|k−1 is a mean of the zk, Pzz,k|k−1 is a variance of the zk, zk|k−1 is a measurement at k moment predicted from the state at k−1 moment, h(x) is a measurement equation, wi=1/2nx, and Rk is a measurement noise variance matrix;
S35, calculating a posterior distribution function of a state variable x;
wherein

OG Complex Work Unit Drawing
wherein in the formula, xk+, is a posterior estimate of the state variable at k moment, a mean and a variance of the xk+ are xk|k and Pk|k, respectively, Kk=Pxz,k|k−1(Pzz,k|k−1)−1 is a Kalman gain matrix, Pxz,k|k−1 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 Ξk+=Pk|k, w=[w1 . . . w2nx]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
xi,k|k−1w=0,
xi,k|k−1diag(w)xi,k|k−1T=Pk|k−1−Qk,
wherein in the formula, xi,k|k−1is 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,
xi,k|kw=0,
xi,k|k+diag(w)(xi,k|k+)T=Pk|k,
wherein in the formula, Xi,k|k+is an updated cubature point;
S37, assuming both Ξkand Ξk+are symmetric positive definite matrices, and xi,k|k =B·Xi,k|K−1, then Ξk=Lk(Lk)T, Ξk+=Lk+1(Lk+1)T wherein B is a transformation matrix to be solved, xi,k|k+is an updated cubature point error matrix; further Ξk+=BLk (Lk)TBT , B=Lk+1Γ(Lk)−1, wherein Γ is an arbitrary orthogonal matrix that satisfies ΓΓT=Inx, when Γ is taken as a unit matrix, B=Lk+1(Lk)−1 is obtained;
S38, obtaining an updated cubature point based on a posterior state estimate mean xk|k and an updated cubature point error matrix as x+i,k|k=xk|k=xi,k|k+, 0≤i≤2nx.