cst0
commented
2 years ago Answered my own question here:
"Roadmap: [...] We would like to add velocity to the state of the extended Kalman filter."
So it's something that nobody has tackled because this seems to be working well enough as-is. I'll close since I've answered my own question about this functionality.
I was using this node and poking around the source, when I uncovered something I'm a bit confused by. As far as I can tell, this implementation is accidentally skipping the state-prediction step of the kalman filter.
Starting with the construction of the state model:
https://github.com/ros-planning/robot_pose_ekf/blob/fd6cef32b447e8b344a1111373e515aa2f8bfc50/src/odom_estimation.cpp#L60-L66
So my reading of that is that the system is being modeled as a 6-dof matrix of gaussians, each with a very high covariance. That makes sense, our sensor/state inputs will resolve to x,y,z,r,p,y and a high covariance is a fine initial assumption.
But this is never updated with platform movement, so when we get to the update stage, the only thing this is used for is to introduce random noise: https://github.com/ros-planning/robot_pose_ekf/blob/fd6cef32b447e8b344a1111373e515aa2f8bfc50/src/odom_estimation.cpp#L177-L181
As far as I can tell, no state estimation off of a movement model is ever performed. I think when this was first being written someone intended to do that. This line suggests to me that someone aimed to create a subscriber to
cmd_vel, which would allow for that state update: https://github.com/ros-planning/robot_pose_ekf/blob/fd6cef32b447e8b344a1111373e515aa2f8bfc50/include/robot_pose_ekf/odom_estimation_node.h#L75 But that shared pointer is never accessed, so it doesn't seem to be in use.It's possible that I'm misunderstanding something, but if I'm right, then this implementation isn't actually performing the state prediction step of the kalman filter, which prevents us from fully taking advantage of this algorithm's capabilities.