Added kalman filter test for gyro data filtering
Kalman fixes