Notes on “Fusion Thoughts”

Coupla things on the second half about video macro block fusion from the previous post. First, it’s

δx2 = h2 tan(θ)


δx2 = atan(δx2 / h2)

The latter is working out θ which is known anyway.  Just a bit of careless writing down thoughts by me.

More interestingly, the picture represents forward positive movement from left to right across the page.  However, the pitch angle is negative following the right hand rule which my code adheres to.  Therefore, the full conversion equation is:

δx = δx1 + h2 tan(θ)

The situation on the Y / roll axis is different. If you mentally flip the diagram left / right, following the right to left represents a positive move on the Y axis. The roll angle in this case is also positive according to the right hand rule so the equation looks like this:

δy = δy1 - h2 tan(φ)

There are other places in the code which already follow this ruling.

Better angles

Here’s the current code used for calculating pitch, role, and yaw angles, which in turn are used to extract gravity from acceleration, and integrate the results to get velocities:

# Get angles in radians for Euler and quad frame: rotate the accelerometer readings to earth
# frame, pass them through the butterworth filter, rotate the new gravity back to the quad
# frame, and get the revised angles.
eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)
egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)
qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)
uap, uar = GetRotationAngles(qgx, qgy, qgz)

# Convert the gyro quad-frame rotation rates into the Euler frames rotation rates using the
# revised angles from the Butterworth filter
urp, urr, ury = Body2EulerRates(qry, qrx, qrz, uap, uar)

# Merge rotation frames angles with a complementary filter and fill in the blanks
tau_fraction = tau / (tau + i_time)
pa = tau_fraction * (pa + urp * i_time) + (1 - tau_fraction) * uap
ra = tau_fraction * (ra + urr * i_time) + (1 - tau_fraction) * uar
ya += i_qrz

The flaw in this is that it is using the previous set of angles to rotate the latest set of quad-frame accelerometer readings into the earth frame, as shown in red.

The revised code looks like this:

# Update the previous pitch, roll and yaw angles with the latest gyro output
urp, urr, ury = Body2EulerRates(qry, qrx, qrz, pa, ra)
pa += urp * i_time
ra += urr * i_time
ya += ury * i_time

# Based upon the revised angles, rotate the latest accelerometer readings to earth frame.
# Next, run the earth frame acceleration through the Butterworth LPF to extract gravity.
# Next, rotate and revise gravity back to the quad frame.
eax, eay, eaz = RotateQ2E(qax, qay, qaz, pa, ra, ya)

egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)

qgx, qgy, qgz = RotateE2Q(egx, egy, egz, pa, ra, ya)

In this case, the angles are updated first, based upon converting the gyro anglular rates into Euler anglular rates, and integrating these onto the existing Euler angles. Only then with the updated angles are the accelerometer readings reoriented to the earth-frame in order to extract gravity.

Although this definitely fixes one problem with the gravity extraction (and hence potential drift), I do have concerns about the revised version: primarily, you’ll notice that the pitch and role angles are never now defined by accelerometer readings, only increments to gyro readings. As a result, you’ll notice the complementary filter has vanished, as the gyros are not prone to noise. However, this may actually be a double edged sword: while there is now a reliance on zero drift in gyro accuracy, the fact the angles now don’t rely on the accelerometer might mean the results are less prone to accelerometer drift. Without testing, it’s not possible to say which side wins.

Worst case, I could re-add the complementary filter, but that introduces it’s own problems with lag. Not sure yet when I have the change to take Phoebe out to test this. The predicted weather for the coming week isn’t great.

An itch that needs scratching.

It’s bothered me for a long time that the two sources of angles fed into the complementary filter do not have the same meaning.  Since adding the rotation matrix code this has been increasingly evident.  Finally with the graphs from yesterday, and the consistent forward drift, understanding this has become a top priority.

The nub of the problem is the difference in the context of the angles:

Euler angles are cumulative: they represent the rotation of gravity between the inertial reference frame (earth axes aligned with north, west and up) and the quadcopter X, Y, and Z axes respectively.  To perform the rotation each angle is applied in sequence.

Integrated Gyro angles are independent – they are all in the quad frame, and represent rotation rate around each of the quads axes all within the quad reference frame.  Gravity doesn’t enter the equation.

