Tune IMU Fusion performance

[SwapnilPande]

Currently, the Madgwick filter works, but is very slow to respond to changes in the robot orientation. Update parameters so that it updates quickly enough for balancing and other similar tasks

[SwapnilPande]

Mostly tuned now, test with 3-axis tilting rig