Robotics & Autonomy
Kalman / EKF from Scratch
A hands-on state-estimation library: KF and EKF implemented from scratch in NumPy, with seven progressive examples from 1D tracking to sensor fusion, EKF radar and a real drone flight-log case study.
- My role
- Author
- Context
- Open-source tutorial
- Stack
- Python · NumPy · SciPy · State estimation
7
worked examples
KF+EKF
from scratch
Real
drone flight log
Why I wrote it
State estimation is the quiet backbone of robotics: every drone, rover and phone fuses noisy sensors into one belief about where it is. I wanted a version I fully understood, so I wrote the filters myself and built seven runnable examples that each add one idea. The same ideas run through my state-estimation internship work and the GPS-denied UAV project.
The library
Two small classes with minimal dependencies (NumPy / SciPy): a linear KalmanFilter with a Joseph-form covariance update for numerical stability, and an ExtendedKalmanFilter that linearises non-linear models through their Jacobians.
kf = KalmanFilter(
F=np.array([[1, dt], [0, 1]]), # constant-velocity model
H=np.array([[1, 0]]), # we measure position only
Q=np.diag([0.05, 0.05]), # process noise
R=np.array([[9.0]]), # measurement noise
x0=[0, 0], P0=np.diag([10, 10]),
)
for z in measurements:
kf.predict()
kf.update(z)Seven examples, one idea at a time
- A noisy measurement: the problem statement.
- 1D constant velocity: from position alone, the filter also infers velocity. Position RMSE 2.7 → 1.7 m.
- Constant acceleration: follows a manoeuvring target.
- 2D tracking: a 4-state
[x, vx, y, vy]filter along a curved path. Track RMSE 2.2 → 0.9 m. - Sensor fusion: accelerometer drives the prediction, barometer corrects it.
- EKF radar: range and bearing are non-linear in position, so the EKF linearises each step.
- Real flight log: a real ~45-minute tethered-drone flight.
The real-data case study
The last example leaves simulation behind. It fuses the onboard barometer with IMU vertical acceleration, rotated into the world frame with the attitude quaternion, and validates against survey-grade ground truth. The fused estimate stays within 2.7 m of ground truth while raw GPS is 5.5 m off, and the zoom panel shows the filter smoothing real barometer jitter.
