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.

估计系统是由一个动态或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}$

wherekis the time step,x_kis the system state at time stepk,f(x_k) is the state-dependent equation of motion,h(x_k) is the state dependent measurement equation, andy_kis 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}$

wherew_kandv_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 matrixP, which is written asx∼N(μ,P). Given a Gaussian distribution, the mean, which is also the most likely value ofx, is defined by expectation (E) as:

$μ = E [x]$

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

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

The covariance is also called the second moment ofxabout its mean.

If the dimension ofxis one,Pis only a scalar. In this case, the value ofPis usually denoted byσ²and called variance. The square root,σ, is called the standard deviation ofx. The standard deviation has important physical meaning. For example, the following figure shows the probability density function (which describes the likelihood thatxtakes 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 ofx, 95% of the data fall within the 2σboundary, and 99.7% of the data fall within the 3σboundary.

Gaussian Distribution

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|kandP_k|kdenote the state estimate and covariance after correction at time stepk, whereasx_k+1|kandP_k+1|kdenote the state estimate and covariance predicted from the previous time stepkto the current time stepk+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 asx_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|kis the state estimate before (priori) correction andx_k+1|k+1is the state estimate after (posteriori) correction.K_kis the Kalman gain governed by an optimal criterion,y_kis the true measurement, andh(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 ofxusing the distribution information ofy_k+1. This is called the posterior probability density ofxgiveny. In a filter, the prediction and correction steps are processed recursively. The flowchart shows the general algorithms for Kalman filters.

卡尔曼滤波器Algorithm Flow Chart

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.

卡尔曼滤波器

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)在每个更新,过滤器需要计算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, seeLinear 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 theA_kandH_kmatrices in the Kalman filter are replaced by the Jacobian matrices off(x_k) andh(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 usetrackingMSCEKF.

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.

True, Linear, and UT Transformations

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, wherenis 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 2nsample points used to propagate the covariance distribution, wherenis 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. SeetrackingUKFfor the definition of these parameters.

高斯和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}$

whereNis the number of Gaussian states maintained in the filter, andc_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. SeeTracking with Range-Only Measurementsfor an example.

互动多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}$

wherei= 1, 2, …,M, andMis 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. SeeTracking Maneuvering Targetsfor 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.

Particle Filtering Algorithm

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.
高斯和	✓	✓ (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	使用weighte样本分布的不确定性d 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.