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:
[true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false] [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false] [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] [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]
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?
↧