Platform is a Husky A200 with smooth tires, driving on a linoleum over concrete floor, IMU is a Lord Microstrain 3DM-GX5-25. I'm using ROS Melodic on Ubuntu 18.04 (bionic) on an x86 laptop (Asus U46E). The drivers for the Husky are the ones provided by Clearpath on Github, the drivers for the IMU are the ros_mscl library supplied by Lord on Github. At the moment, I think both of these components are working correctly and I have misconfigured robot_localization somehow.
My test procedure was to drive the robot in a square by teleop, and record a bag file (which is attached). I play back the bagfile while visualizing the wheel odometry from /husky_velocity_controller/odom and the fused odometry from /odometry/filtered. I'm trying to tune the process_noise_covariance and initial_estimate_covariance so that the fused odometry shows the robot driving in a square.
What I see in RViz is that the wheel odometry looks very good. The square looks square to within my ability as a driver, and the edges look straight. The fused odometry has straight-ish paths of what appears to be the right length, but the angles of each turn are too small, and the robot appears to swing in the right direction and then swing mostly back before continuing on.
I can't upload images, so a RViz screenshot is at https://imgur.com/a/MLkpB7i red is the wheel odometry, green is the fused odometry.
What I would like to see is that the fused odometry matches the wheel odometry to some degree, showing the robot turning in a square.
I don't have a covariance matrix for the IMU as provided by a datasheet, but the driver provides the ability to explicitly set one and I wrote a node to change it when the bag is playing, so I could try different values. Setting it to 0.01 or 0.001 did not appear significantly different.
I'm not using 2D mode because the eventual goal is to drive outside and do SLAM on uneven terrain, so the roll/pitch/yaw of the robot ends up mattering.
My current localization.yaml is
odom_frame: odom
base_link_frame: base_link
world_frame: odom
two_d_mode: false
frequency: 100
odom0: husky_velocity_controller/odom
odom0_config: [false, false, false, # x, y, z
false, false, false, # roll, pitch, yaw
true, true, true, # x vel, y vel, z vel
false, false, false, # roll vel, pitch vel, yaw vel
false, false, false] # x accel, y accel, z accel
#odom0_differential: true
odom0_queue_size: 10
imu0: ned_imu #imu_converter/ned_to_enu
imu0_config: [false, false, false, # x, y, z
true, true, true, # roll, pitch, yaw
false, false, false, # x vel, y vel, z vel
true, true, true, # roll vel, pitch vel, yaw vel
false, false, false] # x accel, y accel, z accel
imu0_differential: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
#x y z R P Y x_vel y_vel z_vel R_vel P_vel Y_vel x_acc y_acc z_acc
process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.05, 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.001, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.0025, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0004, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 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]
#x y z R P Y x_vel y_vel z_vel R_vel P_vel Y_vel x_acc y_acc z_acc
initial_estimate_covariance: [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, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.005, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 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]
Fused odometry at the end of the run shows the position covariance has grown a bunch (8-10 or so), while the angle covariance hasn't (~4.4e-5).
header:
seq: 16886
stamp:
secs: 1604433259
nsecs: 991405964
frame_id: "odom"
child_frame_id: "base_link"
pose:
pose:
position:
x: -24.2360850036
y: -3.19585717591
z: 0.253668646449
orientation:
x: 0.00448050413206
y: -0.000571877037255
z: 0.999818469957
w: 0.0185101370194
covariance: [8.461155308944512, -3.085013813809409e-07, -1.4569404253909023e-06, 1.9501914057338175e-19, 5.774504551138615e-15, 7.584439603538485e-14, -3.0850138138081206e-07, 8.46115839531135, 3.7929404136743135e-07, 7.428970217642526e-18, -2.135016067522118e-16, 2.0554091034599122e-12, -1.4569404253908593e-06, 3.792940413674474e-07, 10.14967989727414, -8.779029750512138e-18, 6.439895319387443e-13, -7.805430454917561e-19, 1.950191405733817e-19, 7.428970217642527e-18, -8.779029750512138e-18, 4.411994619293552e-05, -2.473864997877824e-15, -1.389364859879805e-10, 5.774504551138615e-15, -2.1350160675221175e-16, 6.439895319387443e-13, -2.4738649978840173e-15, 4.411994554061842e-05, -1.0323582166128937e-12, 7.584439603538483e-14, 2.0554091034599122e-12, -7.805430454917557e-19, -1.3893648598798049e-10, -1.032358216612894e-12, 1.7735512338065326e-05]
twist:
twist:
linear:
x: -6.26275439251e-06
y: 0.0
z: 0.0
angular:
x: -0.00113261547645
y: -0.00208262085874
z: -0.00175177927318
covariance: [0.0009095285209163614, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0009095285209163614, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0006793146412084853, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.7733233582427964e-05, -7.378965545909667e-16, 7.386214325338173e-13, 0.0, 0.0, 0.0, -7.378965545909665e-16, 1.7733233581472372e-05, 9.522500905499948e-13, 0.0, 0.0, 0.0, 7.386214325338172e-13, 9.522500905499944e-13, 1.7732283207454367e-05]
Originally posted by abend on ROS Answers with karma: 3 on 2020-11-04
Post score: 0
Original comments
Comment by johnconn on 2020-11-05:
When just inputting velocity/accel measurements, your covariance will always increase.
You are inputting orientation from your imu, so it's expected that the kalman filter maintains low covariance on it's orientation output.
Are you sure your imu is working correctly? If your wheel odom is giving you distance info, and your imu is giving you orientation info, and you trust the wheel odom, the imu should be suspected.
Comment by abend on 2020-11-05:
I think it's working correctly.
I did have it getting sampled 20x more frequently than the wheel odom, but even with them both at the same frequency (10Hz), I get the same results. I tried graphing the IMU and wheel odom reported yaw velocity, and they agreed well in magnitude and direction, although with the IMU very slightly lagging the wheel odom.
My IMU doesn't report its own covariance matrices for the outputs, but I can specify one to the driver that gets repeated in each message. I haven't had a lot of luck getting different results by changing that, though.
Comment by johnconn on 2020-11-06:
What about the orientation values themselves, not the velocities? You are also adding those absolute measurements from the imu. If the velocities looked good, I would remove those absolute orientation inputs from your kalman filter and see what happens.
Comment by abend on 2020-11-06:
That was it, if you want to make that an answer instead of a comment, I can accept it.
Comment by johnconn on 2020-11-06:
sure I can do that :)