Target Tracking Algorithm Using Two-Stage Cubature Kalman Filter

Introduced in 2009 by Arasaratnam and Haykin [1], the CKF is a nonlinear filtering methodology, and accurately estimates the posterior mean and covariance of nonlinear function transmission using the third-order spherical-radial cubature rule. Different from the unscented Kalman filter, this approach entails careful mathematical calculations to estimate the values of the five Gaussian integrals in nonlinear Gaussian filtering. Theoretical resilience is achieved by guaranteeing that the cubature points within the filter have positive weights, thereby successfully inhibiting the occurrence of covariance matrices that are not positive definite. The CKF, considered the most optimal approach for approximating Gaussian integrals [2], [3], quickly received extensive recognition in several disciplines due to its simplicity, superior estimate accuracy, and numerical stability in solving a variety of nonlinear estimating problems [4], [5].

Precise system models and accurate random information are required for the standard CKF. However, system models often contain unknown random biases in practical applications. A common solution for this is to treat the biases as part of the system state, augment the state, and then utilise Kalman filter while estimating both the system state and biases, which is known as the augmented state Kalman filter (ASKF) algorithm. However, it creates a substantial computational demand as system dimensions increase. Thus, this often leads to computational overflow and failures in digital implementations [6], [7], [8], [9].

To address the aforementioned challenges, researchers worldwide have made various improvements. The square root CKF algorithm was updated by Zhong [10] using the Huber method for nonlinear statistical regression. In the presence of anomalous noise interference in the observed system values, Huber's robust filtering method was proposed by intersecting the observation values and modifying the affected data. Zhao et al. [11] developed an integrated algorithm combining factor graphs and CKF, enabling the effective use of historical information for measurement updates to overcome uncertainties in the observation environments. Zhang et al. [12] proposed an adaptive $\mathrm{H} \infty$ CKF algorithm, based on a fading factor. This algorithm prevents a reduction in estimation accuracy in the face of uncertainty in system models. Huang et al. [13] introduced a filter algorithm based on variational Bayesian methods to determine state information in complex maritime conditions with erroneous inputs. Zhang et al. [14] proposed an adaptive Kalman filter algorithm based on variational Bayesian methods. This approach is specifically designed to simultaneously estimate the variables that represent the current state of the system, the matrices that describe the uncertainty in future predictions, and the matrices that represent the uncertainty in the measurements, thereby effectively dealing with the complex relationships that naturally exist in state estimation and predictive covariance matrices.

The TSCKF algorithm is introduced in this study. Utilizing nonlinear T-transform, the TSCKF algorithm applies matrix diagonalization to effectively diagonalize the state error covariance matrix. This methodological advancement circumvents the need for additional computation of cross-covariance between the state and biases, significantly reducing the computational burden. Such an approach not only streamlines the filtering process but also assures the uninterrupted and efficient operation of the system.

The primary emphasis is placed on a model of a discrete stochastic system that is nonlinear and includes random biases of unknown kind. The model is described in the following manner:

Let $x_k \in R^n$ represent the state vector, $z_k \in R^m$ represent the observation vector, and $b_k \in R^p$ represent the system bias vector. The nonlinear functions $f(\cdot)$ and $h(\cdot)$, which represent the state transition and observation functions respectively, are continuously differentiable at point $x_k$. The matrices $D_k$ and $F_k$ are the coefficient matrices for the bias state equation and observation equation, respectively, with the correct dimensions. The noise sequences $\omega_k^x, \omega_k^b$ and $v_k$ are independent zero-mean Gaussian white noise sequences with variances as indicated:

Given that $Q_k^x, Q_k^b$ and $R_k$ are positive definite symmetric matrices, and $\delta_{k j}$ represents the Kronecker function.

The starting state values $x_0$ and $b_0$ are supposed to be uncorrelated white noise processes, with each being a Gaussian random variable possessing defined features.

$\begin{aligned} & E\left[x_0\right]=\hat{x}_0, E\left[\left(x_0-\hat{x}_0\right)\left(x_0-\hat{x}_0\right)^T\right]=P_0^x>0 \\ & E\left[b_0\right]=\hat{b}_0, E\left[\left(b_0-\hat{b}_0\right)\left(b_0-\hat{b}_0\right)^T\right]=P_0^b>0\end{aligned}$

