Discrete Ekf on Continuous Time Model

Filter for nonlinear state estimation

In estimation theory, the extended Kalman filter (EKF) is the nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance. In the case of well defined transition models, the EKF has been considered^[1] the de facto standard in the theory of nonlinear state estimation, navigation systems and GPS.^[2]

History [edit]

The papers establishing the mathematical foundations of Kalman type filters were published between 1959 and 1961.^{[3] [4] [5]} The Kalman filter is the optimal linear estimator for linear system models with additive independent white noise in both the transition and the measurement systems. Unfortunately, in engineering, most systems are nonlinear, so attempts were made to apply this filtering method to nonlinear systems; most of this work was done at NASA Ames.^{[6] [7]} The EKF adapted techniques from calculus, namely multivariate Taylor series expansions, to linearize a model about a working point. If the system model (as described below) is not well known or is inaccurate, then Monte Carlo methods, especially particle filters, are employed for estimation. Monte Carlo techniques predate the existence of the EKF but are more computationally expensive for any moderately dimensioned state-space.

Formulation [edit]

In the extended Kalman filter, the state transition and observation models don't need to be linear functions of the state but may instead be differentiable functions.

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k})+{\boldsymbol {w}}_{k}}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k})+{\boldsymbol {v}}_{k}}$

Here w _k and v _k are the process and observation noises which are both assumed to be zero mean multivariate Gaussian noises with covariance Q _k and R _k respectively. u _k is the control vector.

The function f can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, f and h cannot be applied to the covariance directly. Instead a matrix of partial derivatives (the Jacobian) is computed.

At each time step, the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearizes the non-linear function around the current estimate.

See the Kalman Filter article for notational remarks.

Discrete-time predict and update equations [edit]

Notation ${\displaystyle {\hat {\mathbf {x} }}_{n\mid m}}$ represents the estimate of ${\displaystyle \mathbf {x} }$ at time n given observations up to and including at time m ≤ n .

Predict [edit]

Update [edit]

where the state transition and observation matrices are defined to be the following Jacobians

${\displaystyle {{\boldsymbol {F}}_{k}}=\left.{\frac {\partial f}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k}}}$

${\displaystyle {{\boldsymbol {H}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {x}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

Disadvantages [edit]

Unlike its linear counterpart, the extended Kalman filter in general is not an optimal estimator (it is optimal if the measurement and the state transition model are both linear, as in that case the extended Kalman filter is identical to the regular one). In addition, if the initial estimate of the state is wrong, or if the process is modeled incorrectly, the filter may quickly diverge, owing to its linearization. Another problem with the extended Kalman filter is that the estimated covariance matrix tends to underestimate the true covariance matrix and therefore risks becoming inconsistent in the statistical sense without the addition of "stabilising noise" ^[8] .

Having stated this, the extended Kalman filter can give reasonable performance, and is arguably the de facto standard in navigation systems and GPS.

Generalizations [edit]

Continuous-time extended Kalman filter [edit]

Model

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} (t)&=h{\bigl (}\mathbf {x} (t){\bigr )}+\mathbf {v} (t)&\mathbf {v} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {R} (t){\bigr )}\end{aligned}}}$

Initialize

${\displaystyle {\hat {\mathbf {x} }}(t_{0})=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]}{\text{, }}\mathbf {P} (t_{0})=Var{\bigl [}\mathbf {x} (t_{0}){\bigr ]}}$

Predict-Update

${\displaystyle {\begin{aligned}{\dot {\hat {\mathbf {x} }}}(t)&=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}+\mathbf {K} (t){\Bigl (}\mathbf {z} (t)-h{\bigl (}{\hat {\mathbf {x} }}(t){\bigr )}{\Bigr )}\\{\dot {\mathbf {P} }}(t)&=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}-\mathbf {K} (t)\mathbf {H} (t)\mathbf {P} (t)+\mathbf {Q} (t)\\\mathbf {K} (t)&=\mathbf {P} (t)\mathbf {H} (t)^{T}\mathbf {R} (t)^{-1}\\\mathbf {F} (t)&=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}\\\mathbf {H} (t)&=\left.{\frac {\partial h}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t)}\end{aligned}}}$

Unlike the discrete-time extended Kalman filter, the prediction and update steps are coupled in the continuous-time extended Kalman filter.^[9]

