How to find a good covariance matrix in robot_localizaiton

Question

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:

---
header: 
  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 -->
      <param name="base_link_frame" value="base_link"/>
      <!-- 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,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.03, 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,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.025, 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.02, 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]</rosparam>

           <rosparam param="initial_estimate_covariance">[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, 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]</rosparam>

output of ekf:

---
header: 
  seq: 39422
  stamp: 
    secs: 1315
    nsecs: 64000000
  frame_id: map
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: 492816.442878
      y: 5527539.17343
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.75453971807
      w: 0.65625438197
  covariance: [9.975232789043764e-09, -2.7211403317856447e-13, 0.0, 0.0, 0.0, -2.024543778171654e-07, -2.721140331787132e-13, 9.978358886778933e-09, 0.0, 0.0, 0.0, 4.2968139320611683e-07, 0.0, 0.0, 9.99334221236771e-07, 1.0797297191010833e-14, 2.43905109137402e-15, 0.0, 0.0, 0.0, 1.0797297191010833e-14, 9.986702151855713e-07, -3.955545639126125e-20, 0.0, 0.0, 0.0, 2.4390510913740193e-15, -3.955545639121582e-20, 9.986702151857375e-07, 0.0, -2.0245437781715935e-07, 4.296813932061041e-07, 0.0, 0.0, 0.0, 6.92441896268007]
twist: 
  twist: 
    linear: 
      x: -0.110295949949
      y: 0.487712246076
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -0.0284413512762
  covariance: [1.6395474509339376, 0.36944552744331216, 0.0, 0.0, 0.0, 0.49711919164115514, 0.36944552744331216, 0.08551321315239684, 0.0, 0.0, 0.0, 0.1121471685471008, 0.0, 0.0, 9.990019961300908e-07, -9.63676432645194e-24, 2.5526644039518165e-23, 0.0, 0.0, 0.0, -9.636764326451947e-24, 9.960317146497115e-07, 5.937695665719259e-28, 0.0, 0.0, 0.0, 2.5526644039518177e-23, 5.937688657720329e-28, 9.960317146497115e-07, 0.0, 0.49711919164115503, 0.11214716854710079, 0.0, 0.0, 0.0, 0.20076052394621322]
---

Is there any strategy to find the good covariance matrix?

---

Originally posted by Devin on ROS Answers with karma: 62 on 2016-02-29

Post score: 2

---

Original comments

Comment by Devin on 2016-02-29:
I think I need to reset the ekf_localization when I change the velocity direction. how?

Comment by Tom Moore on 2016-03-01:
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.

Comment by Devin on 2016-03-01:
filter works well at the beginning but it becomes inaccurate after robot changing direction. I think I need to reset the filter. Right?

Comment by Tom Moore on 2016-03-01:
No, there should be no need to reset the filter, especially just from turning. Can you describe what you mean by inaccurate?

Comment by Devin on 2016-03-01:
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.

Answer 1

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.

---

Originally posted by Tom Moore with karma: 13689 on 2016-03-01

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Devin on 2016-03-01:
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?

Comment by Devin on 2016-03-01:
How about reseting kalman filter after turning?

Comment by Arowana on 2016-03-01:
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.

Comment by Devin on 2016-03-04:
Thank you, Tom. It works.