The subsequent parts utilise the augmented state cubature Kalman filter (ASCKF) algorithm to initially estimate the system states and biases. An analysis is conducted on the limits of this augmented technique, which is then followed by the introduction of the TSCKF algorithm and its subsequent algorithmic analysis. Ultimately, experimental validation is performed.

Let

$X_{k+1}=\left[\begin{array}{l}x_{k+1} \\ b_{k+1}\end{array}\right], f\left(X_k\right)=\left[\begin{array}{c}f\left(x_k\right)+B_k b_k \\ b_k\end{array}\right], \omega_k=\left[\begin{array}{c}\omega_k^x \\ \omega_k^b\end{array}\right], h\left(X_k\right)=h\left(x_k\right)+F_k b_k$

The Eqs. (2)-(36) can be reformulated as:

where, $E\left(\omega_k, \omega_j^T\right)=\left[\begin{array}{cc}Q_k^x & 0 \\ 0 & Q_k^b\end{array}\right] \delta_{k j}$

$b_k$ is incorporated into the current state of the system, and both $x_k$ and $b_k$ are considered as additional dimensions of the expanded state. The ASCKF algorithm operates in the following manner:

Step 1: Time update

(a) The Cholesky decomposition for $P_{k-1 \mid k-1}$ is applied to the posterior density function $p\left(x_{k-1} \mid D_{k-1}\right)=$ $N\left(x_{k-1} ; \hat{x}_{k-1}, P_{k-1}\right)$ at time $k-1$ to obtain:

(b) Determine the cubature points $\left(i=1,2, \ldots, m=2 n_x\right)$:

(c) Calculate the propagated cubature points $(i=1,2, \ldots, m)$:

(d) Calculate the anticipated state at time $k$:

(e) Calculate the estimated covariance of the state error at time $k$:

Step 2: Measurement update

(a) Compute the Cholesky decomposition:

(b) Determine the cubature points $(i=1,2, \ldots, m)$:

(c) Calculate the propagated cubature points $(i=1,2, \ldots, m)$:

(d) Calculate the anticipated value of the measurement at time $k$:

(e) Calculate the covariance of the measurement error at time $k$:

(f) Calculate the cross-covariance at time $k$:

(g) Determine the gain at time $k$:

(h) Calculate the state estimation at time $k$:

(i) Calculate the estimation of the state error covariance at time $k$:

In the algorithm, $\hat{X}(\cdot)=\left[\begin{array}{l}\hat{x}(\cdot) \\ \hat{b}(\cdot)\end{array}\right], K_k=\left[\begin{array}{c}K_k^x \\ K_k^b\end{array}\right], P(\cdot)=Cov\{\hat{X}(\cdot)\}=\left[\begin{array}{cc}P_{(\cdot)}^x & P_{(\cdot)}^{x b} \\ \left(P_{(\cdot)}^{x b}\right)^T & P_{(\cdot)}^b\end{array}\right]$.

The dimensions of the augmented matrix are given by the sum of $n$ and $p$. When the values of $p$ and $n$ are similar, the dimension of the new state vector $X_k$ grows greatly in comparison to the dimension of the original state. The increase in computational burden for the ASCKF filter results in a significant escalation, which might potentially lead to overflow and system crashes during digital calculation. The main factor contributing to this problem is the supplementary calculation of $P_{(\cdot)}^{x b}$. In response to this, the notion of a two-stage filter was established. Based on the literature, there are two approaches to execute a two-stage filter. Friedland proposed an approach that suggests separating the augmented filter into two parallel reduced-order filters: a bias-free filter and a bias filter [15]. The bias-free filter operates under the assumption that there is no inherent bias in the system. On the other hand, the bias filter generates an estimation of the bias vector, which is subsequently employed to rectify the output of the bias-free filter. This process allows for the estimation of both the state and bias vectors. Friedland's two-stage method is considered best within specific algebraic constraints. However, these requirements are seldom attainable in practical scenarios, rendering most conventional two-stage methods less than optimal. Another approach utilises a matrix transformation technique referred to as the two-stage transformation. This transformation, devoid of algebraic limitations, can attain optimal performance [16], [17]. In linear systems, the two-stage transformation is observed as matrix block diagonalization. In nonlinear systems, it is observed as matrix block diagonalization achieved by deriving the covariance matrix.

