Mobile Robotics: Extended Kalman Filter for 2D Localization

A hands-on project to implement and tune an Extended Kalman Filter (EKF) in MATLAB, fusing odometry and sensor data to accurately track a robot's position and orientation.

At a Glance

Situation: Autonomous mobile robots suffer from cumulative errors in their odometry (wheel encoder data), leading to significant drift over time. To achieve accurate localization, they must fuse this internal data with external sensor measurements.
Task: To implement a complete Extended Kalman Filter (EKF) in MATLAB to solve this problem. This involved writing the non-linear evolution model, implementing the EKF's prediction and update steps, and tuning the filter's noise parameters for optimal performance.
Action: I implemented the robot's motion model based on its differential drive kinematics. I then completed the core EKF algorithm, coding the prediction step using the motion model and the update step using measurements from magnetic field sensors. A critical part of the work was the systematic tuning of the process noise ($Q_\alpha$) and measurement noise ($Q_\gamma$) covariance matrices to achieve a stable and accurate state estimate.
Result: The implemented EKF was successful. It significantly corrected the drift from pure odometry, providing a much more accurate and robust estimate of the robot's trajectory across various test datasets (e.g., `circles.txt`, `twoloops.txt`). The final tuned filter effectively balanced the confidence between the robot's motion model and the external sensor readings.

Technical Deep Dive

System Architecture & Design

The project centers on the implementation of an Extended Kalman Filter, a standard algorithm for state estimation in non-linear systems. The EKF linearizes the system around the current state estimate at each time step. The filter operates in a continuous predict-update cycle:

Prediction Step: The robot's state (its posture $[x, y, \theta]^T$) and its covariance (uncertainty) are propagated forward in time using the non-linear motion model derived from the robot's odometry.
Update Step: When an external measurement is available (i.e., the robot's sensor detects a magnet with a known world position), the Kalman gain is calculated. This gain determines how much the prediction should be corrected based on the innovation (the difference between the actual measurement and the predicted measurement). The state and covariance are then updated.

Core Implementation Details

My main contribution was to complete the core components of this EKF framework in MATLAB:

Motion Model (`EvolutionModel.m`): I implemented the non-linear function $X_{k+1} = f(X_k, U_k)$ which describes the robot's evolution. This function takes the current posture and the odometry inputs (distance and angle change) and computes the next posture, forming the basis of the prediction step.
EKF Algorithm (`MagnetLoc.m`): I completed the missing code for the main EKF loop, which involved:
- Calculating the Jacobians of the evolution model ($A_k$) and the observation model ($C_k$) required for linearization.
- Implementing the covariance prediction equation: $P_{k+1|k} = A_k P_{k|k} A_k^T + Q_\alpha$.
- Implementing the state and covariance update equations after a measurement is received.
Filter Tuning (`DefineVariances.m`): This was a critical and practical part of the lab. I was responsible for determining the correct values for the noise covariance matrices. I analyzed the system to estimate the measurement noise variance ($Q_\gamma$) based on the sensor's physical properties (spacing of Reed sensors) and tuned the process noise parameters to account for odometry errors like wheel slippage.

Challenges & Solutions

The biggest challenge in any Kalman Filter implementation is tuning the filter. If the filter is told to trust the odometry too much (low process noise), it will ignore valid sensor measurements and drift. If it trusts the sensors too much (low measurement noise), it can become overly sensitive to sensor errors and produce a jittery, inaccurate estimate.

My solution was a methodical, data-driven tuning process. I started by estimating the measurement noise from the sensor's physical characteristics, as guided by the lab instructions. Then, I iteratively tested different values for the process noise parameters on various datasets. By analyzing the resulting plots (especially the innovation, or error between predicted and actual measurements), I could find a balance where the filter produced a smooth, drift-corrected path that was consistent with both the robot's motion and the ground-truth magnet locations. This hands-on tuning process was key to achieving a robust and accurate localization system.