Introduction to Estimation Filters

Background

Estimation Systems

For many autonomous systems, the knowledge of the system state is a prerequisite for designing any applications. In reality, however, the state is often not directly obtainable. The system state is usually inferred or estimated based on the system outputs measured by certain instruments (such as sensors) and the flow of the state governed by a dynamic or motion model. Some simple techniques, such as least square estimation or batch estimation, are sufficient in solving static or offline estimation problems. For online and real time (sequential) estimation problems, more sophisticated estimation filters are usually applied.

An estimation system is composed of a dynamic or motion model that describes the flow of the state and a measurement model that describes how the measurements are obtained. Mathematically, these two models can be represented by an equation of motion and a measurement equation. For example, the equation of motion and measurement equation for a general nonlinear discrete estimation system can be written as:

$\begin{array}{l} x_{k + 1} = f (x_{k}) \\ y_{k} = h (x_{k}) \end{array}$

where k is the time step, x_k is the system state at time step k, f(x_k) is the state-dependent equation of motion, h(x_k) is the state dependent measurement equation, and y_k is the output.

Noise Distribution

In most cases, building a perfect model to capture all the dynamic phenomenon is not possible. For example, including all frictions in the motion model of an autonomous vehicle is impossible. To compensate for these unmodeled dynamics, process noise (w) is often added to the dynamic model. Moreover, when measurements are taken, multiple sources of errors, such as calibration errors, are inevitably included in the measurements. To account for these errors, proper measurement noise must be added to the measurement model. An estimation system including these random noises and errors is called a stochastic estimation system, which can be represented by:

$\begin{array}{l} x_{k + 1} = f (x_{k}, w_{k}) \\ y_{k} = h (x_{k}, v_{k}) \end{array}$

where w_k and v_krepresent process noise and measurement noise, respectively.

For most engineering applications, the process noise and measurement noise are assumed to follow zero-mean Gaussian or normal distributions, or are at least be approximated by Gaussian distributions. Also, because the exact state is unknown, the state estimate is a random variable, usually assumed to follow Gaussian distributions. Assuming Gaussian distributions for these variables greatly simplifies the design of an estimation filter, and form the basis of the Kalman filter family.

A Gaussian distribution for a random variable (x) is parametrized by a mean value μ and a covariance matrix P, which is written as x∼N(μ,P). Given a Gaussian distribution, the mean, which is also the most likely value of x, is defined by expectation (E) as:

$μ = E [x]$

The mean is also called the first moment of x about the origin. The covariance that describes of the uncertainty of x is defined by expectation (E) as:

$P = E [(x - μ) {(x - μ)}^{T}]$

The covariance is also called the second moment of x about its mean.

If the dimension of x is one, P is only a scalar. In this case, the value of P is usually denoted by σ² and called variance. The square root, σ, is called the standard deviation of x. The standard deviation has important physical meaning. For example, the following figure shows the probability density function (which describes the likelihood that x takes a certain value) for a one-dimensional Gaussian distribution with mean equal to μ and standard deviation equal to σ. About 68% of the data fall within the 1σ boundary of x, 95% of the data fall within the 2σ boundary, and 99.7% of the data fall within the 3σ boundary.

Even though the Gaussian distribution assumption is the dominant assumption in engineering applications, there exist systems whose state cannot be approximated by Gaussian distributions. In this case, non-Kalman filters (such as a particle filter) is required to accurately estimate the system state.

Filter Design

The goal of designing a filter is to estimate the state of a system using measurements and system dynamics. Since the measurements are usually taken at discrete time steps, the filtering process is usually separated into two steps:

Prediction: Propagate state and covariance between discrete measurement time steps (k = 1, 2, 3, …, N) using dynamic models. This step is also called flow update.
Correction: Correct the state estimate and covariance at discrete time steps using measurements. This step is also called measurement update.

For representing state estimate and covariance status in different steps, x_k|k and P_k|k denote the state estimate and covariance after correction at time step k, whereas x_k+1|k and P_k+1|k denote the state estimate and covariance predicted from the previous time step k to the current time step k+1.

Prediction

