Revision history [back]

It really depends on how your state estimation software works. Ideally, you'd want to feed a filter velocity estimates and let it use its kinematic model to integrate those velocity estimates for you. In that case, the velocities can (usually) have static covariances.

However, I believe robot_pose_ekf works a bit differently, as it doesn't integrate velocities directly. For reasons I won't go into here, I believe it takes the difference between a given sensor measurement value and the last value, and adds that to the current overall state estimate, and produces a new measurement that way (robot_pose_ekf maintainers, please correct me if I'm wrong). In that case, I think what you'd really want is for that measurement to have a covariance that is equal to the last state estimate's covariance plus the small covariance generated from the current measurement delta. I'm not sure if that's what robot_pose_ekf is doing or not.

If you'd rather integrate the velocities and not worry about the covariances of absolute position measurements (at least for odometry), I would suggest using the ekf_localization node in the robot_localization package.

It really depends on how your state estimation software works. Ideally, you'd want to feed a filter velocity estimates and let it use its kinematic model to integrate those velocity estimates for you. In that case, the velocities can (usually) have static covariances.

However, I believe robot_pose_ekf works a bit differently, as it doesn't integrate velocities directly. For reasons I won't go into here, I believe it takes the difference between a given sensor measurement value and the last value, and adds that to the current overall state estimate, and produces a new measurement that way (robot_pose_ekf maintainers, please correct me if I'm wrong). In that case, I think what you'd really want is for that measurement to have a covariance that is equal to the last state estimate's covariance plus the small covariance generated from the current measurement delta. I'm not sure if that's what robot_pose_ekf is doing or not.

If you'd rather integrate the velocities and not worry about the covariances of absolute position measurements (at least for odometry), I would suggest using the ekf_localization node in the robot_localization package.

dist_traveled = ::hypot(cur_x_pos - last_x_pos, cur_y_pos - last_y_pos);
x_covariance += dist_traveled * error_per_meter * ::cos(yaw); 
y_covariance += dist_traveled * error_per_meter * ::sin(yaw);