Script for Magnetometer Calibration

This is the code that you can use in the R stats package to work out the magnetometer calibration. You need to import two libraries into R, the rgl graphics library and minpack.lm for the Levenberg-Marquardt optimisation library.

The input is a csv file of the output from the AHRS software in CALIBRATE_MAG mode, as previously described. The output is all on screen – graphs in their own windows and data at the R console.

library("rgl")
library("minpack.lm")
# Load data - rows are vectors
magData <- read.csv("magnetometer_data20.csv")

# Split data into two matrices, columns are vectors
magM <- t(magData[,1:3])
rownames(magM) <-c('mx', 'my', 'mz')
R <- t(magData[,4:12]) / 1e5
rownames(R) <- c('xx', 'xy', 'xz', 'yx', 'yy', 'yz', 'zx', 'zy', 'zz')

# Plot on screen
plot3d(t(magM), aspect = c(1,1,1), box = FALSE, col="violet")

# use arithmetic mean to get starting estimate for centre of ellipsoid
pStart <- list(a = 1, b = 0, c = 0, d = 0, e = 1, f = 0, g = 0, h = 0, i = 1, 
 B_x = mean(magM['mx',]), B_y = mean(magM['my',]), B_z = mean(magM['mz',]),
 eb_x = 1, eb_y = 0, eb_z = 0)

# Plot starting estimate for visual check:
rgl.spheres(pStart$B_x, pStart$B_y, pStart$B_z, 20, alpha = 1, col = "green", box = FALSE, add = TRUE)

# Optimize over a matrix M, offset B and and external field eb:
residFun<-function(p, m, r) { 
 Rieb_x = p$eb_x * r['xx',] + p$eb_y * r['yx',] + p$eb_z * r['zx',] 
 Rieb_y = p$eb_x * r['xy',] + p$eb_y * r['yy',] + p$eb_z * r['zy',]
 Rieb_z = p$eb_x * r['xz',] + p$eb_y * r['yz',] + p$eb_z * r['zz',]
 M = matrix(c(p$a,p$b,p$c,p$d,p$e,p$f,p$g,p$h,p$i) ,nrow = 3, byrow = TRUE)
 B = c(p$B_x, p$B_y, p$B_z)
 Rieb = matrix(c(Rieb_x, Rieb_y, Rieb_z), nrow = 3, byrow = TRUE)

 residV <- (M %*% Rieb + B)
 return (colSums((residV - m)^2))
}

# Perform optimisation, increase maxiter for more iterations if necessary
nls.out <- nls.lm(par = pStart, fn = residFun, m = magM, r = R, control = nls.lm.control(nprint = 1, maxiter = 50))
summary(nls.out)

M <- matrix(coef(nls.out)[1:9], ncol = 3, byrow = TRUE) 
B <- c(coef(nls.out)[10:12])
eb <- c(coef(nls.out)[13:15])

# Subtract B from every column then premultiply by inverse of M
magRSR <- solve(M) %*% (magM - B)

# and plot
rgl.open()
bg3d("white")
plot3d(t(magRSR), col = "blue", scale = FALSE, box = FALSE, xlab = 'x', ylab = 'y', zlab = 'z')

# best spherical fit
sphereRadius = mean(sqrt(colSums(magRSR^2)))
rgl.spheres(0, 0, 0, sphereRadius, alpha = 0.2, col = "green", box = FALSE, add = TRUE)

# Plot Earth field estmates in Earth frame
# note: R times a vector measured in body frame gives you earth-frame representation
N = matrix(c( magRSR[1,] * R['xx',] + magRSR[2,] * R['xy',] + magRSR[3,] * R['xz',] ,
 magRSR[1,] * R['yx',] + magRSR[2,] * R['yy',] + magRSR[3,] * R['yz',] ,
 magRSR[1,] * R['zx',] + magRSR[2,] * R['zy',] + magRSR[3,] * R['zz',] ), nrow = 3, byrow = TRUE )
plot3d(t(N), col = "red", box = TRUE, add= TRUE, alpha = 1.0)

# Yellow blob for mean Earth field vector estimate
plot3d(t(eb), col = "yellow", type = "s", radius = sphereRadius * 0.0174, alpha = 0.7, add = TRUE)

# Green blob at "south pole"
rgl.spheres(0, 0, -sphereRadius, radius = sphereRadius / 25, alpha = 1, col = "green", add = TRUE)

