0
$\begingroup$

Rosanswers logo

Hi, My system description is as follows: Turtlebot - ros-kinetic - Ubuntu 16.04

I am running two instances of robot_localization, odometry in the first instance and 'odometry and global pose' in the second.

  1. odom--> base_footprint (this is my equivalent of base_link). I'm adding a high noise (std_dev = 1) to the /odom and publishing it as /odom_noisy : frequency: 100 two_d_mode: true

    map_frame: map # Defaults to "map" if unspecified

    odom_frame: odom # Defaults to "odom" if unspecified

    base_link_frame: base_footprint # Defaults to "base_link" if unspecified

    world_frame: odom # Defaults to the value of odom_frame if unspecified

    odom0: /odom_noisy

    odom0_config: [false, false, false, false, false, false, true, true, false, false, false, true, false, false, false] odom0_queue_size: 5 odom0_nodelay: false odom0_differential: false odom0_relative: false

    use_control: true

    Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to

    false.

    stamped_control: false

    The last issued control command will be used in prediction for this period. Defaults to 0.2.

    control_timeout: 0.1

    Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.

    control_config: [true, false, false, false, false, true]

    Places limits on how large the acceleration term will be. Should match your robot's kinematics.

    acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

    Acceleration and deceleration limits are not always the same for robots.

    deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

    If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these

    gains

    acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

    If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these

    gains

    deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]

    [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal

    value (variance) to a large value will result in rapid convergence for initial measurements of the variable in

    question. Users should take care not to use large values for variables that will not be measured directly. The values

    are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below

    #if unspecified. initial_estimate_covariance: [1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

  2. map-> base_footprint (or map-> odom). The pose data (which is the global absolute orientation) has a std_dev of .01 degrees. Given a high level of accuracy in pose data and very noisy /odom_noisy, I expect the EKF to almost completely trust the pose data and output reasonably accurate data. What I'm seeing is a highly oscillating (especially the yaw) output, similar to that of /odom_noisy. As such, removing /abs_orientation doesn't seem to make any difference. The corresponding yaml file is as follows:

    frequency: 100

    two_d_mode: true

    map_frame: map # Defaults to "map" if unspecified odom_frame: odom # Defaults to "odom" if unspecified base_link_frame: base_footprint # Defaults to "base_link" if unspecified world_frame: map # Defaults to the value of odom_frame if unspecified odom0: /odom_noisy odom0_config: [false, false, false, false, false, false, true, true, false, false, false, true, false, false, false] odom0_queue_size: 5 odom0_nodelay: false odom0_differential: false odom0_relative: false

    pose0: abs_orientation pose0_config: [false, false, false, false, false, true, false, false, false, false, false, false, false, false, false] pose0_differential: false pose0_relative: false pose0_queue_size: 5

    pose0_rejection_threshold: 2 # Note the difference in parameter name pose0_nodelay: false

    use_control: true

    Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to

    false.

    stamped_control: false

    The last issued control command will be used in prediction for this period. Defaults to 0.2.

    control_timeout: 0.1

    Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.

    control_config: [true, false, false, false, false, true]

    Places limits on how large the acceleration term will be. Should match your robot's kinematics.

    acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]

    Acceleration and deceleration limits are not always the same for robots.

    deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

    If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these

    gains

    acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]

    If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these

    gains

    deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

    process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]

    [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal

    value (variance) to a large value will result in rapid convergence for initial measurements of the variable in

    question. Users should take care not to use large values for variables that will not be measured directly. The values

    are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below

    #if unspecified. initial_estimate_covariance: [1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

Below are the outputs:

  1. /odometry_map/filtered - published by the second instance robot_localization

    header: seq: 5253 stamp: secs: 760 nsecs: 663000000 frame_id: "map" child_frame_id: "base_footprint" pose: pose: position: x: 0.0442575591845 y: 0.0155869558694 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.384538862254 w: 0.923108803672 covariance: [2.663882074729417, 0.0008610832095776304, 0.0, 0.0, 0.0, 0.026921653227594575, 0.0008610832095776344, 2.6635865448664076, 0.0, 0.0, 0.0, 0.026110560973774358, 0.0, 0.0, 9.993342210219859e-07, 6.797855885527078e-16, 4.2373098980630226e-16, 0.0, 0.0, 0.0, 6.797855885527075e-16, 9.934210138929739e-07, -4.326463119371932e-22, 0.0, 0.0, 0.0, 4.2373098980630226e-16, -4.326463124267019e-22, 9.934210138929745e-07, 0.0, 0.026921653227594475, 0.026110560973774316, 0.0, 0.0, 0.0, 0.9468182423951728] twist: twist: linear: x: 0.0101380887639 y: 0.0387535834841 z: 0.0 angular: x: 0.0 y: 0.0 z: -0.0794096879414 covariance: [0.0009540229033699445, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0009540229033699445, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.934210328992857e-07, 3.0421620013966033e-25, -6.746667100726666e-26, 0.0, 0.0, 0.0, 3.0421620013966033e-25, 9.934210325162797e-07, 7.921164614905649e-32, 0.0, 0.0, 0.0, -6.746667100726666e-26, 7.9257534448641e-32, 9.934210325162797e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.023089536420008897]

  2. /odometry/filtered

    header: seq: 5215 stamp: secs: 760 nsecs: 297000000 frame_id: "odom" child_frame_id: "base_footprint" pose: pose: position: x: 0.0481059004207 y: 0.0277482869621 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.477356029252 w: 0.878709975667 covariance: [2.644794104685194, 0.0007909579721065899, 0.0, 0.0, 0.0, 0.017066758856500826, 0.0007909579721065879, 2.645671616728246, 0.0, 0.0, 0.0, 0.03529150350258051, 0.0, 0.0, 9.995004994888809e-07, 5.3017097166727106e-17, 1.6334620670616377e-16, 0.0, 0.0, 0.0, 5.3017097166727094e-17, 9.95049485090448e-07, -1.7337851303703824e-23, 0.0, 0.0, 0.0, 1.633462067061637e-16, -1.733785070966406e-23, 9.95049485090448e-07, 0.0, 0.017066758856500774, 0.03529150350258047, 0.0, 0.0, 0.0, 0.9383404560967562] twist: twist: linear: x: -0.0149190117811 y: -0.0123008162423 z: 0.0 angular: x: 0.0 y: 0.0 z: -0.0450499681531 covariance: [0.000921183130561197, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.000921183130561197, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.950494900372747e-07, 6.507436387617446e-26, 1.682587054280008e-26, 0.0, 0.0, 0.0, 6.507436387617443e-26, 9.950494896505053e-07, -5.429910242222473e-33, 0.0, 0.0, 0.0, 1.682587054280008e-26, -5.82064332144405e-33, 9.950494896505053e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.022759494263694245]

  3. /abs_orientation

    header: seq: 552 stamp: secs: 0 nsecs: 0 frame_id: "world" pose: pose: position: x: 0.0 y: 0.0 z: 0.0 orientation: x: 0.0 y: 0.0 z: 0.000459218843978 w: 0.999999894559 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.046174061692489e-08]

Edit-1 :

There is a static_transform from world to map.


Originally posted by Karthikeya Parunandi on ROS Answers with karma: 78 on 2018-04-13

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Your abs_orientation message has a frame_id of world. Are you providing a static transform to convert that to map? If the EKF can't transform the input data to map, odom, or base_footprint, it can't use it.

Also, you should turn on two_d_mode. I'd also turn off control as an input until you get the rest working.

EDIT: your time stamps in abs_orientation are also not filled out. Time stamps matter to the state estimation nodes.


Originally posted by Tom Moore with karma: 13689 on 2018-04-13

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by Karthikeya Parunandi on 2018-04-13:
Yes, I have a static transform from map to world. The two_d_mode is also on (Sorry, forgot to copy that in the question). Updated now. Switching off the control doesn't make any difference too. Is there anything more that I could do? Thanks!

Comment by Tom Moore on 2018-04-13:
Edited my response.

Comment by Karthikeya Parunandi on 2018-04-13:
Filling the time stamps did the trick. Thanks a lot!

$\endgroup$

Your Answer

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