Turning Error Correction

In the last post I wrote about some simulated results for how a mechanical attitude indicator (AI) grows erroneous pitch and bank indications during turning flight.

This is of course well-known to pilots, even if not in detail, and also (certainly in detail!) to the designers of aircraft instruments and the various national aviation authorities.

The United States’ Federal Aviation Administration (FAA) has published a series of Technical Standard Orders which detail what a particular flight instrument has to be capable of in order to be permitted to be installed in a civilian aircraft. The document that applies to gyroscopic attitude indicators is TSO-C4c. It doesn’t say how the instrument has to work internally, instead it gives a list of pass fail tests on instrument performance. Among the critera it lists is 4.5:

THe bank or pitch indicating error resulting from a coordinated turn of 180 degrees in one (1) minute at a true airspeed of 180 mph (156 knots) shall not exceed 3 degrees.

There’s also another interesting comment, in requirement 6.3 (my emphasis):

With the instrument mounted on a level platform and the gyro running at equilibrium an azimuth rotation of the platform of 180 shall result in less than a 1.0 degree change in either the roll or the pitch indication. The rotation of the platform and the 180 degree instrument reading shall be accomplished within a time period of ten (10) seconds. If the gyro’s axis is inclined, for the purposes of turn error correction, there shall be, in addition to the tolerance, a change in the pitch indication equivalent to twice the angle of inclination. 

TSO-C4c demands no more than three degrees of error after a 180 degree turn at standard rate, flown at 156 knots. What slew rate will meet that target? If the maximum error at that point is 2 λ / ω,  then λ must be less than 2 × 8 / 180 = 0.089 (in radians/min) which is 5.1 degrees per minute. A regular AI that has a 5 degree per minute rate will pass but the AHRS code we have at the moment which has a 14 degree per minute rate, won’t.

(As a reminder, remember we need the relatively large correction rate to overcome gyro drift over a wide range of temperatures. We have temperature corrected the gyros to first order, but the remaining error can grow to around the 7 to 10 degrees per minute of drift mark if the temperature varies widely, and we need some leeway to correct integration errors.)

If we’re stuck with a high slew rate what else could we do to improve the turn performance? We could find a way to provide a better estimate of the real direction of the acceleration vector, during a turn. If we knew what component of the acceleration was caused by the turn we could subtract it from the measurement and we’d be left with the gravitation component only. That would mean no turning error at all. So what are our options for doing that?

We could use a GPS unit to measure ground speed, from which we can derive a ground acceleration vector. In a turn this will point towards the centre of the turn. This requires a complex new subsystem, a GPS receiver, which requires a GPS signal and introduces all sorts of new failure modes. So it’s not my favourite idea.

• We could use an air pressure sensor attached to the aircraft Pitot line; then we could measure the dynamic air pressure which when combined with a measurement of the static air pressure (the Mongoose board has an air pressure sensor) will give us an air speed. We could correct this for pressure altitude to get a good estimate of our true air speed. Knowing that airplanes fly almost exclusively in a forward direction we can use the air speed and turn rate to estimate the horizontal portion of the acceleration. This idea has a disadvantage in that it needs a connection to the aircraft Pitot line, which rules it out for certified aircraft. You’re not permitted to connect uncertified hardware to anything, in a certified aircraft. It certainly means the device would no longer be portable since a qualified aircraft mechanic would have to do the connection.
• We could use the magnitude of the measured acceleration vector as a guide: in a turn, the acceleration not only moves off to the side but it gets bigger too. This is one idea I’ve been pursuing; it has its own disadvantages, and I’ll discuss it in a later post
• We could simply guess the airspeed. We could just assume that all turns are flown between 60 and 150 knots, pick a value somewhere in the middle and base a new estimate of the sideways acceleration on the measured rate of turn and that fixed speed. (If we want to be really cheeky then we could pick 156 knots: in the FAA test we’d have no error at all, then.)

Let’s look at the last idea first. Here’s a graph for how the AHRS vertical axis tilts under the following circumstances:

• the AHRS is programmed with a 14 degree per second slew rate
• the AHRS assumes a bank angle which would be correct for an airspeed of 80 knots
• a standard rate turn is flown
• at a range of airspeeds. The graph shows red for 60 knots, blue for 80 knots, green for 100 knots, yellow for 120 knots and magenta for the TSO-C4c specified speed of 138 knots.

This is not too hopeful. At 60 knots and 100 knots the error takes about 15 seconds to stabilize at ±3.3 degrees of bank. There’s no error at 80 knots of course, but at 120 and 138 knots the error is still out of limits.

If we pick a higher assumed airspeed we can reduce the error at higher real airspeeds at the expense of lower. Here’s the result for an assumed airspeed of 120 knots:

At 138 knots the error is just outside the 3 degree limit, the errors for slower airspeeds are considerably worse.

So this scheme doesn’t work so well.

In the next post I’m going to examine how and whether we can use the magnitude of the acceleration vector to tell us something about the turn, to be able to correct the measured acceleration to get a better estimate for the graviational part.

Turning Errors (continued)

In the last post I talked about long-term steady state solutions for the error in the gyroscopic attitude indicator’s axis of rotation (and the consequent error in the indicated bank and pitch) in long duration turns.

In this post I’m going to examine the transient behaviour: how does the AI behave before it reaches a steady state? Does it even reach a steady state? Is the steady state the largest error you can expect to see?

Rather than do this analytically (which I suspect might not be tractable) I wrote a short python script to model the behaviour and produce some graphs. And to simplify even that task I’ve worked in a planar geometry rather than spherical, so the simulations are increasing less valid as the angles involved move away from vertical axis. Nevertheless we should be good for pitch and bank angles up to 20 degrees, say, at least in a qualitative sense. The Python code is included at the end of this post if anyone wants to try it.

The following graphs all simulate an AI with a self-erection rate of 5°/second. If slower, the errors and rates will be smaller than shown in these graphs, and for faster self-erection rate the errors and rates of turn will be larger, but the patterns and qualitative behaviour will remain the same.

Slow turn at 100kts. Tick marks indicate every 10 seconds of flight. After 40 seconds the AI indicates straight and level.

The first example is a slow turn (one fifth standard rate), at 100 knots, with a bank angle of 3.15 degrees. The blue line marks the the path of the gyro axis, and the red dot represents the acceleration vector – and also the bank angle for the coordinated turn. Rather than have the red dot move in a circle around the origin (as in the previous post) I have arranged the red dot to remain stationary, as if the graph itself were rotating with the aircraft. This means the graph corresponds with the pilot’s view rather than the view of an observer on the ground. The graph axes are now pitch and bank, rather than north/south/east/west tilts.

