Closed sbrodeur closed 7 years ago
Adjusted and standardized X-Y-Z axis. In self-reference, Y points straight ahead, X points right and Z points up. In orientation, Y points North, X points East, and Z points up. IMU madgwick (topic /imu/data) gives fairly accurate IMU info now.
I would like to see if we obtain something similar to Figure 7 in the link below after calibrating the magnetometer: http://www.sensorsmag.com/sensors/motion-velocity-displacement/compensating-tilt-hard-iron-and-soft-iron-effects-6475
We suffer from both hard and soft distortion. Hard distortion is more present in the Y axis ( X axis has almost none). We also have the soft-distortion ellipse form. Note that this data was collected with the offsets I made.
I'm not sure if the Madgwick filter we use to fuse mag and acc data takes this soft distortion into account.
It is also to note that we havent removed the bank and elevation angle in the magnetic data like in the article, so that may explain the soft-distortion ellipse we see.
You may be right with this. I think that if we plot the same data in 3D (X-Y-Z), we should be able to estimate the shift to the reference axes, and remove the bank and elevation angle effects. I can do that if you give me the data that was used to generate the figure, including the Z axis data.
mag2.zip Here is the csv and spreadsheet file. The data different from the previous graph since my collected data was overwritten but it gives the same curves and all.
See script for calibration: https://github.com/sbrodeur/ros-icreate-bbb/blob/master/src/imu/scripts/calibrate_mag.py
Usage:
python calibrate_mag.py --input=magPoints_2.csv --display-data
It should display the data in 3D (navigable) and return the transformation (rotation matrix) required for calibration:
INFO:__main__:Using input cvs file: /home/simon/git/ros-icreate-bbb/data/magPoints_2.csv
INFO:__main__:X scale: -0.000020, 0.000018 (range of 0.000038)
INFO:__main__:Y scale: -0.000017, 0.000019 (range of 0.000036)
INFO:__main__:Z scale: -0.000041, -0.000040 (range of 0.000001)
INFO:__main__:Use this rotation matrix:
[[ 1. 0. 0.00988014]
[ 0. 1. -0.07183293]
[-0.00978334 0.07740914 1. ]]
The rotation matrix R should multiply the data x (i.e. xc = R x) directly at the IMU capture. It can be hard-coded or passed as a ROS parameter to the driver node.
Applied corrections and this is the new graph
I applied the offset Y needed as well so its pretty centered. There is a slight ellipse shape, like in the previous graph.
Here is the graph of Z for X and Y , showing that the circle is now "flat"
See last commit efd77a5 for handling soft-iron effects. This requires an additional rotation matrix Rxy and multiplicative factors fx and fy. The idea to correct for the ellipse shape is to apply the rotation Rxy, rescale with fx and fy, then apply the inverse rotation Rxy^T (transposed of the rotation matrix) to get back to the original space.
INFO:__main__:Using input cvs file: /home/simon/git/ros-icreate-bbb/src/imu/scripts/magPoints_2.csv
INFO:__main__:X scale: -0.000019, 0.000018 (range of 0.000037)
INFO:__main__:Y scale: -0.000017, 0.000019 (range of 0.000037)
INFO:__main__:Z scale: -0.000041, -0.000040 (range of 0.000001)
INFO:__main__:Use this rotation matrix to correct for bank angle (z axis):
[[ 1. 0. 0.00988014]
[ 0. 1. -0.07183293]
[-0.00978334 0.07740914 1. ]]
INFO:__main__:Use this rotation matrix to correct for soft-iron effects (x and y axis):
[[-0.78891389 0.61450376 0. ]
[-0.61450376 -0.78891389 0. ]
[ 0. 0. 1. ]]
INFO:__main__:Use these factors to correct for soft-iron effects (x-y plane):
[ 0.93339828 1.07683662]
INFO:__main__:All done.
The new correction works really well! The only thing is the offset on Y that I cant get right, but i think the reason for this is that I apply the offset before the rotations, when I should do it after. I will apply corrections and check it out.
This is the latest calibration. It's practically aligned perfectly and has a good circle shape.
Excellent! We can close the issue when the changes to the offsets will be committed.
Committed. For future reference : a8af559af217208a5e5bfa4e4b416b9da26a271b
While the IMU-calculated odometry follows the reference axes determined below, the odometry calculated from the internal robot sensors switches Y with X axes: https://github.com/sbrodeur/ros-icreate-bbb/blob/master/src/irobot_create/bin/driver.py#L175
I am also able to see that in the visualizations.
See commit ee3e09d for a fix. This will need to be tested with the robot.
Define the X-Y-Z axes on the robot and make sure this is coherent with odometry and IMU output.