Two-Stage Cubature Kalman Filtering Based on T-Transform and Its Application

According to the actual application system model which has bias, this paper analyzes the shortage of the conventional augmented algorithm, the two-stage cubature Kalman filtering algorithm, which is presented on the basis of a two-stage nonlinear transformation. The core ideas of the algorithm are to obtain the block diagonalization of the covariance matrix using the matrix transformation and avoid calculating the covariance of the state and bias to reduce the amount of calculation and ensure a smooth filtering process. Then, the equivalence of the two-stage cubature Kalman filtering algorithm and the cubature Kalman filtering algorithm is proved by updating equivalent transformation. Through the experiment of trajectory tracking of a wheeled robot, it is verified that the two-stage cubature Kalman filtering algorithm can obtain good tracking accuracy and stability with the presence of unknown random bias. Simultaneously, the equivalence of the two-stage cubature Kalman filtering algorithm and cubature Kalman filtering algorithm is verified again using the contrast experiment.