From yesterday’s analysis this particular case is well within the area where the long term error is bank-angle limited. As expected, the gyro axis moves towards the acceleration vector. After a little more than forty seconds (four blue ticks) or 20 degrees of turn, the gyro has erroneously tilted to show a level attitude.

Let’s now increase the rate of turn to 0.30 (of a standard rate).

Once again we are error-limited by bank-angle, and the entire bank angle is eventually nulled by the gyro axis turning. It takes longer than before, and there is a transient pitch error of about a degree that reaches a maximum about 55 seconds into the turn.

Now let’s increase the turn rate to 0.317:

The behaviour starts to change as the speed at which the acceleration vector moves during the turn gets close to the fastest speed at which the gyro axis can tilt. Even after 12 minutes the gyro axis hasn’t caught up.

Increasing the rate of turn to 0.33:

We’ve passed a critical point. The acceleration vector now outruns the speed at which the gyro axis can turn. Only very slightly, but the gyro axis is definitely moving away from the acceleration vector as time goes on.

Turn rate 0.36:

The gyro axis appears to reach a stable point where it lags the acceleration vector by roughly 45 degrees. The green arrow shows what the AI indicates after 12 minutes in the turn: a nose-up pitch of 3 degrees and a bank of 5.7 – 3.5 = 2.2 degrees. The magenta arrow shows what the AI should be indicating: a pitch of zero and a bank of 7.1 degrees. The peak error is bigger than the long term error: the bank error peaks at 100 seconds when the AI shows only 1 degrees of roll, and the peak overall error, which is when the blue line is furthest from the origin, happens at about 130 seconds.

Turn rate o.4:

Now things are starting to get interesting. We’re developing a clear oscillation as the error spirals in towards its long term limit. The error starts at zero, grows at first mainly in bank, then increasingly in pitch, then the bank error  decreases, as does the pitch error, then the bank error increases again and so forth. Over the course of 12 minutes the path of the gyro axis orbits the steady-state point about one and half times.

Turn rate 0.6:

The spiral behaviour of the gyro axis is much more pronounced now. We even have a slight bank error opposite to the direction of turn, from 125 to 180 seconds. The size of the errors has decreased a lot relative to the bank angle and the AI consequently remains proportionately more accurate throughout the turn. This is because the bank angle is bigger and because the aircraft is rotating fast enough that the gyro axis doesn’t have time to drift very far in one direction before the aircraft and the acceleration vector have rotated 180 degrees and the gyro axis drifts back in the opposite direction.

Here’s the result for a standard rate turn, a rate of 3 degrees per second:

At 100 knots the errors are really quite small. The bank error oscillates between positive and negative values and pitch error grows initially to double it’s long term value, then decreases, then increases by not so much, and so on. After 12 minutes the bank error is nearly zero and the aircraft shows a steady nose-up pitch of about 1.5°.

Let’s zoom in and examine this situation for the first 120 seconds:

This is the error we expect to see with a single 360º turn. The error at the end of the turn is down to less than half a degree. If instead we stopped turning at 180º  (six ticks along the line) then the error is about 3 degrees of pitch and close to zero in bank. That’s the error you might expect to see with this AI after flying a course reversal on an instrument approach: about 3 degrees of nose up attitude error.

In the next post I’m going to go back to the AHRS and examine its performance, and also consider the stipulations ofTSO-C4c which is the Federal Aviation Administration’s performance standard for “Bank and pitch instruments” – in other words AIs –  to be approved for use on civil aircraft.

What can we conclude from all this? A few points, I think:

• For turns of standard rate and higher at any airspeed, and for half rate turns at airspeeds greater than 50 knots, the behaviour of the AI is sufficiently limited by the erection rate of the gyro, λ, that the spiral pattern of the gyro axis is the dominant behaviour.
• The long term error is in the AI is given by sin^-1 (λ / ω) which for small λ is approximately equal to λ / ω. It’s therefore proportional to the slew rate and inversely proportional to the rate of turn. The long term error appears almost entirely as a pitch error.
• Approximating the first turn of the spiral by a circle centred on the long term error point, we can see that the maximum error, which is transient, occurs  180° into the turn (or more accurately, just before then.)
• At that point the error is double the long-term error, at 2 λ / ω, and is also almost entirely in pitch.
• The biggest bank error occurs 90° into the turn. It has a size of approximately λ / ω.
• As the turn progresses the errors in bank alternate to each side of the turn, showing an underbank for the first half-turn and then an overbank for the second half.  The pitch error remains nose-up throughout.
• These approximations are usefully accurate during a standard rate turn at 100 knots. The faster the turn rate and the faster the speed of the aircraft, the more accurate they become.

And for those who want to try it out for themselves, here’s the program that produced the graphs on this page:

# simulate AI gyro axis movement in constant rate turn
import math
import numpy
import pylab


l = 5.0             # slew rate of gyro axis, degrees per minute
turnRate = 1       # turn rate 1 = standard rate of 3 deg. / sec
airSpeed = 100.0   # speed in knots
t_max = 720.0       # duration of turn in seconds
steps = 720
ticksEvery = 10    # seconds between tick-marks

rad2deg = 180 / math.pi
deg2rad = math.pi / 180

g = 9.81               # gravity
omega = turnRate * math.pi / 60 # radians per second
v = airSpeed / 1.94   # metres per second
R = math.tan(omega * v / g) # bank angle in degrees
R_deg = R * rad2deg    # bank angle in radians
lam = l * deg2rad / 60  # slew rate in radians per second
delta_t = t_max / steps # time increment between steps
angleTurned = turnRate * t_max * 3

x = 0.000001
y = 0.000001
xx = []
yy = [] 
xxx = [] 
yyy = []

print "Angle of Bank " , R * rad2deg , " degrees"

def r(): return math.sqrt(x**2 + y**2)
def s(): return math.sqrt((R - x)**2 + y**2)
for step in range (0, steps):
    x += delta_t * (lam * (R - x) / s()  -  omega * y)
    y += delta_t * (-lam * y / s()       +  omega * x)
    xx.append(x * rad2deg)
    yy.append(y * rad2deg)
    if (step * delta_t) % ticksEvery == 0:
       xxx.append(x * rad2deg)
       yyy.append(y * rad2deg)

# graph        
pylab.plot(xx,yy)

# where the tick-marks go
pylab.plot(xxx,yyy, 'b+')

pylab.xlabel('bank / degrees')
pylab.ylabel('pitch / degrees')
pylab.title('slew=' + str(l) + 'deg/min  rate=' + str(turnRate) + \
    '  speed=' + str(airSpeed) + 'kts  (' + str(t_max) + 'sec' + \
            ', ' + str(angleTurned) +'deg)')