The enhanced Kalman filter employs the Jacobian matrix to calculate the error covariance matrix and the gain matrix. However, the Jacobian matrix is unnecessary in the CKF calculation. Although the two-stage augmented Kalman filter methods have been suggested by researchers [18], [19], [20], it is not possible for the TSCKF to directly utilise the framework of the augmented Kalman filter. To produce the effects of matrix block diagonalization observed in the augmented Kalman filter, it is essential to determine the relationship of matrix block diagonalization inside the CKF framework. This section presents the matrix block decoupling approach as a means to develop the TSCKF algorithm, building upon the aforementioned concept.

The T-transform in linear systems is defined as:

Therefore, the linear two-stage transformation can be represented by the attributes of the T-transform as follows:

where, $\bar{P}=diag\left\{\bar{P}^1, \bar{P}^2\right\}$.

The nonlinear T-transform and two-stage transformation are obtained for the given Eq. (1). This represents a nonlinear discrete system with unknown biases. This derivation is based on the linear T-transform and two-stage transformation from Eq. (18) and Eq. (19).

Lemma 1: By applying techniques from studies [8], [9], the linear system T-transform may be extended to nonlinear systems, resulting in the following formula for the nonlinear T-transform:

where, $X=\left\{\left(X^1\right)^T,\left(X^2\right)^T\right\}^T, X^1 \in R^{n-p}, X^2 \in R^p, F\left(X^2\right)$ are non-linear functions of the sub-state $X^2$. The nonlinear T-transform properties are used to obtain formulas for a two-stage transformation:

where, $\Phi$ and $\Psi$ represent two specific nonlinear functions.

The lemma concludes.

The lemma has the subsequent characteristics:

Therefore, it can be inferred that:

The TSCKF operates as follows for the nonlinear discrete stochastic system with unknown random biases, as depicted in Eq. (1):

(a) Unbiased filter:

(b) Bias filter:

The interdependence between the two filters:

where, $P_{k \mid k-1}=M_{k-1}+Q_{k-1}=\left[\begin{array}{cc}M_{k-1}^{11}+Q_{k-1}^{11} & M_{k-1}^{12}+Q_{k-1}^{12} \\ \left(M_{k-1}^{12}+Q_{k-1}^{12}\right)^T & M_{k-1}^{22}+Q_{k-1}^{22}\end{array}\right], \quad K_k=\left[\begin{array}{c}K_k^1 \\ K_k^2\end{array}\right]$.

The nonlinear functions $\Phi$ and $\Psi$ can be calculated by utilising Eq. (28) and the backward difference equation.

$\begin{gathered}\Phi\left(\bar{X}_{k \mid k-1}^2\right)=\Phi\left(\bar{X}_{k-1 \mid k-2}^2\right)+U_k\left(\bar{X}_{k \mid k-1}^2-\bar{X}_{k-1 \mid k-2}^2\right) \\ \Psi\left(\bar{X}_{k \mid k}^2\right)=\Psi\left(\bar{X}_{k-1 \mid k-1}^2\right)+V_k\left(\bar{X}_{k \mid k}^2-\bar{X}_{k-1 \mid k-1}^2\right)\end{gathered}$

The TSCKF algorithm is obtained by applying a non-singular two-stage transformation to the CKF method, which establishes their mathematical equivalence.

Theorem 1: The TSCKF method is mathematically equivalent to the CKF algorithm, as proven in Eqs. (4)-(14).

Proof: Employing inductive reasoning, let us assume that at time k:

By applying Eqs. (5), (6), (7), (20), (29), (34), and (41), it can be deduced that:

Subsequently, by utilising Eqs. (16), (22), (25), (30), (35), and (42), it can be deduced that:

Eq. (43) implies that at time $k+1$, Eq. (41) remains valid.

Evidence has been established.

This section first provides details about the experimental setup and the simulation system model to assess the effectiveness of the TSCKF method, and then utilises the TSCKF algorithm for target tracking in this system, validating the algorithm's efficacy. Afterwards, a comparison is made between the CKF and TSCKF algorithms in the identical experimental setting. The purpose is to confirm the equality of both methods.

