# Butterworth implementation details

I did a 1 minute hover flight today with the down-facing LiDAR + video reinstated to track the performance of the dynamic gravity values from the Butterworth as temperature shifts during the flight:

60s hover stats

To be honest, it’s really hard for me to work out if the Butterworth is working well extracting gravity from acceleration as the IMU temperature falls.  There are so many different interdependent sensors here, it’s hard to see the wood for the trees!

As an example, here’s my pythonised Butterworth class and usage:

```####################################################################################################
#
# Butterwork IIR Filter calculator and actor - this is carried out in the earth frame as we are track
# gravity drift over time from 0, 0, 1 (the primer values for egx, egy and egz)
#
# Code is derived from http://www.exstrom.com/journal/sigproc/bwlpf.c
#
####################################################################################################
class BUTTERWORTH:
def __init__(self, sampling, cutoff, order, primer):

self.n = int(round(order / 2))
self.A = []
self.d1 = []
self.d2 = []
self.w0 = []
self.w1 = []
self.w2 = []

a = math.tan(math.pi * cutoff / sampling)
a2 = math.pow(a, 2.0)

for ii in range(0, self.n):
r = math.sin(math.pi * (2.0 * ii + 1.0) / (4.0 * self.n))
s = a2 + 2.0 * a * r + 1.0
self.A.append(a2 / s)
self.d1.append(2.0 * (1 - a2) / s)
self.d2.append(-(a2 - 2.0 * a * r + 1.0) / s)

self.w0.append(primer / (self.A[ii] * 4))
self.w1.append(primer / (self.A[ii] * 4))
self.w2.append(primer / (self.A[ii] * 4))

def filter(self, input):
for ii in range(0, self.n):
self.w0[ii] = self.d1[ii] * self.w1[ii] + self.d2[ii] * self.w2[ii] + input
output = self.A[ii] * (self.w0[ii] + 2.0 * self.w1[ii] + self.w2[ii])
self.w2[ii] = self.w1[ii]
self.w1[ii] = self.w0[ii]

return output

#-------------------------------------------------------------------------------------------
# Setup and prime the butterworth - 0.01Hz 8th order, primed with the stable measured above.
#-------------------------------------------------------------------------------------------
bfx = BUTTERWORTH(motion_rate, 0.01, 8, egx)
bfy = BUTTERWORTH(motion_rate, 0.01, 8, egy)
bfz = BUTTERWORTH(motion_rate, 0.01, 8, egz)

#---------------------------------------------------------------------------------------
# Low pass butterworth filter to account for long term drift to the IMU due to temperature
# change - this happens significantly in a cold environment.
#---------------------------------------------------------------------------------------
eax, eay, eaz = RotateVector(qax, qay, qaz, -pa, -ra, -ya)
egx = bfx.filter(eax)
egy = bfy.filter(eay)
egz = bfz.filter(eaz)
qgx, qgy, qgz = RotateVector(egx, egy, egz, pa, ra, ya)
```

Dynamic gravity is produced by rotating the quad frame accelerometer readings (qa(x|y|z)) back to earth frame values (ea(x|y|z)), passing it through the butterworth filter (eg(x|y|z)), and then rotating this back to the quad frame (qd(x|y|z)).  This is then used to find velocity and distance by integrating (accelerometer – gravity) against time.

Sounds great, doesn’t it?

Trouble is, the angles used for the rotation above should be calculated from the quad frame gravity values qg(x|y|z).  See the chicken / egg problem?

The code gets around this because angles for a long time have been set up initially on takeoff, and then updated using the gyro rotation tweaked as an approximation to the rotation angle increments.  During flight, the qa(x|y|z) angles are fed in over a complimentary filter.

Thursday is forecast to be cold, sunny, and wind-free; I’ll be testing the above with a long GPS waypoint flights which so far lost stability at about the 20s point.  Fingers crossed I’m right that the drift of the accelerometer, and hence increasing errors on distance and velocity resolves this.  We shall see.

P.S. I’ve updated the code on GitHub as the Butterworth code is not having a negative effect, and may be commented out easily if not wanted.

# A thought experiment…

Do we need to know the angle of take-off?

This springs from another night’s sleep and a general concern about angles:

• I have a strong suspicion that now angles alone are what are causing drift
• There will be a slight mismatch between angles from the accelerometer and gyros unless both are perfectly calibrated – the initial take-off platform angles is determined by the accelerometers alone, and then angles throughout the flight come from both gyro and accelerometer ‘fused’ by the complimentary filter.
• Another one of those long term niggles that I’m sure the flight controllers for off-the-shelf quadcopters don’t need to be aligned accurately with the quad frame itself.

So lets run through the thought experiment:

