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 Karlsson IEEE Aerospace Conf 2007 Motivating Example Range-Only Measurements Two Sensors with range uncertainties Performance? General Software for filtering

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

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

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

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

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?

Rickard Karlsson IEEE Aerospace Conf 2007 Kullback-Leibler Information

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.

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

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

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

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

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

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

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

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 OOP & Design Patterns

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

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:

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

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

Rickard Karlsson IEEE Aerospace Conf 2007 Class: Model

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

Rickard Karlsson IEEE Aerospace Conf 2007 Class: Noise

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

Rickard Karlsson IEEE Aerospace Conf 2007 Class: Estimator

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

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

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

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

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

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

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

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

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

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

Rickard Karlsson IEEE Aerospace Conf 2007 Summary Rickard Karlsson Automatic Control Linköping University, SWEDEN Proposed KD as a performance measure General Filtering Software