# SSDD

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

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.

# Dynamic accelerometer calibration II

Picking up from where I got to before, I’m going to follow the path where the accelerometer raw data ellipsoidal space is near enough to spherical that I can forget the rx*_gain values.

The errors this will introduce should be relatively inconsequential: first the gains are only a per-cent or two; secondly, the critical factor here is that the sensors read zero when horizontal (i.e. corrected by offsets), but any minor gain errors will lead to angle and speed errors, but critically, these will still read zero when they should, and the errors will be handled by the PIDs.

On that basis, let’s see where that leads.  Here’s what we have to work with so far

```qx = (rx + rxoff) * rxgain
qy = (ry + ryoff) * rygain
qz = (rz + rzoff) * rzgain

qx2 + qy2 + qz2 = 1

ex = 0
ey = 0
ez = 1

|ex|   |cos(pitch),         0,          -sin(pitch)||qx|
|ey| = |         0, cos(roll),           -sin(roll)||qy|
|ez|   |sin(pitch), sin(roll), cos(pitch).cos(roll)||qz|

pitch = atan(qx / qz)
roll = atan(qy / qz)```

So without the gain excluded, we have the following – and we have 3 variables fewer to calculate.

```qx = (rx' + rx'off)
qy = (ry' + ry'off)
qz = (rz' + rz'off)```

Sat on the ground, the only force acting on Phoebe is vertical gravity, so the earth axis values should read (0, 0, 1) g, as should the offseted sensors once matrixed.

```ex = 0 = (rx' + rx'off) * cos(pitch) - (rz' + rz'off) * sin(pitch)
ey = 0 = (ry' + ry'off) * cos(roll) - (rz' + rz'off) * sin(roll)
ez = 1 = (rz' + rz'off) * cos(pitch) * cos(roll) + (rx' + rx'off) * sin(pitch) + (ry' + ry'off) * sin(roll)```

Replacing pitch and roll with the raw data gives

```pitch = atan((rx' + rx'off) / (rz' + rz'off))
roll = atan((ry' + ry'off) / (rz' + rz'off))```

That leaves us with only the 3 offset variables, r*off to calculate based upon the raw sensor data, r*.

```(rx' + rx'off) * cos(pitch) = (rz' + rz'off) * sin(pitch)
(ry' + ry'off) * cos(roll) = (rz' + rz'off) * sin(roll)
(rz' + rz'off) * cos(pitch) * cos(roll) + (rx' + rx'off) * sin(pitch) + (ry' + ry'off) * sin(roll) = 1```

Not the easiest of equations to solve as I don’t think I can simplify the atan’s sine’s and cosine’s, but I’ll have a go.

`(rx' + rx'off) * cos(pitch) = (rz' + rz'off) * sin(pitch)`

Dividing both sides by cos(pitch) leads to

`(rx' + rx'off) = (rz' + rz'off) * tan(pitch)`

`(rx' + rx'off) = (rz' + rz'off) * (rx' + rx'off) / (rz' + rz'off)`

Thus successfully proving that

` 1 = 1`

10/10 for effort, 0/10 for results!

# “Hmm, that’s interesting but…”

“but what?” (sorry, too much Matrix recently – there is a contextual link though)

Here’s the plot from the same test run, but this time with accelerometer data aligned with Phoebe’s axes – virtually raw from the sensors rather than converted to earth coordinates.

Phoebe axis accelerometer

If you compare it to the plot from the previous post, there are similarities in the X and Z axes: Phoebe climbs to roughly 1m and drifts in the X direction by a meter showing signs of drift correction in action; but from Phoebe’s Y axis point of view, she drifted nearly 5 meters towards the wall she ended up hitting, and yet there’s no sign of that in the Earth coordinate plot from earlier.

Given the difference between these readings is just the coordinate conversion matrix (see, told you so), it seems I definitely have a bug there in the Y axis conversion!

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

# Tom West’s Request

Tom asked me in a previous comment to show diagrammatically how all the bits fit together. Here you go, Tom.

Software Layout

From left to  right:

• the target for the horizontal speed PID is defined by user input (remote control or pre-programmed)
• the input is integrated from the accelerometer
• the output is a desired horizontal acceleration to make input and target match
• the horizontal acceleration output is converted to the angle target for the absolute angle PID by a slightly arbitrary atan2() function comparing horizontal acceleration against gravity to produce the desired target angle
• the input comes from both the integrated gyro and the Euler angles derived from the accelerometer merged together by a complementary filter
• the output provides the target for the angular rate PID
• the input comes direct from the gyro
• the output value is then used to change the PWM driving the motors ESCs to achieve the desired horizontal speed.

The resultant output is then applied to the propeller motors ESC PWM signals – so for a forward tilt, half the output is added to the rear motors, and half the output is subtracted from the front motors.  The same applies to the Y axis.

The Z axis PID is hardly worth showing a diagram but it’s there at the bottom, just one PID – target is a desired vertical speed, input is integrated from the accelerometer, output is fed to all props equally.

The matrix is there to convert sensors readings from the tilted quadcopter to earth coordinates – essentially accelerometer readings get matrixed into earth axis vectors before integrating to give horizontal and vertical speeds to feed the left-most of the PIDs.

# Fixing the Matrix

The Murder / Suicide flight is almost certain to be due to a bug in the current Matrix:

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

I’ve arranged it such that if Phoebe is nose down, or left-side down, the pitch and roll angles are negative respectively.

Forward, leftward and upward movement results in positive accelerometer values.

The diagram below shows the vectors representing Phoebe pitching nose down and moving right to left as a result.  Her sensor vectors are in gold, and their translation to earth axis vectors in black.  Phoebe is pitched nose downwards, so the angle of pitch is negative.

Vector conversion

Here’s the revised value of the Matrix that ensures the earth vectors match those shown in the picture.

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

Critically, this means that when calculating eax based upon the faz vector, because the pitch angle is negative, so is sin(pitch), yet the eax vector is showing forward (and therefore positive) acceleration.  Hence right hand column of the matrix where it shows -sin(pitch) and -sin(roll).

The same is not necessary when deriving eaz from the fax vector; eaz is shown as negative, and so fax*sin (pitch) already produces the correct negative result.

The case for the y axis is identical.

Hopefully there will a slot in the weather tomorrow for me to test this.

# Attempted Murder Suicide

It’s been a bit quiet here for the past few days as I’ve been testing drift control and until yesterday, there wasn’t a lot to report, so I didn’t bother.

Yesterday’s testing seemed to show progress in the right direction – Phoebe sort of held her own against a 20mph tail wind, before tilting into the wind at an increasing angle towards me.  So I called it a day, knowing the problem was with the ‘matrix’ I was using, and set out to find the problem.

So I went out to the park earlier today with a revised version of the matrix to try.  And that’s when it happened – despite the similar 20mph tail wind, she shot up off the ground, and headed straight for me, directly into the wind.  I had to dive off the stool I was on (she just slashed open the sleeve of my down jacket) and having missed me, she smashed herself into the ground, destroying one propeller and having a really good go at chopping several cables, which luckily they were out of reach.

I have no idea what the lesson is from the software / sensors point of view yet, but I have definitely learned Phoebe is a long way from being trustworthy!

P.S. And do you know what’s worse, Phoebe managed to unplug her power supply, meaning no stats were written (they’re stored in shared memory and dumped to file at the end of each run), meaning I’ve got to do the same thing again tomorrow!

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