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.


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.

  • $\begingroup$ I don't think it's a gimbal lock. Also, I didn't set both roll and pitch to zero. I am talking about either one being zero. When I tested I had roll near zero and pitch at an angle close to 45 degress. $\endgroup$ – skr_robo Apr 24 '18 at 12:58
  • $\begingroup$ I have already calculated standard deviation and coded a Gaussian in C++ as described in the link. I believe Matlab and Excel does the same thing. I want to confirm that part while also searching for better models. I think I need to add better details to the question for clarity. $\endgroup$ – skr_robo Apr 24 '18 at 13:05

Your Answer

By clicking "Post Your Answer", you acknowledge that you have read our updated terms of service, privacy policy and cookie policy, and that your continued use of the website is subject to these policies.

Not the answer you're looking for? Browse other questions tagged or ask your own question.