0
$\begingroup$

Rosanswers logo

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?


Originally posted by hobbeshunter on ROS Answers with karma: 3 on 2018-05-24

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Tom Moore with karma: 13689 on 2018-05-24

This answer was ACCEPTED on the original site

Post score: 2

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.