In the prediction step, the state propagation is straightforward. The filter only needs to substitute the state estimate into the dynamic model and propagate it forward in time as x_k+1|k = f(x_k|k).

The covariance propagation is more complicated. If the estimation system is linear, then the covariance can be propagated (P_k|k→P_k+1|k) exactly in a standard equation based on the system properties. For nonlinear systems, accurate covariance propagation is challenging. A major difference between different filters is how they propagate the system covariance. For example:

A linear Kalman filter uses a linear equation to exactly propagate the covariance.
An extended Kalman filter propagates the covariance based on linear approximation, which renders large errors when the system is highly nonlinear.
An unscented Kalman filter uses unscented transformation to sample the covariance distribution and propagate it in time.

How the state and covariance are propagated also greatly affects the computation complexity of a filter. For example:

A linear Kalman filter uses a linear equation to exactly propagate the covariance, which is usually computationally efficient.
An extended Kalman filter uses linear approximations, which require calculation of Jacobian matrices and demand more computation resources.
An unscented Kalman filter needs to sample the covariance distribution and therefore requires the propagation of multiple sample points, which is costly for high-dimensional systems.

Correction

In the correction step, the filter uses measurements to correct the state estimate through measurement feedback. Basically, the difference between the true measurement and the predicted measurement is added to the state estimate after it is multiplied by a feedback gain matrix. For example, in an extended Kalman filter, the correction for the state estimate is given by:

$x_{k + 1 | k + 1} = x_{k + 1 | k} + K_{k} (y_{k + 1} - h (x_{k + 1 | k}))$

As mentioned, x_k+1|k is the state estimate before (priori) correction and x_k+1|k+1 is the state estimate after (posteriori) correction. K_k is the Kalman gain governed by an optimal criterion, y_k is the true measurement, and h(x_k+1|k) is the predicted measurement.

In the correction step, the filter also corrects the estimate error covariance. The basic idea is to correct the probabilistic distribution of x using the distribution information of y_k+1. This is called the posterior probability density of x given y. In a filter, the prediction and correction steps are processed recursively. The flowchart shows the general algorithms for Kalman filters.

Estimation Filters in Sensor Fusion and Tracking Toolbox

Sensor Fusion and Tracking Toolbox™ offers multiple estimation filters you can use to estimate and track the state of a dynamic system.

Kalman Filter

The classical Kalman filter (trackingKF) is the optimal filter for linear systems with Gaussian process and measurement noise. A linear estimation system can be given as:

$\begin{array}{l} x_{k + 1} = A_{k} x_{k} + w_{k} \\ y_{k} = H_{k} x_{k} + v_{k} \end{array}$

Both the process and measurement noise are assumed to be Gaussian, that is:

$\begin{array}{l} w_{k} ~ N (0, Q_{k}) \\ v_{k} ~ N (0, R_{k}) \end{array}$

Therefore, the covariance matrix can be directly propagated between measurement steps using a linear algebraic equation as:

$P_{k + 1 | k} = A_{k} P_{k | k} A_{k}^{T} + Q_{k}$

The correction equations for the measurement update are:

$\begin{array}{l} x_{k + 1 | k + 1} = x_{k + 1 | k} + K_{k} (y_{k} - H_{k} x_{k + 1 | k}) \\ P_{k + 1 | k + 1} = (I - K_{k} H_{k}) P_{k + 1 | k} \end{array}$

To calculate the Kalman gain matrix (K_k) in each update, the filter needs to calculate the inverse of a matrix:

$K_{k} = P_{k | k - 1} H_{k}^{T} {[H_{k} P_{k | k - 1} H_{k}^{T} + R_{k}]}^{- 1}$

Since the dimension of the inverted matrix is equal to that of the estimated state, this calculation requires some computation efforts for a high dimensional system. For more details, see Linear Kalman Filters.

Alpha-Beta Filter

The alpha-beta filter (trackingABF) is a suboptimal filter applied to linear systems. The filter can be regarded as a simplified Kalman filter. In a Kalman filter, the Kalman gain and covariance matrices are calculated dynamically and updated in each step. However, in an alpha-beta filter, these matrices are constant. This treatment sacrifices the optimality of a Kalman filter but improves the computation efficiency. For this reason, an alpha-beta filter might be preferred when the computation resources are limited.