# Examine angular deviation from b0 in z-plane
Dev = matrix(c( 57 * asin((eb[1] * N[2,] - eb[2] * N[1,]) / (sqrt(eb[1]^2 + eb[2]^2 ) * sqrt(N[1,]^2 + N[2,]^2)))))
plot(Dev)
# RESULTS:
# This is the mag offset:
B
# This is the rotation matrix from magRaw to body-frame calibrated B-field measurement:
solve(M) / solve(M)[1,1]
# And this is the magnetic field vector at the calibration site,
# relative to the startup datum for the calibration run:
eb <- eb / sqrt(sum(eb^2))
eb
# the magnetic declination (angle of dip) at your calibration site was measured to be (degrees)
180 * asin(eb[3]) / 3.14159

Configuring the WiFly with Levi’s own App

It turns out that Levil have an iPhone/iPad app for use with their G Mini AHRS, and one of its features is the ability to reconfigure the WiFly module to connect via UDP. I tried it the other day and it happily reconfigured the WiFly module attached to my AHRS to connect via UDP too. Which is handy.

Kalman Filters: yea or nay?

If you look through the academic literature on this topic you won’t get very far without reading about Kalman filters, Extended Kalman Filters, Unscented Kalman filters and so forth. Kálmán himself was a Hungarian engineer and mathematician who came up with some great ideas about how to to use the output of noisy measurements to get a better understanding of what they were actually measuring (looking past the noise, so to speak.) and his theories were put to use originally in NASA’s Apollo programme, allowing the Apollo guidance computers to make best estimates of the orientation and position of the spacecraft from gyroscopic data and observation of the positions of various stars. Since then Kalman filters have found applications in all sorts of dynamic control systems; we really couldn’t do without them.

A basic Kalman filter will let you take a linear system measurements of which are affected by a Gaussian random noise and filter out as much of the noise as possible. An Extended Kalman filter does the same for a non-linear system by linearizing around the current state, an Unscented Kalman filter uses an Unscented Transform to get a better estimate on the statistics of the model and there are many further extensions of these techniques designed to give better results in given circumstances. My inexpert opinion is that they have limited applicability to our problem for three main reasons:

• The errors in the output come from things like thermal drift and unknown accelerations, rather than from random processes
• The computing power we have available in the Arduino hardware is very limited. Kalman filter complexity scales as the cube of the number of state variables, and we have a lot of variables we could include: three body angles, three rotation rates, three orthogonal accelerations, and three orthogonal magnetic field measurements.
• The simple algorithms which don’t involve Kalman filters, like the ones described by Mahony et al. work very well in practice, so there doesn’t appear to be a requirement for anything more complicated.

Imagine using a Kalman-type filter for the roll and pitch measurements. We’d apply as inputs to the filter the roll and pitch rates measured by the gyroscopes and the accelerations sensed by the accelerometer, and we’d get an output that removed as much of the noise from the gyroscopic measurements as possible and followed closely the orientation that kept the acceleration in a vertical sense.

However – this is almost exactly what we don’t want to do. The gyro outputs aren’t very noisy at all. And more importantly we want a correction towards the local acceleration that’s as slow as possible, not as fast as possible. If the aircraft enters a steep turn for thirty seconds the local gravitation acceleration no longer matches the correct “down”, and a Kalman filter will shift the horizon to match in very short order – exactly the wrong thing to do.

For the yaw measurements, which again come from the angular rotations rates but are stabilized by the measured direction of the external (earth’s) magnetic field, there may be an application for a Kalman filter. I want the heading output to track the measured field, which is a noisy measurement, and a Kalman filter will let me smooth out the noise by incorporating the gyroscopic information about how fast I’m turning. I can use just two state variables, heading, and turn rate, which means the mathematical complexity is very low. A second simplification is that I get direct observations of each state variable by itself rather than in combination.

There are, however, two difficulties with this idea: firstly the heading angle has a discontinuity at zero. Go one degree west of zero and my heading is 359, not -1. That’s going to cause problems when I calculate the residual terms, the difference between the estimator and the most recent observation. I could get a huge error angle of nearly 360 degrees. The second issue is that the heading angle and the rate of turn – the two observations that I need to put into the filter – should be specified in the Earth frame, that is, relative to the surface of the planet. But all my measurements are in the aircraft frame. From the Direction Cosine Matrix I can distill the heading angle and the turn rate, in fact I do it already as part of the output. But one of the features of the Kalman filter is that I need to update my state variables with the output of the filter, so I’d have to find a way to input a new heading angle and a new turn rate (both in the Earth frame) into the Direction Cosine Matrix without changing the existing roll and pitch angles and roll and pitch angular rates. I’d have to change the entire matrix. That would be possible, but inolve a lot of trigonometrical operations.

