1 of 4

KF/ EKF - Modelling and tuning noise matrices and other parameters

I am developing C++ code to estimate roll and pitch of a camera using accelerometer and gyroscope. The roll, pitch and yaw are in my state space ($X_t$) and the process is modeled as:

$\bar{X_t} = X_{t-1} + Eu$

Here $u$ is a vector of gyro rates in x, y and z axis while $E$ is the matrix to convert gyro rates to Euler rates. Note that even though yaw is part of the state space, it is not corrected by accelerometer. It is present so that the short term reliability of gyros can be made use of, in the future development of the project.

Now, the covariance matrix($P_t$) is calculated as: $\bar{P_t} = GP_{t-1}G^T + Q$

and the Kalman gain($K_g$) is calculated as: $K_g = PH^T(HPH^T + R)^{-1}$

The $Q$ is the process noise matrix and $R$ is the observation noise matrix. The $G$ is the Jacobian of process model. The observation model $H$ is an identity matrix (This model gets multiplied by state space vector to produce predicted observation. The actual observation contains roll and pitch angles calculated from accelerometer, and yaw angle which simply is a copy of predicted yaw).

I am getting good results in most poses of the camera. However, when one of the angles (roll or pitch) is close to zero, I see drift or zero crossing patterns in the plots of roll or pitch. Can I avoid them by better modeling or tuning of parameters? I would like to know if there are any systematic methods for modeling:

1) The process noise $Q$? I am currently using a Gaussian matrix with mean = 0 and std deviation of 1. I used this as reference. How can I model them better?

2) All diagonal values of $P$ are set to an initial value of 0.05 to represent uncertainty in initialization. The initialization of state space vector is done by calculating roll and pitch values from initial readings of accelerometer. The yaw is initially set to 0. I prefer the accelerometer to be trusted more than gyro. Hence the initial uncertainty, 0.05, is a value lower than the lowest value(0.059) in $Q$. Is this a good approach? Are there better ways?

3) The observation noise $R$? Right now, I have calculated std deviation of accelerometer readings in x, y and z. I use their square as first, second and third diagonal elements of $R$. The rest of the values in the matrix are 0. As I type this question, I have realized that I should first convert accelerometer readings to roll, pitch angles before calculating std deviation. However, are there better suggestions to model this?