ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# Kalman Filter in robot_localization: Why are velocities and accelarations given relative to the robot frame?

I'm currently digging myself through the Extended Kalman Filter of the robot_localization package. And I was wondering why an EXTENDED Kalman Filter is needed. As Tom Moore states in this comment, the velocities and accelerations of the state are given relative to the robot frame, in contrary to the position that is (of course) relative to the world frame. Because of that the dynamics function for the x position becomes unlinear:

X = X + cos(yaw) * cos(pitch) * dt * X_velocity + 0.5 * cos(yaw) * cos(pitch) * dt^2 * X_acceleration


If I would have a state that safes the velocities and accelerations relative to the world frame I would get a linear dynamics function and thus wouldn't have to use an EXTENDED Kalman Filter.

X = X + dt * X_velocity + 0.5 * dt^2 * X_acceleration


So what is the reason for having the state contain the velocities and accelerations not relative to the world but to the robot frame?

edit retag close merge delete

Sort by ยป oldest newest most voted

It's mainly because velocities in ROS are commonly reported in the robot's body frame. The nav_msgs/Odometry message provides different frame_ids for the pose and twist data, and pretty much every odometry source I've encountered produces velocity data in the robot's body frame.

If you remove that assumption in the filter, then you'd have to do the rotations when you receive the sensor data anyway. That would get awkward because of the way we handle measurement queuing. It would also mean we'd have to predict, rotate the measurement using the predicted world state rotation to get the velocity into the world frame, then correct.

In any case, while I'm sure there are other ways to skin this cat, the CPU usage for typical use cases is minute, and the Jacobians are calculated analytically, so we're not having to do any kind of auto-differentiation.

more

## Stats

Seen: 435 times

Last updated: May 24 '18