Magnetometer handling in INS

GPS · Mar 27, 2014

The Tau Labs INS (written by Dale Schinstock) works really well and has been used for various navigation tasks for the last few years. However, one aspect that I have found problematic is the magnetometer handling. The way it works currently is that the Earth's 3D magnetic vector is rotated by the attitude to predict the 3D measurement we should get.

In principle that is a great idea, and on planes probably fine. However, on multirotors that generate a lot of local field distortions from the motors, it is rather problematic. The reason is that as the Z (down) component is altered, it creates a bias in the roll and pitch components of attitude to deal with this. We already know (from years of experience flying the complementary filter) that the gyros and accels are sufficient to stabilize those components. As a work around we generally have just used fairly high variances for the magnetometer. This isn't optimal though, as I've seen the INS bias estimate for the Z axis sometimes destabilize since it isn't sufficiently constrained by the mag when initially converging if things are fairly miscalibrated.

I have been wanting to alter the EKF for some time to only allow the magnetometer to influence the heading estimate, but the path wasn't obvious. Today I decided to sit down and implement this, and I'm fairly happy with the result.

Derivation

We want to measure just the heading aspect of the mag, in order to get rid of the mag influencing attitude. Currently the predicted measurement of the magnetometer is

B¯b=Rbe(q)BeSo we want a version that only depends on the heading component of q. That turns out to be less trivial that it sounds. To achieve this, we essentially want to only work with magnetic information projected into a plane parallel to the earth (called the h plane). The rotation Rbe can be decomposed into two matricies, Rbe=RbhRhe, where Rbh applies the pitch and roll transformation and Rhe applies the yaw transformation. We can rotate the raw mag measurements in order to undo the influence of just the roll and pitch by using the inverse (in this case transpose) of Rbh, Bh=RTbhB, and then only keep the components in x and y.

To calculate both Rbh and Rhe, we can get the equations from the code. I then used this code in matlab and the symbolic toolbox to get the matrix. It took a little bit of massaging afterwards to get an efficient implementation:
Code:
syms q1 q2 q3 q4 real
q = [q1 q2 q3 q4];

q0s = q(1) ^ 2;
q1s = q(2) ^ 2;
q2s = q(3) ^ 2;
q3s = q(4) ^ 2;

R13 = 2.0 * (q(:,2) .* q(:,4) - q(:,1) .* q(:,3));
R11 = q0s + q1s - q2s - q3s;
R12 = 2.0 * (q(:,2) .* q(:,3) + q(:,1) .* q(:,4));
R23 = 2.0 * (q(:,3) .* q(:,4) + q(:,1) .* q(:,2));
R33 = q0s - q1s - q2s + q3s;

roll = atan2(R23, R33)
pitch = asin(-R13)
yaw = atan2(R12, R11)

% compute the rotation matrix that is for the attitude component
% this is the matrix that will rotate from the earth frame to the
% current roll and pitch. The inverse will take the magnetometer
% reading back into a horizontal place
sF = sin(roll); cF = cos(roll);
sT = sin(pitch); cT = cos(pitch);

r = sqrt(R23^2 + R33^2)
sF = R23 / r;
cF = R33 / r;
sT = -R13;
cT = sqrt(1 - R13^2);
Rbe_bh = [cT, 0, -sT;
 sF*sT, cF, cT*sF;
 cF*sT, -sF, cT*cF]
Click to expand...
Read more and see photos via the source link.

Source:
http://buildandcrash.blogspot.com/2014/03/magnetometer-handling-in-ins.html

Log in or Sign up

Magnetometer handling in INS

GPS Registered

Share This Page

Log in or Sign up

Magnetometer handling in INS

GPS Registered

Share This Page

Useful Searches