# 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(θ)`

not

`δ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.

# Blind, deaf, disorientated, lost and scared of heights…

and yet, amazing.

So in that last video of a ~10s flight, Phoebe drifted about 1m when she shouldn’t have.  To me, that’s amazing; to you, that may be underwhelming.  So here’s why you should be amazed.  Basic school math(s):

distance = ½ x acceleration x time² ⇒ plugging in the numbers above says she was accelerating at 0.02m/s² (2cm/s²) instead of zero to travel 1 meter in 10 seconds.

Gravity is roughly 10m/s².  The accelerometer is set to read up to ±4g.  So that’s a range of 8g or roughly 80m/s²

So 0.02/80 * 100 = 0.025% error or a ±8 error value in the sensor range of ±32768.

Now the critical part – the sensors are only rated at 2% (±655) accuracy, and yet I’m getting 0.025% or 80 times that degree of accuracy.

And that’s why I don’t think I can get things very much better, whatever I do.

There is a slight chance that when the A2 is released (sounds like that’s shifted to next year now), I may be able to run the sampling rate at 1kHz without missing samples (I’ve had to drop to 500Hz to ensure I don’t miss samples at the moment).

Other than that though, she needs more sensors:

• camera for motion tracking (!blind)
• ultrasonic sensors for range finding (!deaf)
• compass (!disorientated)
• GPS (!lost)
• altimeter (!scared of heights).

but they’ll also need an A2 for the extra processing.  So this is most definitely it for now.

# Moron reference frames

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!

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

# Phoebe is right handed

Because of the math(s) problems, I had to start questioning anything to do with angles in Phoebe’s code.

I’ve confirmed the MPU6050 uses the right hand rule consistently throughout.  For the accelerometer:

• forward acceleration (X-axis) is a positive value (index finger points forward))
• leftward acceleration (Y-axis) is a positive value (middle finger points left)
• vertical acceleration (Z-axis) is a positive value (thumb points up)

This results in nose down being a negative pitch angle and port down also being a negative roll angle, which matches Phoebe’s code.

For the gyroscope, with your right hand in a fist and thumb pointing in a positive direction, and viewing the axis from behind, clockwise rotation is positive.  This does lead to an oddity: positive rotation around the Y-axis leads to positive pitch rotation which when integrated from zero produces a positive angle, which is the opposite from the accelerometer angle.  Luckily, the code negates the Y-axis gyro readings to they both agree on the sign of the angle.

18 months ago, I just worked this all out through experimentation, but it’s good to know the right-handed rule for my next post.

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

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

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

Equal scale axes

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

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

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