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

Question

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 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? What should be its order of magnitude?

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?

Edit: 1) In the above situation only either roll or pitch are close to zero. Not both.

2) My intention is to code the Gaussian noise models in C++, rather than using readily available functions in Matlab or Excel, if they are the most suitable models. I am also looking for suggestions on better models, if any.

Answer 1

You are suffering from “gimbal lock” when roll and pitch are close to zero. If you want to minimize the effects of that you might consider using quaternions, it is another form of representing angles.

For the standard deviation of the signals is simple. Just give the sensor a fixed input value and read the output. The output has to be a noisy signal around the mean, so you can make a Gaussian fit of that (excel, matlab can do it) and get the standard deviation.

Answer 2

The issue related to zero crossing patterns, which is one of the key questions, was related to the implementation. The issue was resolved and this program was actually completed almost 11-12 months ago. An answer was not posted considering that the main issue was not explained properly.

However, I have recently learned that the interest on this question is geared towards setting the noise matrices. I would like to add my understanding as an answer.

1) The noise is assumed to have zero mean and a variance. The matrix $Q$ represents the variance of process noise and this depends on gyroscope readings. First a large number of gyro readings were taken at a known position and orientation without any motion. The mean and std. deviation of this data was calculated for x, y and z axes. The std. deviations corresponding to the three axes were denoted as $noise\_gx$, $noise\_gy$ and $noise\_gz$.

The last part of this page shows how gyro readings are converted to Euler angle rates.

$\begin{bmatrix}\dot \phi\\\dot \theta\\\dot \psi\end{bmatrix} = \begin{bmatrix}p + qsin(\phi)tan(\theta) + rcos(\phi)tan(\theta)\\qcos(\phi) -rsin(\phi)\\qsin(\phi)/cos(\theta) + rcos(\phi)/ cos(\theta)\end{bmatrix}$

Here $p, q, r $ are gyro rates and $\dot \phi, \dot \theta, \dot \psi$ are Euler rates. This is then multiplied by appropriate time quantity to obtain change in roll, pitch and yaw, required to update the state. A similar logic is used to convert the raw noise values from the gyroscope into values inside matrix $Q$. Substitute $noise\_gx, noise\_gy, noise\_gz$ on the RHS instead of $p, q, r$. Multiply the result with time. This result will have three values. Square the first value and set it as the first diagonal element of $Q$. Then the square the second value and set it as the second diagonal element. Repeat the same for the third value.

The rest of the values are zero. This is done under the assumption that there is no covariance between the different variables of the state. However, this is not always true and should be considered for accurate modeling.

2) The initial value for uncertainty was set based on information on the system and some trail and error.

3) The matrix $R$ was calculated in a method similar to that of $Q$. However, in this case, $noise\_ax, noise\_ay, noise\_az$ were calculated after converting sensor reading to angles (I don't quite remember how this value was obtained for yaw. Will update after checking notes). So, these values were squared and set as diagonal elements of $R$.

So, as a general approach:

1) Take sample data (in large quatity) from the sensor.

2) Calculate std. deviation before or after manipulating the data to bring it to the same space or physical dimensions of the state variables or observation variables, depending on convenience and the implementation.

3) Square the std. deviation values or multiply them to generate variance or covariance depending on assumptions regarding the independence of variables involved.