ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
OK, so there are really two questions here:
For (1), the EKF's cycle looks like this:
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.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.t_m0
, and update the EKF state. Now t_state
= t_m0
.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.