Testing and tunning imu_filter_madgwick
Hi, I build a 2 wheeled rover. It has an IMU (ICM20602) and a magnetometer (HMC5883). I make code on my uC to read all data and publish it to a ROS master (running on an odroid computer) using rosserial. I would like to tune imu_filter_madgwick in order to get reliable orientation.
I get good result using just IMU data, but as soon as I enable magnetometer the results is worse. I would like some help debuging it.
I attached a bag file for a test. I drive my rover in (almost) straight line for a few seconds, then make a 180º CCW turn while drive forward (I mean it's not a pivot turn). Then drive straight get to initial position. Stop for a few seconds and start a couple of full circle pivot turns. I have odometry, from wheel encoders too.
I'm running:
rosrun imu_filter_madgwick imu_filter_node _use_mag:=false _use_magnetic_field_msg:=true _publish_debug_topics:=true
and
rosbag play --clock test2.bag /chori/imu/data:=/imu/data_raw /chori/imu/mag:=/imu/mag
in another terminal I run rviz and add an IMU object
Without magnetometer data the result seems to be ok. But for some reason adding magnetometer data make a worse orientation estimation.
I need some guidance to debug the problem.
The final goal is fuse odometry (you can see on bag file orientation problems with it) with orientation from imu_filter_madgwick in order to get a reliable odometry data.
Thank!
Hi I am assuming by using "just IMU data", you are referring to the accelorometer and gyroscope values. Mind you, these cannot give "absolute" orientation but rather local orientation (relative yaw change since start of motion). However, a magnetometer works by calculating the heading with respect to magnetic north. Hence, fusing these 2 unrelated values will undoubtly give bad estimations as they are measuring different things in different coordinate frames. Is this what you were trying?
Yes, that is what I'm trying and that is the purpose of imu_filter_madgwick. Aligning GYRO+ACCEL yaw estimation with magnetometer estimation is just a a simple add operation. The problem here is that for some reason I get worse results on some parts of the path when I add magnetometer.
Have you tried visualizing the raw magnetometer estimates on rqt graph and compare them with the estimate from the Gyro+accel? Perhaps you are in an indoor environment where the magnetic field is not strong resulting in high variance magnetometer readings.