# Two-Stage Cubature Kalman Filtering Based onT-Transform and Its Application

Authors: Lu Zhang; Qiugen Xiao; Hailun Wang; Yinyin Hou
Journal: Complexity
 (2021)

---

## Abstract

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.


---

## Body

## 1. Introduction

The cubature Kalman filter proposed by Ienkaran Arasaratnam and Simon Haykin in 2009 is a nonlinear filtering method [1]. Its core is the use of the third-order spherical rules of the radial displacement of the posterior mean and covariance of the nonlinear function approximation. Different from unscented Kalman filtering, cubature Kalman filtering has a rigorous mathematical derivation when it approximates five Gaussian integrals in nonlinear Gaussian filtering, and the algorithm has strict guarantee in theory. Moreover, the weights of cubature points in cubature Kalman filtering are all positive, so the covariance is not positive definite; it can be considered as the most ideal approximate Gaussian integral algorithm. Therefore, the proposed algorithm is widely accepted in various fields because of its simple structure, high estimation accuracy, and good numerical stability [2, 3].The standard cubature Kalman filtering algorithm requires an accurate system model and accurate random information. However, in the real world, the system model typically contains unknown random deviations [4, 5]. In the past few years, many scholars have devoted themselves to this field. The new CECKF based on the embedded cubature Kalman filter can increase filter precision by means of reusing the embedded cubature rule in the process of numerical approximation, which can realize higher precision than ECKF in theory [6]. A novel class of fractional interpolatory cubature Kalman filters (FICKFs) based on the interpolatory cubature rule (ICR) is presented, which can achieve a custom degree of accuracy under the Bayesian filtering framework [7]. The high-order UKF algorithm is applied to quick access recorder (QAR) data in order to solve in localising and tracing an airplane [8]. The quasi-stochastic integration filter (QSIF) solves the nonlocal sampling problem based on the unbiased property of the stochastic spherical integration rule, which can also reduce systematic error and improve filtering accuracy [9]. A novel algorithm for training FNNs is that it is capable of training the initial network and optimizing the network structure simultaneously [10]. A sparse representation-based framework, termed SRS, is able to optimize the network structure and training performance simultaneously [11]. For the state estimation of the dynamic system has unknown bias, a common solution is to take the bias as part of the system state to enlarge the dimensional system status and then use the Kalman filter to estimate the states of the system and the deviation simultaneously. This method is called the expansion dimension Kalman filtering algorithm (augmented state Kalman filter, ASKF). However, in a practical application, as the system dimensions increase, the calculation amount of the ASKF algorithm increases greatly, which can easily cause overflow and failure on a digital computer [12, 13].A two-stage cubature Kalman filter (TSCKF) is proposed in this paper. The algorithm, with the help of a nonlinearT-transform, makes the state error covariance matrix diagonalization by using the matrix diagonalization method, avoids the additional calculation condition and the deviation of cross-covariance, reduces the filter algorithm for the calculation, and ensures the normal operation of the system.


## 2. System Model

