A message from our CEO about the future of Stack Overflow and Stack Exchange. Read now.

 Bumped by Community user occurred Apr 19 '19 at 17:04 Tweeted twitter.com/StackRobotics/status/1076855072267608068 occurred Dec 23 '18 at 15:00 Bumped by Community user occurred Dec 20 '18 at 17:03 Bumped by Community user occurred Nov 20 '18 at 17:03 Bumped by Community user occurred Oct 21 '18 at 17:00 Bumped by Community user occurred Sep 21 '18 at 16:00 Bumped by Community user occurred Aug 22 '18 at 15:02 Bumped by Community user occurred Jul 23 '18 at 14:47 Bumped by Community user occurred Jun 23 '18 at 14:10 Bumped by Community user occurred May 24 '18 at 13:40 4 added 318 characters in body edited Apr 24 '18 at 13:14 skr_robo 20711 silver badge77 bronze badges 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. 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? 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. 3 added 39 characters in body edited Apr 23 '18 at 15:18 skr_robo 20711 silver badge77 bronze badges 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? 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? 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? 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? 2 deleted 3 characters in body edited Apr 23 '18 at 14:43 skr_robo 20711 silver badge77 bronze badges 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? 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? 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? 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? 1 asked Apr 23 '18 at 14:22 skr_robo 20711 silver badge77 bronze badges