Rosanswers logo

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 ?


Originally posted by ros_geller on ROS Answers with karma: 92 on 2015-10-05

Post score: 1


1 Answer 1


Rosanswers logo

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.

Originally posted by Tom Moore with karma: 13689 on 2015-10-06

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by ros_geller on 2015-10-06:
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?

Comment by Tom Moore on 2015-10-07:
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).

Comment by ros_geller on 2015-10-08:
Ah ok, thanks! Does this work well in your experience? I mean, does the state estimation improve upon not having it as an input?

Comment by Tom Moore on 2015-10-09:
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.

Comment by Tom Moore on 2016-05-08:
Update: check out the indigo-devel branch on the main repo. I've added a control term.

Comment by Nick S on 2016-09-01:
Could you provide an example of how to provide the control input in a launch file?

Comment by Tom Moore on 2016-09-01:

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

Comment by cxxzju on 2017-03-21:
good! I am testing it

Comment by Marvin on 2018-01-05:
Ref' comment Oct 8, 15 regarding using a TwistWithCovarianceStamped message to input cmd_vel inputs to the robot_localization node. What would be a reasonable covariance matrix and how would it be derived? An explained example would be most useful.

Comment by Tom Moore on 2018-01-05:
I'd start here:



Your Answer

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