Considering a nonlinear discrete random system with unknown random deviation [14], the model is expressed as follows:(1)xk+1=fxk+Bkbk+ωkx,bk+1=bk+ωkb,zk=hxk+Fkbk+υk,where xk∈Rn is a state vector, zkϵRm is the observation vector, bk∈Rp is the system deviation vector, and nonlinear functions f⋅ and h⋅ are the state transfer function and observation function, respectively, and continuously differentiable in xk. Matrix Dk and matrix Fk are coefficient matrices in the bias equation and the observation equation, respectively, and they have appropriate dimensions. Noise sequences ωkx, ωkb, and υk are noncorrelated zero-mean Gaussian white noise sequences, and the variance has the following relation:(2)EωkxωkbυkωjxωjbυjT=Qkx000Qkb000Rkδkj,where Qkx,Qkb,andRk are positive definite symmetric matrices and δkj is the Kronecker function.Assuming that the initial state valuesx0 and b0 are noncorrelated white noise processes, they are Gaussian random variables with the following characteristics:(3)Ex0=x^0,Ex0−x^0x0−x^0T=P0x>0,Eb0=b^0,Eb0−b^0b0−b^0T=P0b>0.As shown in formula (1) of the system, the computational complexity of the extended dimension cubature Kalman filter (ASCKF) increases with the increase of the extended dimension matrix. If the extended dimension matrix dimension is n + p, compared with the initial state of the system, the dimensions p and n are comparable, the dimension of the new state vector Xk increases significantly, the computation of the ASCKF increases greatly, and it is easy to cause overflow and system crash when running on a digital computer. The main reason for this result is the additional calculation of P⋅xb. To solve this problem, the idea of the two-stage filter is introduced. According to the literature, there are two approaches to implement the two-stage filter. One is the traditional method proposed by Friedland [15]. In this method, the extended dimension filter is transformed into two parallel reduced-order filters by decoupling unbiased filter and biased filter. In the unbiased filter, it is assumed that the system bias does not exist, and the bias filter generates the estimation of the bias and then uses the result of the bias filter to correct the output value of the unbiased filter, so as to obtain the estimation of the state and the bias. Friedland’s two-stage method is optimal under certain algebraic constraints, but in practical applications, these algebraic constraints can be implemented very limitedly [16]. Therefore, the traditional two-stage method is mostly a suboptimal algorithm. The other is to use the matrix transformation technique called two-stage transformation. In the transformation process, there is no algebraic constraint, so the optimal performance can be obtained [17]. In the linear system, the two-stage transformation is the matrix block diagonalization [18]; in the nonlinear system, the two-stage transformation is represented by obtaining the partial derivative function of the covariance matrix to realize the matrix block diagonalization [18].


## 3. Two-Stage Cubature Kalman Filtering Algorithm