pylab.grid(True)
pylab.axes().set_aspect('equal', 'datalim')

##pylab.arrow(xx[steps-1], yy[steps-1], \
##            R * rad2deg - xx[steps-1], -yy[steps-1], \
##            color='green',shape='full',\
##            length_includes_head = True,\
##            head_width = 0.2, overhang = 0.2)
##
##pylab.arrow(0, 0, \
##            R * rad2deg, 0, \
##            color='magenta',shape='full',\
##            length_includes_head = True,\
##            head_width = 0.2, overhang = 0.2)

pylab.plot(R * rad2deg, 0, "r-o")
#pylab.savefig('graph_' + str(turnRate) + '_' + str(airSpeed) + '.png' )
pylab.show()

Turning Errors

The gravitational feedback mechanism currently implemented in the AHRS code causes the vertical axis of the display (the down direction of the roll/pitch) to align itself with (to slew towards) the measured acceleration, at a fixed rate. At the moment I’ve set the fixed rate to 0.004 radians per second, which works out at about 14 degrees per minute. This is required for two reasons: firstly to overcome drift in the gyros, which look like a slow continuous roll in a random and changing direction, and secondly to overcome integration and other errors that get included over time into the direction cosine matrix. That does mean that during a turn an error is induced, because then the detectable acceleration vector is the sum of gravity (down) and the sideways centripetal acceleration to the outside of the turn.

In this part I want to look at the nature and size of this error.

If you look at the way a mechanical gyroscopic Attitude Indicator (AI) works you’ll see it has a self-erecting mechanism that works via a set of pendulous vanes. If the spin axis of the gyro isn’t aligned with gravity then small vanes at the bottom of the assembly hang to one side and open up air jets. The air jets blow and push so that the gyroscope axis returns to the vertical. This happens at a fixed rate – usually 5 to 8 degrees per minute. That means that it works in the same way as the AHRS code, and suffers from the same accelerational and turning errors, so the analysis here is just as applicable to the mechanical AI. A qualitative description of the errors induced in a turn would go something like this:

• Initially the gyro axis is vertical and the measurable acceleration is purely vertical due to gravity. The aircraft flies straight, let’s say southwards.
• The aircraft banks into a right turn. Imagine it’s a coordinated turn, so the bank angle is fixed by the speed and rate of turn. The gyro axis is still vertical but the new acceleration tilts to the east. The angle of tilt is the same as the aircraft bank angle.
• Because of the self-erecting mechanism the gyro axis starts to tilt eastwards, towards the acceleration vector.
• Meanwhile the aircraft is in a clockwise turn. The acceleration vector always tilts towards the left wing. So during the turn it moves to south-east, then south, then south-west, then west, and so on.
• The gyro axis continues always to move towards the acceleration vector at the same fixed rate, no matter where they both are. So after starting to swing east, it bends south, then south west, then west.
• Overall, the gyro axis travels outwards from the centre, on some kind of spiral path.

Imagine you are running around the outside of a circle. Beginning at the centre of the circle is a dog that chases you as you go. If the dog is very fast it will run at full speed until it catches up with you, then slow down and stay at your heels. If you run faster than the dog can it will wind up running in a circle that’s smaller than yours, while you run ahead of it. So it is with the acceleration vector and the gyro axis: if the acceleration vector moves around wide circle (i.e. large bank angle or rate of turn) then the gyro axis can’t catch up with it. If the bank angle and rate of turn are small then it can. If the gyro axis ever does catch up to the acceleration of course that means the AI will be indicating straight and level, even if the aircraft is really in a bank.

(a) The dog can run as fast as the man, and catches him. (b) The man runs a little faster than the dog (c) The man runs much faster than the dog. The magenta line is in the direction of the instantaneous velocity of the dog and is tangent to the dog's circle.

In the diagram above you can see this graphically. (As depicted, the aircraft would be at the centre of the circle, flying west turning and to the right. The dog and the man give you an overhead view of the gyro axis and the acceleration vector, respectively.)

On the left (a) is the situation where the gyro axis can tilt as fast as the acceleration vector is moving. The gyro axis catches up with the acceleration vector, just as the dog catches up with the man. The attitude indicator will then wrongly show only straight and level.

In (b) is the situation where the acceleration vector is moving only a little faster than the gyro axis is able to. The angle between the blue lies is relatively small and the gyro axis (dog) is displaced from the vertical (centre) in nearly the same direction as the acceleration vector (man). In this case the error would be mostly showing too little bank, and a small erroneous nose-up pitch.

In (c) the man runs very fast around a large circle, corresponding to a large bank angle and a high rate of turn. The gyro axis is displaced from vertical by the same amount as in (b)  because the dog is running at the same speed, but lags behind the acceleration vector by nearly 90 degrees. The AI will show almost the correct bank angle but most of the error will appear as an erroneous nose-up pitch.

For the sake of completeness I should say that although this is the steady-state, We still don’t know anything about the short term behaviour, and how long – or whether at all – the gyro reaches this steady state. If the aircraft starts straight, level and fast, and rolls quickly into a steep turn then the initial error is in bank and the pitch error appears later as the acceleration vector moves ahead of the gyro axis.

To get a more detailed insight we need to look at the situation mathematically. Unfortunately a complication is that the real geometry is spherical and not planar; it’s as though the man and the dog were running around the inside of a great big hemispherical bowl, where their circular paths are meridians of latitude, and the magenta straight line from the dog to the man is actually a great circle.

Let’s have some terminology:

• Θ will be the  the angular deviation of the acceleration vector from the vertical, the same as the bank angle of the aircraft if the turn is coordinated.
• ω is the rate of turn of the aircraft, in unit of radians per second, a standard rate turn of 180 degrees in one minute means ω = π / 60.
• v is the flight speed, in metres per second. 1 knot is 1.94 m/s.
• g, the surface gravity of the earth: 9.81 m/s²
• θ is the angle of deviation of the gyro axis from the true vertical. It corresponds to the radius of the dog’s circle. It is an angle rather than a distance because of the spherical geometry.
• θ_max is the biggest angle of deviation possible for a given rate of turn. It corresponds to the radius of the biggest circle the dog can run, which depends on the maximum speed of the dog and its rate of turn.  θ can be smaller than θ_max but not bigger.
• λ is the slew-rate of the gyro axis. For the AHRS code it’s set at 0.004 radians / second, or 14°/s. For a mechanical gyro-driven AI I understand it’s usually between 5 and 8 degrees per second. In the dog analogy it corresponds to the fastest speed the dog can run.