So currently the complementary filters is trying to merge two different types of angles and can only produce garbage as a result.

I may to convert gyro angles into Euler angles, and for a change I found a site which explained these well, and in clear language and equations that don’t obfuscate the facts behind fancy symbols.

Euler vs Gyro Angles

There’s a few other links from that page that explain other parts of the quad code really clearly too.  Well worth reading.

Or I may need to use Kalman to smooth out the Euler angles, and dispose of the complementary filter altogether.

Or there may be a plan C that I’ve not had time to think of yet.

But I’m stuck on a bus for 2 hours tomorrow, so I’ll have plenty of time to muse and snooze!

Phoebe’s metaphysical weight loss.

Ever since things took a turning point a few months ago, and every change I made had a positive impact on Phoebe’s flights, I’ve been very aware that the code was shrinking.

Her largest point was when I added the rotation matrix, and that definitely was the turning point.  Some stats on the number of lines of code, comments and spaces

Beard   2420 code   425 comments  337 blank lines
Now     1931 code   314 comments  279 blank lines

No significant conclusions to be drawn, other than code’s much simpler when you really understand what you’re trying to code!  KISS principle.

Iterative dynamic accelerometer calibration

Currently, my accelerometer calibration of offset / gains against temperature requires two days of action, reading the sensors at various angles across a broad range of temperatures to put into Excel to produce trend lines allowing calculation of offset and gain at the current temperature for the X, Y and Z accelerometers.  If you think that sounds difficult and tedious, multiply by two and you’re getting there.  It works but I don’t like it for several reasons:

  • off-the-shelf quad manufacturers / DIYers don’t do this
  • it’s tuned to a specific sensor – replace a broken sensor with a new one and the calibration needs to be redone
  • the calibration can only really be done on a sunny day in winter to be able to get termperature ranges from 10°C (Phoebe sleeps in the garage overnight) to 25°C (Phoebe moves indoors, and as her temperature rises, many samples are taken).

So I’ve been considering how to calibrate the accelerometer in flight.  It goes a bit like this: Prior to take-off

  • read the accelerometer many times and take the average while she sits quiet and stable in the ground – these readings are a gravity vector in the quadcopter’s frame (QFGV) and will be flawed due to offset and gain errors.
  • Using the QFGV, calculate her angles wrt earth using Euler – again the angles will be incorrect, but testing shows the error isn’t far from the truth
  • Use the quadcopter to Earth matrix to convert the QFGV to earth coordinates – the Earth frame gravity vector (EFGV) – in the perfect world, it should read 0, 0, 1 – the perfect world earth frame gravity vector (PWEFGV) but doesn’t in reality
  • Subtract the PWEFGV from the EFGV to come up with earth-frame error vector (EFEV)
  • Convert that EFEV back from the Earth frame to the quad frame to produce the quad-frame error vector (QFEV).

Now set Phoebe running:

  • read the raw accelerometer values every loop, integrating them over the time between each read
  • Periodically (roughly 40Hz compared to her spin rate of roughly 440Hz) process the readings by dividing by the elapsed time since last processing to come up with averaged accelerometer readings.
  • Subtract the QFEV from the readings to produce a ‘better’ set of quad-frame averaged accelerometer results
  • Use this better set of accelerometer results to come up with better angles via Euler
  • Use the better angles to convert the original fixed QFGV to a new, more accurate EFGV
  • Again subtract the perfect world PWEFGV from the new EFGV to come up with a new EFEV.
  • Convert the EFEV back to a QFEV
  • Repeat ad infinitum

This should produce an ever improving set of offsets by taking the initial fixed QFGV and constantly refining the quadcopter accelerometer readings with the QFEV.  In doing so, there should be convergence on an accurate value for the QFEV – i.e. the accelerometer is calibrated on-the-fly. Initial testing suggests this might just work, but Phoebe is not getting her @R$£ off the ground fast enough, so tilting to stop drift is causing the props to clip the ground. More testing later with a more energetic take-off rate will hopefully be more conclusive.

P.S.  All the above is based on the assumption that calculation of Euler angles from raw accelerometer readings is more accurate than the raw sensor readings themselves.  Currently I’m working on gut-feel, but I need to spend the time producing a mathematical proof.