Discrete-time measurements [edit]

Most physical systems are represented as continuous-time models while discrete-time measurements are frequently taken for state estimation via a digital processor. Therefore, the system model and measurement model are given by

${\displaystyle {\begin{aligned}{\dot {\mathbf {x} }}(t)&=f{\bigl (}\mathbf {x} (t),\mathbf {u} (t){\bigr )}+\mathbf {w} (t)&\mathbf {w} (t)&\sim {\mathcal {N}}{\bigl (}\mathbf {0} ,\mathbf {Q} (t){\bigr )}\\\mathbf {z} _{k}&=h(\mathbf {x} _{k})+\mathbf {v} _{k}&\mathbf {v} _{k}&\sim {\mathcal {N}}(\mathbf {0} ,\mathbf {R} _{k})\end{aligned}}}$

where ${\displaystyle \mathbf {x} _{k}=\mathbf {x} (t_{k})}$ .

Initialize

${\displaystyle {\hat {\mathbf {x} }}_{0|0}=E{\bigl [}\mathbf {x} (t_{0}){\bigr ]},\mathbf {P} _{0|0}=E{\bigl [}\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)\left(\mathbf {x} (t_{0})-{\hat {\mathbf {x} }}(t_{0})\right)^{T}{\bigr ]}}$

Predict

${\displaystyle {\begin{aligned}{\text{solve }}&{\begin{cases}{\dot {\hat {\mathbf {x} }}}(t)=f{\bigl (}{\hat {\mathbf {x} }}(t),\mathbf {u} (t){\bigr )}\\{\dot {\mathbf {P} }}(t)=\mathbf {F} (t)\mathbf {P} (t)+\mathbf {P} (t)\mathbf {F} (t)^{T}+\mathbf {Q} (t)\end{cases}}\qquad {\text{with }}{\begin{cases}{\hat {\mathbf {x} }}(t_{k-1})={\hat {\mathbf {x} }}_{k-1|k-1}\\\mathbf {P} (t_{k-1})=\mathbf {P} _{k-1|k-1}\end{cases}}\\\Rightarrow &{\begin{cases}{\hat {\mathbf {x} }}_{k|k-1}={\hat {\mathbf {x} }}(t_{k})\\\mathbf {P} _{k|k-1}=\mathbf {P} (t_{k})\end{cases}}\end{aligned}}}$

where

${\displaystyle \mathbf {F} (t)=\left.{\frac {\partial f}{\partial \mathbf {x} }}\right\vert _{{\hat {\mathbf {x} }}(t),\mathbf {u} (t)}}$

Update

${\displaystyle \mathbf {K} _{k}=\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}{\bigl (}\mathbf {H} _{k}\mathbf {P} _{k|k-1}\mathbf {H} _{k}^{T}+\mathbf {R} _{k}{\bigr )}^{-1}}$

${\displaystyle {\hat {\mathbf {x} }}_{k|k}={\hat {\mathbf {x} }}_{k|k-1}+\mathbf {K} _{k}{\bigl (}\mathbf {z} _{k}-h({\hat {\mathbf {x} }}_{k|k-1}){\bigr )}}$

${\displaystyle \mathbf {P} _{k|k}=(\mathbf {I} -\mathbf {K} _{k}\mathbf {H} _{k})\mathbf {P} _{k|k-1}}$

where

${\displaystyle {\textbf {H}}_{k}=\left.{\frac {\partial h}{\partial {\textbf {x}}}}\right\vert _{{\hat {\textbf {x}}}_{k|k-1}}}$

The update equations are identical to those of discrete-time extended Kalman filter.

Higher-order extended Kalman filters [edit]

The above recursion is a first-order extended Kalman filter (EKF). Higher order EKFs may be obtained by retaining more terms of the Taylor series expansions. For example, second and third order EKFs have been described.^[10] However, higher order EKFs tend to only provide performance benefits when the measurement noise is small.

Non-additive noise formulation and equations [edit]

The typical formulation of the EKF involves the assumption of additive process and measurement noise. This assumption, however, is not necessary for EKF implementation.^[11] Instead, consider a more general system of the form:

