Ask Your Question

How would you add control commands to robot_localization?

asked 2015-10-05 14:40:27 -0500

ros_geller gravatar image

updated 2015-10-05 14:41:18 -0500

How would one go about adding the control input the to extended kalman filter? Normally this is added in the predict phase of the kalman filter as part of the state transition function, x(k+1) = Ax(k) + Bu(k) + w(k).

Say that my control inputs are pitch, roll, yaw ratio and z (height) ratio. My initial thoughts would then be to just add these control commands to the sensor signal in some external code, and then send the fused signal to its original destination, for instance to nav_msgs/Odometry.msg.

Would this work, or is there some built in functionality to do this in robot_localization ?


edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2015-10-06 09:26:21 -0500

Tom Moore gravatar image

There isn't currently support for a control term, but it would definitely be a good feature addition. In the meantime, you could add the commanded velocity as a noisy measurement.

edit flag offensive delete link more


Ok. Do you mean adding it as a seperate sensor with noise, or just increasing the noise matrix for the existing sensor(s) and affected states?

ros_geller gravatar image ros_geller  ( 2015-10-06 13:55:47 -0500 )edit

No, I mean using your cmd_vel (control) as a new input. You'll need to make sure it's a TwistWithCovarianceStamped message, but you can just feed that straight to the filter then (after making its covariance reasonable).

Tom Moore gravatar image Tom Moore  ( 2015-10-07 20:40:32 -0500 )edit

Ah ok, thanks! Does this work well in your experience? I mean, does the state estimation improve upon not having it as an input?

ros_geller gravatar image ros_geller  ( 2015-10-08 01:11:57 -0500 )edit

I'm advocating making your control commands an actual "sensor" input for the EKF. First, your control inputs would have to be velocities, not pose data. Put them into a TwistWithCovarianceStamped message, then use that as an input to ekf_localization_node.

Tom Moore gravatar image Tom Moore  ( 2015-10-09 07:15:28 -0500 )edit

Update: check out the indigo-devel branch on the main repo. I've added a control term.

Tom Moore gravatar image Tom Moore  ( 2016-05-08 20:20:54 -0500 )edit

Could you provide an example of how to provide the control input in a launch file?

Nick S gravatar image Nick S  ( 2016-09-01 00:42:08 -0500 )edit

I added control term support, but haven't yet documented it (apologies).

Tom Moore gravatar image Tom Moore  ( 2016-09-01 02:16:03 -0500 )edit

good! I am testing it

cxxzju gravatar image cxxzju  ( 2017-03-21 11:54:56 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2015-10-05 14:40:27 -0500

Seen: 431 times

Last updated: Oct 06 '15