# Further thoughts from the same flight data.

Angles vs. Distance

The above is a plot of distance travelled in the x axis, (edx, cyan) – positive is forwards – and the pitch angle Phoebe is tilted by (c pitch, green) – positive is nose-up providing reverse acceleration.

It does show drift control in action: as distances change (i.e. there is a non-zero velocity), the pitch angle changes to produce acceleration in the opposite direction to kill the velocity and stop the drift.

OK, it’s a bit wavey, but at least the idea’s right.  That’s enough motivation to keep tuning.

# A different lack of drift

I did a couple more test flights this morning after replacing the blades with 10 x 3.3’s, removing the accelerometer calibration and tweaking the horizontal velocity PIDs.

You may remember from a previous post with a different setup that drift occured during takeoff and landing, but not during hover.

Well today’s run was similar yet different: no drift during take-off, but drift starting during hover and increasing during landing:

Zero take-off drift

It’s as though the drift control PID is tailored to a specific section of the flight.  The difference between the 3 phases of flight is primarily the vertical acceleration.

The flight from the previous run coped with 1g (hover), but not with > 1g (takeoff) or < 1g (landing)

This flight coped with > 1g vertical acceleration (take-off) but not with <= 1g (hover and landing).

The primary role the vertical accelerometer plays with horizontal acceleration is in the angles calculated.  With 1g vertical acceleration and 0g horizontal, angles are spot on (ignoring gain / offset errors).  However, with > 1g vertical acceleration, calculated angles are smaller, and similarly with < 1g, the calculated angles are larger.

It’s these angles which are then used as the targets in the absolute angle PIDs to define the angle of tilt for the quad required to counteract any horizontal drift.  And that gives me a couple of ideas to try.

Tomorrow is looking to be a lovely day for testing in the park with temperatures in the teens, and wind at 7mph.  Looking forward to trying out my theories.  I’ll let you know what I find tomorrow.

# Unbalanced Complementary filter?

UPDATE: My speculation from yesterday’s blog below is almost certainly complete bollox. There is no indication from the gyro specs that the gyro drifts other than due to temperature; it’s the integration of gyro output over time which is bound to drift. The complementary filter uses the change in angle since the last sample to update the overall filtered angle – any drift gets filtered out to 50% by the tau period. Since the angular rate is not drifting, this short term update is fine – the key here is it does not use the integrated gyro as an absolute angle – it only uses the angular rate.

As you can tell, I’m starting to get a bit deperate trying to work out why Phoebe drifts and how to control it.  I have completely OTT calibration and yet she still drifts – I really am running out of ideas short of implementing a Kalman filter which I really don’t want to do as I hate to use someone else’s Magic Box solution.

Phoebe drifts, regardless of what I do, and it’s driving me nuts.

The temperature / sensor calibration I did recently was once attempt to correct this, but it’s hard to see whether that’s had any real effect – it may well be a factor but does not appear to be the dominant factor.

Which moves me on to the complementary filter – it looks unbalanced to me. Let’s have a look in more detail:

```tau_fraction = tau / (tau + delta_time) c_angle = tau_fraction * (prev_c_angle + gyro * delta_time) + (1 - tau_fraction) * euler_angle prev_c_angle = c_angle ```

As a reminder, tau is a tuned period of time where the gyro and accelerometer inputs contribute equally to the overall filtered angle. For the sake of a real example, my tau is 0.5s, and the loop time is 50Hz (0.02s) leading to a tau_fraction of 96% per processing cycle.

The complementary filter is a simple fractional mix of short term accuracy in integrated gyro plus long term accuracy of accelerator Euler angles. This wipes out the long term gyro drift by increasingly using the long term stable Euler angles. To that extent, it works.

But imagine that the quad is hovering, and has been so for long enough that the gyro drift has been overwhelmed by the accelerometer Euler angles. But the gyro still has drift, and this drifted value is what’s used in the short-term reaction to events.

Now surely it would be better to use a (gyro – drift) value to update the short term changes in angles? But how to measure drift long term? Well my speculation is that it’s just this:

`drift = i_angle - c_angle`

where i_angle is the previously integrated angle from the gyro over the whole of the flight. As a result, it’s dominated by drift.

So removing the tau filter for the moment, and add in the drift compensation:

`c_angle = (prev_c_angle + (gyro * delta_time - drift))`

or

`c_angle = (prev_c_angle + (gyro * delta_time) - (prev_i_angle - prev_c_angle))`

or collapsing that down