• Phoebe is sat on a a very tilted take-off platform – lets call this the base-frame – her nose is pointing downwards compared to horizontal – a positive pitch angle compare to horizontal in the earth-frame.  But critically, we do not measure any angles of gravity here – angles are relative to the base-frame so pitch, roll and yaw are all zero
• Her flight plan in her quadcopter reference frame (which the sensors live in) is zero velocities Vqx and Vqy on the X and Y axes, and a fixed velocity Vqz in the Z axis direction.
• She takes off
• Because of the pitch of the base-frame compared to horizontal, gravity starts acting on her X axis such that Vqx starts to increase.
• The result of the PID processing is that the she rotates negatively around her Y axis until Vqx is 0, and as a result, she ends up horizontal without any knowledge of what ‘horizontal’ is – this is all simply about zero movement detected by her X axis accelerometer.
• There still needs to be a record of gravity in the base-frame, so that it can be tracked / updated in the quad-frame to get velocities.

There’s bound to be details I’ve missed, but essentially I believe this could fly (ahem).  But why bother?

• angular control is now completely (and therefore consistently) with the gyros
• velocity control is now completely in the quad-frame
• the above removes most of the time consuming rotation matrices, reducing them to just one – rotation of distributed gravity from the previous orientation to the next orientation of the quad-frame based upon the change in gyro angles.

It’s actually a relatively small rework of the code.  I have two known unknowns:

• what does this do with the Butterworth filter extracting gravity from acceleration in the earth frame – if this is still needed then the 3rd advantage listed above ceases to exist – can it be throw away now we don’t use the accelerometer for angles, and instead just use a start of flight fixed reading for distributed gravity
• similarly, how to handle flight plans in the earth-frame?  This doesn’t matter for take-off and hover, but it does for lateral movement to maintain a fixed vertical height and a fixed horizontal speed.

Not sure yet – more sleep needed to work this out more.

# Test flight with new angles

I took Phoebe out to try the new angles code in a small window between torrential rain and winds gusting to over 20mph.  The weather made it hard to draw any solid conclusions, but certainly her behaviour was no worse than prior to the new code.  She took off and hovered at a fixed height, but with some lateral drift that looked like she was correcting, albeit slowly.  So it’s definitely worth further testing on a day with better weather conditions.

Next step will be to collect flight diagnostics to see whether what I see happening matches what her sensors believe is happening; this’ll give real evidence whether the new way of calculating angles is better without the complementary filter for removing accelerometer noise, and whether the use of the low-noise gyro for angle increments results in better readings for lateral and vertical drift matching what’s happening in the real world.

Looks like Thursday is predicted to have the conditions I’m looking for.  Fingers crossed.

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

# Linearity

PIDs model a linear system without having to know all the factors which may affect the system, nor what effect they may have, as long as the affect is linear.

