The data-sheet for the ITG3200 suggests that the uncorrected thermal stability is about 40 degrees per second from one end of the temperature range (-40C) to the other (85C). Yesterday I read up on the datasheet for the L3G4200D device from STMicrolelectronics. It lists the zero-rate level change at 0.03 degrees per degree Celsius, which is an improvement by about an order of magnitude.

That doesn’t mean the non-linearities, hysteresis and other difficult-to-correct stuff is any less, but there’s a good chance. So I’ve ordered a breakout board with the L3G4200D on it and I’ll wire it up and use that gyro chip instead of the ITG3200 for some testing, so see if we can get a more stable platform. And I’ll write up here whatever I find.

However, they’ve got a GPS included so they’ve “cheated” on solving some of the more difficult technical issues.

http://ukga.com/news/view?contentId=27090

My source says the price will be around $500. I look forward to hearing more about it and assessing its performance.

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.

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()

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.

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.

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.

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

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.