P.P.S.  On the other hand, it’s easy to prove in real-world testing – if not true, then Phoebe will progressively go mad instead of settling into a stable state of affairs.

Moron reference frames

Best go read here and here if you haven’t already.

Software Layout

Software Layout

There are two reference frames with Phoebe:

  • the Earth-frame is defined by gravity and two other orthogonal axes which together define horizontal
  • the quadcopter-frame defined by the sensor axes assuming the sensors are aligned well with the quad’s arms.

Because I don’t (can’t) compensate for yaw, the Earth-frame horizontal axes point in the same direction as the quadcopter-frame sensor X and Y axes as viewed from above.

The key to the following train-of-though is that the inputs on the left of the diagram are in the Earth-frame (horizontal and vertical velocities), and the output’s on the right-hand side are in the quadcopter-frame because that’s where the motors and propellers reside.

There is a rotation matrix for converting from the Earth-frame to the quadcopter-frame as described in the Beard PDF linked above1.

OK, lets work through the details of moving through the PIDs from LHS inputs to RHS outputs, starting with the simpler example of the vertical velocity PID at the bottom right of the diagram.

The target is an Earth-frame vertical velocity.  The input is the quadcopter-frame accelerometer Z-axis sensor, matrixed2 to Earth-frame before integrating to give the current Earth-axis vertical speed.  However, the output needs to be in the quadcopter-frame to be correct for driving the PWM error compensation.  This cannot be done on the output side directly as the relationship between PWM to the ESC and ESC output is non-linear. To me, that suggests the only option is for the the vertical PID target to be adjusted from the Earth-frame back to the quadcopter-frame to allow for any tilt between the Earth- and quadcopter-frames.  In other words, if Phoebe has an overall small tilt of θ (derived from short term averaged Euler angles), then the vertical velocity PID’s current Earth-frame target, e,  needs to be converted to the quad-frame target, q, thus:

e/q = cos θ
q = e/cos θ

In other words, the vertical velocity PID target (configured at 0 for hover for example or 0.3 as 30cm per second climb) is the Earth-frame target speed increased by 1/cosθ to ensure the correct Earth-frame lift is provided.

Having sorted the vertical velocity, let’s move on to the horizontal, where things are less clear to me.

The leftmost PID (top left) is Earth-frame horizontal velocity input and target with an Earth-frame output representing corrective Earth-frame acceleration.  That output is converted to a desired pitch / yaw target for the absolute angle PID.  But I think there’s a bug there: the input for the absolute angle PID is a combination of Euler angles (long term) and integrated gyro (short term) passed through a complementary filter.  The Euler angles are correct for rotating between the two frames, but I’m concerned but not convinced that the integrated gyro is not, the reason being that the gyro sensor is in the quad-frame; should the outputs be rotated to Earth-frame before integrating2?

The output of this ‘middle’ PID is the rotation rate target. This lies in the quadcopter-frame aligned with the gyro axes. The input comes direct from the gyro, also in the quad-frame. The output needs to be in the quadcopter-frame. So I think all’s well, but I’m not 100% sure.

So net is I need a quadcopter- to Earth-frame matrix (the inverse of Beard), and I need to consider whether I need to add a cos θ tilt angle to the vertical velocity PID earth-frame target.

Answers on a postcard please as to whether I’ve gone wrong here.!


  1. I have an Earth to Quadcopter rotation matrix from Beard.  However, I misread it and am using it as a quadcopter- to Earth-frame matrix which can’t be a “good thing”TM.
  2. I don’t have a quadcopter- to Earth-frame matrix – Beard only shows an Earth- to quadcopter-frame matrix.  I should have spotted that.  I need to invert Beard to produce the matrix I need.  More messy math(s), yuck!

Reference Frames and Rotation Matrices

As background, here’s my current matrix which is flawed

|eax| = | cos(pitch), sin(roll), -sin(pitch)         | |qax|
|eay| = | sin(pitch), cos(roll), -sin(roll)          | |qay|
|eaz| = | sin(pitch), sin(roll), cos(pitch).cos(roll)| |qaz|