Jacobian matrix is needed to obtain error covariance and gain matrix in the calculation of extended Kalman filtering (EKF), but it is not needed in the calculation of cubature Kalman filtering (CKF). At present, although some scholars have proposed a two-stage extended Kalman filtering (TSEKF), the two-stage cubature Kalman filtering cannot directly apply the framework of TSEKF, but needs to find out the matrix block relationship under the CKF framework in the calculation process so that it can realize the role of the matrix block in EKF. Based on this idea, the matrix block decoupling method is introduced to deduce TSCKF.Lemma 1.
TheT-transform of a linear system is extended to a nonlinear system using the method in [12, 13]. The formula of the nonlinear T-transform is obtained as follows:(4)TF,X=X1+FX2X2,where X=X1T,X2TT, X1∈Rn−p, X2∈Rp, and FX2 is a nonlinear function of a substate X2.
By using the nonlinearT-transform property, the following nonlinear two-stage transformation formula can be obtained:(5)X^k|k−1=TΦ,X¯k|k−1,(6)X^k|k=TΨ,X¯k|k,(7)Pk|k−1=∂TΦ,X¯k|k−1∂X¯k|k−1P¯k|k−1∂TΦ,X¯k|k−1∂X¯k|k−1T,(8)Pk|k=∂TΨ,X¯k|k∂X¯k|kP¯k|k∂TΨ,X¯k|k∂X¯k|kT,(9)Kk=∂TΨ,X¯k|k∂X¯k|kK¯k,where Φ and Ψ are two nonlinear functions.
The formula in the lemma has the following properties:(10)∂TΦ,X¯k|k−1∂X¯k|k−1=In−pUk0Ip≡TUk,∂TΨ,X¯k|k∂X¯k|k=In−pVk0Ip≡TVk,and UkandVk can be obtained as follows:(11)Uk=∂ΦX¯k|k−12∂X¯k|k−12,Vk=∂ΨX¯k|k2∂X¯k|k2.
For nonlinear discrete stochastic systems with unknown random deviations (1), the TSCKF is(1)
Unbiased filter:(12)X¯k|k−11=1m∑i=1mf1Sk−1|k−1ξi+TΨ,X¯k−1|k−1,uk−1−ΦX¯k|k−12,X¯k|k1=X¯k|k−11+ΦX¯k|k−12+VkX¯k|k2−X¯k|k−12−ΨX¯k|k2+K¯k1Zk−1m∑i=1mhSk|k−1ξi+TΦ,X¯k|k−1,uk,P¯k|k−11=Mk−111+Qk−111−UkMk−122+Qk−122UkT,P¯k|k1=P¯k|k−11+UkP¯k|k−12UkT−VkP¯k|k−12VkT−K¯k1Pzz,k|k−1K¯k1T−K¯k1Pzz,k|k−1K¯k2TVkT−K¯k1Pzz,k|k−1K¯k2TVkTT,K¯k1=Nk1−VkNk2.(2)
Biased filter:(13)X¯k|k−12=1m∑i=1mf2Sk−1|k−1ξi+TΨ,X¯k−1|k−1,uk−1,X¯k|k2=X¯k|k−12+K¯k2Zk−1m∑i=1mhSk|k−1ξi+TΦ,X¯k|k−1,uk,P¯k|k−12=Mk−122+Qk−122,P¯k|k2=P¯k|k−12−K¯k2Pzz,k|k−1K¯k2T,K¯k2=Nk2.
The coupling relation between the two filters is(14)Uk=Mk−112+Qk−112Mk−122+Qk−122−1,Vk=Uk−K¯k1Pzz,k|k−1K¯k2TP¯k|k−12−1,where Pk|k−1=Mk−1+Qk−1=Mk−111+Qk−111Mk−112+Qk−112Mk−112+Qk−112TMk−122+Qk−122andKk=Kk1Kk2.
The derivation process is as follows.
Bring formulasx^k|k−1=1/m∑i=1mXi,k|k−1∗ and x^k|k=x^k|k−1+Kkzk−z^k|k−1 to the left side of formulas (5) and (6), and bring formula (4) to the right side:(15)X¯k|k−11+ΦX¯k|k−12X¯k|k−12=1m∑i=1mfSk−1|k−1ξi+TΨ,X¯k−1|k−1,uk−1,(16)X¯k|k1+ΨX¯k|k2X¯k|k2=X¯k|k−11+ΦX¯k|k−12X¯k|k−12+K¯k1+VkK¯k2K¯k2Zk−1m∑i=1mhSk|k−1ξi+TΦ,X¯k|k−1,uk.
Formula (9) is brought into formula (16) and expanded:(17)X¯k|k−11=1m∑i=1mf1Sk−1|k−1ξi+TΨ,X¯k−1|k−1,uk−1−ΦX¯k|k−12,(18)X¯k|k1=X¯k|k−11+ΦX¯k|k−12+VkX¯k|k2−X¯k|k−12−ΨX¯k|k2+K¯k1Zk−1m∑i=1mhSk|k−1ξi+TΦ,X¯k|k−1,uk,(19)X¯k|k−12=1m∑i=1mf2Sk−1|k−1ξi+TΨ,X¯k−1|k−1,uk−1,(20)X¯k|k2=X¯k|k−12+K¯k2Zk−1m∑i=1mhSk|k−1ξi+TΦ,X¯k|k−1,uk,where f⋅=f1⋅Tf2⋅TT and K¯k=K¯k1TK¯k2TT.
For formulaPk|k−1=1/m∑i=1mXi,k|k−1∗Xi,k|k−1∗T−x^k|k−1x^k|k−1T+Qk−1, let (21)Mk−1=1m∑i=1mXi,k−1|k−1∗∑i=1mXi,k−1|k−1∗T−X^k|k−1X^k|k−1T.
According to the dimension of the block matrix in the two-stage transformation formula,Mk−1 is divided into blocks:(22)Mk−1=Mk−111Mk−112Mk−112TMk−122.
Similarly, the state noise variance matrix is divided into blocks:(23)Qk−1=Qk−111Qk−112Qk−112TQk−122.
Substitute two block matrices into formulaPk|k−1=1/m∑i=1mXi,k|k−1∗Xi,k|k−1∗T−x^k|k−1x^k|k−1T+Qk−1, and the results are as follows:(24)Pk|k−1=Mk−1+Qk−1=Mk−111+Qk−111Mk−112+Qk−112Mk−112+Qk−112TMk−122+Qk−122.
Take formula (24) to the left side of formula (7), and then multiply the matrix on the right side of formula (7) to get the following results:(25)P¯k|k−11=Mk−111+Qk−111−UkMk−122+Qk−122UkT,P¯k|k−12=Mk−122+Qk−122,Uk=Mk−112+Qk−112Mk−122+Qk−122−1.
By sorting out formula (8) and bringing in formulas (7) and (9), we can get the following results:(26)P¯k|k1=P¯k|k−11+UkP¯k|k−12UkT−VkP¯k|k−12VkT−K¯k1Pzz,k|k−1K¯k1T−K¯k1Pzz,k|k−1K¯k2TVkT−K¯k1Pzz,k|k−1K¯k2TVkTT,P¯k|k2=P¯k|k−12−K¯k2Pzz,k|k−1K¯k2T,Vk=Uk−K¯k1Pzz,k|k−1K¯k2TP¯k|k−12−1.
According to the corresponding dimensions, the formulaKk=Pxz,k|k−1Pzz,k|k−1−1 is divided into blocks to obtain the block gain matrix as follows:(27)Kk=Kk1Kk2.
By taking formula (27) into formula (9) and unfolding, we can get the following results:(28)K¯k1=Kk1−VkKk2,K¯k2=Kk2.
The nonlinear functions Φ and Ψ can be determined using formula (11) and the backward difference equation:(29)ΦX¯k|k−12=ΦX¯k−1|k−22+UkX¯k|k−12−X¯k−1|k−22,ΨX¯k|k2=ΨX¯k−1|k−12+VkX¯k|k2−X¯k−1|k−12.
The derivation is completed.
The flowchart of the two-stage cubature Kalman filtering (TSCKF) algorithm is shown in Figure1.Figure 1
The flowchart of the two-stage cubature Kalman filtering (TSCKF) algorithm.


