Ask Your Question
2

How to find a good covariance matrix in robot_localizaiton

asked 2016-02-29 19:02:20 -0500

Devin gravatar image

updated 2016-03-01 12:17:15 -0500

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 ...
(more)
edit retag flag offensive close merge delete

Comments

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

Devin gravatar imageDevin ( 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.

Tom Moore gravatar imageTom Moore ( 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?

Devin gravatar imageDevin ( 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?

Tom Moore gravatar imageTom Moore ( 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.

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

1 Answer

Sort by ยป oldest newest most voted
2

answered 2016-03-01 13:49:18 -0500

Tom Moore gravatar image

updated 2016-03-03 07:55:38 -0500

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.

edit flag offensive delete link more

Comments

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?

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

How about reseting kalman filter after turning?

Devin gravatar imageDevin ( 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.

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

Thank you, Tom. It works.

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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2016-02-29 19:02:20 -0500

Seen: 692 times

Last updated: Mar 03 '16