Sigma-Point Kalman Filter

An alternative in the non-linear case to the Extended Kalman Filter is the Sigma Point Kalman Filter. In the results reported from various experiments, in the case of the non-linear functions $f$ and $h$, the Sigma Point Kalman Filter (SPKF) tends to provide better performance compared to the EKF: the statistical linearized error propagation (SPKF) is generally superior to the propagation based on Taylor series expansion (EKF).

Not only the state, but also the various points around the mean (the sigma points) are propagated through the functions that constitute the state update and observation in the Kalman filter. The advantage of the Sigma Point Kalman Filter (SPKF) is that it does not require the computation of Jacobians and typically allows for a better estimation of the mean and variance of the process.

The Unscented Kalman filter is one of the various versions of the Kalman filter based on Sigma Points. In this case, the theory for uncertainty propagation discussed in section 2.6.2 is utilized to estimate the mean and covariance of the a priori state and the observation error.

Even with the Unscented filter, it is possible to handle the case where noise enters the system in a non-additive manner. To generalize the case of non-additive noise, we define, in order to maintain a syntax consistent with that discussed in section 2.6.2, a variable called augmented state $\mathbf{x}^{a} \in \mathbb{R}^{n^{a}}$ with $n^{a}=n+q$ formed by the state $\mathbf{x} \in \mathbb{R}^{n}$ and the process noise $w$, which has zero mean, so as to use the function

$${\bm{\mathcal{X}}}^{-} = f(\mathbf{x}^{a}_{k-1}, \mathbf{u}_{k} )$$ (2.115)

for state update that allows for the nonlinear and non-additive consideration of the contribution from the process noise. Similarly, we define the augmented covariance matrix as:

$$\mathbf{P}_{\mathbf{x}}^{a} = \begin{bmatrix} \mathbf{P}_{\mathbf{x}} & 0 \\ 0 & \mathbf{Q} \end{bmatrix}$$ (2.116)

In the case where the process noise is additive, the system becomes similar to that of the linear Kalman filter in the form

$$\mathbf{P}^{-}_{k} = \sum_{i=0}^{2n} w^{c}_i ({\bm{\mathcal... ...{i} - \bar{{\bm{\mathcal{X}}}}^{-}_{i} )^{\top} + \mathbf{Q}_k$$ (2.117)

From the sigma points ${\bm{\mathcal{X}}}^{-}_{i}$, projected through $f$ and representing the a priori state distribution, it is possible to generate additional sigma points in order to obtain the a priori observation estimate:

$${\bm{\mathcal{Z}}}_i = h({\bm{\mathcal{X}}}^{-}_{i})$$ (2.118)

with which to calculate the most probable value of the observation $\hat{\mathbf{z}}$ by weighting the results ${\bm{\mathcal{Z}}}_i$ with the weights of the associated sigma points as in equation (2.39). In this case as well, the observation noise can be incorporated as an augmented state or, if assumed to be additive and independent, it can be added to the covariance matrix.

By utilizing the knowledge of the sigma points ${\bm{\mathcal{X}}}^{-}_{i}$ and ${\bm{\mathcal{Z}}}_i$, it is possible to easily obtain the covariance $\cov (\bm{\mathcal{Z}})$ and also the cross-covariance $\cov (\bm{\mathcal{X}},\bm{\mathcal{Z}})$ by generalizing the equation (2.39):

$$\cov \left( \bm{\mathcal{X}},\bm{\mathcal{Z}}\right) \appro... ...bar{\mathbf{x}})(\bm{\mathcal{Z}}_i - \bar{\mathbf{z}})^{\top}$$ (2.119)

Given the knowledge of the covariance $\cov (\bm{\mathcal{Z}})$ and the cross-covariance $\cov (\bm{\mathcal{X}},\bm{\mathcal{Z}})$, the Kalman sigma-point gain becomes exactly as expressed by equation (2.104), and the covariance update $\mathbf{P}_{k}$ follows the equation (2.105).