Extended Kalman Filter (EKF)

1. Extended Kalman Filter (EKF) And some other useful Kalman stuff!

2. The Linear (normal) KF

3. What if the system are not described in a linear manner? I.e. if we divide by a state variable, multiplies two state variables or e.g. applies a function (e.g. a sinus) on one or more of our state variables. A lot of real world models are non-linear. How can we extend the Kalman filter – which is an linear optimal state estimator – to real world problems, which not always, in the first point of view, may not be described in a linear manner?

4. We goes from this system description, which is linear To this system description, which may be non-linear

5. How should we handle the Kalman gain?

6. First approach The obvious solution is to use a Kalman filter on a non-linear system, by linearize the model around a point of operation. But, this could require a significantly higher order of model, to be able to describe the non-linear behavior. Should we use a non-linear function in place of K?

7. The solution is much smarter! K will be replaced by another K, just obtained “slightly” different. Extension of the linear Kalman filter to the non-linear cases requires a few more steps in the implementation. But the most of the calculations may be done in compile time, which make the online calculations only a little more intense, in case than the non-linear functions themselves are not computationally heavier. We still only have to compute a single matrix inverse.

8. Partial Derivative Matrix A.k.a. Jacobian matrix will be the tool to handle the EKF. Remember that the Jacobian matrix is defined as Remember this!

9. Quiz Time!

10. Quiz Consider the system described by the following state update function: 1,1 1,2 2,1 2,2

11. The EKF algorithm (1/4) The system and measurement equations are given as follows: Initialize the system:

12. The EKF algorithm (2/4) For each time step k=1,2,…, compute the following: • Compute the following partial derivative matrices (Jacobian):

13. The EKF algorithm (3/4) • Perform the time update of the state estimate and estimation-error co-variance: • Compute the partial derivative matrices:

14. The EKF algorithm (4/4) • Perform the measurement update of the state estimate and the estimation error co-variance matrix: …and go back to a). Note that numerical more precise equations are available for the co-variance update function.

15. Discretization (1/5) There are different approaches to discretization. E.g. we could have a linear continuous time state space model described by the system equations This could be transformed to a discrete model by

16. Discretization (2/5) Another, and maybe the simplest approach, will be to transform the individual sums of products in the continuous time update equation from the definition of the derivative. E.g. like where Δt is the samplingstime.

17. Discretization (3/5) An obviously approach will also be to use Taylor expansion. This could make us skip the step of going through the continuous time state space model to reach the discrete time model . E.g. if M is a constant and F and ϴ (moment and angularposition) are state variables we could discretize the expression

18. Discretization (4/5)

19. Discretization (5/5) Using up to second order the discrete time model will be

20. Other Need to Know about Kalman System is Reachable((F,G) pair is Reachable) if, starting from a null initial state, we can always find an input signal which leads the system to any a-priori selected final state in finite time.

21. Condition to check Reachability Systemis Reachable if and only if the reachability matrix is full rank. I.e. its rank is n. Note that the system may be reachable only from the process noise point of view. Here the must be a factorization of process noise co-variance matrix .

22. Other Need to Know about Kalman System S is called Observable(the (F,H) pair is observable) if two different initial states do not exist, such that their corresponding outputs are exactly the same, for each t≥0.

23. Condition to check Observability Systemis observable if and only if the observability matrix is full rank. I.e. it has rank n, where n is the system order.

24. Let’s check Observability We have the system

25. Let’s check Observability We have the system

26. Nice to Know about Kalman If the following conditions hold: • Uncorrelated process and measurement noise! • And one of either: a) System is reachable from the noise’s point of view, and the system is observable. b) Or all F eigenvalues are strictly inside the unitary circle. It will make the system asymptotically converge.

27. Gain of Asymptotic Convergence If a linear system asymptotically converges, we can calculate the estimation error co-variance matrix and hence the Kalman gain analytical at compile time, which will give us a much more, computational, efficient filter. Otherwise we have to analyze in each time instance.

28. Exercise on the class! See the uploaded document: A Mechanical System.pdf

29. References [1] S. M. Savaresi, ”Model Identification and Adaptive Systems - KALMAN FILTER”, Milan: Politecnico di Milano, 2011. [2] D. Simon, “Optimal State Estimation, Kalman, H∞and Nonlinear Approaches”, Hoboken, New Jersey: Wiley, 2006. [3] Welch, G and Bishop, G. 2001. “An introduction to the Kalman Filter”, http://www.cs.unc.edu/~welch/kalman/