We finally got Kalman filter implemented for gyro pitch reading. Gyros are quite noisy and since the gyro output, which is essentially angular acceleration, has to be integrated to calculate the actual pitch angle, continuous drift is unavoidable. However, pitch angle can be calculated from accelerometer as well. Accelerometer is not affected by drift. Kalman Filter corrects the pitch angle and gyro bias continuously by comparing the pitch calculated from gyro and accelerometer. Below is the image of our IMU arrangement for testing.
Below is the video of the Kalman Filter performance. There is 1-to-1 correspondance between IMU movements and the "KALMAN FILTERED" readings. You can also see that when the IMU is violently shaken (that is when "ACCELEROMETER" readings and display change erratically), the "KALMAN FILTERED" readings are quite stable. Also, you can observe that the "GYRO" readings drift with time from 114 to 106 and goes on decreasing even when the IMU is left undisturbed. The filtered values remain unchanged showing the effectiveness of the Kalman Filter.
Our next step is to implement Extended Kalman Filter for pitch readings and then move onto Kalman Filter implementation for full 6DOF IMU realization. Simultaneously, we have started work on Autopilot.