${\displaystyle {\boldsymbol {x}}_{k}=f({\boldsymbol {x}}_{k-1},{\boldsymbol {u}}_{k-1},{\boldsymbol {w}}_{k-1})}$

${\displaystyle {\boldsymbol {z}}_{k}=h({\boldsymbol {x}}_{k},{\boldsymbol {v}}_{k})}$

Here w _k and v _k are the process and observation noises which are both assumed to be zero mean multivariate Gaussian noises with covariance Q _k and R _k respectively. Then the covariance prediction and innovation equations become

${\displaystyle {\boldsymbol {P}}_{k|k-1}={{\boldsymbol {F}}_{k-1}}{{\boldsymbol {P}}_{k-1|k-1}}{{\boldsymbol {F}}_{k-1}^{T}}{+}{{\boldsymbol {L}}_{k-1}}{{\boldsymbol {Q}}_{k-1}}{{\boldsymbol {L}}_{k-1}^{T}}}$

${\displaystyle {\boldsymbol {S}}_{k}={{\boldsymbol {H}}_{k}}{{\boldsymbol {P}}_{k|k-1}}{{\boldsymbol {H}}_{k}^{T}}{+}{{\boldsymbol {M}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {M}}_{k}^{T}}}$

where the matrices ${\displaystyle {\boldsymbol {L}}_{k-1}}$ and ${\displaystyle {\boldsymbol {M}}_{k}}$ are Jacobian matrices:

${\displaystyle {{\boldsymbol {L}}_{k-1}}=\left.{\frac {\partial f}{\partial {\boldsymbol {w}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k-1|k-1},{\boldsymbol {u}}_{k-1}}}$

${\displaystyle {{\boldsymbol {M}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {v}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1}}}$

The predicted state estimate and measurement residual are evaluated at the mean of the process and measurement noise terms, which is assumed to be zero. Otherwise, the non-additive noise formulation is implemented in the same manner as the additive noise EKF.

Implicit extended Kalman filter [edit]

In certain cases, the observation model of a nonlinear system cannot be solved for ${\displaystyle {\boldsymbol {z}}_{k}}$ , but can be expressed by the implicit function:

${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z'}}_{k})={\boldsymbol {0}}}$

where ${\displaystyle {\boldsymbol {z}}_{k}={\boldsymbol {z'}}_{k}+{\boldsymbol {v}}_{k}}$ are the noisy observations.

The conventional extended Kalman filter can be applied with the following substitutions:^{[12] [13]}

${\displaystyle {{\boldsymbol {R}}_{k}}\leftarrow {{\boldsymbol {J}}_{k}}{{\boldsymbol {R}}_{k}}{{\boldsymbol {J}}_{k}^{T}}}$

${\displaystyle {\tilde {\boldsymbol {y}}}_{k}\leftarrow -h({\hat {\boldsymbol {x}}}_{k|k-1},{\boldsymbol {z}}_{k})}$

where:

${\displaystyle {{\boldsymbol {J}}_{k}}=\left.{\frac {\partial h}{\partial {\boldsymbol {z}}}}\right\vert _{{\hat {\boldsymbol {x}}}_{k|k-1},{\hat {\boldsymbol {z}}}_{k}}}$

Here the original observation covariance matrix ${\displaystyle {{\boldsymbol {R}}_{k}}}$ is transformed, and the innovation ${\displaystyle {\tilde {\boldsymbol {y}}}_{k}}$ is defined differently. The Jacobian matrix ${\displaystyle {{\boldsymbol {H}}_{k}}}$ is defined as before, but determined from the implicit observation model ${\displaystyle h({\boldsymbol {x}}_{k},{\boldsymbol {z}}_{k})}$ .

Modifications [edit]

Iterated extended Kalman filter [edit]

The iterated extended Kalman filter improves the linearization of the extended Kalman filter by recursively modifying the centre point of the Taylor expansion. This reduces the linearization error at the cost of increased computational requirements.^[13]

Robust extended Kalman filter [edit]

The extended Kalman filter arises by linearizing the signal model about the current state estimate and using the linear Kalman filter to predict the next estimate. This attempts to produce a locally optimal filter, however, it is not necessarily stable because the solutions of the underlying Riccati equation are not guaranteed to be positive definite. One way of improving performance is the faux algebraic Riccati technique ^[14] which trades off optimality for stability. The familiar structure of the extended Kalman filter is retained but stability is achieved by selecting a positive definite solution to a faux algebraic Riccati equation for the gain design.

Another way of improving extended Kalman filter performance is to employ the H-infinity results from robust control. Robust filters are obtained by adding a positive definite term to the design Riccati equation.^[15] The additional term is parametrized by a scalar which the designer may tweak to achieve a trade-off between mean-square-error and peak error performance criteria.

Invariant extended Kalman filter [edit]

The invariant extended Kalman filter (IEKF) is a modified version of the EKF for nonlinear systems possessing symmetries (or invariances). It combines the advantages of both the EKF and the recently introduced symmetry-preserving filters. Instead of using a linear correction term based on a linear output error, the IEKF uses a geometrically adapted correction term based on an invariant output error; in the same way the gain matrix is not updated from a linear state error, but from an invariant state error. The main benefit is that the gain and covariance equations converge to constant values on a much bigger set of trajectories than equilibrium points as it is the case for the EKF, which results in a better convergence of the estimation.

Unscented Kalman filters [edit]

A nonlinear Kalman filter which shows promise as an improvement over the EKF is the unscented Kalman filter (UKF). In the UKF, the probability density is approximated by a deterministic sampling of points which represent the underlying distribution as a Gaussian. The nonlinear transformation of these points are intended to be an estimation of the posterior distribution, the moments of which can then be derived from the transformed samples. The transformation is known as the unscented transform. The UKF tends to be more robust and more accurate than the EKF in its estimation of error in all the directions.

"The extended Kalman filter (EKF) is probably the most widely used estimation algorithm for nonlinear systems. However, more than 35 years of experience in the estimation community has shown that is difficult to implement, difficult to tune, and only reliable for systems that are almost linear on the time scale of the updates. Many of these difficulties arise from its use of linearization."^[1]

A 2012 paper includes simulation results which suggest that some published variants of the UKF fail to be as accurate as the Second Order Extended Kalman Filter (SOEKF), also known as the augmented Kalman filter.^[16] The SOEKF predates the UKF by approximately 35 years with the moment dynamics first described by Bass et al.^[17] The difficulty in implementing any Kalman-type filters for nonlinear state transitions stems from the numerical stability issues required for precision,^[18] however the UKF does not escape this difficulty in that it uses linearization as well, namely linear regression. The stability issues for the UKF generally stem from the numerical approximation to the square root of the covariance matrix, whereas the stability issues for both the EKF and the SOEKF stem from possible issues in the Taylor Series approximation along the trajectory.

Ensemble Kalman Filter [edit]

The UKF was in fact predated by the Ensemble Kalman filter, invented by Evensen in 1994. It has the advantage over the UKF that the number of ensemble members used can be much smaller than the state dimension, allowing for applications in very high-dimensional systems, such as weather prediction, with state-space sizes of a billion or more.

Fuzzy Kalman Filter [edit]

Fuzzy Kalman filter with a new method to represent possibility distributions was recently proposed to replace probability distributions by possibility distributions in order to obtain a genuine possibilistic filter, enabling the use of non-symmetric process and observation noises as well as higher inaccuracies in both process and observation models. ^[19]

See also [edit]

• Kalman filter
• Ensemble Kalman filter
• Fast Kalman filter
• Invariant extended Kalman filter
• Moving horizon estimation
• Particle filter
• Unscented Kalman filter

References [edit]

1. ^ ^{a b} Julier, S.J.; Uhlmann, J.K. (2004). "Unscented filtering and nonlinear estimation" (PDF). Proceedings of the IEEE. 92 (3): 401–422. doi:10.1109/jproc.2003.823141. S2CID 9614092.
2. ^ Courses, E.; Surveys, T. (2006). Sigma-Point Filters: An Overview with Applications to Integrated Navigation and Vision Assisted Control. Nonlinear Statistical Signal Processing Workshop, 2006 IEEE. pp. 201–202. doi:10.1109/NSSPW.2006.4378854. ISBN978-1-4244-0579-4. S2CID 18535558.
3. ^ R.E. Kalman (1960). "Contributions to the theory of optimal control". Bol. Soc. Mat. Mexicana: 102–119. CiteSeerX10.1.1.26.4070.
4. ^ R.E. Kalman (1960). "A New Approach to Linear Filtering and Prediction Problems" (PDF). Journal of Basic Engineering. 82: 35–45. doi:10.1115/1.3662552.
5. ^ R.E. Kalman; R.S. Bucy (1961). "New results in linear filtering and prediction theory" (PDF). Journal of Basic Engineering. 83: 95–108. doi:10.1115/1.3658902.
6. ^ Bruce A. McElhoe (1966). "An Assessment of the Navigation and Course Corrections for a Manned Flyby of Mars or Venus". IEEE Transactions on Aerospace and Electronic Systems. 2 (4): 613–623. Bibcode:1966ITAES...2..613M. doi:10.1109/TAES.1966.4501892. S2CID 51649221.
7. ^ G.L. Smith; S.F. Schmidt and L.A. McGee (1962). "Application of statistical filter theory to the optimal estimation of position and velocity on board a circumlunar vehicle". National Aeronautics and Space Administration.
8. ^ Huang, Guoquan P; Mourikis, Anastasios I; Roumeliotis, Stergios I (2008). "Analysis and improvement of the consistency of extended Kalman filter based SLAM". Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on. pp. 473–479. doi:10.1109/ROBOT.2008.4543252.
9. ^ Brown, Robert Grover; Hwang, Patrick Y.C. (1997). Introduction to Random Signals and Applied Kalman Filtering (3 ed.). New York: John Wiley & Sons. pp. 289–293. ISBN978-0-471-12839-7.
10. ^ Einicke, G.A. (2019). Smoothing, Filtering and Prediction: Estimating the Past, Present and Future (2nd ed.). Amazon Prime Publishing. ISBN978-0-6485115-0-2.
11. ^ Simon, Dan (2006). Optimal State Estimation. Hoboken, NJ: John Wiley & Sons. ISBN978-0-471-70858-2.
12. ^ Quan, Quan (2017). Introduction to multicopter design and control. Singapore: Springer. ISBN978-981-10-3382-7.
13. ^ ^{a b} Zhang, Zhengyou (1997). "Parameter estimation techniques: a tutorial with application to conic fitting" (PDF). Image and Vision Computing. 15 (1): 59–76. doi:10.1016/s0262-8856(96)01112-2. ISSN 0262-8856.
14. ^ Einicke, G.A.; White, L.B.; Bitmead, R.R. (September 2003). "The Use of Fake Algebraic Riccati Equations for Co-channel Demodulation". IEEE Trans. Signal Process. 51 (9): 2288–2293. Bibcode:2003ITSP...51.2288E. doi:10.1109/tsp.2003.815376. hdl:2440/2403.
15. ^ Einicke, G.A.; White, L.B. (September 1999). "Robust Extended Kalman Filtering". IEEE Trans. Signal Process. 47 (9): 2596–2599. Bibcode:1999ITSP...47.2596E. doi:10.1109/78.782219.
16. ^ Gustafsson, F.; Hendeby, G.; , "Some Relations Between Extended and Unscented Kalman Filters," Signal Processing, IEEE Transactions on , vol.60, no.2, pp.545-555, Feb. 2012
17. ^ R. Bass, V. Norum, and L. Schwartz, "Optimal multichannel nonlinear filtering(optimal multichannel nonlinear filtering problem of minimum variance estimation of state of n- dimensional nonlinear system subject to stochastic disturbance)," J. Mathematical Analysis and Applications,vol. 16, pp. 152–164, 1966
18. ^ Mohinder S. Grewal; Angus P. Andrews (2 February 2015). Kalman Filtering: Theory and Practice with MATLAB. John Wiley & Sons. ISBN978-1-118-98496-3.
19. ^ Matía, F.; Jiménez, V.; Alvarado, B.P.; Haber, R. (January 2021). "The fuzzy Kalman filter: Improving its implementation by reformulating uncertainty representation". Fuzzy Sets Syst. 402: 78–104. doi:10.1016/j.fss.2019.10.015. S2CID 209913435.

Further reading [edit]

External links [edit]

• Position estimation of a differential-wheel robot based on odometry and landmarks

morrisuponce.blogspot.com

Source: https://en.wikipedia.org/wiki/Extended_Kalman_filter

Share this post