Instead what I chose to to was this: Magnetic measurements are made at 20Hz and for each measurement I run an update cycle of the Kalman filter; at the same time I tear down the old DCM and build a new one. I use the previous roll and pitch angles, but replace the old heading angle with the most recent prediction of the Kalman filter. I let the Kalman filter free-wheel between magnetic measurements, effectively ignoring the output. The DCM is updated at 200Hz but using only the raw gyro measurements, whose noise is acceptably small. So I will grow a slight discrepancy – some fractions of a degree – in the twentieth of a second  before the DCM is next rebuilt. But when I rebuild it I reset the discrepancy to zero. So it’s a kind of hybrid system for yaw stabilization: kept in close sync with the Kalman-smoothed output but without the mathematical complexity of trying to merge the Kalman output back into the DCM.

Rebuilding the DCM is a little costly in computational terms but it has one advantage: I no longer need to renormalize; by rebuilding it from roll, pitch and heading angles I automatically ensure that the orthogonality and other constraints in the DCM are met. So I make up a little of what I lost.

To get around the discontinuity at zero I use another trick. Inside the code I measure the magnetic field in the aircraft frame and then rotate it (by using the DCM) to the earth frame. It should always point exactly North. To the extent that when rotated to the Earth frame it doesn’t appear to point north, that is the error, or in Kalman terms, the residual, for the heading angle state variable. Rather than obtaining the residual by subtracting the estimator for the heading angle from the observation (and possibly coming up with an angle of nearly 360 degrees) I get the residual by rotating the measurement in space and see how far it is from where I expect it to be.

How does the code work?

Let’s move on to the more interesting parts of this project: turning a circuit board with some sensors and a microprocessor into a thing that can tell which way is up, and which way is North.

The sensors on the board tell us three things:

1. The gyroscopic sensors tell us how fast we’re rotating about each of three different axes
2. The accelerometers measure the local acceleration. That’s usually (but not always) the force of gravity
3. The magnetometers measure the local magnetic field.

There are lots of ways to describe how we put this information together. The thirty-second explanation is this: we’ll integrate (mathematically) the rates-of-turn about the three axes to tell us the angles we’ve reached, we’ll assume that the local acceleration averages to “down” to correct errors in the integration, and finally we’ll use the magnetometer data to tell where North is.

At this stage I’m going to point you towards this document written by William Premerlani. Premerlani is a leading light amongst hobbyist designers and builders of autonomous aerial vehicles and he’s written a very clear explanation of the basis of how an AHRS works. You can find out more about what he does at his page on the DIYDrones website. If you follow it through you should get a good understanding of what the Direction Cosine Matrix is and how it connects to our problem. Building and maintaing an accurate DCM is the problem our software has to solve.

The software that I’ve worked on started with code that implements a version of what he (somewhat inaccurately) describes as the “Direction Cosine Matrix algorithm”, and itself is based on theory described in some academic work by Robert Mahony. If you’re interested in some of the theory I also recommend reading some of Sebastian Madgwick’s works, such as this one.

If you’re using the Mongoose board you can download some sample software that implements the “DCM algorithm”, and that’s in fact what I started with. Over the last month or so I’ve extensively rewritten and refactored the code to make it clearer and to implement some different algorithms, but it maintains the same flow structure.

Among the differences I’ve implemented are the following:

• I’ve added some C++ classes to represent matrices and vectors, so matrix multiplications, rotations of vectors, dot- and cross- products and the like can be done in a single line. That reduces a lot (all?) of the clutter.
• Mahony (and Premerlani, by extension) advocate a proportional & integral (PI) feedback mechanism for roll and pitch stability; Madgwick prefers a fixed feedback. I also prefer the fixed rate roll and pitch feedback, with some caveats, as it is more appropriate to our requirements (I can say more about why, later.) We can therefore talk about a fixed “slew rate” at which we reorient our vertical vector, rather than a rate that’s proportional to the error.
• In order to minimise the sensitivity to lateral accelerations we want to reduce the slew rate as much as possible. Since the slew rate is what corrects for gyro drift we must make the gyro drift as low as possible. I choose to measure and store in EEPROM the null output from the gyros at startup time which requires the unit to be absolutely still. (In order to allow for warm-starts, if motion is detected then previously stored null output values are used instead.) I also model the temperature dependence of the null outputs as a linear function of the gyro chip die temperature and include correction for this. The gyro drift measured experimentally is reduced to a maximum of 7 degrees per minute over a range of temperatures from -10 to 50C, so the slew rate is set to double that, or 0.004 radians per second.
• I include code that makes it straightforward to calibrate the gain of the gyroscopes, so that a real rotation of (say) 10 degrees is correctly measured as exactly 10 degrees.
• Output is in a format comptible with the Levil G Mini, as well as an optional JSON format.
• Since the Mongoose includes a barometric pressure sensor on the board the code calculates the pressure altitude and the rate of climb.
• I’ve included a Kalman filter for the magnetic heading. This is a big topic, and I hope to write some comments on why, and how, later.
• You have the option, by setting a compiler constant in the code, to choose one of two basic modes: in reorientation mode the unit calculates its orientation at startup time and measures everything relative to that. In other words, straight-and-level is defined as how the unit sat at startup. If it was on a 45 degree bank, or a 30 degree pitch – or some combination – that’s the “new straight”. So you don’t have to worry about how the AHRS is mounted. Alternatively, non-reorientation mode means measure angles relative to the circuit board where that is. (More accurately, it means measure the angles relative to the internals of the sensor chips – which might not lie quite flat on the PCB. That brings up another topic of sensor misalignments and their effects, something I also hope to cover later.)
• Calibrating the magnetometers to get an accurate measurement of north is another big topic, which has been exercising me for the last couple of weeks. For now, I include code which gives you information from which you can externally calculate the correct values to insert into the code to get good measurements. That, too, is a topic for later.
• Most of the code is functions which read the sensors. This is effectively library code and has been copied more-or-less complete from other similar projects under the terms of the various licences, such as the GPL. The bit that does the interesting work, is quite short – a couple of pages perhaps.