`c_angle = (2 * prev_c_angle) + (gyro * delta_time) - (prev_i_angle)`

finally adding back the complementary filter

`c_angle = tau_fraction((2 * prev_c_angle) + (gyro * delta_time) - (prev_i_angle)) + (1 - tau_fraction) * euler_angle`

It seems possible this could be more accurate by removing long-term drift from short-term changes in angle processing – thus enabling longer flights. In this case, a long flight only means a few seconds.

Something else for me to try next week once the weather improves and the replacement blades arrive (don’t ask!).

# Phoebe, where on earth do you think you’re going?

The YouTube motion control video got me thinking: in their setup, their sponge ball tracking systems gives them centimeter accuracy of the speed, position and orientation of the quad(s).

Phoebe uses the accelerometer with coordinates reorientated to the earth’s axes using the various angle calculations to do the same for speed and position.  She currently doesn’t track yaw ( => orientation) – I just try to minimize it using the z-axis gyro – I’ll need a compass to track it accurately.

And despite all my best attempts, she drifts and I’m really struggling to track down why, primarily due to a lack of a plan.  I’d just assumed the drift was due to bad PID tuning, or poor angles reorientating the quad accelerometer axis to earth coordinates; my approach has just been to tweak PIDs and other config parameters in search of the holy grail combination, but so far to no avail.

So in today’s tests, I’ve converted her sensor plots into a map of where Phoebe thinks she’d been compared to where I saw her go.

Phoebe’s Flight Map

And actually, despite having to double integrate acceleration to distance, it’s a reasonable match of the flight.

She moves forward by roughly 0.9 meters, and moves to the right (negative is reverse / right direction) by about 0.5 meter.

However…

1. it’s clear she’s trying to correct left / right drift for a bit before she just goes off on one
2. it’s also clear she doesn’t make a similar effort to correct the forward drift
3. the left / right scale is out by a factor of 5 – she eventually drifted right by about 2.5 meters

The first problem suggests that once the complementary filters starts to favour the accelerometer, then drift protection starts to fail.

The second problem suggests I may have my forward / backward drift compensation the wrong way round, so she accelerates forward in compensation for forward acceleration – though I’m not convinced here.

The cause of the third problem I can’t explain, but the difference in resolution in the accelerometer axes makes it hard for consistent calculations for drift – not good

For me, I’m delighted, as by mapping her path, I’ve found lots of useful facts that previously I’ve just been speculating on.

Further thoughts a few hours later…

The consistent forward acceleration (rather than drift compensation) is due to a bug I fixed a while back that seems to have crept back in  – my bad.