Then by geometry and a little trigonometry the following results are true:

• Θ = tan^-1(vω / g) ; 0 ≤ Θ < π/2 This is the usual formula for the angle of bank in a turn.
• θ_max (λ, ω) = λ ≤  ω :  sin^-1 (λ / ω);  λ >  ω : π/2  ; and where 0 ≤ θ < π/2
  note θ_max is a function only of λ and ω
• θ = min (Θ , θ_max)
• the cusp between θ = Θ and θ = θ_max occurs when  tan^-1(vω / g) = sin^-1 (λ / ω)
  As far as I know there’s no analytic way to solve that equation, but it can be solved numerically for any set of parameters.

We can look at this in a variety of ways. Let’s keep a fixed λ and examine the relationship between θ, ω and v. In piloting terms it’s interesting to ask how the long-term error in the AI changes with:

1. Airspeed, if flying a fixed rate turn
2. Rate of turn, if flying at at a fixed airspeed

The first is the easiest to answer. If ω and λ are both fixed then θ_max is fixed too. Because of the form of the inverse tan function, Θ is a monotonic increasing function of v, so as the airspeed increases, θ equals the bank angle, until it reaches this fixed value of θ_max - then  θ remains constant as you fly faster.

If I fly a standard rate turn for a long-enough time, with a regular AI – lets say λ is 5 degrees per minute – then the biggest the roll or pitch error it will settle to is sin^-1 ( 5 / 180) = 1.59°. (In fact, if all the angles involved are small then for a standard rate turn the error is limited approximately to the slew rate (in degrees per minute) divided by π; which in this case is 5° / 3.14159 is 1.59°). This is a maximum error; if you’re flying soooo slowly that a standard rate turn involves a bank of less than 1.59°, then that lower angle is the long-term error instead.

For a turn at half standard rate, the the maximum error is actually bigger: sin^-1 ( 5 / 90) = 3.18°. To obtain that error you’d have to have a bank angle of at least that, and therefore your airspeed would have to be greater than 114 knots. Comparing to the three diagrams (a), (b), and (c) above: fly a half-standard rate turn at 114 knots or below with this AI, and eventually it will show straight-and-level. Fly at 41 knots (say you’re in a microlight!) where your bank angle is fractionally more than 3.18°, and eventually that tiny fraction is all that will be left showing on the AI. But if you could fly a half-rate turn at 4700 knots then you’d be in an 80° bank and you’d have most of that 3° degrees of error show as a incorrect nose-up attitude, while the indicated bank would be correct. (You’d also be pulling about 6g and have a turn radius of 50 miles, so it might be time to talk to you national air force about your aircraft technology, they’d like to hear from you.)

Why is there a bigger potential error in a slower rate turn? Because if you reverse your direction more quickly – standard rate compared to a half-standard rate – you have less time in any one direction for the error to build up. Your easterly error that you got while flying south is more quickly cancelled out because you’re sooner flying north again. And in terms of the dog and the man, if the dog has to go around only half a turn in a given time, the radius of its path can be bigger.

Let’s examine the second option. What happens to the AI errors if I peg my airspeed at say 120 knots, and vary my rate of turn? The long term error will equal the bank angle, up to a maximum angle, and then it go down as I tighten the turn. It’s probably easiest to look at this graphically, so here is some data plotted out:

The black line shows θ_max for λ = 5º/min. The coloured lines are the bank angles at speeds from 80 to 200 knots, for a range of turn rates. The long term gyro error for any rate of turn is the lower of the black line and which ever coloured line applies (according to speed).

Angles of bank and theta_max for different speeds, vs. turn rate.
Red=80kts, green=100kts, blue=120kts, yellow=140kts, magenta=200kts

A few observations:

• The biggest errors are for very fast flight combined with slow turn rates. Fly a 0.75-degree per second turn at 200 knots for a sufficient time and eventually your AI will show close to wings-level and therefore be in error by 7 degrees.
• For standard rate turns at all reasonable air speeds, long-term errors are limited by slew rate and not bank angle.
• For standard rate turns, the long-term error is a small proportion of the  true bank angle. The AI remains approximately accurate, and certainly useful, even for very long duration turns. It’s more accurate for standard rate turns at higher speeds.
• For turns of half standard rate, the long term error is a significant proportion of the bank angle. The AI will indicate significantly closer to level flight than it should. Only when the speed gets above 200kts does the AI maintain a reasonable indication of the correct bank angle for a half-rate turn.

As a final example let’s calculate what happens in every pilot’s favourite test maneuver, the steep turn. In this case the aircraft is a continuous turn at 45° of bank, travelling at say 100 knots. That being the case, the rate of turn is about 3.65 times standard rate, just short of 11° turn per second, or 660° / min.   With the same AI with λ = 5º/min, the maximum error is clearly small and slew-rate limited, indeed to a value of about 0.4°. So I don’t think you need to worry about AI errors while practicing steep turns.

This section has been all about the long term maximum error. But we haven’t any real idea how the error to build up. And how much of the error accumulates in a fixed angle of turn – a course reversal or 180° turn for instance. I hope to look at that in the next section.

How Garmin say you should do it

I’ve found online a copy of the installation manual for the Garmin G1000 glass panel cockpit display system, which is written for builders of experimental aircraft who are installing the G1000 system. I’m interested in how they say the magnetometer subsystem should be installed, and whether it needs to be flight calibrated.

The GMU44 is the magnetometer unit and Garmin specify (table 1-1) minimum distances from sources of magnetic interference.

Garmin table 1-1

They forbid installation within 2.5 metres of the engine structure of the aircraft, which is the largest piece of ferromagnetic material. But they also have limits for distance to the landing gear (spring steel, often) and electrical conductors.

There’s also a GMU 44 Location Survey Tool Software which uses a runtime version of another mathematical tool suite – Matlab – that you have to use, which measures the magnetic changes from a whole long list of operations (nothing done in flight though, as far as I can see.)

Interesting reading. One of the things I am coming to see is that the idea of having a portable device you can fly with that will give you an accurate magnetic heading is infeasible. Anything portable isn’t going to be accurate; and for accuracy you need to tie your sensors to a fixed location and them calibrate them in place – or at least be absolutely certain that there are no magnetic interferences in that location.

Magnetometer misalignments

In a previous post I said that you could tell something about the magnetometer misalignments from the matrix M.