But there’s a non-linearity in the code at the moment (and a bug actually that became cler as I gave the none linearity some thought:

The velocity PIDs’ output is essentially a number representing the desired corrective acceleration.  The bigger the velocity error, the bigger the acceleration required to fix it; they are proportional.

For constant acceleration:

`v = a * t`

Or if the acceleration is not constant:

`v = ∫ a dt`

Similarly the rotation PID’s input is essentially representing the desired change in angle, and the output is the desired rotation rate applied to the ESCs / motors to carry out that change; again they are proportional.

For a fixed rotation rate, R:

`θ = R * t`

Or if the rotation rate is not constant:

`θ = ∫ R dt`

But there’s a problem turning the corrective acceleration (the output from the velocity PID) into the desired angle (the input to the rotation rate PID).  Here’s the current code.

``` pr_target = qvx_out
rr_target = -qvy_out```

There’s two problems with the code above.  It’s mapping the qv*_out accelerations to a rotation rate.  But the acceleration in the quad X and Y axes is not proportional to the angle (for examply 0º change gives 0 change in acceleration; 45° gives 1/√2 acceleration change and 90° gives full power acceleration change.  Notice the underlined above – they don’t match and that’s the bug.  To add to the complexity, it’s not actually known what the angle change actually is – it’s not an absolute angle wrt the earth gravity; it’s the change in angle since last time so an integration of the gyro output or the gyro change since the last set of motion processing. But currently that’s not what is fed into that PID – it’s a rotation rate.

So once more, I have a rewrite of the code shown above to allow for the non-linearity, and the result is fed into the inner PID as a desired angle change and compared to the integrated gyro angle change to come up with the rotation rate.

A good thing then that the weather has gone lousy again, so I have plenty of time to think it through properly and implement the changes.

P.S I’m going to apply these changes to the thread + signal code as although its use of threads provided no performance improvement in data collection rate, the code is a lot cleaner / tidier both in allocation / ownership and freeing of variables, and has a nice clean split between locally owned and global variables.

# 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

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

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!

# Circles

Another day, another type of test to track down where these strange readings are from.  This time, Phoebe sits on a flat platform which sits on a set of sponge balls on the floor.

I then roll her round in circles on the floor, which should produce sine waves of acceleration from the X and Y sensors 90° out of sync.  And ignoring the noise (I’d turned down dlpf to 40Hz, that’s what’s shown:

Circular acceleration

Scale seems plausible at up to 0.4g acceleration. qax has a slight negative offset suggesting she’s leaning slightly – she’s picking up -0.025g or as a first order estimate of the angle -0.025 radians or -1.5° – again plausible.

So the deadzone theory is dead!  But there is something interesting here: above is the complete acceleration including gravity; once gravity is subtracted, then this is what you get:

net acceleration

And this is where it gets interesting – the -0.025g offset has gone, which is good, but so has the bulk of the sine wave shape leaving only spikes – compare the scales of the two graphs.

The measurement for gravity in the earth reference frame is fixed throughout the flight – it’s measured as part of the pre-flight checks.  But critically, the measured acceleration is used to calculate the angles used to redistribute fixed gravity to the new quadcopter reference frame – and I think it’s these angles and their reorientation of gravity between reference frames that’s also removing the sine waves. And sure enough, the angles graph below shows Phoebe pitching and rolling by ±20°, whereas I know full well she is not because she’s sitting on a flat platform that can’t pitch and roll.

Pitch and Roll

So what I need is a way to calculate these rotation matrix angles ignoring the net acceleration; again we’re back to complementary or Kalman filters and using the gyro angles.  Except there’s another problem: the gyro angles are all in the quadcopter reference frame; what’s needed is a way to convert the gyro angles to the various intermediate Euler angles of a rotation matrix which lie in three different reference frame.  I vaguely remember something about this in a paper I read one – time to go on a hunt again.

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

# Surprise surprise…

though not the nice kind Cilla Black used to host on prime time UK TV until the turn of the millenium.

Very wonky pitch angles

Although angles have been ‘sorted’ for a long while now, I took a look at them from the last test flight to try to work out why Phoebe insisted on thinking she was drifting backwards, resulting in her real world compensation of drifting forwards.

Look at the 3 green lines – they start off well (she took off with her nose up by about 10°), but around 3 seconds, the gyro (i_pitch) and the accelerometer (e_pitch) clearly have a major disagreement.  Net result of the complementary filter (c_pitch) is that Phoebe thinks her nose is pointing upwards, and as a result, she’s going to think she’s going backwards (which is exactly what yesterday’s graph shows), and so she compensates by dropping her nose, and hence start moving forwards, which is what happened yesterday in real life.

At a guess the complementary filter is now paying way too much heed to the gyro now I’ve got everything so beautiful tuned and averaged with the accelerometer readings.

Time to tweak tau in the complementary filter down from 0.5s to something a lot smaller, and see what happens although to be honest, that’s just a workaround / hack – I’d rather understand why the gyro and accelerometer suddenly disagree so much, but for the moment, I’m very short of ideas – perhaps it’s time to retire the gyro from angle calculations completely and instead use it solely for angular stability?  Hmm now, there’s a thought.

# Another one bites the dust:

another flaw fixed. After some more PhD / Masters paper reading yesterday, I realized the angles being calculated should not all be in comparison to the the initial inertial reference frame; instead as the rotation matrix is applied, each angle is calculated against the reference frame the previous rotation had achieved.

So my pitch angles were wrong, which lead to bad gravity calculation prior to take-off and that’s never going to be a good thing!  With the code change, the values read of the sloping takeoff platform are as follows where qfrgv_* are gravity read in the quad reference frame, and efrgv* are those reorientated back to the earth frame.  The X and Y values are perfect, but Z is out – that doesn’t surprise me as the Z-axis accelerometer is twice as inaccurate as the X and Y.  Never the less, the ‘drift’ it shows is 7cms-2 which isn’t bad, though it is making it hard for phoebe getting as far off the ground as far as she should be.

```pitch 8.722535, roll -0.448118
qfrgv_x: 0.152762 qfrgv_y: -0.007787 qfrgv_z: = 0.995656
efrgv_x: 0.000000 efrgv: 0.000000 efrgv_z: = 1.007337```

There’s still something wrong though as this plot of the flight shows:

Mucky lawn flight path

She thinks she’s moving backwards according to the sensors and her compensation for that actually has her moving forwards in the real world.

The accurate measure of gravity values suggests that calibration isn’t the problem.  My next suspicion is the way the earth frame velocity PID targets are converted to quad frame.  Fingers crossed.