How would you add control commands to robot_localization?

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

ros_geller
92 ●6 ●8 ●15

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 ?

Thanks

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

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

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

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

Comments

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 ( 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 ( 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 ( 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 ( 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 ( 2016-05-08 20:20:54 -0500 )edit

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