To recap: if the magnetometer axes are installed twisted with respect to the platform axes the calibration procedure detects and corrects for this by including an extra rotation in the matrix M. Also I said that the singular value decomposition (SVD) of M breaks M down to a rotation, an orthogonal scaling along the coordinate axes, then a second rotation, each of which can be represented by an individual matrix V, S, and U, in turn. V and U are rotations and are therefore orthogonal matrices, and S is a diagonal matrix. In fact it’s more convenient to use the transpose of V (and since V is an orthogonal rotation, its transpose is also its inverse) and write

• M = U S V^T

any rotational misalignment in the magnetometer data will be apparent if the two rotations (the one before and the one after scaling) are not equal and opposite. That is, in perfect alignment, U V^T  = I. Returning to Data Set A previously I can quickly form the product U V^T, in the R console window:

> svd(M)$u %*% t(svd(M)$v)
             [,1]         [,2]        [,3]
[1,] -0.999739709  0.009058857 -0.02093924
[2,]  0.009619606  0.999593562 -0.02683607
[3,]  0.020687625 -0.027030510 -0.99942052
>

This is very close to the identity – with ones along the leading diagonal and zeros elsewhere – or at least it would be if the signs of the first and last entries were positive instead of negative. What’s going on here?

Let’s look at this matrix as being very close to the following:

-1  0  0 
 0  1  0
 0  0 -1

You should recognize this as a matrix that rotates 180º around the y axis because it switches the sign of the x- and z- coordinates. (It’s a flip long-end to long-end). I have written earlier when discussing the accelerometer calibration that I needed to change the sign of the x and z calibration values in order to make the board face the correct way. What the result above is telling me is that the raw output of the magnetometer is misaligned by almost exactly 180º. Which is true, since with the board upright the sensor axes are front/back and top/bottom reversed. But I was actually able to measure this reversal from the magnetometer calibration, instead of assuming it.

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

Magnetometer Thoughts (continued)

We were looking at the kind of magnetic measurements that could be made when the AHRS is subject to constraints on its orientation during the period when calibration is done. I took a set of measurements by rotating the system only about a vertical axis, with three slightly different orientations.

If I run the data through the least squares algorithm, here are the results, in graphical format. It’s animated again, so click on the graphic to see it rotate.

Limited attitude magnetic data after calibration (blue) along with estmates for the earth field vector (red). The green pimple is the south pole - where the z axis is most negative.

Apart from a couple of outliers the blue points lie close to a sphere, and the red points lie in a close bunch. (To recap: the blue points are the estimated vectors of the Earth field in the sensor frame – they rotate around as the sensor rotates. The red points are the estimator for the earth field in the Earth frame. They should all fall at the same place. The green dot represents the “south pole” of the coordinates for each set of data: the Earth’s south pole with reference to the red dots and the minus-z direction in the sensor frame with respect to the blue dots.

The angular deviation from mean north for each measurement looks like this:

Angular devations of north estimates, from the restricted attitude calibration

This data is (apart from some outliers) constrained to ±10° of error, which I expect to reduce with appropriate filtering.

I have also implemented the least-squares sphere fitting method from the paper by Vasconcelos et. al. and on this data set it fails to map the data onto the sphere – there isn’t enough of the ellipsoid present for the optimisation to converge.  However the method I’m suggesting here which doesn’t attempt explicitly to fit to sphere but instead bring the red dots to a single point does successfully map the ellipsoidal data onto a sphere.

Magnetometer Thoughts

In the previous post I finished up by looking at the results of the magnetometer calibration. Now I want to consider how this is going to work in a real aircraft.

In order to get a good fit to the theoretical model (ellipsoid-sphere etc.) I want a sample of points that lie around a good proportion of the ellipsoid. (We’ll examine the case where that’s not possible, later.) How do I make that happen? Obviously I need to “point” the AHRS in all sorts of different directions. It’s not obvious, however, how the direction the AHRS points maps to where on the ellipsoid the data point of my magnetometer measurements will lie.

Consider this:

• The measurement from the magnetometer has three variables: a value for each of the x, y and z axes.
• The measurement from the magnetometer – in the theoretical model – lies on an ellipsoid.
• An ellipsoid is isomorphic (topologically identical) to a sphere. You can specify a point on both a sphere and an ellipsoid with two variables: latitude and longitude, for instance,
• So although the magnetometer output has three variables it has only two degrees of freedom.
• The orientation of the AHRS has three degrees of freedom: it is a function of three independent variables: roll, pitch and yaw angles. (There are other ways to specify an orientation in space: the direction cosine matrix has nine variables but also six constraints, so it also has three degrees of freedom. Quaternion representation of orientations: four variables and one constraint: three degrees of freedom again.)
• The space of orientations is often described as SO(3) – the space of 3×3 orthogonal non-inverting matrices – so a mathematician would say that the magnetometer provides a map from SO(3) to S2, ie. from an orientation in space to a point on the 2-sphere.
• The loss of one degree of freedom implies there’s an invariance – somewhere in the orientation there’s a hidden variable, whose value doesn’t affect the magnetometer outputs.

So what is this invariance? How does it show up? The answer is, over any rotation around the Earth field direction. Wherever I hold the AHRS if I spin it around an axis that’s parallel to the Earth’s field the magnetometer output won’t change: I’ll get the same x, y, and z measurements from the chip. (You can see that from a symmetry argument too, and it’s the same reason that you can’t detect a change in your north/south/east/west orientation by measuring local gravity: North/south/east/west is a function of a rotation around a vertical axis - parallel to the gravity field lines – and any measurement of gravity is invariant to such a rotation.)

Let’s go back to the ellipsoid again: each orientation of the AHRS is mapped to a point on the ellipsoid. But because of this rotational invariance, each single point on the ellipsoid represents a whole family of orientations all of which differ only by a rotation around the field line.

The corollary is that to take measurements that lie around a large proportion of the ellipsoid I need to sweep the AHRS by rotating it around axes orthogonal to the field lines.

Another way to think of it is this: imagine the ellipsoid fixed to the AHRS in the centre of a dark room. Imagine the Earth’s field represents a tiny beam of light, shining towards the centre of the room. I can see the ellipsoid only at the point where the narrow beam of light hits it. By rotating the AHRS and ellipsoid in space I can map out its surface by seeing where the beam of light hits it as it twists and turns. To map large portions of the ellipsoid I need to rotate across the direction of the beam; if I turn it parallel to the beam of light then the light falls on the same spot.

Large and small lumps of ferromagnetic material distort the earth’s magnetic field, and the purpose of the magnetometer calibration was to correct for the distortions caused by such material that’s fixed to the AHRS platform. Unfortunately I’m now going to put the AHRS in an aircraft, in close proximity to 426lbs of mixed cast steels in the shape of a Teledyne Continental IO-470F aircraft engine. That means I’ll end up measuring the magnetic field as disturbed by all that ferromagnetic iron.

Alternatively I can strap the AHRS down in the aircraft and treat the whole thing as the platform – perform a calibration by taking measurements while the aircraft rotates in space. This is where I run into problems. For a model aircraft I can just pick the thing up and turn it over, spin it around and around, and reach all sorts of orientations as I sweep all over the ellipsoid of magnetic measurements. Most aircraft aren’t certified for aerobatic flight – certainly, mine isn’t – so there’s a very limited range of orientations I can reach without danger. And because the angle of dip at my location is quite steep – only 20° away from the vertical – the most comfortable axis to rotate the aircraft around, which is the vertical one, is quite close to parallel with the field lines.

Let’s look at a diagram. Of course I don’t need to be in the aircraft to limit the range of orientations of the AHRS. Here are the results of a set of measurements made while rotating the AHRS multiple turns about a vertical axis (only) in i) a level attitude, ii) nose down approximately 30º and iii) banked left 30º. It’s an animated GIF, but you’ll have to click on it to see the animation.

