## **Lab 11** | Tuning EKF ### Design of Autonomous Systems ### CSCI 6907/4907 - Section 86 ### Prof. **Sibin Mohan** ### Spring '25 | SEH 4040 --- ## **Objectives** - Understand the role of **Kalman Filters** in autonomous systems - Learn how **Extended Kalman Filters (EKF)** improve estimation in nonlinear systems - Explore **tuning strategies** for EKF using both empirical and systematic methods - Simulate EKF to visualize effects of tuning --- ## **1. Introduction** State estimation allows robots to track internal states despite noisy or incomplete observations --- In this lab: - We build on MP-3 where EKF was used with Navio sensors - We dive deeper into **EKF** and how to **tune it** for optimal performance --- ## **2. Kalman Filter Basics** Kalman Filters perform two main steps: 1. **Prediction**: Estimate next state using the model 2. **Correction**: Update estimate using sensor data --- ### Key Terms - **State vector (x)**: Quantities like position, velocity - **Process model**: Defines how the state evolves - **Measurement model**: Maps sensor data to state - **Q**: Process noise covariance - **R**: Measurement noise covariance --- ## **3. Extended Kalman Filter (EKF)** Used for **nonlinear** systems. Linearizes the system at current estimate ### 3.1 State Prediction ``` x̂ₖ|ₖ₋₁ = f(x̂ₖ₋₁|ₖ₋₁, uₖ₋₁) ``` - `f`: Nonlinear process model - `uₖ₋₁`: Control input --- ### 3.2 Covariance Prediction ``` Pₖ|ₖ₋₁ = Fₖ Pₖ₋₁|ₖ₋₁ Fₖᵀ + Qₖ ``` - `Fₖ`: Jacobian of `f` --- ## **3.3 Measurement Update** **Innovation**: ``` ẏₖ = zₖ - h(x̂ₖ|ₖ₋₁) ``` **Innovation Covariance**: ``` Sₖ = Hₖ Pₖ|ₖ₋₁ Hₖᵀ + Rₖ ``` --- **Kalman Gain**: ``` Kₖ = Pₖ|ₖ₋₁ Hₖᵀ Sₖ⁻¹ ``` **State & Covariance Update**: ``` x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ ẏₖ Pₖ|ₖ = (I - Kₖ Hₖ) Pₖ|ₖ₋₁ ``` --- ## **4. Tuning Kalman Filters** Goal: Choose the best Q and R to balance model trust vs sensor trust --- ### 4.1 Noise Covariance Matrices - **Q (Process Noise)**: Higher → less model trust - **R (Measurement Noise)**: Higher → less sensor trust --- ### 4.2 Empirical Tuning **Trial-and-error** approach: - Start with small values for Q and R - Tune based on performance/behavior - **Pros**: Simple - **Cons**: Time-consuming, suboptimal --- ### 4.3 Systematic Tuning - **Statistical analysis**: Estimate noise from logs - **Parameter identification tools** (e.g. MATLAB) - **Offline simulation** to optimize RMSE --- ## **5. Simulating EKF** Before hardware deployment, simulate EKF to understand tuning effects. --- **Online Simulators**: - [Wolfram EKF Tuning Demo](https://demonstrations.wolfram.com/TuningAnExtendedKalmanFilter/) - Visualize effect of Q and R --- ## **6. Demo: Tuning EKF** --- ## **7. Summary** - EKF is critical for nonlinear state estimation - Tuning Q and R is vital for good performance - Use both empirical and systematic methods - Simulators help visualize and debug EKF behavior --- ## **References** - [Automatic Addison: Extended Kalman Filter Python Example](https://automaticaddison.com/how-to-implement-an-extended-kalman-filter-in-python/) - [ArduPilot EKF Docs](https://ardupilot.org/dev/docs/extended-kalman-filter.html) - [MATLAB: Tuning a Kalman Filter](https://www.mathworks.com/help/control/ref/kalman.html) - [Wolfram Demonstration: Tuning an Extended Kalman Filter](https://demonstrations.wolfram.com/TuningAnExtendedKalmanFilter/)