The TSCKF algorithm is utilised to estimate the posture, velocity, and oscillation of a two-wheeled robot, which is operated by the use of an inertial sensor and a real-time Kinematic global positioning system (GPS), aiming to assess the effectiveness of the algorithm. The system model is depicted in the following manner:

$\begin{gathered}x_{k+1}=f\left(x_k\right)+B_k b_k+\omega_k^x \\ b_{k+1}=b_k+\omega_k^b \\ z_k=h\left(x_k\right)+b_k+v_k\end{gathered}$

where,

$f\left(x_k\right)=\left[\begin{array}{c}X_k+\Delta t V_{l, k} \cos \left(\theta_k\right)-\Delta t V_{y, k} \sin \left(\theta_k\right) \\ Y_k+\Delta t V_{l, k} \sin \left(\theta_k\right)-\Delta t V_{y, k} \cos \left(\theta_k\right) \\ V_{l, k}+\Delta t V_{y, k} \gamma_k+\Delta t \alpha_{x, k} \\ V_{y, k}-\Delta t V_{l, k} \gamma_k+\Delta t \alpha_{y, k} \\ \theta_k+\Delta t \gamma_{m, k}\end{array}\right]$

$h\left(x_k\right)=\left[\begin{array}{c}X_k \\ Y_k \\ \cos \left(\theta_k\right) V_{l, k}-\sin \left(\theta_k\right) V_{y, k} \\ \sin \left(\theta_k\right) V_{l, k}+\cos \left(\theta_k\right) V_{y, k} \\ \theta_k\end{array}\right]$

The state vector $x_k=\left[\begin{array}{lllll}X_k & Y_k & V_{l, k} & V_{y, k} & \theta_k\end{array}\right]^T$ is unaffected by the bias vector $b_k$ and initially takes on the value of $x_0 \sim N\left(2,0.05^2\right)$. The measurement vector $z_k=\left[\begin{array}{lllll}z_{X, k} & z_{Y, k} & z_{X^{\prime}, k} & z_{Y^{\prime}, k} & z_{\theta, k}\end{array}\right]^T$ consists of the precise values for position, velocity, and angle. The state and measurement noises are both zero-mean Gaussian white noises that are independent of each other, with the state noise vector being denoted as $\omega_k^x \sim N\left(0,0.5^2\right)$, and the measurement noise vector as $v_k \sim N\left(0,0.05^2\right)$.

The parameter $\left\{\begin{array}{lll}\alpha_{x, k} & \alpha_{y, k} & \gamma_{m, k}\end{array}\right\}$ varies over time, reflects the acceleration on the $x$ and $y$ axes and the orientation at time $k$, and is obtained via the accelerometer and gyroscope. $\Delta t$ represents the sample interval of the discrete system. The gyroscope is believed to measure the yaw rate, denoted as $\gamma_k$, which is equivalent to $\gamma_m$.

The TSCKF algorithm represents the bias equation as:

$b_{k+1}=b_k+\omega_k^b, \omega_k^b \sim N\left(0,0.05^2\right)$

The computational hardware utilised in this simulation experiment consists of a quad-core Intel Core i5 4258U 2.4 GHz CPU, 4GB RAM, working on a Windows 7 64-bit Professional Edition operating system. The simulation programme employed is Matlab2013a.

The simulation experiment initialised the system with mean values of 2 and a variance of 0.05 for five random integers, which takes $x_0=[ 2+0.05 * \operatorname{rand}(5,1)]$ as the initial estimated value. The matrix with a variance of $P_0=\operatorname{diag}([ 0.05,0.05,0.05,0.05,0.05])$ was a $5 \times 5$ matrix with diagonal elements of 0.05 . The bias was initially assigned a value of $b_0=[ 5+0.01 * \operatorname{rand}(1,1)]$, with a variance of 0.05 . The simulation lasted for a duration of 200 seconds, and the TSCKF method underwent 1,000 Monte Carlo simulations.

The estimation error for each component of the state vector is defined by Eq. (44), which quantifies the level of agreement between the estimated and real state values.

The Root Mean Square Error (RMSE) is a metric used to evaluate and compare the effectiveness of different filtering algorithms.

where, $M$ indicates the number of Monte Carlo simulations, whereas $x_{*, k}^{(i)}$ and $\hat{x}_{*, k \mid k}^{(i)}$ refer to the actual state values and estimated values of $x_*$ in the $n$-th Monte Carlo simulation, respectively.

