Week 8 — Control Systems Implementation
Day 5 of 7 | Difficulty: ⭐⭐⭐⭐
The Problem: No Sensor Is Perfect
- Encoders measure joint angles accurately but don’t tell you where the robot is in the world.
- IMU (accelerometer + gyroscope) measures acceleration and rotation rate, but drifts over time.
- Camera / LiDAR gives absolute position but updates slowly and drops out.
Sensor fusion combines all of them to produce a state estimate better than any single sensor.
The Kalman Filter
The Kalman Filter (KF) is the optimal estimator for linear systems with Gaussian noise. It works in two steps:
Predict Step
Propagate the state estimate using the model:
$$\hat{x}{k|k-1} = F \hat{x}{k-1|k-1} + B u_k$$ $$P_{k|k-1} = F P_{k-1|k-1} F^T + Q$$
Update Step
Correct the estimate using the sensor measurement:
$$K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1}$$ $$\hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}{k|k-1})$$ $$P{k|k} = (I - K_k H) P_{k|k-1}$$
Where:
- $F$ = state transition matrix
- $H$ = observation matrix (maps state to measurement)
- $Q$ = process noise covariance
- $R$ = measurement noise covariance
- $K$ = Kalman gain (blending factor)
Extended Kalman Filter (EKF)
Robot dynamics are nonlinear. The EKF linearizes around the current estimate using Jacobians:
$$F_k = \frac{\partial f}{\partial x}\bigg|{\hat{x}{k-1}}, \quad H_k = \frac{\partial h}{\partial x}\bigg|{\hat{x}{k|k-1}}$$
Then run the same predict/update equations with these local linearizations.
Trade-off: EKF is approximate. For highly nonlinear systems, use UKF (Unscented) or particle filter.
EKF for Robot Localization
State Vector
$$x = \begin{bmatrix} x \ y \ \theta \ v \ \omega \end{bmatrix}$$
Position $(x, y)$, heading $\theta$, linear velocity $v$, angular velocity $\omega$.
Sensors
| Sensor | Measurement | Rate | Noise |
|---|---|---|---|
| Wheel encoders | $v, \omega$ | 1 kHz | Low |
| IMU | $a_x, a_y, \omega$ | 200 Hz | Medium (gyro drift) |
| GPS (outdoor) | $x, y$ | 10 Hz | High (±3m) |
| Visual odometry | $x, y, \theta$ | 30 Hz | Medium (feature-dependent) |
Fusion Architecture
IMU ──┐
├──▶ EKF ──▶ (x, y, θ, v, ω)
Enc ──┤
GPS ──┤
VO ───┘
The EKF runs at the highest sensor rate (1 kHz from encoders/IMU) and incorporates slower sensors (GPS, VO) when they arrive.
Tuning Q and R
These matrices encode your trust in the model vs. the sensors:
- Q (process noise): How wrong is the model? Larger Q → trust sensors more.
- R (measurement noise): How noisy are the sensors? Larger R → trust the model more.
Heuristic: Set $R$ based on sensor datasheets. Set $Q$ empirically — increase until estimates become smooth without lag.
Practical Tips
1. Outlier Rejection
GPS can jump 10 meters in urban canyons. Use a Mahalanobis distance gate — reject measurements where $(z - H\hat{x})^T S^{-1} (z - H\hat{x}) > \chi^2_{threshold}$.
2. Initialization
Start the EKF with a good initial pose from GPS or manual alignment. Bad initialization → slow convergence or divergence.
3. Covariance Reset
If the robot is kidnapped (moved without sensing), reset $P$ to a large value so the filter re-converges quickly.
Key Takeaway
Sensor fusion is how robots know where they are. The EKF is the industry workhorse — it blends fast but drifting sensors (IMU, encoders) with slow but absolute sensors (GPS, vision) into a single, smooth state estimate. Tune Q and R to reflect your trust in each information source.
Next: Day 6 — Python Practice: Full Robot Control Loop in ROS2 + Gazebo