Wrong base_link translation using ekf_localization_node
Hello there! I wan't to use ekf_localization_node to fuse data from odom and IMU in my (2,0) class robot, but I have problem to set proper parameters. At first I wanted to check how covariance matrices affect the result based only on odometry data. And I have problem with translation.
After starting all nodes, when I send linear velocity command it's fine. Also rotation works good. Problem occurs when I rotate robot from the starting pose and then say robot to move in the rotated direction. Frame base_link comming from ekf move in wrong direction. But the "raw" odometry from controler with encoders is good.
Could you explain me, what am I doing wrong? Its hard to explain so I attached the rosbag. Below is my code, and rosbag containing: /robot_0/odom, /robot_0/odom_filtered/ and /cmd_vel.
rosbag: https://drive.google.com/file/d/1CQanWBfs2-Z1P4hsT_xgxT77E0foy65c/view?usp=sharing
Here is video showing how it looks like: https://www.youtube.com/watch?v=NjQIzocDBrQ&feature=youtu.be
tf tree: https://imgur.com/Ir3Rhfe
My launch:
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find ekf_test)/params/ekf_rosbag_params.yaml" />
<remap from="odometry/filtered" to="robot_0/odom_filtered"/>
</node>
<node name="controler_tf_publisher" type="pub_tf_from_odom_topic.py" pkg="ekf_test">
<rosparam param="odom_topic">robot_0/odom</rosparam>
<rosparam param="parent_frame_id">controler_odom</rosparam>
<rosparam param="child_frame_id">controler_base_link</rosparam>
</node>
<node name="ekf_tf_publisher" type="pub_tf_from_odom_topic.py" pkg="ekf_test">
<rosparam param="odom_topic">robot_0/odom_filtered</rosparam>
<rosparam param="parent_frame_id">robot_0/odom</rosparam>
<rosparam param="child_frame_id">robot_0/base_link</rosparam>
</node>
<node pkg="tf" type="static_transform_publisher" name="world_frame_publisher1" args="0 0 0 0 0 0 world robot_0/odom 100"/>
<node pkg="tf" type="static_transform_publisher" name="world_frame_publisher2" args="0 0 0 0 0 0 world controler_odom 100"/>
</launch>
Example odom msg:
header:
seq: 761
stamp:
secs: 1576944830
nsecs: 664253217
frame_id: "robot_0/odom"
child_frame_id: "robot_0/base_link"
pose:
pose:
position:
x: 0.971138656139
y: -0.000453197862953
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.615555694251
w: 0.788093387407
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01]
twist:
twist:
linear:
x: 0.000464193755761
y: 0.00186030799523
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.00590989086777
covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10000000000.0, 0.0, 0 ...