Magnetometer measurements from the AHRS in constrained orientations

You can see three distinct loops, one for each attitude of the AHRS. You can also see they lie on some kind of curved surface, but you’d be hard pushed to say it’s a section of an ellipsoid. What is clear is that as I suggested, because of the steep angle of the “illuminating” Earth field, only a small portion of the surface has been traced. What is also clear is that the orientation of the loops in the data has no obvious correlation to the axis of rotation which was the same in all three cases.

At this point I’d like to refer to the paper by Vasconcelos, Elkaim et al. that I mentioned previously. They discuss the fact that “the swinging of the magnetometer triad is is constrained by the vehicle’s maneuverability and, consequently, only some sections of the ellipsoid can be traced.” The structure of their method of magnetometer calibration is firstly to fit the magnetometer data – without any orientation information – to a sphere, and then in a second step, to use external information about the Earth field to find the optimal estimator for the magnetometer alignment. For this method to work for data collected over a limited range of orientations it’s necessary to show that whatever algorithm you use has a chance of fitting the data to a sphere (by finding the matrix M, in my notation) when provided with points that lie on only a limited portion of the ellipsoid.

Vasconcelos et. al. do this but but with (simulated) data that lies on an equatorial portion of the ellipsoid (their fig. 3(a) and 3(b)). There is an unstated assumption in their analysis that the yaw and pitch angles of the sensor correspond to the longitude and lattitude angles of the corresponding point on the ellipsoid, and that the magnetic measurement is invariant with respect to the roll body angle of the sensor. This can in fact never be true, since the invariance is with respect to any angle of rotation about the Earth field which is an axis not at all fixed with respect to the sensor but instead fixed with respect to the Earth.

If the calibration measurements are carried out at an equatorial location of the planet, where the Earth field lies horizontal, then rotating the AHRS platform (aircraft or vehicle, as you will) about a vertical axis will trace a series of equatorial paths around the ellipsoid. But these paths will be intersecting, and not lie in a smooth band, as suggested by Vasconcelos et. al. At higher lattitudes it would be impossible to collect anything similar to the kind of data they use in their analysis without rotating the AHRS outside of the body angle limits they set.

In the next section I’m going to examine the results of trying the calibration algorithm on this data set.

Calibration Part 6: Magnetometers (continued, again)

Now that I have numbers for M and B I can insert them into the code, in the Parameters.h file:

#define MAGOFFSET_X -62.3 //B_x
#define MAGOFFSET_Y -247.3 //B_y
#define MAGOFFSET_Z -232.7 //B_z
//nine components of the matrix M
#define MAGGAIN_1 -1.0000
#define MAGGAIN_2 0.0279
#define MAGGAIN_3 0.0001 
#define MAGGAIN_4 -0.0102
#define MAGGAIN_5 0.9117
#define MAGGAIN_6 -0.0046
#define MAGGAIN_7 -0.0422
#define MAGGAIN_8 -0.0477
#define MAGGAIN_9 -1.0489

I want some kind of measure as to how well these figures work in practice. This is hard to determine. Firstly without a surveyor’s quality compass I can’t tell where north actually is to better than a degree or two. Google maps will only get you so far. Secondly I can line things up with the way the Mongoose PCB is oriented to maybe another degree, but I have no hope of accurately measuring anything with respect to any of the chip packages on the circuit board.

Thirdly, the AHRS can be oriented to any {roll, pitch, heading} angle combination, and to see they quality of my calibration I should check the accuracy and precision throughout this three dimensional search space. I would only have a hope of doing that accurately if I had a gimbal system into which I could the AHRS to rotate to a carefully measured orientation. And that system would have to be entirely made of brass, or wood, or something non ferromagnetic or it would affect the measurements. Unsurprisingly I don’t have such an apparatus.

So I’m going to make one simple set of measurements, which is the set I care most about: with the AHRS in a level configuration (and the supposed aircraft to which the AHRS is attached in level flight) how does the indicated magnetic heading vary as the yaw angle rotates? At a minimum I expect a heading of zero to align with something that by eye at least points northwards, and also that the heading increases by 10° for every 10º of clockwise rotation.

Here are the results of just such a set of measurements (with the calibration data derived from data set A):

Deviation between physical and indicated heading angle. (Both axes in degrees)

What this indicates is that in a level attitude, the heading angle is consistent to ±1.5°. The curve fit is the first order error which I expect to be sinusoidal in nature, and that has a magnitude of ±0.5°. I think those results are quite respectable.

Calibration Part 5: Magnetometers (continued)

Now we’ve looked at some of the theory behind calibrating the magnetometer outputs, let’s see how we can actually do it. In the last section I said that we had

•  n vector equations of the form m_i = M ( R_i ^eb + B ), where M is a matrix that represents both the stretch and the rotation.

And that we need to find the fixed values of M, ^eb and B that make all these equations most nearly correct at the same time. We can do this with a least squares optimization method, using

• J (M,^eb, B) =  Σ ( M ( R_i ^eb + B ) - m_i )²

as the cost function and optimizing over all 15 individual parameters in M, ^eb and B (there are 9 in the matrix M and 3 each in ^eb and B.) This corresponds to minimizing a negative log likelihood function (I haven’t given it here) under the assumption that all samples have the same standard deviation.

At some point I want to return to this to consider how the errors contribute: the m_i measurements can be considered as subject to a white Gaussian noise process, and the sequence of orientation matrices R_i is subject to error which increases in time as gyro drift (which is uncorrected in yaw, during the calibration period) takes it’s toll. Therefore we might expect to get a more accurate answer if the early samples are weighted more heavily than the later ones.