Figure 1, Figure 2, Figure 3, Figure 4, Figure 5 illustrate the comparison between the actual and estimated values of the states in relation to position, velocity, and angle on the $X$ and $Y$ axes. These comparison charts show that the actual values are found within the area of estimated values that are most likely to be correct. This suggests that the algorithm has effective noise reduction abilities and accurate estimation.

Figure 6, Figure 7, Figure 8, Figure 9, Figure 10 depict the error graphs for each constituent of the state vector, obtained from Eq. (44). These graphs demonstrate that the error in the state vector $x_k$ remains within an insignificant range for practical applications. This inaccuracy is actually caused by the inherent computational imperfections in digital computing. Therefore, the estimation findings are considered acceptable.

The graphs in Figure 11 and Figure 12 illustrate the comparison and error between the real and estimated values of the bias vector in the $b_k$ state.

Figure 12 demonstrates that the error in $b_k$ closely resembles that of the state vector $x_k$, staying within a small range that is insignificant in practical scenarios, thus making the results satisfactory.

To summarise, the simulation studies have confirmed that the TSCKF technique is suitable for state estimation.

The simulation system model used for this comparative experiment is the same as the one described in Section 5.1, where the state vector $x_k$ is not influenced by the bias vector $b_k$, with $x_0 \sim N\left(5,0.01^2\right)$. The state and measurement noises are zero-mean Gaussian white noises that are independent of each other. The state noise vector is denoted as $\omega_k^x \sim N\left(0,0.02^2\right)$, while the measurement noise vector is denoted as $v_k \sim N\left(0,0.1^2\right)$. The bias noise vector is represented by the symbol $\omega_k^b \sim N\left(0,0.01^2\right)$.

In the simulation experiment, the initial estimated value of the system was designated as $x_0=5+0.01 *$ rand $(5,1)$, using mean values of 5 and a variance of 0.01 for five randomly generated integers. Additionally, the matrix with a variance of $P_0=\operatorname{diag}([ 0.02,0.02,0.02,0.02,0.02])$ was defined as a $5 \times 5$ matrix with diagonal elements of 0.02 . The bias was initially set at a value of $b_0=2+0.02 *$ rand $(1,1)$, with a variance of 0.01 . The simulation lasted for a duration of 200 seconds, and the TSCKF method underwent 1,000 Monte Carlo simulations.

Figure 13, Figure 14, Figure 15, Figure 16, Figure 17, Figure 18, Figure 19, Figure 20, Figure 21, Figure 22 illustrate the comparison of estimated values and RMSE for the state components using both the CKF and TSCKF algorithms. By examining Figure 13, Figure 15, Figure 17, Figure 19, and Figure 21, it can be noted that the estimated value curves of the state vectors for both algorithms are nearly identical, thereby verifying the suitability of the CKF approach. Figure 14, Figure 16, Figure 18, Figure 20, and Figure 22 indicate that the estimation accuracy of both techniques is similar, thereby establishing their equivalency.

Figure 23 and Figure 24 display the estimation curves and RMSE comparison for the bias vector $b_k$ of both techniques. The RMSE accuracy of both algorithms is similar, which further supports their equivalency.

The RMSE of both techniques can be determined using Eq. (45), and the results are shown in Table 1 for comparison. The data presented in this table provides quantitative evidence that both algorithms have comparable accuracy, thereby establishing their equality.

Confronting the ubiquitous biases present in system models within practical settings, the ASCKF algorithm was initially developed. An in-depth analysis of this augmented algorithm highlighted certain limitations, leading to the proposition of the TSCKF. Central to the TSCKF algorithm is its innovative approach of block-diagonalizing the covariance matrix via matrix state transformations. This pivotal strategy effectively obviates the need for calculating cross-covariance between the state and biases, thereby substantially diminishing the computational load. As a result, this approach facilitates a smoother and more efficient filtering process. The mathematical equivalence of the TSCKF algorithm with the established CKF algorithm was rigorously demonstrated, underscoring the theoretical robustness of the proposed method. Subsequently, a series of simulation experiments were conducted to assess the practical applicability of the TSCKF algorithm. These experiments not only verified the usability of the TSCKF algorithm but also, through comparative analysis, established its equivalence with the CKF algorithm.