There is a question about the best way to represent rotations, in the code. Some say that quaternions are better than matrices, and some say that matrices are better than quaternions. If you’re not sure about what either of them are, don’t worry about it. If you know only matrices, quaternions are another way of representing a rotation in space with a set of numbers. A rotation matrix has 9 numbers (and six constraints), and a rotation quaternion has 4 numbers and one constraint. You can switch from matrix to quaternion and back with some trigonometrical formulae and anything you can do with one you can do equally with the other. The differences come down to speed (number of basic operations) and numerical accuracy. Wikipedia has a good comparison. It turns out that quaternions are faster for every operation except calculating rotations of a vector; matrices use half as many operations for that. Since most of the work with rotations we do in the code are rotations of vectors it makes sense to use matrices. Quaternions sound sexy and exotic but actually once you’ve worked through the algebra they’re just as dull as matrices.

Configuring the WiFly module

The RN-XV WiFly module is for the most part a TCP/IP to serial convertor. We use it to be able to stream data from the AHRS to the iPad or tablet. If you want a serial cable connection or a USB connection you don’t need the WiFly module at all. However all the sofware for the iPad that I want to use requires a WiFi connection, so that’s what I’m using.

Overall I found configuring the WiFly module the most unsatisfying part of the whole project. I found it very hard to get the right settings to “stick”, and have on occasion found it nearly impossible to connect to to see why it wasn’t working. The good news is that once it starts to go it seems to keep on going. So I’d give it 4 out of 10 for ease of configuration, but 9 or 10 out of 10 for reliability in the field. So far.

You can find the full instructions for what it can do and how to set it up via a link at the Sparkfun website; here I’ll give a summary of the commands you need to apply. To be able to connect to the XV in the first place I used the ad-hoc networking mode, which you can invoke by powering up with PIO9 pulled high. PIO9 is connected to pin 8, and 3.3V power is at pin 1 which is the pin with the square pad on top. Pin 8 is the one three from the end of the same row. Short those two together with something at power up and look at your list of WiFi networks for a device called something like WiFly-GSX-51. The last two digits will be different in your case.

Associate to it then wait a minute for your laptop to self-assign an IP address on this ad-hoc network. Eventually you should be able either to telnet to the device (address 169.254.1.1 port 2000), or I had more success with Netcat.

nc 169.254.1.1 2000

I’d like to provide you with a transcript of a session where the WiFly is configured but the WiFly I have seems to have a strange quirky mode where instead of acting on any configuration commands it simply echoes them back to the console in a dumb and insolent manner. So I’m recreating this list from memory, the entries after the # are my comments, you don’t need to type them; I hope you have better luck than me (I did say they were temperamental to configure!)

$$$     #enter command mode - should see a response CMD
set wlan join 4     #create an ad-hoc network
set wlan channel 1     #some channel no. required for ad-hoc mode
set wlan ssid Wifly-GSX-01     #can call it what you like, but existing sofware requires Wifly-GSX-xx format
set wlan tx 6     #reduce broadcast power to save battery and reduce regulator load
set uart baud 115200     #baud rate to match Mongoose etc.
set ip address 169.254.1.1     #required by software
set ip netmask 255.255.0.0
set ip dhcp 0     #don't try to use DHCP
set ip protocol 2     #expect incoming TCP connection. If you want to try UDP, set this to 1, or for either, use the value 3
save     #store configured values to NVRAM - don't forget this or you'll have to start over
reboot     #hope that it all worked.

Once you’ve correctly configured it you should be able to:

• Power up the device from cold
• Associate to it via the WiFi SSID Wifly-GSX-xx
• After a few seconds telnet or nc to 169.254.1.1 on port 2000 (very much as you did when you pull pin 8 high at powerup)
• Receive data from the Mongoose or ArdIMU