The short term left / right drift compensation seems to just need PID tuning – I think oscillations like this are usually due to an excessive P, so I need to reduce that, and consider adding some I (for “return to where you a started”) and D (for “get a shift on now!”.

The longer term left / right drift’s (lack of) compensation is most interesting: the equidistant dots suggest strongly Phoebe believes she is moving to the right at constant speed and is horizontal; that matches what I saw. Bearing in mind this is sensor data, it’s used as PID feedback, so the Y axis horizontal speed PID should do something about this, yet it doesn’t appear to.  For the moment, I have no idea how this can come about so it’s frustrating – interesting too, but primarily frustrating.

I have no idea where to start about the factor of five scaling difference between the axes; I’ll just have to keep my eye on it during Monday’s testing.  Currently, the only solution is a dirty hack to multiply the left-right earth coordinate acceleration by 5 before integrating.

Finally, I’m still delighted that producing this map works, and provides a new kind of insight into flight problems.

P.S. Anyone else think this looks like an eagle soaring?

# Building “The Matrix”

Sadly not the Keanu Reeves version – I think we’d need an spherical cluster of Raspberry Pi’s much larger than the Sun to build an earth Matrix, and once you’ve got that many, you may as well let gravity take its course and let it collapse into a real solar system akin to Deep Thought 2.

Anyway, the matrix in question is the one that can convert from the quadcopter axes accelerometer outputs to the earth coordinate system. Currently it looks a bit like this:

````|Xe| = |0, 0,          sin(pitch)||Xq|`
`|Ye| = |0, 0,           sin(roll)||Yq|`
`|Ze| = |0, 0, cos(pitch)cos(roll)||Zq|````

The matrix is only converting data from Phoebe’s Z axis into data for the Earth’s X, Y and Z axes.

The empty 2 columns add Phoebe’s X and Y accelerometers to the equation.  Filling in the rest of the Matrix is a case of drawing a lot of triangles.

The triangle for the X accelerometer maps from Xq to Xe and Ze suggesting

```Ze = Xqsin(pitch)
Xe = Xqcos(pitch)```

Adding these couple of entries to the Matrix:

````|Xe| = |cos(pitch), 0,          sin(pitch)||Xq|`
`|Ye| = |0,          0,           sin(roll)||Yq|`
`|Ze| = |sin(pitch), 0, cos(pitch)cos(roll)||Zq|````

Doing the same for the Y axes with roll angles yields

````|Xe| = |cos(pitch),         0,          sin(pitch)||Xq|`
`|Ye| = |0,          cos(roll),           sin(roll)||Yq|`
`|Ze| = |sin(pitch), sin(roll), cos(pitch)cos(roll)||Zq|````

Currently, there’s a high probability this is wrong, as although it looks right to me and my aged math(s), it doesn’t ‘feel’ right – my brain needs a night’s sleep to mull it over. And for the moment, that’s fine – the code is running pretty well on just that third column of the matrix, and tomorrow is looking like a good day for testing again, so once I’ve retested the hover to build confidence and hopefully shoot a video, I might try some horizontal movement looking for errors that the additional columns in the matrix would fix.

# Park testing

It’s a lovely day here at the edge of the Cotswold; bright sunshine and absolutely no wind.  So I went to the park.

First flight was perfect – wow I thought, wish I’d brought my camera.

Second flight drifted by 10m and I was glad I hadn’t bothered.

After numerous flights trying to work out what on earth was causing the drift, lots of watching take-off for common features revealed the truth:  take-off from non-horizontal ground means there is some lateral power applied by the blades, leading to the drift.  As the take-off tilt is corrected, the drift stops accelerating, but still continues at constant speed due to momentum.  Hence the overall drift across the flight.

By having pre-flight Euler angle checks, Phoebe knows immediately she’d not on a flat surface, so the problem is all due to how fast she can compensate and get herself level using the mix of the DLPF and complementary filters along with the absolute angle PID.  Now I know I’ve mentioned these loads of times; the difference here is that I now have a greater understanding of how they are interacting.  And with that in mind, I could watch a take-off carefully, looking at the take-off angle, watch how long into the flight it took for Phoebe to correct that angle, and therefore how much drift speed she gained until she levelled out.

And as a result, I have a much better set of default configuration figures for DLPF, complementary filters and absolute angle PID gains – not perfect perhaps, but possibly good enough that if there is wind on another day, I can rely on these default to then start looking at the horizontal drift kill testing using the horizontal velocity PID.  And that is a big step forwards because once that’s sorted, it’s on to phase 2 and a remote control

# I used to be indecisive…

but now I’m not so sure…

whether I actually need a horizontal acceleration PID.  I’d considered adding the horizontal acceleration PID between the horizontal speed PID and the pitch / roll angle PIDs.  Its output would set the angle target for those PIDs, but I’ve come to the conclusion that there is a direct mathematical link between horizontal acceleration (the output from the horizontal speed PID) and the tilt angle target set for the angle PIDs

Consider we want 1g acceleration horizontally, and 1g acceleration vertically to give us horizontal acceleration – i.e. no rise or fall in flight height.  Then the power must be applied equally horizontally and vertically, meaning a 45º tilt.  Or putting it slightly clearer, the acceleration from the accelerometer is measured in g’s, so if we setup the gains of the horizontal speed PID to output in (approximate) g’s, and denote earth axis acceleration along the x and z axes as aeax and aeaz respectively then

tan(θ) = aeax / aeaz

But for horizontal flight aeaz = 1g. So

θ = atan(aeax)

This means the target acceleration from the horizontal speed PID output (i.e. target acceleration) if measured in units of g maps directly to a target angle for the pitch / roll angle PIDs which is “a good thing”TM as it’s much better than adding a noisy accelerometer PID into the system.

# Too much test tinkering to do today.

I managed to squeeze in 10 or 12 test flights (3s take-off, 3s hover, 3s land).  Phoebe’s new legs are holding up beautifully – nutlock + a multitude of washers has definitely given her extra strength.

I’m convinced now I have my anti-drift / non-flat ground take-off code right – just the fact I made so many flights without damage means things are heading in the right direction, but they also raised several issues.

• DLPF vs tau – low Digital Low Pass Filter (5Hz) gives stable take-off from vertical speed PID, but gives inaccurate angles due to valid data being filtered out. Upping DLPF to 20 or 40Hz lets in more accelerometer accuracy but also noise, leading to bumpier take-offs and a need for the complementary filter for angle accuracy to be upped from 10Hz to 5Hz to keep the noise out of the angles.  Hmmm, not sure what to do here yet if anything.
• YAW is still plaguing power distribution – the anti-YAW pid is working well, but it’s clear on landing that one pair of blades (front right and back-left I think) are spinning very much faster than the other pair to suppress the yaw. All the blades are new, well balanced, and attached firmly to their motors, so perhaps I have a duff-motor.  Hmmm, I wonder which one?
• Intermediate PID tuning – horizontal speed P-gain at between 0.25 and 0.3 is having the right effect, but is sluggish. I suspect the problem is I need to tweak the PID gains of the angle PID to make sure we get higher angular acceleration targets for faster self correction.

YAW is the priority – the whole point of setting up the quadcopter with different spin direction is to suppress yaw so I shouldn’t have to be dealing with it, and yet it’s the dominant factor now, smothering some of the more subtle motor spin PIDs’ effects.

After YAW, the DLPF / complementary filter conflict comes next – essentially we want the highest DLPF frequency and also the highest complementary filter frequency.  To make that work, noise needs to be removed from the system.  I’ve already got rubber standoffs supporting the sensor breadboard, but now I’ve also removed all PID D-gains – they provide super fast reaction to problems but at the expense of noise – fast reaction = fast acceleration = noise that registers as the DLPF frequency is increased.  We shall see (hopefully) tomorrow the effect this has.

# Weather to do Christmas testing?

Phoebe is rebuilt, and due to the lousy weather, I’ve been browsing the code and found a couple of bugs that might have been affecting her accuracy in angles (thanks Federico!). Trouble is the weather forecast says the best days for testing are Christmas Day and Boxing Day (which also happens to be my wife’s birthday). To make it worse, the kid’s grandparents and aunty are with us over Christmas, so I’m looking for excuses to take an hour out for Phoebe testing – I’m open to suggestions – please comment if you have any inspired ideas!

# Current testing strategy

I’ve lost a degree of confidence in my previous diagnosis of the randomness of Phoebe’s current testing due to the breadboard adhesive.

So I’ve been trying some more structured testing, purely focussed on vertical takeoff from a non-horizontal platform. I’ve tweaked the code so it only does a take-off and landing, skipping the long hover stage which is the most prone to drift (and crashes). I’ve also changed the filter frequency for the complementary filter – the transition between integrated gyro and Euler accelerometer previously happened at about 2Hz (0.5s) meaning that take-off is controlled primarily by gyro which doesn’t ‘see’ the non-horizontal take-off platform. Because Phoebe’s code loops between 125 – 175 Hz depending on the level of diagnostics, I can safely change the filter frequency to 10Hz (0.1s) which means the accelerometer Euler angle can play more of a role during takeoff but still allowing the filter to remove the accelerometer noise from the sample.

And this had a good result – she was able to take off vertically from a none horizontal platform; then she’d seem to change her mind, and start to drift;  I suspect strongly this is due to the accuracy of the Euler angles adding increasingly into the complementary filter over time, so I tried increasing the filter frequency from 10Hz back to about 2.5Hz prolonging the interval where the Euler angles start to dominate, and things got better.  Time to review my filter code, and the Euler angle code, I think.

Again though, on the plus side (as ever), this did seem to be a level of control I’d not seen previously – which is “a good thing”TM.

P.S.  Found the bug (I think but only tomorrow’s testing will tell): the accelerometer outputs had been calibrated to read (0, 0, 0) in the X, Y, Z axes respectively when horizontal to then work out horizontal and vertical speeds.  But the Euler calculation expects the values to be (0, 0, 1) – i.e. including gravity in the z axis to produce accurate angles.  The code achieves this by using the uncalibrated values.  But in doing so, it meant that the horizontal then read (0.001523, -0.001071, 0.956401) which are the calibration offsets themselves instead of the expected (0, 0, 1).  So as Euler started to dominate the complementary filter calculation, Phoebe started to think she was at (0.1, 0.06, 89.9) degrees to the horizontal – subtle, but enough to trigger a compensatory tilt and send her off accelerating.  In fact the calibrated values should be used for Euler, but the z-axis value needs +1g  adding to it to ensure horizontal returns the expected values of (0, 0, 1) exactly.

P.P.S. Today’s testing suggested that the P.S. is right – right up until the breadboard did break loose from the frame producing all kinds of nutty test flights – so I may well have been right about the breadboard too – we’ll see tomorrow once the glue has dried on the breadboard.