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

asked 2018-05-24 05:08:47 -0600

hobbeshunter
3 ●1 ●1 ●3

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 flag offensive close merge delete

add a comment

answered 2018-05-24 05:54:50 -0600

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

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.

edit flag offensive delete link

add a comment

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

1 Answer

Question Tools

Stats

Related questions

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

1 Answer

Question Tools

Stats

Related questions

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