## 4. Equivalence Proof of Two-Stage Cubature Kalman Filtering

The two-stage cubature Kalman filtering (TSCKF) algorithm is derived from the cubature Kalman filtering (CKF) algorithm through nonsingular two-stage transformation, so they can be proved to be equivalent mathematically.Proof.
By inductive reasoning, it is assumed that, atk time,(30)X^k|k=Xk|k.
According to formulasXi,k−1|k−1=Sk−1|k−1ξi+X^k−1|k−1, Xi,k|k−1∗=fXi,k−1|k−1,uk−1, x^k|k−1=1/m∑i=1mXi,k|k−1∗, (4), (17), (19), and (30),(31)Tϕ,X¯k+1|k=X¯k+1|k1+ϕX¯k+1|k2X¯k+1|k2=1m∑i=1mfSk|kξi+Tψ,X¯k|k,uk=1m∑i=1mfSk|kξi+X^k|k,uk=Xk+1|k.
According to formulasx^k|k=x^k|k−1+Kkzk−z^k|k−1, (6), (9), (18), (20), (27), and (31),(32)X^k+1|k+1=X¯k+1|k1+ΦX¯k+1|k2+VkX¯k+1|k+12−X¯k+1|k2+K¯k+11Zk+1−1m∑i=1mhSk+1|kξi+TΦ,X¯k+1|k,uk+1X¯k+1|k2+K¯k+12Zk+1−1m∑i=1mhSk+1|kξi+TΦ,X¯k+1|k,uk+1=Tϕ,X¯k+1|k+K¯k+11K¯k+12Zk+1−Z^k+1|k+Vk+1X¯k+1|k+12−X¯k+1|k20=Tϕ,X¯k+1|k+K¯k+11+Vk+1K¯k+12K¯k+12Zk+1−Z^k+1|k=Xk+1|k+Kk+1Zk+1−Z^k+1|k=Xk+1|k+1.
It can be seen from formula (32) that formula (30) also holds at time k+1.
The proof is completed.


## 5. Experiment and Analysis

To verify the performance of the TSCKF algorithm, the experimental environment and simulation system model are provided first. In this system, the CKF algorithm and TSCKF algorithm are compared to verify the equivalence of the two algorithms.


### 5.1. Simulation System Models

