Specifics of sensor fusion algorithm of robot_localization package

edit

OK, so there are really two questions here:

1. How do we handle updates when we have sensors coming in with different frequencies?
2. How do we update only part of the state vector?

For (1), the EKF's cycle looks like this:

• ROS does a spinOnce, and we collect all the sensor measurements that are waiting in the ROS callback queue. We then put them in a C++ priority_queue, which is ordered by their time stamps. So after all the callbacks fire, we have an ordered measurement queue that is waiting to be processed. If you have different sensor frequencies, you may have, e.g., two IMU measurements and one wheel encoder odometry measurement in the queue.
• We then look at the time stamp of our most recent state/EKF output (t_state), and the time stamp of the first sensor measurement in the queue (t_m0). We compute the time delta, and then we carry out an EKF prediction step over that time delta.
• We then "correct" the prediction using the measurement obtained at t_m0, and update the EKF state. Now t_state = t_m0.
• We then pop the next measurement from the priority queue, and repeat, until the priority queue is empty.
• Finally, we publish the EKF state in a ROS message and via tf

For (2), the EKF always carries out a full prediction for the EKF state at every measurement. In other words, no matter what the sensor data is measuring, we project the full state across the time delta, as described in (1). However, we only correct the state for the variables that were measured by that sensor. We do this by exploiting the state-to-measurement space matrix, commonly referred to as H in the literature. We compute an MxN matrix for H, where M is the number of variables in the measurement, and N is the size of the full EKF state (currently 15). We construct similar sub-matrices for the other relevant EKF matrices, and carry out our correction as in any standard EKF formulation.