robot_localization orientation does rotate randomly (or jump)

[tranqkhue]

The orientation should be consistent with the vehicle (for example, when the vehicle is rotating with a constant angular velocity or standing still). However, in some cases, the robot's tf generated by robot_localization does rotate randomly (or jump). How does this bug happen?

[tranqkhue]

First, try to run without robot_localization and detect any unexpected values from sensors (Odom, IMU, GPS). Make sure the values from sensors seem valid, like output orientation and position.

• For example, with package roboclaw_ros, we need to configure 2 parameters relating to the kinematics of a differential-driving vehicle with encoder feedback: base_width and ticks_per_meter. FYI, refer to https://github.com/tranqkhue/bugcar/commit/5921186f6e47bf44278b673022ded5231cb3827d, https://github.com/tranqkhue/bugcar/commit/8f33c0daad6c2e81d90e03f732e121921ec4e55d, and roboclaw_ros/README.md
• Currently, there is also a bug relating to the bno055 imu_msgs, that is it clockwise positive, while ROS standard is counter-clockwise positive. That could create jumps when fusing another standard ROS message, like the Odom above, when one physical state but two very different sensor reading inputs for the robot_localization

[tranqkhue]

Second, try to check rostopic info tf, as one tf, for example odom->base_link, may be published by more than ONE NODE! That inconsistent could create the conflicts lead to tf jumping!

• In particular, robot_localization fuses odom and imu and publishes odom->base_link transform. However, roboclaw_ros also publishes its own odom->base_link transform (from only odom reading)! In this case, only robot_localization's publishing is allowed!

[tranqkhue]

Also there is a bug in the covariance of sensors, in particular, BNO055, which has been corrected in https://github.com/tranqkhue/bugcar/commit/107958655677b3709454e2c094c82aa23fb08398

• This bug messes up the whole EKF estimation when includes the BNO055 input and also covariance singularity happens (negative value in covariance matrix)
• The bug happens when the covariance matrix is NOT a diagonal matrix. As the sensor data between axes are linearly independent (a very strong assumption in this case), the covariance matrix except the diagonal (which are the variance of each axis itself) should be equal to zero! (Just a caution, mathematically, the covariance matrix of linearly independent values should be a diagonal matrix, but it's different in sensor params of robot_localization).

Just a Probability and Statistics lessons throwback :smiley:

• Moreover, a little mathematics here. Covariance is calculated as If X, Y is linearly independent; then their covariance should be zero. However, as we put all non-zero values into the covariance matrix, they become linearly dependent. That linearly dependent creates singularity. Singularity means that the value of E[X]E[Y] > E[XY], which creates some weird negative covariance, and also exploding covariance phenomena.

Just one more reason to think about Mathematics, more carefully :smiley: