RE: ROS GPS robot pains
Jim DiNunzio
Hi All and (@Mike Ferguson if you are there. I don’t have your email)
I’m trying to get my ROS2 GPS equipped robot to navigate, but have been stuck for a month trying to stabilize and fuse the magnetometer and GPS into ROS messages using an industry standard fusing Madgiwck filter for IMU accel + gyro + Magnetometer magnetic field into consolidated stabilized IMU (accel, gyro, orientation) and Robot Localization nodes (EKF) to fuse the stabilized IMU with GPS and wheel odometry.
I’m still having some magnetometer issues, but I’ve made progress.
The worst problem is a location drift away issue for /odom and /odometry/gps. Please see drifting away webm video attached. When I use a script that simply repeats the same GPS location over and over, then there is no drift.
- I have tried adjusting the noise covariance values in the EKF to favor the sensor values over the internal state model by increasing the values for x,y from the GPS. This helped reduce the drifting considerably, but it still happens, and I’m not sure if I can find a value where it converges to become stable.
- I wrote python scripts to take 1000 (or any n) measurements of each IMU sensor and compute the standard deviation while robot is not moving and covariance values and publish them along with the values.
- I employ a compass calibration routine that takes measurements for 20 seconds (while you rotate the magnetometer in a full circle) of what they call hard iron and soft iron deposits (basically find an offset value to center the magnetic field readings with themselves and scale them to be in the same range)
Bottom line. I’m not sure how to fix this. I tried changing the covariance values from the GPS to be bigger or smaller, but neither helped.
Any help from ROS2 or outdoor GPS navigation experts would be greatly appreciated!
Jim
The rest of this email includes my configuration files and some sample output from the topics. All code is in github.
My configuration files, originally copied and adapted after weeks of research following the ROS2 localization and GPS related web pages.
https://docs.nav2.org/tutorials/docs/navigation2_with_gps.html
https://docs.ros.org/en/melodic/api/robot_localization/html/navsat_transform_node.html
https://docs.ros.org/en/melodic/api/robot_localization/html/configuring_robot_localization.html
https://docs.ros.org/en/melodic/api/robot_localization/html/integrating_gps.html
I use the nmea_navsat_driver to control my GPS:
https://wiki.ros.org/nmea_navsat_driver
I use the MPU 9250 IMU + magnetometer with sparkfun driver:
https://www.arduino.cc/reference/en/libraries/sparkfun-mpu-9250-9-dof-imu-breakout/
Of note which took a long time for me to fully grasp:
Because of the sensor chips layout of the 9250,
I had to switch the Magnetometer readings to the ENU (East-North-Up) navigation frame from NED (North-East-Down) frame by swapping x and y and inverting z magnetic field measurements.
The accel and gyro already use the Front, Left, Up body frame which is standard for ROS2 and compatible with ENU inertial (world) frame. This was necessary for both to line up and be compatible for the Madgwick filter input.
My Robot localization Yaml:
https://github.com/jimdinunzio/trucksaurus/blob/humble/linorobot2_base/config/ekf_gps.yaml
I’ve tried with two-d mode on and off. On seems to compensate when the compass is tilted, but off does not.
My Madgwick filter Yaml:
https://github.com/jimdinunzio/trucksaurus/blob/humble/linorobot2_base/config/imu_filter.yaml
RQT_GRAPH of my setup
Adafruit Ultimate GPS
GPS Readings from /fix which vary a little as expected, but don’t keep drifting away.
rte /fix | grep -i "latitude"
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364666666667
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65364833333334
latitude: 33.65365
latitude: 33.65365
latitude: 33.65365
latitude: 33.65365166666667
latitude: 33.65365
latitude: 33.65365
latitude: 33.65364833333334
latitude: 33.65365
latitude: 33.65365
latitude: 33.65365
latitude: 33.65365
latitude: 33.65364833333334
latitude: 33.65364833333334
^C
jim@Robuntu:~/Documents/git/truck_ws$ rte /fix | grep -i "longitude"
longitude: -117.86949166666666
longitude: -117.86949166666666
longitude: -117.86949166666666
longitude: -117.86949
longitude: -117.86949166666666
longitude: -117.86949333333334
longitude: -117.86949166666666
longitude: -117.86949333333334
longitude: -117.869495
longitude: -117.86949166666666
longitude: -117.86949
longitude: -117.86949
longitude: -117.86948833333334
longitude: -117.86948833333334
longitude: -117.86948833333334
longitude: -117.86948833333334
longitude: -117.86948666666666
longitude: -117.869485
longitude: -117.869485
longitude: -117.86948333333333
longitude: -117.869485
longitude: -117.86948333333333
longitude: -117.869485
longitude: -117.86948666666666
longitude: -117.86948833333334
longitude: -117.86948666666666
longitude: -117.86948333333333
longitude: -117.869485
longitude: -117.869485
longitude: -117.869485
longitude: -117.869485
longitude: -117.86948333333333
longitude: -117.86948333333333
longitude: -117.869485
longitude: -117.86948666666666
longitude: -117.86948666666666
longitude: -117.86948833333334
longitude: -117.86948833333334
[Jim D]
Odom drifts away: (output from EKF #1)
s/git/truck_ws$ rte /odom | grep -i position -A 4
position:
x: 0.06896762975465025
y: -0.0037901593849700116
z: 0.18884388181245676
orientation:
--
position:
x: 0.0705685080622421
y: -0.0036183911695689226
z: 0.19051567123956764
orientation:
--
position:
x: 0.07128769143453731
y: -0.0035566359355475577
z: 0.19207743305710862
orientation:
--
position:
x: 0.07267318519655247
y: -0.0033946281441688394
z: 0.19367009671495947
orientation:
--
position:
x: 0.07359147467047644
y: -0.0033004253574522705
z: 0.19520985246281236
orientation:
--
position:
x: 0.0754844758593558
y: -0.0030748991563973767
z: 0.19684785744338748
orientation:
--
position:
x: 0.07647189829393089
y: -0.002918322019610263
z: 0.19836293927848134
orientation:
--
position:
x: 0.07816628866252899
y: -0.0026366478878085066
z: 0.20001658667237857
orientation:
--
position:
x: 0.07884143662909439
y: -0.0026776323825669288
z: 0.20151423229833607
orientation:
--
position:
x: 0.08002992430549795
y: -0.002816016657517632
z: 0.2030773744519186
orientation:
--
position:
x: 0.08080361583908205
y: -0.0028788284772178533
z: 0.20469037525961717
orientation:
--
position:
x: 0.0826598559214726
y: -0.0028139873800323944
z: 0.2064235878492896
orientation:
--
position:
x: 0.08321077099743308
y: -0.0027947350284388537
z: 0.20797588864068556
orientation:
--
position:
x: 0.0842555820654813
y: -0.0027610444525229014
z: 0.20964501495998275
orientation:
--
position:
x: 0.08468378766478157
y: -0.0027794152766340095
z: 0.21128231949898865
orientation:
--
position:
x: 0.08596734889088409
y: -0.0026680569566273687
z: 0.2129480556766276
orientation:
--
position:
x: 0.08640832710215313
y: -0.002749453134611652
z: 0.21458163310434045
orientation:
--
position:
x: 0.08758283625510063
y: -0.002769121852921556
z: 0.21624700774084404
orientation:
--
position:
x: 0.0880619397061978
y: -0.002850914169904894
z: 0.21787249274167567
orientation:
--
position:
x: 0.08936784481687915
y: -0.0028480914537117774
z: 0.21952657734781372
orientation:
--
position:
x: 0.08994748803500681
y: -0.0029015787185720323
z: 0.22116794094753878
orientation:
--
position:
x: 0.09132310884933709
y: -0.002849691966683679
z: 0.22281927599129206
orientation:
Odometry/gps output from downstream Navsat Transform away too.
jim@Robuntu:~/Documents/git/truck_ws$ rte /odometry/gps | grep -i position -A 4
position:
x: 0.32539586817715427
y: -0.6548911284624968
z: 0.0
orientation:
--
position:
x: 0.328402549016271
y: -0.6512198207279163
z: 0.0
orientation:
--
position:
x: 0.34696523439559035
y: -0.6400588643429961
z: 0.0
orientation:
--
position:
x: 0.3638900683864962
y: -0.6297391981166444
z: 0.0
orientation:
--
position:
x: 0.34107282741298384
y: -0.7708482798191455
z: 0.0
orientation:
^C
And therefore /odom/global also drifts:
jim@Robuntu:~/Documents/git/truck_ws$ rte /odom/global | grep -i position -A 4
position:
x: -12.612060800985494
y: -19.765236818846844
z: -15.848569914137281
orientation:
--
position:
x: -12.611625261014327
y: -19.76534574548266
z: -15.8466596304372
orientation:
--
position:
x: -12.609657236664033
y: -19.76523376176626
z: -15.845407866683956
orientation:
--
position:
x: -12.608811378732822
y: -19.765318396730983
z: -15.843526582528407
orientation:
--
position:
x: -12.607528548571688
y: -19.764920519465814
z: -15.842289041705115
orientation:
--
position:
x: -12.607376596585214
y: -19.764880681100866
z: -15.84041327920116
orientation:
--
position:
x: -13.471405968901065
y: -21.105885240832386
z: -16.401251133727982
orientation:
--
position:
x: -13.47070819482514
y: -21.10594757546258
z: -16.399350939696262
orientation:
--
position:
x: -13.469347731363404
y: -21.105704096298897
z: -16.39809570538825
orientation:
--
position:
x: -13.469020636740764
y: -21.105714173906534
z: -16.396242595374595
orientation:
--
position:
x: -13.467390965123677
y: -21.105681388281326
z: -16.394950515294354
orientation:
--
position:
x: -13.466898816787799
y: -21.105838360078792
z: -16.393093109280677
orientation:
--
position:
x: -13.4651940049392
y: -21.105704453132997
z: -16.391777375589285
orientation:
--
position:
x: -13.464677686463846
y: -21.105801823135526
z: -16.38987334551594
orientation:
--
position:
x: -13.46297316792487
y: -21.10566921273147
z: -16.38855300236142
orientation:
--
position:
x: -14.143890810919515
y: -22.471663915404257
z: -16.952662776181853
orientation:
--
position:
x: -14.142513573377418
y: -22.471442200963118
z: -16.95139983700341
orientation:
--
position:
x: -14.14200688454535
y: -22.471425626334515
z: -16.949509194897427
orientation:
--
position:
x: -14.140206824516179
y: -22.47137080699079
z: -16.948178464688635
orientation:
--
position:
x: -14.139568128258805
y: -22.471503958146943
z: -16.94625298273359
orientation:
--
position:
x: -14.138099491789182
y: -22.47130980782059
z: -16.944977725212432
orientation:
--
position:
x: -14.137823576509291
y: -22.471380222111033
z: -16.94308314112525
orientation:
--
position:
x: -14.135920389151734
y: -22.47122321195525
z: -16.941728690162115
orientation:
--
position:
x: -14.135251844948108
y: -22.471266691277645
z: -16.939794106800175
orientation:
--
position:
x: -14.133747027828482
y: -22.47104123974748
z: -16.938501066258535
orientation:
--
position:
x: -14.763549114192173
y: -23.515081901073838
z: -17.51390038799834
orientation:
--
position:
x: -14.762045808148837
y: -23.514945711107647
z: -17.51260069804261
orientation:
--
position:
x: -14.761392143679519
y: -23.514978717949315
z: -17.510682534155517
orientation:
--
position:
x: -14.759918894514026
y: -23.5148736374915
z: -17.509348766944722
orientation:
--
position:
x: -14.759668194263137
y: -23.514994692625955
z: -17.507439237114774
orientation:
--
position:
x: -14.757829313385765
y: -23.51473926742096
z: -17.506065799217634
orientation:
--
position:
x: -14.75715905404296
y: -23.514732236672383
z: -17.504135343776326
orientation:
--
position:
x: -14.755754507358331
y: -23.51463067251883
z: -17.50286695212106
orientation:
--
position:
x: -14.755595428848883
y: -23.514765245097866
z: -17.50094720277235
orientation:
--
position:
x: -14.754060085885548
y: -23.51461671221511
z: -17.499640611430706
orientation:
--
position:
x: -15.547725910524031
y: -24.34112151700228
z: -18.085352755343145
orientation:
IMU data is reasonable – output from Madgwick filter. Input is magnetometer and raw imu from robot.
im@Robuntu:~/Documents/git/truck_ws$ rte /imu/data
header:
stamp:
sec: 1720829723
nanosec: 862000000
frame_id: imu_link
orientation:
x: 0.06459618864534807
y: 0.0871541111573901
z: -0.008091090234665104
w: 0.9940654040754434
orientation_covariance:
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
angular_velocity:
x: -0.02288654061786842
y: -0.04180830016288134
z: 0.0
angular_velocity_covariance:
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
linear_acceleration:
x: -0.8460406856029294
y: 1.5441889088251628
z: 10.482402181776706
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
---
header:
stamp:
sec: 1720829723
nanosec: 960000000
frame_id: imu_link
orientation:
x: 0.08062682201742968
y: 0.010404580767383697
z: -0.06975128619881195
w: 0.9942463569679479
orientation_covariance:
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
angular_velocity:
x: -0.023419172835884267
y: -0.040609877672345696
z: -0.010249841245442481
angular_velocity_covariance:
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
linear_acceleration:
x: -0.8382568717934191
y: 1.5633490658947267
z: 10.463840779615566
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
---
header:
stamp:
sec: 1720829724
nanosec: 63000000
frame_id: imu_link
orientation:
x: 0.06565902743345743
y: 0.10113108648585065
z: -0.026995842689339727
w: 0.992336948793189
orientation_covariance:
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
angular_velocity:
x: -0.02168811812733276
y: -0.03981092934532193
z: -0.01051615735445044
angular_velocity_covariance:
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
linear_acceleration:
x: -0.8364606070681475
y: 1.5645465757115744
z: 10.484198446501978
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
---
header:
stamp:
sec: 1720829724
nanosec: 160000000
frame_id: imu_link
orientation:
x: 0.08000231843175488
y: 0.01826521130688868
z: -0.07831626998041741
w: 0.9935454559091965
orientation_covariance:
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
- 0.0
- 0.0
- 0.0
- 0.003733504883962753
angular_velocity:
x: -0.02248706645435654
y: -0.03994408739982591
z: 0.0
angular_velocity_covariance:
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
- 0.0
- 0.0
- 0.0
- 3.03999989625936e-08
linear_acceleration:
x: -0.862207068130374
y: 1.5465839284588583
z: 10.465038289432414
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
---
Chris Albertson
linear_acceleration:
x: -0.8460406856029294
y: 1.5441889088251628
z: 10.482402181776706
On Jul 12, 2024, at 6:37 PM, Jim DiNunzio <j...@dinunzio.com> wrote:...
The rest of this email includes my configuration files and some sample output from the topics. All code is in github.My configuration files, originally copied and adapted after weeks of research following the ROS2 localization and GPS related web pages.
compatible for the Madgwick filter input.My Robot localization Yaml:I’ve tried with two-d mode on and off. On seems to compensate when the compass is tilted, but off does not.My Madgwick filter Yaml:RQT_GRAPH of my setup
<image001.png>
--
You received this message because you are subscribed to the Google Groups "RSSC-List" group.
To unsubscribe from this group and stop receiving emails from it, send an email to rssc-list+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/rssc-list/01a601dad4c4%24828eb500%2487ac1f00%24%40dinunzio.com.--
You received this message because you are subscribed to the Google Groups "RSSC-List" group.
To unsubscribe from this group and stop receiving emails from it, send an email to rssc-list+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/rssc-list/01c501dad4c5%24400cd6c0%24c0268440%24%40dinunzio.com.
<20240712_165410.jpg><20240712_165354.jpg>
Jim DiNunzio
x: -0.9927356381667778
y: -0.926872598240152
z: 9.018446430680342
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
x: -0.9843530694488436
y: -0.9142987451632507
z: 8.995693744160235
linear_acceleration_covariance:
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
- 0.0
- 0.0
- 0.0
- 8.660000275995117e-06
---
You received this message because you are subscribed to the Google Groups "HomeBrew Robotics Club" group.
To unsubscribe from this group and stop receiving emails from it, send an email to hbrobotics+...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/hbrobotics/458B3B1F-5674-41FF-9174-D43BF9B24D71%40gmail.com.
Chris Albertson
To view this discussion on the web visit https://groups.google.com/d/msgid/rssc-list/CABrUjRHGCoSTpS58E9K6LdQn5O9MU2c_vnzE9VtOw6AH_OH_sA%40mail.gmail.com.