During the last two weeks, the Kalman filter for BallE has evolved. We now have the mathematical model as well as the Matlab implementation. 3 crucial questions were raised before reaching this point.
System and measurement equations:
x_k= F_(k-1)*x_(k-1) + G_(k-1)*u_(k-1) + w_(k-1)
where v_k and w_k are the measurements’ noise and the process’ noise respectively
X_k describes the state of the system at moment k, Y_k the measurements, F_k the transition matrix, H_k the measurement matrix, u_k the command (in our case we consider we don’t have any command applied for the time being)
1) What type of Kalman filter should we use?
After a literature review and the study of the possible filters, I decided that the Sequential Kalman Filter (SKF) was the most appropriate for our problem: SKF does not require matrix inversion for computing the gain matrix (K). However, in order to use this type of filter we must be sure that the measurement covariance is a diagonal matrix.
2) Knowing that we obtain data from the accelerometer, gyroscope and magnetometer, how can we model the state? How should we deal with the gyroscope’s drift?
To answer this question, I first analyzed filters with 6DOF where the state is defined by X_k= [alpha bias]k (as presented here: http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data ). However, as we have the magnetometer we can use only the accelerometer and magnetometer to compute the Euler angles (Y_k = [roll pitch yaw]k ). We can put as well the bias of the gyroscope in X_k but what exactly should define the state of the system?
Starting on this path, we searched more information about the approach used by Copterix last year (http://copterix.perso.rezel.net/?page_id=26). After studying “Airborne attitude estimation using a Kalman filter”, the Master thesis of Matthieu Marmion(1), Richard Murray’s lectures on Sensor Fusion and Factorized Quaternion Algorithm, we used the following model:
Y_k=[roll pitch yaw]k
F_k the same as the one presented in (1)
H_k the Jacobian of the Euler angles in fonction of the quaternion
3) Do we really need to compute H_k or can we find a simpler form for it?
We know the conversions for Euler to quaternion/ quaternion to Euler, therefore we can write directly the Euler angles in fonction of the X_k. However, we will further need H_k for the filter (for computing the gain matrix and the a posteriori state estimate and a posteriori state covariance). I computed the Jacobian and implemented the whole approach in Matlab.
After testing it with real data obtained from our IMU, the filter didn’t give satisfactory results…Once I finished my code and after calibration (measurement noise, initialization, time step) and after performing the tests I tried to understand where the problem might be. Yesterday I looked at the Copterix implementation and noticed that the set Y_k= [quaternion]. This simplifies enormously H_k. If we take Y_k=[quaternion]k we obtain H_k=[O4x3 I4]. However I wonder if the measurement noise covariance is still diagonal in this case.
Therefore, after today’s presentations I did the necessary changes to my code to work with H_k=[O4x3 I4]. The results are better but the filter doesn’t seem to work properly yet…