1. Rickard Karlsson IEEE Aerospace Conf 2007 Target Tracking Performance Evaluation A General Software Environment for Filtering Rickard Karlsson Gustaf Hendeby Automatic Control Linköping University, SWEDEN rickard@isy.liu.se

2. Rickard Karlsson IEEE Aerospace Conf 2007 Motivating Example Range-Only Measurements Two Sensors with range uncertainties Performance? General Software for filtering

3. Rickard Karlsson IEEE Aerospace Conf 2007 Motivating Example: General Filtering Software  Surface/UW navigation  Robotics  Bearings-Only Tracking  Track Before Detect Performance usually using RMSE Applications

4. Rickard Karlsson IEEE Aerospace Conf 2007 Outline  Nonlinear filtering using particle filters  Performace measure for nonlinear filtering Kullback-Divergence vs RMSE  General Filtering Software Object oriented design Design Patterns  Examples

5. Rickard Karlsson IEEE Aerospace Conf 2007 Filtering STATE SPACE MODEL Process noise Measurement noise PROBABILISTIC DESCRIPTION

6. Rickard Karlsson IEEE Aerospace Conf 2007 Bayesian Recursions: Probability Density Function (pdf) M.U. T.U.

7. Rickard Karlsson IEEE Aerospace Conf 2007 Filter Evaluation: Mean Square Error (MSE) Mean square error (MSE)  Standard performance measure  Approximates the estimation error covariance  Bounded by the Cramér-Rao Lower Bound (CRLB)  Ignores higher-order moments! Compare the true trajectory with the estimated!!! What can we do instead?

8. Rickard Karlsson IEEE Aerospace Conf 2007 Kullback-Leibler Information

9. Rickard Karlsson IEEE Aerospace Conf 2007 Filter Evaluation: Kullback Divergence (KD) Kullback Divergence (KD)  Compares the distance between two distributions  Captures all moments of the distributions  True PDF from a grid-based method  True PDF from PF, compare sub-optimal filters  Smoothing kernel needed for implementation Compare the true PDF with the estimated PDF.

10. Rickard Karlsson IEEE Aerospace Conf 2007 Generalized Gaussian Generalized Gaussion Distribution Kullback Divergence PDF

11. Rickard Karlsson IEEE Aerospace Conf 2007 Example 1: One-dimensional Nonlinear System Probability Density Function x Time

12. Rickard Karlsson IEEE Aerospace Conf 2007 Example 1: One-dimensional Nonlinear System Kullback Divergence RMSE KD for one realization comparing PF and EKF RMSE for 400 MC simulations

13. Rickard Karlsson IEEE Aerospace Conf 2007 Example 2: Range-Only Measurement  Estimate target position from range-only measurements  Nonlinear measurements but Gaussian noise  Posterior distribution: bimodal  Point Estimate: EKF vs PF the same, i.e. same RMSE

14. Rickard Karlsson IEEE Aerospace Conf 2007 Example 2: Simulation Results for Range-Only MSE KD No Difference! KD Indicates a Difference! EKF PF EKF PF

15. Rickard Karlsson IEEE Aerospace Conf 2007 Calculating the probability EKF PF&True Probability for target within the circle with radius R

16. Rickard Karlsson IEEE Aerospace Conf 2007 Example 3: Linear -- Non-Gaussian Noise  MSE similar for both KF and PF!  KL is better for PF, which is accounted for by multimodal target distribution which is closer to the truth

17. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Environment in C++  MATLAB  Easy to use  Weak typing  Somewhat slow  Object oriented (not really)  C++  More complicated to use  Fast  Strong typing  Object oriented  Can be implemented ! F++: Fairly easy to use Just provide models f(x), h(x), etc Estimators: EKF, PF, IMM, UKF Open Source code available www.control.isy.liu.se/resources/f++ OOP & Design Patterns

18. Rickard Karlsson IEEE Aerospace Conf 2007 Object Oriented Programming (OOP) Inheritance Encapsulation Overloading

19. Rickard Karlsson IEEE Aerospace Conf 2007 Design Patterns – What is it? Smart Pointers Singletons Object factories … “Design patterns are general, programming language independent, conceptual high level solutions to common problems” Example:

20. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML

21. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML

22. Rickard Karlsson IEEE Aerospace Conf 2007 Class: Model

23. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML

24. Rickard Karlsson IEEE Aerospace Conf 2007 Class: Noise

25. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML

26. Rickard Karlsson IEEE Aerospace Conf 2007 Class: Estimator

27. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML Ex: Linear Gaussian system with KF and MATLAB support

28. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML Ex: Non-Linear Gaussian system with PF and MATLAB support

29. Rickard Karlsson IEEE Aerospace Conf 2007 Particle Filtering in Practice General model Common models for tracking and navigation

30. Rickard Karlsson IEEE Aerospace Conf 2007 F++ A General Filtering Framwork in C++ ModelNoiseEstimatorI/O LinModel MultiModel GenericModel Gauss SumNoise … EKF PF IMM UKF MPF MATLAB XML Ex: Linear Dynamics, Non-Linear Measurements Gaussian noise with PF and MATLAB support

31. Rickard Karlsson IEEE Aerospace Conf 2007 Code: Main Estimation Loop Time Update Meas. Update Estimate TU/MU/Est This works for any estimator! estimate u y filter

32. Rickard Karlsson IEEE Aerospace Conf 2007 Code: Main Estimation Loop Estimator Time Update Meas. Update Estimate This works for any estimator! estimate u y filter

33. Rickard Karlsson IEEE Aerospace Conf 2007 Code: Main Program INPUT MC-loop True/Meas Estimate OUTPUT

34. Rickard Karlsson IEEE Aerospace Conf 2007 Code: Main Program INPUT MC-loop True/Meas Estimate OUTPUT

35. Rickard Karlsson IEEE Aerospace Conf 2007 Code: Input (from MATLAB) Just type f(x), h(x),… Nonlinear? Matrices!!

36. Rickard Karlsson IEEE Aerospace Conf 2007 Example 4: Positioning using IMU and GPS MPF part of next release

37. Rickard Karlsson IEEE Aerospace Conf 2007 Summary Rickard Karlsson Automatic Control Linköping University, SWEDEN rickard@isy.liu.se www.control.isy.liu.se/~rickard Proposed KD as a performance measure General Filtering Software