ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

How to find a good covariance matrix in robot_localizaiton

I am using gps data only to compute the position and velocity of robot. In other words, only /fix can be used. I choose utm_odometry_node in gps_common package to transform /fix to /odom and then I feed it to ekf_localization node and it works. In my experiment, the velocity direction of robot will be changed. But the velocity changes slowly from ekf_localization if I change velocity of the robot.Now the question is how to find a good covariance matrix to get the accurate velocity as soon as possible?

input message:

---
seq: 39697
stamp:
secs: 993
nsecs: 600000000
frame_id: map
child_frame_id: ''
pose:
pose:
position:
x: 492824.687572
y: 5527528.38954
z: 0.144843751748
orientation:
x: 1.0
y: 0.0
z: 0.0
w: 0.0
covariance: [1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
twist:
twist:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
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, 0.0]
---


launch file:

<?xml version="1.0"?>

<launch>

<node pkg="robot_localization" type="ekf_localization_node" name="human1_ekf_localization_global" clear_params="true">

<param name="frequency" value="30"/>

<param name="sensor_timeout" value="0.3"/>

<param name="two_d_mode" value="true"/>

<!-- Defaults to "map" if unspecified -->
<param name="map_frame" value="map"/>
<!-- Defaults to "odom" if unspecified -->
<param name="odom_frame" value="odom"/>
<!-- Defaults to "base_link" if unspecified -->
<!-- Defaults to the value of "odom_frame" if unspecified -->
<param name="world_frame" value="map"/>

<param name="transform_time_offset" value="0.0"/>

<param name="odom0" value="/human1/navsat/odom"/>

<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>

<param name="odom0_differential" value="false"/>

<param name="odom0_relative" value="false"/>

<param name="print_diagnostics" value="true"/>

<remap from="odometry/filtered" to="/human1/odometry/global"/>

</node>

</launch>


a part of my covariance matrix:

<rosparam param="odom0_config">[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]</rosparam>

<rosparam param="process_noise_covariance">[1e-4, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    1e-4, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
0,    0 ...
edit retag close merge delete

I think I need to reset the ekf_localization when I change the velocity direction. how?

( 2016-02-29 19:14:40 -0500 )edit

Those covariance matrices are of less interest to me than the input data. Can you please post your entire EKF launch file (please remove the covariance matrices), and then post one sample input message (odom0)? Thanks.

( 2016-03-01 11:49:39 -0500 )edit

filter works well at the beginning but it becomes inaccurate after robot changing direction. I think I need to reset the filter. Right?

( 2016-03-01 12:18:59 -0500 )edit

No, there should be no need to reset the filter, especially just from turning. Can you describe what you mean by inaccurate?

( 2016-03-01 13:13:05 -0500 )edit

It changes very slowly and it is obviously wrong. For example, velocity.linear.x is a positive number but position x is decreasing. I am sure it is accurate and correct at the beginning. But inaccurate after changing direction.

( 2016-03-01 13:24:59 -0500 )edit

Sort by ยป oldest newest most voted

It changes very slowly and it is obviously wrong. For example, velocity.linear.x is a positive number but position x is decreasing.

Two things:

1. I think you may want to review REP-105. The velocity is reported in the base_link frame, which is rigidly attached to the robot. Imagine a robot that is at position (10, 10) and is facing the odom frame origin, and driving towards it. In that case, linear velocity in X will be positive, but the X position, which is reported in a world frame like odom, will decrease.
2. You are not providing any kind of orientation data whatsoever. This means the filter has no idea which way the robot is facing. Furthermore, it means that your yaw variance is going to explode. Just looking at your output message from the EKF, I can see that the yaw variance is already 6.92.

In any case, it seems that you were expecting the EKF to figure out your robot's heading based on differentiated position, and it won't do that. You should have at least a yaw velocity measurement for your setup.

EDIT in response to comment

I'm not sure what resetting the filter buys you. I don't think it's going to solve your problem. One option would be to duplicate your odom0 configuration as odom1, but turn on differential mode for odom1. That will produce a velocity from the pose data, but you're effectively feeding the filter the same information twice.

more

I see. Thank you very much. But I still believe that it is possible to compute the absolute velocity(what I need) by absolute positions(GPS). is it right?

( 2016-03-01 14:23:13 -0500 )edit

How about reseting kalman filter after turning?

( 2016-03-01 14:33:27 -0500 )edit

Cars get inaccurate average velocity over extra long distances. If it is a robot with a smaller average speed I don't think you will get any good speed estimation.

( 2016-03-01 22:58:06 -0500 )edit

Thank you, Tom. It works.

( 2016-03-04 11:41:53 -0500 )edit