robot localization using only GPS and IMU
I want to use GPS and IMU to localize my WAM-V.
The GPS and IMU I used are Hemisphere V103 and Microstrain 3DM-GX5-25, respectively.
I use microstrain_25.launch to launch my IMU from this repo.
Since I want to read both 'GGA' and 'RMC' sentences from my GPS receiver, I didn't use nmea_navsat_driver to launch my GPS, while I referred to the package and completed this one. The vector_gps_node.py will publish not only /fix (with type: sensor_msgs/NavSatFix) but /vel (with type: geometry_msgs/TwistWithCovarianceStamped) topics.
The following two files are my ekf and navsat_transform nodes configuration file.
wam_v_ekf.yaml
wam_v_nav_sat.yaml
And the launch files I ran are
wam_v_ekf.launch
wam_v_navsat.launch
For the links between all frames, statis_tf.launch defines the static transformation between my WAM-V and the sensors.
After all setup, gps_imu_localization.launch turns on the filter and start to update the pose.
I pushed my sensor tower and traversed around a 100-meter square in our campus to test the result, and the following bag was logged. localization_test.bag
The position data from /odometry/filtered seems good while the twist data seems weired (almost zero all the time), I wondered if the twist data from /vel have fuse into the filter or not, or is there anything that I ran wrong?
Thanks you.
I set only velocity of x and y of /vel to true because the GPS received the speed over ground. I used the angular velocity from IMU instead.
The 'linear' twist in my bag is almost zero in all bag time. For example, one message from
$ rostopic echo /odometry/filtered
header:
seq: 281
stamp:
secs: 1532064627
nsecs: 614916086
frame_id: odom
child_frame_id: base_link
pose:
pose:
position:
x: 0.0439098967791
y: 0.0465766089414
z: 4.6586169512e-17
orientation:
x: -4.76634038632e-08
y: -1.00457682814e-07
z: 0.99873880299
w: 0.0502076030221
covariance: [0.23705621692608908, -1.948553713304518e-07, -3.211042843507399e-15, 7.69128559205973e-17, -7.016659155262905e-16, -4.296271258830678e-22, -1.9485537133189546e-07, 0.23705596497155082, -6.319626611416414e-15, 3.389351376770283e-17, -3.0920713101511945e-16, -1.892249709476833e-22, -3.2110428427468623e-15, -6.319626611081266e-15, 4.993822867834366e-07, 8.990013640452487e-20, -4.34598940609993e-19, -2.1269664814804854e-25, 8.91475471176e-22, 3.467902683877184e-23, -1.8507687791485761e-19, 9.950410108674838e-07, -2.6955834579856635e-06, 1.27021331820798e-14, -1.0752241709029304e-22, 5.165295186679301e-23, 5.07370312031434e-20, 9.818024348636693e-12, 9.950585474636392e-07, -4.8901998783387225e-15, 1.331106186670209e-21, -1.4044567207079472e-22, -3.7404809766608605e-19, 9.949686290362372e-07, -6.3993967208974425e-06, 1.0000284199430183e-09]
twist:
twist:
linear:
x: -1.28488571722e-06
y: 2.27016817711e-06
z: -5.75735358866e-22
angular:
x: 3.12629915024e-05
y: -0.0169196098021
z: -0.000954064614961
covariance: [0.0037598143027063827, 2.8670448525224797e-14, -4.680865634469344e-19, -1.0351445899851551e-22, 1.9894169327049684e-25, -2.161844916136863e-22, 2.867044852451675e-14, 0.0037598143027521985, -1.2080053781032023e-18, -2.6796387196790525e-22, 5.149875357385759e-25, -5.596265147549989e-22, -4.680865634469346e-19, -1.2080053781032025e-18, 4.990745723093383e-07, -1.4186406852968232e-23, 2.730775443423136e-26, -2.964368244431797e-23, -2.160681353268554e-25, -5.593555364393698e-25, -2.937152821480802e-26, -6.513806285925216e-06, 1.3895180093342896e-08, -1.4117143291187117e-05, 1.1269814495359424e-22, 2.9175244888896195e-22, 1.5316792408755735e-23, 0.0037832272066786153, -6.513806354746521e-06, 0.00761925568471093, -3.324888360174464e-34, -8.577600280788058e-34, -7.044456178721454e-35, 7.678061987700381e-16, -1.5249302083239389e-18, 9.999941155233127e-10]
---
which shows that x and y velocity are almost zero, while my walking speed is about 0.7 meter per second.
