POV

The problem: the camera point of view is in the quad frame; the garmin point of view is in the earth frame.  They need to both be in the same frame to produce a vector that’s meaningful.  A pretty radical rewrite of this area last night resulted.  A test flight this morning sadly was pretty much the same as yesterday: a very stable hover, but shooting off right when she should have gone left.  More stats:

POV

POV

The top pair of accelerometer vs camera show pretty good alignment, right up to the point of 0.4m to the right.  I believe this is correct, but I wouldn’t put money on it yet!

The middle pair are accelerometer vs LiDAR height over time, which is excellent.

The bottom pair are the flight plans in earth and quad frames (the quad one is simply the earth one rotated from my to her POV) – this is where there’s clearly a problem – they should be the same but they are wrong once the flight rotates.  I can’t see an obvious bug in the code, which makes me suspect there’s an obvious bug in my understanding instead.

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.

Wot no Kalman!

Before I’d even finished my previous post, I’d found a solution that meant I didn’t need a Kalman filter just yet

It all started with this, describing how to merge accelerometer Euler angles with gyro angular rate transformed from body frame to the 3 Euler frames.  But as per usual, it wasn’t enough; while I understand the complementary filter well, I didn’t understand the conversions of gyro rotation rates in the body frame to the Euler frames.  Until I found this.

That now allows a complementary filter to track Euler angles short term via gyro and long term with the accelerometer.  I know I’ve done this before, but this time, I’m merging measurements in the same same set of Euler frames so the results should be correct.

A bit of manic coding last night, combined with a repeat of yesterday’s tests showed this.

Complementary Euler

Complementary Euler

Compare this with the graph from yesterday’s equivalent tests:

net acceleration

net acceleration

I hope you agree this is much better – as expected there are roughly sinusoidal lines about 90° out of phase as Phoebe rolled around the floor in circles.

No option now but to test this new code live – need to find where I left my bravery first though!

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!

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.

What’s tilt for?

Tilt is the angle between the Phoebe-frame Z-axis, and Earth-frame ‘vertical’.  It drops out of Euler calculations.  It seems the ideal way to work out the conversion from the earth-frame vertical velocity target to the quad frame vertical velocity target thus:

qvz = evz / cos(tilt)

i.e. if the quad is tilted then the power applied to all four motors is increased to maintain the correct earth axis vertical speed.  Seems obvious doesn’t it?

Yet up to now, I’ve been using the rotation matrix to take Earth-frame vertical velocity target to the quad-frame.  Here’s what that looks like:

qvz = evx * (c_ra * s_pa * c_ya + s_ra * s_ya) + evy * (c_ra * s_pa * s_ya - s_ra * c_ya) + evz * c_pa * c_ra

where

  • c_pa = cos(pitch angle)
  • s_pa = sin(pitch angle)
  • c_ra = cos(roll angle)
  • s_ra = sin(roll angle)
  • c_ya = cos(yaw angle)
  • s_ya = sin(yaw angle)

I’m struggling to believe these two very different equations produce the same results.  So I ran a test.  Without the motors running I picked Phoebe up and rotated her around her X and Y axes producing a broad range of tilt:

Euler vs. Beard

Euler vs. Beard

  • green  is the vertical velocity target in the Earth-frame
  • yellow is using the Euler tilt angle to transform the earth target to quad-frame target
  • blue uses beard to do the same.

The flight plan is rise for 3s, hover for 5s and descend for 4.5s – the rest of the time is spent softening the transitions between these targets.

Anyway, back to the plot.  While the target is 0, the tilt has no effect; that happens only for non-zero targets as you can see on the graph.  I’m sure you’ve also noticed the Euler tilt and Beard matrix results don’t match.  Similar in shape but inverted.

Euler looks right to me; as Phoebe tilts, Euler ups the quad-target to ensure the earth-target remains as set.  Beard seems to be doing the exact opposite.  I need to understand this discrepancy as I’m relying on Beard for horizontal velocity too where Euler can’t help.

Any thoughts?  Probably an axis error in my implementation of Beard I guess?

I tried a test flight using Euler tilt angles rather than Beard’s matrix for conversion of the vertical velocity PID target in Phoene’s frame.  Phoebe climbed higher, but seemed less stable.  Then went back to Beard, and yet another near perfect flight.  So Beard wins even though Euler’s tilt seems more logical to me!

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.!

Footnotes:

  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!

Dodo!