The TSCKF is used to estimate the attitude, speed, and swing of a two-wheel drive robot in order to verify the effectiveness of the algorithm. The robot is controlled by an inertial sensor and real-time kinematic global positioning system. The system model is as follows:(33)xk+1=fxk+Bkbk+ωkx,bk+1=bk+ωkb,zk=hxk+bk+υk,where(34)fxk=Xk+ΔtVl,kcosθk−ΔtVy,ksinθkYk+ΔtVl,ksinθk−ΔtVy,kcosθkVl,k+ΔtVy,kγk+Δtαx,kVy,k−ΔtVl,kγk+Δtαy,kθk+Δtγm,k,hxk=XkYkcosθkVl,k−sinθkVy,ksinθkVl,k+cosθkVy,kθk.The state vectorxk=XkYkVl,kVy,kθkT is independent of the deviation vector bk, and the initial value is x0∼N2,0.052. The measured direction zk=zX,kzY,kzX′,kzY′,kzθ,kT is composed of the absolute position, velocity, and angle. The state noise and measurement noise are all zero-mean Gaussian white noises and are not related to each other. The state noise vector is ωkx∼N0,0.52, and the measurement noise vector is υk∼N0,0.052.The time-varying parameterαx,kαy,kγm,k is the acceleration and azimuth of the X-axis and Y-axis at time k, the data are derived from an accelerometer and gyroscope, and Δt is the sampling time of the discrete system. Assuming that the yaw rate γk is measured by using the gyroscope, γk=γm.The bias equation in the TSCKF algorithm is as follows:(35)bk+1=bk+ωkb,ωkb∽N0,0.052.In the simulation experiment, the computer hardware configuration consisted of an Intel Core i54258u 2.4 G processor and 4 GB memory, the operating system was Windows 7 64 bit Professional Edition, and simulation software was MATLAB 2013a.


### 5.2. Comparison between the CKF Algorithm and TSCKF Algorithm

The state vectorxk is independent of the bias vector bk, and x0∼N5,0.012. The state noise and measurement noise are all zero-mean Gaussian white noises and are not related to each other. The state noise vector is ωkx∼N0,0.022, and the measurement noise vector is υk∼N0,0.12. The bias noise vector is ωkb∼N0,0.012.In the simulation experiment, the initial estimated value of the system wasx0=5+0.01×rand5,1, that is, five random numbers with a mean value of 5 and variance of 0.01; variance was P0=diag0.02,0.02,0.02,0.02,0.02, that is, diagonal elements of 0.02 in a 5 × 5 matrix; deviation of the initial value was b0=2+0.02×rand1,1; variance was 0.01; simulation time was 200 seconds; and Monte Carlo simulation was 1000.Figures2–11 show the comparison chart of the estimated values in the TSCKF algorithm. It can be seen from Figures 2, 4, 6, 8, and 10 that the estimated value curves of the two algorithms are approximate, which proves the usability of the CKF algorithm. Figures 3, 5, 7, 9, and 11 show that the estimation accuracies of the two algorithms are equivalent, and the two algorithms are equivalent.Figure 2
Comparative diagram of the estimation values of the two types of algorithms.Figure 3
Comparison of the root-mean-square error of the two algorithms.Figure 4
Comparative diagram of the estimation values of the two types of algorithms.Figure 5
Comparison of the root-mean-square error of the two algorithms.Figure 6
Comparative diagram of the estimation values of the two types of algorithms.Figure 7
Comparison of the root-mean-square error of the two algorithms.Figure 8
Comparative diagram of the estimation values of the two types of algorithms.Figure 9
Comparison of the root-mean-square error of the two algorithms.Figure 10
Comparative diagram of the estimation values of the two types of algorithms.Figure 11
Comparison of the root-mean-square error of the two algorithms.Figures12 and 13 show the estimation curve of bias vector bk for the two algorithms and the comparison of the root-mean-square error. It can be seen from the figure that the root-mean-square error accuracies of the two algorithms are equivalent, and the equivalence of the two algorithms is proved again.Figure 12
Comparative diagram of the estimation values of the two types of algorithms.Figure 13
Comparison of the root-mean-square error of the two algorithms.


## 5.1. Simulation System Models

