Key Engineering Takeaways
- •The Kalman Filter computes the statistically optimal estimate by weighing prediction uncertainty against measurement noise covariance.
- •Prediction Step uses kinematics to propagate state; Update Step uses measurements (IMU, Encoders) to correct the state.
- •The robot_localization ROS 2 package uses an EKF to produce high-frequency robust odometry.
Prerequisites
- • Probability & covariance matrices
- • Linear algebra
The 2-Step Prediction and Update Cycle Equations
1. Predict Step:
$$\hat{\mathbf{x}}_k^- = \mathbf{F}_k \hat{\mathbf{x}}_{k-1} + \mathbf{B}_k \mathbf{u}_k$$ $$\mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^T + \mathbf{Q}_k$$
2. Update Step:
$$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1}$$ $$\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_k^-)$$ $$\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^-$$
Tags:#Kalman Filter#EKF#Sensor Fusion#IMU#Odometry#State Estimation