Or in fact “D’oh! D’oh!”.  As you might guess, two bugs / enhancements found while snoozing on the sofa, musing over some of this morning’s flights:

  1. if you have nested PIDs, try to only include the integral gain in the outermost PID to ensure best performance.  Therefore tune from inner to outer (i.e. next outer gains set to 0 when tuning inner), and once tuned, increase the tuned inner P gain by up to 50%, set the I gain to 0, and reenable the next PID gains – i.e. tune angular rate, absolute angle and horizontal speed in that order.
  2. If you’re going to change how you calculate the Euler angles to match the coordinate system of the gyro angles, you really ought to change the Matrix used so that X, Y and Z angles are wholly independent (i.e. no sines).

Not finished yet with the inside-out re-tuning, but the progress so far looks promising.

Yet more park testing

Went to the park again today to test my new angles calculation; complete garbage – with Phoebe whizzing 25m forward before I killed the flight.  The change to the angles had left me worried about calibration, specifically for the accelerometer, and to be honest I still do, but luckily I can leave that for another day: after checking the stats, the cause is quite clearly the complementary filter.

Pitch angles

Pitch angle

As I mentioned, the flight had Phoebe whizzing forwards across the park – i.e. nose down, and that’s quite clear from the integrated gyro pitch angle (i-pitch); it’s not surprising that it’s not visible in e-pitch, the Euler angles, but the fact the complementary filter (c-pitch) had effectively ignored the i-pitch clearly shows the problem.  The tau was set to 0.25s but quite clearly from the graph the i-pitch contains critical, accurate information for at least a few seconds without obvious signs of drift.  I think I need to start playing with tau at nearer 2.5s!

Just for comparison, I’ve dug out some stats from when I first added the complementary filter; in this test, the blades were not running, I was just manually tilting Phoebe – look how the i-pitch and e-pitch track each other, while the filter output (c-pitch) cancels out the gyro integral drift (i-pitch) – all shown in shades of blue:

Sensor Angles

Sensor Angles

Hopefully with tau sorted, I can go back to checking my new angles and putting my mind at rest about whether gravity calibration is correct / useful – more on that once I’ve made a decision.

Just an afterthought: it’s clear from the tests that the accelerometer Euler angles are varying wildly, so perhaps it’s better, at the same time as (say) doubling up tau to 0.5, I also increase the dlpf to 5Hz.  Only testing will tell.

P.S. The big downward spike at the end was Phoebe doing a forward roly-poly at the end of the flight as her front legs clipped the ground while she was still travelling at several mph.  Funny to watch and zero damage.

Park Life

I took Phoebe for a play in the village park just now – it was a huge amount of fun for us both!  When the local kids are at school, it’s empty, and there’s a football pitch sized area of just grass.  It’s also just a few meters higher than our garden (we’re at the lowest point in the village), so while our lawn is currently somewhere between a quagmire and a lake, the park just had soft grass to land on, which, it turned out, is a very good thing!

The only downside is our garden has 6 – 10′ stone walls around it shielding it from the worst of any wind; the park is next to an open field, and today a 20mph wind was blowing.  So I sat, back to the wind and let her rip.  Off she flew at probably 20mph and reached the other side of the pitch in no time at all.  Her wind-drift protection wasn’t engaged so that’s actually what I’d expected (which is why I sat where I did), but I was surprised at how effective the wind was!

She did takeoff at a tilt and self corrected as the complementary filter started kicking in.  With dlpf set to 5Hz, I think I should be able to pull in the accelerometer much quicker.  I got it down to 10ms but I suspect I could go lower still.

And best of all, it was fun – there was nobody and nothing other than me that Phoebe could harm, meaning I could just watch the details of the flight and the change of behaviour as I tweak the config – very satisfying!

I’ll definitely be back there next time it’s not raining – Monday’s forecast is looking promising, and the wind is only expected to be just 14mph!

P.S. After reviewing the stats from the final flight, something useful has come from the 20mph flights too:  I’d been tuning the DLPF / complementary filter the wrong way round.  Under the belief that a low DLFP cuts out noise, I’d been reducing the fraction of time that accelerometer Euler angles became dominant.  But as a result, the stats showed I was tuning out the integrated gyro data.  This meant long term stability was fine, but Phoebe lacked the ability to react to the initial tilt she took off with.  So Tuesday’s priority is to see the net effect of the changes I’ve made – to allow more accelerometer data through (DLPF @ 10Hz rather than 5Hz), but then take longer to allow it through the complementary filter (0.2s rather than 0.01s).  Looking forward to seeing the result.