Integrate IMU feedback into PID control loop

[jasonappah]

Objective
Use IMU yaw/accel/gyro to provide feedback for heading and straight-line controllers.

Acceptance Criteria

• IMU data fused (e.g., complementary/Madgwick) to stable yaw estimate
• PID loops consume filtered orientation and/or linear acceleration as needed
• Drift mitigation approach documented

Work

• Add IMU driver and calibration routine
• Implement filter; expose stable yaw rate and angle
• Wire into PID and add diagnostics

Artifacts

• Calibration steps doc
• Sample logs of raw vs filtered signals

Risks/Notes

• Sensor mounting and magnetic environment sensitivity.