Another option would be to consider the uncorrected gyro drift as a reorientation of the R_i in space over the duration of the sampling, and which appears as a “wobble” in R.  We should replace R_i with  Ω^-1(t_i) R_i , where Ω(t) represents the rotation by which the reference frame has drifted by time t and t_i is the time of the i’th sample.  A fair assumption for the structure of this drift is that each of the body gyro rates has an independent fixed null offset. It would be convenient to be able to linearize as Ω_i = I + (i/n).Δt.Ω where Δt is the duration of the calibration period and Ω is the matrix form of a differential rotational element whose nine elements row-wise are [(0, -ω₂, ω₃), (ω₂, 0, -ω₁), (-ω₃, ω₁, 0)] , that is, to assume that the samples are equally spaced in time and that to first order the drift in R  is a continuous rotation at a fixed rate about an axis fixed in space. However this would not match the assumption of a fixed gyro drift rate: the aggregate rotation from a drifting gyro is strongly dependent on the history of the orientation of that gyro. (Eg: a drifting “yaw” gyro shows up as a pitch error if the roll angle is 90 degrees.)

In the mean time let’s go on without considering the errors in the samples, and see what kind of results we get. If we get a decent convergence to an answer then as a check we can determine from the fitted value for ^eb the angle of magnetic dip (the angle at which the Earth field lines go into the ground) for the location where the calibration data was collected. And we can compare that with a value from a dip meter, or else look up a value from the world geomagnetic model, via one of a number of websites like the British Geological Survey. We’ll then include the values for M and B in the AHRS code as part of the calibration parameters. For each magnetic measurement thereafter we can calculate the Earth field vector in the Earth frame. Wait, you say, shouldn’t that be a constant? Surely the Earth field doesn’t move? Why measure something that’s constant? Because, to the extent that our measurement appears to move, it indicates an error in our heading angle. Suppose I have a heading of 032. I measure “north” and come up with an answer that points two degrees to the right of my x-earth axis (i.e the angle is 2 degrees instead of zero). I determine from this that my earth-x axis needs to be rotated to the left by two degrees to align it correctly. In actual fact I’m going to insert the angle of the measured north relative to my earth-x axis directly into the Kalman filter as the residual of the heading angle state variable. The Kalman filter will rotate the direction cosine matrix to make the residual go to zero, and that means that the earth-x axis will be tied to magnetic north instead of drifting freely due to gyro errors.

To perform the least squares fit I’ve been using a multi-platform package called R – which bills itself as “a free software environment for statistical computing and graphics”. R  gives us a command line environment from which we can load and graph data, and also run least squares optimization queries without having to write any code. Fabulous. So I have written a script for R, that loads a file of data output by the AHRS, and runs through a least squares optimisation procedure to give values of M, ^eb and B. It also draws some 3D graphs so you can see what the data looks like. It makes the whole ellipsoid thing much easier to imagine when you can see it in front of you! Also the script takes the best values for M, ^eb and B and works out a posteriori how far each of the individual magnetic measurements (k) would have predicted ^eb_k to be from the overall best value ^eb - that is, the angular deviation in the z-plane from the mean. Which is an indication of how consistently we would have determined a fixed direction for north (noting, however, that consistency isn’t the same thing as accuracy.)

Let’s take a run-through. Firstly I need to record some data. I set the output mode of the AHRS for the magnetic calibration mode, and make sure to turn off the magnetic tracking (I can’t calibrate the compass if the DCM is already being rotated to match some other inaccurate magnetic calibration).

#define PRINT_JSON 0
#define PRINT_LEVIL 0
#define PRINT_LEVILBARO 0
#define PRINT_POWER 0
#define CALIBRATE_MAG 1
#define CALIBRATE_GYROACCEL  1
#define REORIENT 0
#define IGNORE_MAG 1

Just like for the gyro calibration I use telnet or nc to log the output, which I’m going to call data set A:

-21,-392,-651,38895,-92121,934,92092,38906,2317,-2498,-41,99969,0.10
-22,-415,-658,40727,-91307,-2078,91306,40652,3263,-2134,-3226,99925,0.08
-52,-426,-634,44407,-89454,-5092,89541,44102,6118,-3227,-7277,99683,0.04
-60,-432,-629,47936,-87582,-5618,87628,47411,8576,-4848,-9034,99473,0.00
-39,-456,-626,51096,-85748,-6045,85734,50325,10823,-6239,-10713,99229,-0.03
-32,-467,-627,52295,-85062,-5444,84816,51299,13215,-8448,-11528,98973,-0.05
.
.
etc

Somewhere away from localized magnetic disturbances (i.e. not directly above or near some furniture with iron or steel parts) I rotate the AHRS slowly through a variety of orientations and around a variety of axes, for about 30 seconds, while logging. I copy and paste the data into a text file in a directory next to the script and run it through R.

