Tilt Compensated Compass

So, as I posted earlier that the magnetic field here in Wisconsin is mostly straight down into the ground. Normal procedure with a 3-axis magnetometer seems to be: use a multi-axis accelerometer to get the pitch and roll angles of the compass, correct the detected 3D vector, then toss out the Z-component and compute the angle difference between the projected sensed vector and the front of the sensor. However, since most of the field is down, the projection seems to me like it tosses out a bunch of resolution. I'm going to try to write down what I think might be an alternative technique that won't trash the resolution by avoiding a projection operation. (Or at least waiting to project until most of the magnitude is not in the direction of projection).

I first want to define the heading plane in 3D space. This plane is normal to gravity and due north is right along its axis. (X or Y. Doesn't matter.) When the robot/compass is heading due north the sensed magnetic vector points sharply down from the plane, northwards and just a touch west. I'll call this magnetic north. Magnetic North's direction is related to due north's by a pitch angle and yaw angle. If you remove the pitch and then the yaw from magnetic north you then have a vector that is in the direction of due north in the heading plane. Let's remember these rotation operations as unpitch magnetic north and unyaw magnetic north.

In general, when the robot is out and about it is pitched and rolled verses the gravity vector and it is yawed versus the magnetic north vector. The idea here is to rotate the sensed magnetic vector into the heading plane where a meaningful angle calculation can occur between the rotated vector and the due north vector. So if we take our sensed vector and unroll the roll detected by the accelerometer, unpitch the pitch detected by the accelerometer, then unpitch magnetic north and finally unyaw magnetic north the resulting vector ought to be in the heading plane and a simple trig operation can be performed to get the robot heading.

In practice it the result won't be in the plane, but it will be close enough to not worry about the Z-component. One reason it won't be quite in the is that getting the exact value of magnetic north is not all that easy. For the time being I will probably use the USGS predicted magnetic north.

Anyway, this was my effort to type up my thinking on this whole 3-axis tilt compensated digital compass thing. Hopefully it makes sense. Hopefully I can make it work in code.