The site is read-only. Please transition to use Robotics Stack Exchange
| ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
 | 1 | initial version |
All three of your IMU inputs are measuring absolute orientation (right now, just pitch). Are you sure they all agree with one another? In other words, if you were to start up your IMU node and plot the pitch angle over time, would they start to diverge from one another? If so, is that divergence captured in the covariance data? If they diverge, then your state estimate is going to jump around between them as it receives each message, unless you get your covariances down.
For r_l to work, you need a pose and velocity reference for every pose variable. In other words, you need to measure X, Y, Z, roll, pitch, and yaw, or their respective velocities (in two_d_mode, this is limited to just X, Y, and yaw). Any pose variable that you don't measure either directly or through its velocity will have its covariance explode rapidly, since the filter adds error at every time step (the process_noise_covariance). That error growth can either be drastically slowed by measuring velocities, or completely bounded by measuring the pose data directly.
Your use case is not typical; r_l assumes you want the entire state vector to be estimated. The correlation between state variables means that huge covariance values for unmeasured variables will affect your measured ones, and vice-versa.
You could always read the three measurements from your IMU sensors, then do a straight average of them and compute the covariance manually. Since they are all the same IMU, their covariance values ought to be the same per IMU, so a simple average is all the EKF would end up doing anyway (if you ignore the state prediction step, that is). An EKF is just doing weighted averaging based on relative covariance values.
Alternatively, you could try publishing all of them to the same topic, then feeding that topic to imu_filter_madgwick.
Finally, if you want to watch the orientation of the sensor in rviz, I think imu_filter_madwick can support that. If not, you can just manually publish a PoseWithCovarianceStamped message using rostopic pub that feeds 0s to the filter for all the unmeasured variables.
ROS Answers is licensed under Creative Commons Attribution 3.0 Content on this site is licensed under a Creative Commons Attribution Share Alike 3.0 license.
