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:
---
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 ...
I think I need to reset the ekf_localization when I change the velocity direction. how?
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.filter works well at the beginning but it becomes inaccurate after robot changing direction. I think I need to reset the filter. Right?
No, there should be no need to reset the filter, especially just from turning. Can you describe what you mean by inaccurate?
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.