> # Perform optimisation
> nls.out<- nls.lm(par = pStart, fn = residFun, m = magM, r = R, control = nls.lm.control(nprint = 1, maxiter = 50))
It.    0, RSS = 1.22339e+13, Par. =          1          0          0          0          1          0          0          0          1   -9.72222   -262.489   -308.181          1          0          0
It.    1, RSS = 1.22094e+13, Par. = -0.0744783   0.456607   0.162882  -0.134675    1.45067   0.376037   0.041931    0.13083   0.468529   -9.83206   -262.458    -308.02    0.74496 -0.0085705  -0.283312
It.    2, RSS = 1.2177e+13, Par. =  -0.539431   0.965673   0.245232  -0.271866    1.20153   0.827138  0.0667927   0.305385    0.85613   -9.98764   -262.413   -307.788    1.14094  0.0620342  0.0159279
It.    3, RSS = 1.20435e+13, Par. =   -2.43952    1.76911    0.61229   -0.54322    1.99737    1.49328   0.137573   0.525754  -0.121397   -10.2024   -262.349   -307.465     1.6774   0.399639  0.0936926.
.
<etc>
.
It.   46, RSS = 5.73868e+07, Par. =    -35.271    1.07938 -0.0110288  -0.386153    38.6756  -0.168991    1.43572   -1.80098   -33.6084   -62.3397   -247.288   -232.752    3.79514   -1.75578    12.3681
It.   47, RSS = 5.73868e+07, Par. =    -35.271    1.07938 -0.0110153  -0.386153    38.6756  -0.169001    1.43571   -1.80098   -33.6084   -62.3397   -247.288   -232.752    3.79521   -1.75585    12.3681
It.   48, RSS = 5.73868e+07, Par. =    -35.271    1.07939 -0.0110145  -0.386158    38.6756  -0.168995    1.43567   -1.80097   -33.6084   -62.3397   -247.288   -232.753    3.79518   -1.75581    12.3681
It.   49, RSS = 5.73868e+07, Par. =    -35.271    1.07939 -0.0110058  -0.386159    38.6756  -0.168999    1.43564   -1.80097   -33.6084   -62.3396   -247.288   -232.753     3.7952   -1.75584    12.3681
It.   50, RSS =        nan, Par. =    -35.271    1.07939 -0.0110058  -0.386159    38.6756  -0.168999    1.43564   -1.80097   -33.6084   -62.3396   -247.288   -232.753     3.7952   -1.75584    12.3681
Warning message:
In nls.lm(par = pStart, fn = residFun, m = magM, r = R, control = nls.lm.control(nprint = 1,  :
  lmdif: info = -1. Number of iterations has reached `maxiter' == 50.

> summary(nls.out)

Parameters:
       Estimate Std. Error t value Pr(>|t|)    
a     -35.27102  382.61185  -0.092    0.927    
b       1.07939  731.24259   0.001    0.999    
c      -0.01101 1125.04689   0.000    1.000    
d      -0.38616  550.44043  -0.001    0.999    
e      38.67563    2.99421  12.917  < 2e-16 ***
f      -0.16900 4700.28057   0.000    1.000    
g       1.43564  160.15873   0.009    0.993    
h      -1.80097  587.09787  -0.003    0.998    
i     -33.60840   20.84056  -1.613    0.108    
B_x   -62.33962    8.77291  -7.106 1.19e-11 ***
B_y  -247.28845  268.12997  -0.922    0.357    
B_z  -232.75322    4.80571 -48.433  < 2e-16 ***
eb_x    3.79520 1868.76346   0.002    0.998    
eb_y   -1.75584 2065.83362  -0.001    0.999    
eb_z   12.36808  643.52944   0.019    0.985    
---
Signif. codes:  0 ‘***’ 0.001 ‘**’ 0.01 ‘*’ 0.05 ‘.’ 0.1 ‘ ’ 1 

Residual standard error: 474.4 on 255 degrees of freedom
Number of iterations to termination: 50 
Reason for termination: Number of iterations has reached `maxiter' == 50.

Parameters a to i are the 9 entries of the matrix M, and b0x, b0y, b0z and B0x, B0y, Boz are the three entries each of ^eb and B. For this data set the optimization doesn’t quite converge, within 50 iterations. But the residual sum of squares (RSS) is hardly moving by the end; I could either try again with a higher limit to the number of steps, or press on and see what kind of results I get. If this were an academic exercise I’d be studying the convergence conditions and using different algorithms. But for now I’m just interested in the answers: do they work?

> # RESULTS:
> # This is the mag offset:
> B
       B_x        B_y        B_z 
 -62.33962 -247.28845 -232.75322 
> # This is the rotation matrix from magRaw to body-frame calibrated B-field measurement:
> solve(M) / solve(M)[1,1]
           [,1]        [,2]          [,3]
[1,] 1.00000000 -0.02791751 -0.0001870903
[2,] 0.01016883 -0.91176870  0.0045814641
[3,] 0.04217190  0.04766630  1.0489039771
> # 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
      eb_x       eb_y       eb_z 
 0.2906893 -0.1344868  0.9473188 
> # the magnetic declination at your calibration site was 
> 57.1 * asin(eb[3])
    eb_z 
71.07571

The script rescales M so that the top left entry is identically 1.0, and rescales b_0,e to be of unit length. This is permissible because I don’t care about the absolute magnitude of the Earth field anywhere; I only want to know its direction. To check the angle of dip I note that the z-component of  b_0,e is the sin of the dip angle. In this case the script has measured the angle as 71.32º. According to the BGS website the world geomagnetic model predicts an inclincation of 70.147º, so I’m off by a little more than 1 degree. That’s not too shabby. Here’s the 3d plot of the original data, lying on an ellipsoid:

Raw magnetometer measurements. They lie closely on an ellipsoid (which appears close to spherical as a result of the scaling). The green dot is the centre of mass, used as an intiai value for the centre of the ellipsoid in the least squares fit.

If you use R then you can rotate the axes of the plot by dragging the mouse pointer within the window, and you get a much better idea of the shape of the data as it rotates in front of your eyes. Here’s the plot of the same data after scaling to the sphere and rotating. If the least-squares fit was successful then the blue dots now represent the Earth field vector in the sensor frame-of-reference,  each measurement. The dots move around as the sensor rotates. The green sphere is centred on the origin and has radius equal to the mean distance of the blue dots. So the blue dots should lie on it. The red dots are the same measurements of the Earth field presented in the Earth frame-of-reference (by using the orientation data that accompanies each measurement). Ideally all the red dots should lie at one place, representing the Earth field.

The same data scaled to the sphere, and then reoriented to show the best estimate for the Earth field vector. The absolute size of each measurement and of the sphere is unimportant.

For the purposes of stabilizing the heading output of the AHRS we are interested entirely in the angular variation of the measurement of the Earth field when projected into the earth-xy plane, since this gives us an indication of how stable the compass heading will be. (If we used the Earth field measurement to stabilize in pitch and roll too then we would be interested in the z component too.) The next graph shows this angular deviation from the mean, in degrees. The horizontal axis is the measurement index:

Angular deviation from the mean direction of measured magnetic north vs. index number

The plot shows that most measurements lie with ±5º. There is some structure visible in the graph (we would hope for pure white noise) which indicates drift or inaccuracy in the direction cosine matrix trending with time. I can remove the noisy component with smoothing (which is what the Kalman filter on the heading variable does). This dataset was obtained after careful calibration of all three axis gyroscopes to better than 0.1%. The next graph shows the angular deviation in a data set created with one of the gyroscopes miscalibrated by 1% (only):

Angular deviation with x- gyroscope miscalibrated by 1%

The results show larger errors and, significantly, errors which are structured in time. They would appear in the output since they can’t be filtered. The time structure results from incorrect angular rates from the miscalibrated gyroscope being integrated into the direction cosine matrix. Roll and pitch errors will be corrected by the gravitational correction but yaw errors persist through the calibration period. Here are the results of another data set (call it B) with the gyro measurement corrected. This time I moved the AHRS more slowly than before during the calibration period:

The results of another calibration period

Angular deviations for the calibration above

The data doesn’t appear to fit the sphere as well as the previous set, but the angular deviations are smaller. The least squares algorithm reached convergence in 32 iterations. Also this time the angle of dip was assessed as 70.52º, very close to the “book” value of 70.147º.