I am trying to overcome yaw drift problem by using magnetometer's reading of CF2.0.
First, I did the calibration for hard iron and soft iron by rotating the quadrotor around in all direction in the space I'll use it.
The offset is calculated by (max + min)/2
The radius of each axis is r_x = (max_x - min_x)/2, and the scale for each axis is r/scale_x where r = (r_x+r_y+r_z)/3
The magnetic field in each axis is calculated by m_x = (mag_x - off_x)*scale_x, where mag_x is the raw data from the quadrotor.
First, I tried it simply by heading = arctan2(m_y,m_x) and got quite good result in spanning 360 degrees, with 45-degree steps.
Then the current comes into the play
I tried searching and found from here. They calibrated the effect of current on the magnetic field read from IMU by increasing the thrust gradually and inside their code, they try to find linear model according to the data.
As we cannot get the current directly, I used the value of thrust command sent to the quadrotor instead. Then I experimented by increasing the thrust gradually and obtain the magnetic reading. Plotting the graph of thrust vs mag, I found it is not exactly linear, but maybe better approximated as a quadratic relationship.
So I tried to find this model.
m_x, m_y, m_z is first obtained without current, and set as the base of the field.
Then the thrust is applied from minimum to maximum, with the flie held steady. m_x', m_y', m_z' is obtained.
the effect of the thrust can be seen in m_x' - m_x and so on. then we find the quadratic model between thrust and (m_x' - m_x) and get p_x[0], p_x[1], p_x[2]
(I just state only for x, but the same things are done to y and z)
- thrust vs m_x
- thrust vs m_y
- thrust vs m_z
We can obtain m_x_nocurrent = m_x - (p_x[0]*thrust^2 + p_x[1]*thrust + p_x[2]*(thrust!=0))
Then, these value is transformed back from the sensor's frame to the world frame parallel to the surface by
M_x = -m_x*cos(pitch) + m_y*sin(roll)*sin(pitch) - m_z*cos(roll)*sin(pitch)
M_y = m_y*cos(roll) + m_z*sin(roll)
and the heading angle is obtained from arctan2(M_y,M_x)
(There are still some confusions in the sign of magnetometer's reading and angles.)
To reduce the noise I also tried to average it and reduce the rate to 5 Hz (from 30 Hz).
This is all done on the PC side using ROS, based on the package by Oliver.
I tried flying by using Oliver's controller (modified) to make it hover in a place, and rotate it around (from 0 to -180, then increasing 45 degree step to 180, then 90 and 0).
- magnetic heading (green) and IMU yaw heading (blue)
Also I found that the calibration is a little tricky. I already made a few times of calibration and some days later those calibrated values give a weird result. This is the value I calibrated today. Maybe tomorrow it won't be like this
If I can improve this, and I have more time after my thesis, I will try to include this on the firmware.
Any suggestion for improvement is very very welcome!