1. Estimation and the Kalman Filter David Johnson

2. The Mean of a Discrete Distribution “I have more legs than average”

3. Gaussian Definition --   Univariate  Multivariate 2 2 )( 2 1 2 2 1 )( :),(~)(        x exp Nxp )()( 2 1 2/1 2/ 1 )2( 1 )( :)(~)( μxΣμx Σ x Σμx    t ep,Νp d 

4. Back to the non-evolving case Two different processes measure the same thing Want to combine into one better measurement Estimation

5. What is meant by estimation? Data + noise Estimator Estimation H z ŷ Stochastic process estimate

6. A Least-Squares Approach We want to fuse these measurements to obtain a new estimate for the range Using a weighted least-squares approach, the resulting sum of squares error will be Minimizing this error with respect to yields

7. A Least-Squares Approach Rearranging we have If we choose the weight to be we obtain

8. For merging Gaussian distributions, the update rule is A Least-Squares Approach Show for N(0,a) N(0,b)

9. What happens when you move? derive

10. Moving As you move –Uncertainty grows –Need to make new measurements –Combine measurements using Kalman gain

11. The Kalman Filter “an optimal recursive data processing algorithm” OPTIMAL: -Linear dynamics -Measurements linear w/r to state -Errors in sensors and dynamics must be zero-mean (un-bias) white Gaussian RECURSIVE: -Does not require all previous data -Incoming measurements ‘modify’ current estimate DATA PROCESSING ALGORITHM: The Kalman filter is essentially a technique of estimation given a system model and concurrent measurements (not a function of frequency)

12. The Discrete Kalman Filter Estimate the state of a discrete-time controlled process that is governed by the linear stochastic difference equation: with a measurement: The random variables w k and v k represent the process and measurement noise (respectively). They are assumed to be independent (of each other), white, and with normal probability distributions In practice, the process noise covariance and measurement noise covariance matrices might change with each time step or measurement. (PDFs)

13. The Discrete Kalman Filter First part – model forecast: prediction “prior” estimate Process noise covariance State transition State prediction Error covariance prediction Control signal Prediction is based only the model of the system dynamics.

14. The Discrete Kalman Filter Second part – measurement update: correction “posterior” estimate state correction “prior” state prediction Kalman gain actual measurement predicted measurement update error covariance matrix (posterior)

15. The Discrete Kalman Filter The Kalman gain, K: “Do I trust my model or measurements?” variance of the predicted states = ------------------------------------------------------------ variance of the predicted + measured states measurement sensitivity matrix measurement noise covariance As measurement error covariance, R, approaches zero, the actual measurement, z k is “trusted” more and more. is trusted less and less But, as the “prior” (predicted) estimate error covariance, P, approaches zero, the actual measurement is trusted less, and predicted measurement, is trusted more and more

16. Estimate a constant voltage Measurements have noise Update step is Measurement step is

17. Results

18. Variance

19. Parameter tuning

20. More tuning

21. The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm The EKF allows for estimation of non-linear processes or measurement relationships This is accomplished by linearizing the current mean and covariance estimates (similar to a first order Taylor series approximation) Suppose our process and measurement equations are the non- linear functions The Extended Kalman Filter (EKF) Kalman FilterExtended Kalman Filter

22. Linearity Assumption Revisited

23. Non-linear Function

24. EKF Linearization (1)

25. EKF Linearization (2)

26. EKF Linearization (3)