To make sense of what’s below, please go and read this PDF, particularly section 1.2 describing the frames of reference of a quadcopter.  As a quick summary…

  • Inertial frame  (ƒi) – this doesn’t move – it’s axes (in Phoebe’s case) are North, West and Down (gravity). This then converts to the…
  • Vehicle Frame (ƒv) – the axes remain the same as above (North, West and Down, so the quad could be skew against this axis, but centred on the quad center of gravity. This then converts via yaw angle psi (ψ) to the…
  • Vehicle 1 Frame (ƒv1) – this aligns the frame with the quad’s fore-aft pitch axis. This then converts via pitch angle theta (θ) to the…
  • Vehicle 2 Frame (ƒv2) – this aligns the frame with the quad’s left-right roll axis. This then converts via roll angle phi (φ)  to the…
  • Body Frame (ƒb)- the frame whose axis align with the quad’s sensors.

I don’t have a compass (magnetometer) sensor, so will not be using the Inertial Frame as my starting point.

In addition, yaw measurement should really use the compass as the other option of integrating the gyro Z axis is only accurate short term. I might try it though when I’m next stuck for something to do.

So I’ll be starting from Vehicle Frame 1 but may, once this is working, add yaw and start from Vehicle Frame.

Here’s the matrix pretty much copied straight from Beard.

| 1    0     0    | | cos θ  0 -sin θ | | cos ψ  sin ψ  0 |
| 0  cos φ  sin φ | |   0    1    0   | |-sin ψ  cos ψ  0 |
| 0 -sin φ  cos φ | | sin θ  0  cos θ | |   0      0    1 |

If we then throw away yaw (ψ = 0), and collapse we get

| 1    0     0    | | cos θ  0 -sin θ | | 1  0  0 |
| 0  cos φ  sin φ | |   0    1    0   | | 0  1  0 |
| 0 -sin φ  cos φ | | sin θ  0  cos θ | | 0  0  1 |

Multiply these three together gives:

|    cos θ          0        -sin θ     |
| sin φ * sin θ   cos φ   sin φ * cos θ |
| cos φ * sin θ  -sin φ   cos φ * cos θ |

There’s a passing resemblance to my flawed matrix up at the top which gives me a level of confidence I hadn’t completely cocked it up, and that the updated matrix won’t have a disastrous effect on the flight, but should make them much more accurate flights.

I’ve updated the code and will test fly it tomorrow.

Math(s) help please!

It ain’t rocket science, but quadcopter science still isn’t simple. I’ve got my calibration wrong for the accelerometer. I’d omitted the fact that if Phoebe is static, depending on how she is leaning, gravity can be shared across 3 axes.  Don’t get me wrong, I’ve had this covered for ages converting from Phoebe’s axes acceleration to earth axes, but not for the accelerometer calibration. Critically, this means the trend lines for acceleration vs. temperature calibration are wrong leading to drift, and odd scaling between axes – i.e. everything I’m seeing going wrong right now.

Earth calibration

Earth calibration

Here’s a diagram of Phoebe’s sensor readings from the rear in red; her left side is tilted down by a negative roll angle θ.  Her pitch angle is 0 (horizontal). The only force is gravity shown in green as a positive value which is how the sensors measure it.  Hence the combination of the the qay and qaz sensors combines together to make up 1g of gravity.  Likewise, a different combination of qay and qaz cancel each other out to make 0g horizontal acceleration. The same applies to the X-axis and pitch angles, but using the Y-axis / roll angles is easier to visualize, and she does have a cute bum!

The result is the following ‘matrix’ for conversion from Phoebe’s accelerometer readings to earth coordinates:

eax = qax * cos(pitch) + qaz * sin(pitch)
eay = qay * cos(roll) + qaz * sin(roll)
eaz = qaz * cos(pitch) * cos(roll) - qax * sin(pitch) - qay * sin(roll)

If Phoebe is not accelerating, then the only force is vertical gravity, so eax and eay are both 0g and eaz is 1g. So assuming the accelerometer readings are perfect, and she’s sitting on the 21° tilt test rig with the fore-aft axis horizontal…

0 = qax * cos(0°) + qaz * sin(0°)
0 = qay * cos(-21°) + qaz * sin(-21°)
1 = qaz * cos(-21°) - qay * sin(-21°)

The calibration then involves calculating the offset and gain to convert accelerometer raw reading to ‘perfect’ qax, qay and qaz readings so that conversion to the earth axes leads to desired (0, 0, 1) as above Accurate calibrated Z axis offset and gain already exists from the previous calibration testing.  So together the equations collapse down to

0 = qax
0 = qaygain(qayraw - qayoffset) * cos(-21°) + qaz * sin(-21°)
1 = qaz * cos(-21°) - qaygain(qayraw - qayoffset)sin(-21°)

The Y offset can be calculated from the raw Y sensor readings with port and starboard pointing down on the rig – any difference between the two gives the offset:

qayoffset = (qayport down + qaystarboard down) / 2

As a result, the calculations collapse further to…

qaygain(qayraw - qayoffset) = -qaz * tan(-21°)
qaygain(qayraw - qayoffset) = (qaz * cos(-21°) - 1) / sin(-21°)

And now I’m bemused / confused.  qayoffset is known which means I have, what seems to me, 2 none equivalent equations for the qazgain Help!  Where have I gone wrong?

Cause of my two biggest problems diagnosed?

  1. lack of drift control by my drift control code
  2. mismatched scales between horizontal and vertical axes

Let’s deal with drift control first:

 pa_target = -math.atan2(evx_out, 1.0 + eaz - eaz_offset)

This essentially says the required pitch angle is the inverse tangent of the desired horizontal acceleration (the corrective output of the horizontal velocity PID) divided by the actual vertical acceleration.

The math(s) is right, but  both top and bottom of the fraction are plagued with noise; the top due to PID d-gain, and the bottom because it’s a direct feed from the accelerometer.  The two noise sources aren’t in phase with each other, so this is what you get as the target pitch angle:

Target pitch angle

Target pitch angle

Removing the noise from the horizontal velocity PID output (evx_out) is just a case of setting the D gain to zero.  Handling the vertical accelerometer noise is a diferent matter – I think only reducing the dlpf will help here.

Regarding the horizontal / vertical scale difference, I’d been tinkering to ensure angles are calculated consistently throughout the code – in particular the Euler angles, conversion matrix, and the equation above.  The aim was to eliminate yaw in all calculations by eliminating any cross-pollination between X and Y axes between Phoebe and the earth  (or putting another way, making sure that if Phoebe yaws, so does the earth, so that viewed from above, the horizontal axes align.  If I had a way to correct yaw, then I’d need to add it back into all the math(s)  but I don’t!

As a result the scaling seemed to improve significantly: all axes now are out by a factor of about 2.5 – but are self consistent.

The flight ascended to ≈2.5m and drifted by ≈4m.

Flight path

Flight path

Equal scale axes

Equal scale axes



Nothing new to see here, but I thought I’d show you more detail of the problem I’m trying to resolve.

The graphs below shows the distances travelled in the X, Y, and Z axes over a flight – derived from accelerometer readings; the top graph transformed to earth axes, the lower one direct from the accelerometer axes.

Earth axis distances travelled

Earth axis distances travelled

Quad axes distances travelled

Quad axes distances travelled

The Z axes are a pretty close match (take-off to 1m, hover and descend), but the X and Y differ significantly; given that Phoebe was close to horizontal, they too should be a close match.

The earth axes version shows horizontal drift under a meter in the X and Y directions.  In contrast, the direct sensor reads show a drift between 4 and 8 meters in the X and Y directions.

In reality, there was a drift of about 8m total in all, suggesting the sensors are working correctly, but that faulty matrix conversion is suppressing this somehow.

Now I’d be happy to tweak the matrix to correct this – the change is simply swapping a couple of minus signs around; except I have a strong belief the matrix in use is correct in that when Phoebe is stationary but on a tilt on the ground, the earth axis readings are 0, 0, ~1) g clearly showing the only force in place is gravity in the earth z axis.  I’ve tested this with her at up to 30° of tilt and it works beautifully.

And yet, it is almost certainly this same matrix conversion which makes the huge drift imperceptible in the earth axes, and the last but most frustrating bane of my life.  You wouldn’t believe the numbers of sheets of paper littered around my desk with diagrams of vectors and angles trying to prove which is the correct implementations.

I’m on hold again waiting for some spare parts (new case for the Raspberry Pi) which I’m expecting Wednesday – once rebuilt my plan will be to actually just try the revised matrices, and see if any give better results than shown above.

Once more, fingers crossed.