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

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
    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 blast from the past

For the last few days, the outdoor temperature dropped to the teens from the twenties.  As a result, she didn’t get off the ground; she just jittered around.  The graph of double integrated (accelerometer – gravity) shows she believed she’s was climbing right at the point the IMU temperature dropped.

Not again :-(

Not again 🙁

This looks awfully similar to this.

The problem is that in these cooler temperatures, once the props start spinning, the IMU cools, and the temperature sensitive accelerometer output shifts as a result.  Because I take a snapshot of gravity prior to the props spinning, net acceleration (accelerometer – gravity) is wrong, so integrating it twice to get distances is very very wrong.

The problem showed up this time because to add the salad bowl lid,  I had to remove her Pimoroni Tangerine Dream PiBow, thus exposing her to the elements. For now, I’ve just moved the initial read of gravity to immediately before take off, but I’m also working out how to reintroduce the PiBox case or similar in a lower profile version to that the salad bowl still fits neatly on top.

Exploring negative G ‘errors’ from the IMU

I’ve been asking myself various questions and checking the answers through test flights:

  • Do the negative G values show up in passive flights?  If the motors aren’t powered up, negative G values are not seen.
  • What about alpf?  With alpf set to 0 or 1 (460 or 184 Hz accelerometer low pass filter), I see the negative G ‘error’, but set to 2 (92Hz) or greater it doesn’t happen.
  • What are the values of the other accelerometer and gyro readings when this happens with alpf set to 0?  These are all believable: ax = -4214, ay = -696, az = -270, qx = 2922, gy = 870, gz = 1531 – no magic hex numbers in there suggesting problems.
  • What about sampling frequency?  With alpf set to 0, but with the sampling frequency dropped from 500Hz to 200Hz (SMPLRT_DIV 1 => 4), there are still negative G ‘errors’.
  • What happens if the range is set to ±4g instead of ±2g with alpf set to 0?  The problem remains – there are negative G ‘errors’.

To me, that suggests that somehow, the negative G readings are not an ‘error’ but are real.  They are filtered out by the lower low pass filter frequencies.  The MPU-9250 is working perfectly.  I’ll continue running with sampling frequency of 500Hz, and alpf of 0, and with the diagnostic / protective code disabled.

This then points the blame for head-butting the ceiling towards the ESCs – it’s as though they have latched at maximum power.  The PWM signals I’m sending range from 1ms to 2ms inclusive, and I’m now wondering whether sometimes, the PWM output does hit 2ms and latches the ESC; I’ve turned down the maximum to 1.999ms and see what happens – it’s a hunch based on a long lost memory of something I’d seen or read.

¡uʍop ǝpᴉsd∩

Short version: if you’re as obsessive as I am, then the Invensense IMU’s may be flawed.

Longer versions: I followed up my duff data errors theory by adding a tiny bit of code that checked whether the Z-axis accelerometer reading was negative; that’s only possible if the quad is upside-down and powering itself towards the ground:

[WARNING] (MainThread) __init__ 1382, Zoe is flying.
[WARNING] (MainThread) __init__ 312, SRD:, 2
[WARNING] (MainThread) __init__ 411, IMU core temp: 23.261427
[WARNING] (MainThread) fly 1507, fly = True, flight plan = fp.csv, calibrate_0g = 0, hover_target = 380, shoot_video = False, vvp_gain = 400.000000, vvi_gain = 200.000000, vvd_gain= 0.000000, hvp_gain = 1.500000, hvi_gain = 0.100000, hvd_gain = 0.000000, prp_gain = 110.000000, pri_gain = 11.000000, prd_gain = 0.000000, rrp_gain = 90.000000, rri_gain = 9.000000, rrd_gain = 0.000000, yrp_gain = 80.000000, yri_gain = 8.000000, yrd_gain = 0.000000, test_case = 0, rtf_period = 1.500000, tau = 5.000000, diagnostics = False
[WARNING] (MainThread) load0gCalibration 536, 0g Offsets:, 0.000000, 0.000000, 0.000000
[WARNING] (MainThread) fly 1646, pitch -1.679818, roll -2.179765
[WARNING] (MainThread) fly 1647, egx 0.000000, egy 0.000000, egz 0.977003
[WARNING] (MainThread) fly 1724, 0 data errors; 0 i2c errors; 0 2g hits
[CRITICAL] (MainThread) fly 1804, ABORT: Duff Data: -228!
[WARNING] (MainThread) fly 2053, IMU core temp: 22.899000
[WARNING] (MainThread) fly 2054, motion_loops 218
[WARNING] (MainThread) fly 2055, sampling_loops 1127
[WARNING] (MainThread) fly 2057, 1 data errors; 0 i2c errors; 0 2g hits

That’s -228 * 2g / 32768 = 0.014g!  No wonder she drifts if that keeps happening.

Yet she wasn’t upside down; the only explanation was that the top byte of the accelerometer Z axis reading had the top bit set meaning it’s a negative value.  Most likely is actually the top bit was 0xFF – i.e. duff data of 0xFF80 was read from the FIFO as the Z axis acceleration.

On the basis the sensor might be damaged, I swapped to a brand-new one, never used and never abused or dropped.  The second sensor also produced the same result.

There were no signs of I2C errors, and I was using the FIFO, so my first finger of blame points at the MPU-9250 pushing duff data into the FIFO.  My only doubt is that this happened consistently for each of the 6+ flights I did, and every time within a second or two of takeoff.

I have some more tests to do before I can point my finger with confidence: I’ll post again when I have more information.

Infinite Impulse Response

In addition to BYOAQ-BAT, I’m still flying Zoë, trying to tackle the accelerometer drift, especially in the Z axis.  That problem is that with drifting Z axis accelerometer, it’s hard to get a decent vertical hover because

The basic code just calculated gravity prior to flight, and then subtracts that from acceleration readings during the flight to come up with net acceleration.  But if the sensors drift during flight, the subtraction of gravity from the sensor readings will also drift, leading to sensors thinking they are hovering while they are still climbing.  This morning’s test had the quad drifting to ground during the hover phase, and on the ground prematurely in the descent phase.

My current approach has been to use a complementary ‘filter’ (though it’s not really filtering) to slowly mix new readings of acceleration into the preset gravity readings.  It works to some extent, but not well enough.  It does suggest filtering gravity may be a suitable approach.  This morning’s test had the flight climbing too high, and continuing to drift slowly upwards during hover.

What I need is a proper digital low pass filter to separate net acceleration from gravity drift.  And thanks to Phil’s comment, I’ve been investigating IIR filters and found a couple of things which are both useful and critically, I understand.

The gravity drift is slow and always in the same direction where as the acceleration is neither, so I’m thinking a very low cut-off (0.5Hz or less) DLPF will work.  Currently I’m implementing a hard-coded second-order Butterworth filter with a sample rate of 71Hz (the frequency of my motion processing) and a cut off of 0.5Hz.  If this is successful, I’ll add the code to actually calculate the Butterworth parameters so I can easily experiment with other filter types, cut-off frquencies and order for the filter.

Code updated on GitHub

I’ve just posted the latest code to GitHub.  With my old-stock much cheaper carbon fiber 11 x 5 props fitted, I was able to do several test flights without a care in the world about smashing into things, and as a result, I know I have code worth posting.

Ironically these cheaper blades are also stronger as they don’t have the cork sandwich like the T-motor ones.  So despite several arguments with walls, swings, swingball posts, and a slide, I’m still using the same set of 4 I started with.

Next steps then are to tinker with dlpf, gravity filter and motion PID settings to kill the fixed velocity drift (which arises during takeoff), and the absolute height attained on take-off.  And I can do so with confidence know I have a stock of cheap blades, and if they do run out, a replacement set is a quarter of the price of the old ones.

Minor update

I’ve added the gravity complementary filter and a few other bits to hopefully work together with it:

  • dlpf is now set to 2 – 94Hz with 3ms lag (from 3 – 44Hz and 4.9ms lag)
  • motion processing frequency of 71Hz (from 43Hz)

The aim of the first change is to ensure we get to see the acceleration spikes, so the filter can take them out of gravity.  We can’t let the dlpf take them out as the motion processing needs to see them to calculate net acceleration with gravity removed.

The aim of the second change is to increase the resolution of the motion processing cycles so that fewer cycles are impacted by acceleration spikes.

Both of the above changes are only possible due to the recent increase of speed in the code from ∼450Hz to ∼900Hz.

Net result though wasn’t significant in the first test flight – same climb well above what the flight plan defined.  Not a problem, I realized it would need tuning.

What was a problem is the battery bank I use to power Phoebe.  It appears they don’t like impacts.  I’ve gone through 4 now – it seems as though the USB B socket gets detached; the bank charges fine, but there’s no power from the USB B socket.  Ironically, they are beautifully made, and I can’t find a way to open them up to fix the problem.

I could do with a super low profile USB B plug, but I’ve never seen such a thing.

Instead another tenner to Amazon for yet another battery bank.


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

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

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

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.

Code updated on GitHub

I’ve reworked the code so the bulk is a python import library (Quadcopter.py), and qc.py is now just a shim header to import the bulk of the code.  Imported libraries are pre-interpreted and the results stored in bitcode (Quadcopter.pyc).  If the .py code hasn’t changed since the previous run, then the interpreter picks up the .pyc file instead; the .pyc file is automatically generated if the .py and .pyc don’t match.

Why bother?  I thought I’d see what sort of performance improvement it would yield.  The answer is minimal – perhaps 2% – not really a surprise as the bulk of the processing is reading the sensors, and that happens at a fixed period that the code can never go faster than, but it was worth a try, and now it’s done, I’ll leave it in place on GitHub.

In passing, you may have noticed that Phoebe’s new built is not using the URF; that’s because I’m missing a connector that Farnell have on back order, but in the meantime I need to test the physical rebuild.  A Cube Calibration of gravity seems to have sorted her consistent forward drift, and now I have the same sort of sluggish self-correcting drift I had previously.

Next step is to do a double check on horizontal velocity tuning, as I think I may not be putting enough P gain into the PID.  Weather looks good this weekend, so I’ll let you know how it goes.

Gravity doesn’t yaw

I’d been using the full rotation matrix to distribute gravity across a tilted Phoebe, so that then it could be deleted from Phoebe’s  accelerometer readings to give a net acceleration, and from there, integrated to net velocity.

But gravity doesn’t yaw: at a fixed pitch / roll tilt, gravity measures the same on the X and Y axes – so when deleting gravity off the rotated X and Y axes to achieve velocity through integration, the reorientation of gravity should not include yaw.

In contrast, the sensor readings, and the earth frame targets do need to allow for yaw so that when the target is ‘move forward’, then Phoebe moves forward from my point of view even if she yaws.

I’ve fixed, and I think it might have secondary positive effects; any sensors errors in one of the axes will have the same axis error in gravity deleted from it.  So this could improve drift by deleting the right gravity error.  Perhaps this is the explanation for why nobody but me is jumping such big loops to calibrated gravity?  Only the next tests will tell, and as always, Murphy is messing things up again and it’s peeing it down outside!