Extended Kalman Filter

The most popular extended Kalman filter (trackingEKF) is modified from the classical Kalman filter to adapt to the nonlinear models. It works by linearizing the nonlinear system about the state estimate and neglecting the second and higher order nonlinear terms. Its formulations are basically the same as those of a linear Kalman filter except that the A_k and H_k matrices in the Kalman filter are replaced by the Jacobian matrices of f(x_k ) and h(x_k):

$\begin{array}{l} A_{k} = {\frac{\partial f (x_{k})}{\partial x_{k}} |}_{x_{k | k - 1}} \\ H_{k} = {\frac{\partial h (x_{k})}{\partial x_{k}} |}_{x_{k | k - 1}} \end{array}$

If the true dynamics of the estimation system are close to the linearized dynamics, then using this linear approximation does not yield significant errors for a short period of time. For this reason, an EKF can produce relatively accurate state estimates for a mildly nonlinear estimation system with short update intervals. However, since an EKF neglects higher order terms, it can diverge for highly nonlinear systems (quadrotors, for example), especially with large update intervals.

Compared to a KF, an EKF needs to derive the Jacobian matrices, which requires the system dynamics to be differentiable, and to calculate the Jacobian matrices to linearize the system, which demands more computation assets.

Note that for estimation systems with state expressed in spherical coordinates, you can use trackingMSCEKF.

Unscented Kalman Filter

The unscented Kalman filter (trackingUKF) uses an unscented transformation (UT) to approximately propagate the covariance distribution for a nonlinear model. The UT approach samples the covariance Gaussian distribution at the current time, propagates the sample points (called sigma points) using the nonlinear model, and approximates the resulting covariance distribution assumed to be Gaussian by evaluating these propagated sigma points. The figure illustrates the difference between the actual propagation, the linearized propagation, and the UT propagation of the uncertainty covariance.

Compared to the linearization approach taken by an EKF, the UT approach results in more accurate propagation of covariance and leads to more accurate state estimation, especially for highly nonlinear systems. UKF does not require the derivation and calculation of Jacobian matrices. However, UKF requires the propagation of 2n+1 sigma points through the nonlinear model, where n is the dimension of the estimated state. This can be computationally expensive for high dimensional systems.

Cubature Kalman Filter

The cubature Kalman filter (trackingCKF) takes a slightly different approach than UKF to generate 2n sample points used to propagate the covariance distribution, where n is the dimension of the estimated state. This alternate sample point set often results in better statistical stability and avoids divergence which might occur in UKF, especially when running in a single-precision platform. Note that a CKF is essentially equivalent to a UKF when the UKF parameters are set to α = 1, β = 0, and κ = 0. See trackingUKF for the definition of these parameters.

Gaussian-Sum Filter

The Gaussian-Sum filter (trackingGSF) uses the weighted sum of multiple Gaussian distributions to approximate the distribution of the estimated state. The estimated state is given by a weighted sum of Gaussian states:

$x_{k} = \sum_{i = 1}^{N} c_{k}^{i} x_{k}^{i}$

where N is the number of Gaussian states maintained in the filter, and c_kⁱ is the weight for the corresponding Gaussian state, which is modified in each update based on the measurements. The multiple Gaussian states follow the same dynamic model as:

$x_{k + 1}^{i} = f (x_{k}^{i}, w_{k}^{i}), for i = 1, 2, \dots, N .$

The filter is effective in estimating the states of an incompletely observable estimation system. For example, the filter can use multiple angle-parametrized extended Kalman filters to estimate the system state when only range measurements are available. See Tracking with Range-Only Measurements for an example.

Interactive Multiple Model Filter

The interactive multiple model filter (trackingIMM) uses multiple Gaussian filters to track the position of a target. In highly maneuverable systems, the system dynamics can switch between multiple models (constant velocity, constant acceleration, and constant turn for example). Modelling the motion of a target using only one motion model is difficult. A multiple model estimation system can be described as:

$\begin{array}{l} x_{k + 1}^{i} = f_{i} (x_{k}^{i}, w_{k}^{i}) \\ y_{k}^{i} = h_{i} (x_{k}^{i}, v_{k}^{i}) \end{array}$

where i = 1, 2, …, M, and M is the total number of dynamic models. The IMM filter resolves the target motion uncertainty by using multiple models for a maneuvering target. The filter processes all the models simultaneously and represents the overall estimate as the weighted sum of the estimates from these models, where the weights are the probability of each model. See Tracking Maneuvering Targets for an example.

Particle Filter

The particle filter (trackingPF) is different from the Kalman family of filters (EKF and UKF, for example) as it does not rely on the Gaussian distribution assumption, which corresponds to a parametric description of uncertainties using mean and variance. Instead, the particle filter creates multiple simulations of weighted samples (particles) of a system's operation through time, and then analyzes these particles as a proxy for the unknown true distribution. A brief introduction of the particle filter algorithm is shown in the figure.

The motivation behind this approach is a law-of-large-numbers argument — as the number of particles gets large, their empirical distribution gets close to the true distribution. The main advantage of a particle filter over various Kalman filters is that it can be applied to non-Gaussian distributions. Also, the filter has no restriction on the system dynamics and can be used with highly nonlinear system. Another benefit is the filter’s inherent ability to represent multiple hypotheses about the current state. Since each particle represents a hypothesis of the state with a certain associated likelihood, a particle filter is useful in cases where there exists ambiguity about the state.

Along with these appealing properties is the high computation complexity of a particle filter. For example, a UKF requires propagating 13 sample points to estimate the 3-D position and velocity of an object. However, a particle filter may require thousands of particles to obtain a reasonable estimate. Also, the number of particles needed to achieve good estimation grows very quickly with the state dimension and can lead to particle deprivation problems in high dimensional spaces. Therefore, particle filters have been mostly applied to systems with a reasonably low number of dimensions (for example robots).

How to Choose a Tracking Filter

The following table lists all the tracking filters available in Sensor Fusion and Tracking Toolbox and how to choose them given constraints on system nonlinearity, state distribution, and computational complexity.

Filter Name	Supports Nonlinear Models	Gaussian State	Computational Complexity	Comments
Alpha-Beta			Low	Suboptimal filter.
Kalman		✓	Medium Low	Optimal for linear systems.
Extended Kalman	✓	✓	Medium	Uses linearized models to propagate uncertainty covariance.
Unscented Kalman	✓	✓	Medium High	Samples the uncertainty covariance to propagate the sample points. May become numerically unstable in a single-precision platform.
Cubature Kalman	✓	✓	Medium High	Samples the uncertainty covariance to propagate the sample points. Numerically stable.
Gaussian-Sum	✓	✓ (Assumes a weighted sum of distributions)	High	Good for partially observable cases (angle-only tracking for example).
Interacting Multiple Models (IMM)	✓ Multiple models	✓ (Assumes a weighted sum of distributions)	High	Maneuvering objects (which accelerate or turn, for example)
Particle	✓		Very High	Samples the uncertainty distribution using weighted particles.

References

[1] Wang, E.A., and R. Van Der Merwe. "The Unscented Kalman Filter for Nonlinear Estimation." IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium. No. 00EX373, 2000, pp. 153–158.

[2] Fang, H., N. Tian, Y. Wang, M. Zhou, and M.A. Haile. "Nonlinear Bayesian Estimation: From Kalman Filtering to a Broader Horizon." IEEE/CAA Journal of Automatica Sinica. Vol. 5, Number 2, 2018, pp. 401–417.

[3] Arasaratnam, I., and S. Haykin. "Cubature Kalman Filters." IEEE Transactions on automatic control. Vol. 54, Number 6, 2009, pp. 1254–1269.

[4] Konatowski, S., P. Kaniewski, and J. Matuszewski. "Comparison of Estimation Accuracy of EKF, UKF and PF Filters." Annual of Navigation. Vol. 23, Number 1, 2016, pp. 69–87.

[5] Darko, J. "Object Tracking: Particle Filter with Ease." https://www.codeproject.com/Articles/865934/Object-Tracking-Particle-Filter-with-Ease.