The TSCKF is used to estimate the attitude, speed, and swing of a two-wheel drive robot in order to verify the effectiveness of the algorithm. The robot is controlled by an inertial sensor and real-time kinematic global positioning system. The system model is as follows:(33)xk+1=fxk+Bkbk+ωkx,bk+1=bk+ωkb,zk=hxk+bk+υk,where(34)fxk=Xk+ΔtVl,kcosθk−ΔtVy,ksinθkYk+ΔtVl,ksinθk−ΔtVy,kcosθkVl,k+ΔtVy,kγk+Δtαx,kVy,k−ΔtVl,kγk+Δtαy,kθk+Δtγm,k,hxk=XkYkcosθkVl,k−sinθkVy,ksinθkVl,k+cosθkVy,kθk.The state vectorxk=XkYkVl,kVy,kθkT is independent of the deviation vector bk, and the initial value is x0∼N2,0.052. The measured direction zk=zX,kzY,kzX′,kzY′,kzθ,kT is composed of the absolute position, velocity, and angle. The state noise and measurement noise are all zero-mean Gaussian white noises and are not related to each other. The state noise vector is ωkx∼N0,0.52, and the measurement noise vector is υk∼N0,0.052.The time-varying parameterαx,kαy,kγm,k is the acceleration and azimuth of the X-axis and Y-axis at time k, the data are derived from an accelerometer and gyroscope, and Δt is the sampling time of the discrete system. Assuming that the yaw rate γk is measured by using the gyroscope, γk=γm.The bias equation in the TSCKF algorithm is as follows:(35)bk+1=bk+ωkb,ωkb∽N0,0.052.In the simulation experiment, the computer hardware configuration consisted of an Intel Core i54258u 2.4 G processor and 4 GB memory, the operating system was Windows 7 64 bit Professional Edition, and simulation software was MATLAB 2013a.


## 5.2. Comparison between the CKF Algorithm and TSCKF Algorithm

The state vectorxk is independent of the bias vector bk, and x0∼N5,0.012. The state noise and measurement noise are all zero-mean Gaussian white noises and are not related to each other. The state noise vector is ωkx∼N0,0.022, and the measurement noise vector is υk∼N0,0.12. The bias noise vector is ωkb∼N0,0.012.In the simulation experiment, the initial estimated value of the system wasx0=5+0.01×rand5,1, that is, five random numbers with a mean value of 5 and variance of 0.01; variance was P0=diag0.02,0.02,0.02,0.02,0.02, that is, diagonal elements of 0.02 in a 5 × 5 matrix; deviation of the initial value was b0=2+0.02×rand1,1; variance was 0.01; simulation time was 200 seconds; and Monte Carlo simulation was 1000.Figures2–11 show the comparison chart of the estimated values in the TSCKF algorithm. It can be seen from Figures 2, 4, 6, 8, and 10 that the estimated value curves of the two algorithms are approximate, which proves the usability of the CKF algorithm. Figures 3, 5, 7, 9, and 11 show that the estimation accuracies of the two algorithms are equivalent, and the two algorithms are equivalent.Figure 2
Comparative diagram of the estimation values of the two types of algorithms.Figure 3
Comparison of the root-mean-square error of the two algorithms.Figure 4
Comparative diagram of the estimation values of the two types of algorithms.Figure 5
Comparison of the root-mean-square error of the two algorithms.Figure 6
Comparative diagram of the estimation values of the two types of algorithms.Figure 7
Comparison of the root-mean-square error of the two algorithms.Figure 8
Comparative diagram of the estimation values of the two types of algorithms.Figure 9
Comparison of the root-mean-square error of the two algorithms.Figure 10
Comparative diagram of the estimation values of the two types of algorithms.Figure 11
Comparison of the root-mean-square error of the two algorithms.Figures12 and 13 show the estimation curve of bias vector bk for the two algorithms and the comparison of the root-mean-square error. It can be seen from the figure that the root-mean-square error accuracies of the two algorithms are equivalent, and the equivalence of the two algorithms is proved again.Figure 12
Comparative diagram of the estimation values of the two types of algorithms.Figure 13
Comparison of the root-mean-square error of the two algorithms.


## 6. Conclusions

According to the general bias of the system model in practical applications, a conventional processing method, the extended dimension cubature Kalman filtering algorithm (ECKF), is introduced. By analysing the shortcomings of ECKF, a two-stage cubature Kalman filtering algorithm (TSCKF) is proposed based on the nonlinear two-stage transformation. The core ideas of TSCKF are to obtain the block diagonalization of the covariance matrix based on the matrix state transformation and avoid calculating cross-covariance of the state and bias, so as to reduce the calculation amount and ensure the smooth progress of the filtering process. Then, the equivalence of TSCKF and CKF is proved by mathematical means. Finally, TSCKF is simulated to verify the availability of the algorithm; at the same time, the comparison experiment between TSCKF and CKF proves the equivalence of the two algorithms.



---

*Source: 5538414-2021-07-07.xml*