Kalman Filter In Progress
Kalman Filter Working with Aggressive Bias Correction
With KF Before branching to a SE
Flying in Vicon with Param and H2, IMU at 800Hz, delay compensation and IMU bias correction working (occasionally stalls)