Sensor Fusion v2

Upgraded from a simple complementary filter to a proper Kalman filter for IMU + time-of-flight sensor fusion.

The complementary filter worked fine on flat ground. It fell apart on stairs — the sharp acceleration spikes during step transitions caused the orientation estimate to drift badly enough to destabilize the gait controller.

The Kalman filter's explicit noise model handles the transient accelerations correctly by treating them as measurement noise rather than signal.

Tuning Q and R matrices was empirical: ran the robot over a 15cm step repeatedly, logged the state estimates, and adjusted until the orientation error stayed within ±2° during transitions.