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 busy week…

First, the result: autonomous10m linear flight forwards:

You can see her stabilitydegrade as she leaves the contrasting shadow area cast by the tree branches in the sunshine.  At the point chaos broke loose, she believed she had reached her 10m target and thus she was descending; she’s not far wrong – the start and end points are the two round stones placed a measured 10m apart to within a few centimetres.

So here’s what’s changed in the last week:

  • I’ve added a heatsink to my B3 and changed the base layer of my Pimoroni Tangerine case as the newer one has extra vents underneath to allow better airflow.  The reason here is an attempt to keep a stable temperature, partly to stop the CPU over heating and slowing down, but mostly to avoid the IMU drift over temperature.  Note that normally the Pi3 has a shelf above carrying the large LiPo and acting as a sun-shield – you can just about see the poles that support it at the corners of the Pi3 Tangerine case.

    Hermione's brain exposed

    Hermione’s brain exposed

  • I’ve moved the down-facing Raspberry Pi V2.1 camera and Garmin LiDAR-Lite sensor to allow space for the Scanse Sweep sensor which should arrive in the next week or two courtesy of Kickstarter funding.
    Hermione's delicate underbelly

    Hermione’s delicate underbelly

    The black disk in the middle will seat the Scanse Sweep perfectly; it lifts it above the Garmin LiDAR Lite V3 so their lasers don’t interfere with each other; finally the camera has been moved far away from both to make sure they don’t feature in its video.

  • I’ve changed the fusion code so vertical and lateral values are fused independently; this is because if the camera was struggling to spot motion in dim light, then the LiDAR height was not fused and on some flights Hermione would climb during hover.
  • I’ve sorted out the compass calibration code so Hermione knows which way she’s pointing.  The code just logs the output currently, but soon it will be both fusing with the gyrometer yaw rate, and interacting with the below….
  • I’ve added a new process tracking GPS position and feeding the results over a shared-memory OS FIFO file in the same way the video macro-block are passed across now. The reason both are in their own process is each block reading the sensors – one second for GPS and 0.1 second for video – and that degree of blocking must be completely isolated from the core motion processing.  As with the compass, the GPS data is just logged currently but soon the GPS will be used to set the end-point of a flight, and then, when launched from somewhere away from the target end-point, the combination of compass and GPS together will provide sensor inputs to ensure the flight heads in the right direction, and recognises when it’s reached its goal.

As a result of all the above, I’ve updated GitHub.

Hermione in wonderland.

Took her outside this morning, and the safety test without LiPo consistently threw I²C errors as yesterday.  I brought her straight indoors, still powered up, both her and my piPad and ran the same test; she worked perfectly. Curiouser and curiouser.

P.S. Shortly after writing the above, I had a eureka moment in the shower: I remembered reading LiPos don’t work well in the cold, and even the Mavic instructions suggest letting it run for a while before take off to let the battery chemistry warm out.  Next test then is to wrap both LiPos (batter bank and the main power) in bubble wrap, boot her up indoors, and then take her outside to fly.  I’ll report back here anon.

P.P.S. It worked!!!!! I wrapped both LiPos in some neoprene foam (normally used for scuba suits), set everything up and running the code and therefore the GLL and PWM to keep the LiPos warm indoors. After a couple of minutes, I lugged everything outside, and she did two flights without a glitch. Roll on spring / summer!

In the deep mid winter…

Hermione got thrown…

…up into the air at full acceleration!

It was less than -5°C when I took Hermione out wearing her salad bowl lid.  I knew the end result would be a destructive crash but I wanted to compare against Zoe’s inability to get of the ground yesterday.

Before 0.9s, things were looking promising…

Double brrr!

Double brrr!

…right up to the point something went wrong with the I2C readings of the IMU:



At that point she launched into the air at high speed to counteract the negative vertical distance she was (falsely) sensing.  By the time I hit Ctrl-C, she was 3m up and climbing hard towards the house roof.  When she hit the ground, she was doing about 7ms-1, so unsurprisingly, there was damage.  Luckily minutes later, the postman arrived with the replacement parts – I knew they were coming which is why I could do the flight bound to end in damage.  It’s worth noting that no “FIFO overflows” or “I2C errors” were detected at the point everything went wrong.

Here’s the equivalent of Zoe’s flight yesterday when she couldn’t get off the ground.  She was flying at about +5°C.  I cut this flight because she was sitting on the ground with the props spinning at the minimal speed.

Zoe's cold

Zoe’s cold

They were running identical code but Zoe’s IMU worked perfectly throughout.  I’m assuming Hermione has power brown outs due to her A+ and all her sensors.  I’m not sure why Zoe’s not taking off well – both she and Hermione showed the same temperature change during takeoff, and the corresponding accelerometer drift.  Next step is to take Zoe out into the sub-zero back garden and see how she reacts at that temperature.

In the deep mid winter…

Zoe can’t be flown…

I took her out this morning, controlled by the accelerometer and gyro, but with the LiDAR and Camera running in parallel passively so I could do a comparison.  But she could hardly drag herself off the ground.  However, in her world, she thought she way climbing throughout:



The props span up to hover speed for the first 0.1s, then she ‘climbed’ for 3 seconds and then “hovered” after 3 seconds.  By 5 seconds she was on the ground with the props spinning at the minimum setting.

Throughout (orange line) she thought she was climbing, which is why she struggled to take off.

If you look at the IMU temperature plot, as she starts to climb, it seems the propeller wind cools off the IMU, and that’s shifting the scale of the accelerometer significantly.  The problem is caused because a snapshot of gravity is taken before takeoff, and then is used throughout the flight to extract net acceleration.  That works fine when the temperature is in the teens, but it’s about 5°C today outdoors.

I’ve seen this in the past, and tried lots to do it either by calibrating the accelerometer against temperature – all I got from that was a beer fridge in the office – and using a butterworth low-pass filter to extract gravity dynamically during the flight – I can’t remember how well this worked but I assume “not well enough” since I dropped it from the code a while back.

What she really needs is a wind-proof jacket or scarf to shield her from the draft from the props.  I might try a duct tape wrap.  Alternatively, I could just bring her indoors into the temperature range where the accelerometer doesn’t drift; I’m just a little nervous as I’ve just popped into the lounge, and the scars on the ceiling are quite revealing of past exploits! Finally, I could take Hermione and her salad bowl lid to see if that helps.

A box it is then!

I’ve tried various ways to acclimatise Zoe’s sensors prior to flight.  The best so far is to set the props spinning at minimum speed, and after 5 seconds, grab a FIFO full of data (42 batches of samples in the 512 byte FIFO and 12 byte batch size), and use these to calculated start-of-flight gravity.  The props then continue to run at this base speed up to the point the flight kicks off.

The net result is a stable flight with no vertical drift during hover, but with horizontal drift of about a meter.  Without this code, horizontal drift is half this but she continues to climb during hover.

I’m not sure how I can improve this, so I’ll leave it alone for now and instead have a look at making a DIY cardboard box to keep Zoe out of the wind.

In passing, I did a quick analysis of the code size: 1021 lines of python code, 756 lines of comments and 301 blank lines giving a total of 2078 lines in Quadcopter.py.  Here’s the script I knocked together quickly FYI:

code = 0
comments = 0
spaces = 0

with open("Quadcopter.py", "rb") as f:
    for line in f.readlines():
        line = line.strip()
        if len(line) == 0:
            spaces += 1
        elif line[0] == '#':
            comments += 1
            code += 1
    print "Code %d, Comments %d, Blank Lines %d, Total %d" % (code, comments, spaces, code + comments + spaces)

I’ve put Zoe’s code up on GitHub as the best yet, although the either / or of vertical / horizontal drift is seriously starting to pᴉss me off.

Note that since I’ve moved the IMU FIFO into the Quadcopter.py code, QCIMUFIFO.py is not longer on GitHub; Quadcopter.py is the latest best working version and QCDRI.py is the best version that uses the data ready interrupt in case you are seeing the I2C errors like I used to.

Gravity vs. Temperature

Here’s a short and slightly chaotic indoor flight tracking IMU temperature, and the value of earth gravity from the sensors.  It looks like it takes roughly 2.5s for the IMU temperature to stabilize once the props are spinning.

Gravity is only being updated during the RTF time used to bring the motors up to near hover speed before lift off – currently 1.5s.

Gravity vs. Temperature

Gravity vs. Temperature

I think I need to separate the cooling period from the RTF period.  For a few seconds before the flight controller code kicks off, run the props a minimum speed to generate the breeze to cool the IMU.  Then read the base-level gravity at that point.  And then kick off the flight which will take less time as the props will already be half way through the standard RTF spin-up process.

Ignore the spikes in temperature;  the temperature readings are not from the FIFO, they are direct reads of the temperature register, and I’m not using the data ready interrupt to trigger these, so periodically, the temperature register is read at the same time as the IMU is updating it, leading to the spikes.

Temperamental @ Temperature

So I lied by omission in my previous post; I’d assumed Zoe would fly better in the cold (it’s only just above freezing outside), but I posted before I tested.  So I took outside, left her sampling without the props running to find her ambient, and flew her a couple of times.  Both flights were about 5m drift forwards, compared to the indoors 1m drift with the same flight plan.

So once more temperature change is critical.  I’ve previously done two different attempts at temperature control:

  1. the BYOAQ-BAT series uses the IMU temperature sensor, and a resistor to try to keep the IMU at a fixed temperature (40°C).  This was a completely OTT approach and I think I fried my IMU at one point.
  2. The Butterworth IIR digital low pass filter extract gravity from acceleration, letting through linear changes but excluding spike – this only works for the DRI code as the filter is timing critical, and required ‘ an indeterminate number of samples for the filter to settle – this is the 20s ‘warm up’ in the DRI code. It also wouldn’t handle the rapid cooling when the props start up in cold-ambient conditions.

But there are two much easier ways to do this (trust me to do it the hard way first).

  1. The base level for gravity is currently taken when the props aren’t spinning and so aren’t cooling the IMU.  But there is a one and a half second period before each flight where the props spin up to hover speed – the ready to fly (RTF) period.  Sampling gravity during this period with a complementary filter will provide a much better base-level value for gravity due to the props providing the cooling air-stream.
  2. Even simpler, popping Zoe into a little cardboard box would shelter her from the cooling breeze from the props.

Option 1 is easier as it’s a simple code change; finding or making the box is hard because there’s little vertical space between the HoG and the frame, but I’ll be keeping my eyes peeled.

P.S. I’ve just taken Zoe out into the pitch black and freezing cold and flew her for 2 short flights with Option 1. The horizontal drift has vanished, but vertical was still there: the first flight descended during hover, and the second ascended. There’s clearly some tuning to be done, but it’s looking promising, and safe enough for further testing indoors tomorrow.

HoG & CoG

What have I been up to?

  1. Phoebe’s HoG PCB is now out for manufacturing – hopefully this will fix the I2C problems and allow me to progress with adding the magnetometer (orientation), URF (height) and camera (motion) sensors into the mix.  The new PCBs are due to arrive on or before 29th.
  2. Zoe’s undergone a minor rebuild of her frame to lift her CoG nearer to prop height to reduce the swinging in an otherwise stable flight – her main battery now resides on the top plate, and her power bank underneath.  I could improve this further by getting rid of the power bank completely, but that will require a new PCB to incorporate the regulator.

Other than that, I’m flying Zoe indoors daily tuning her PIDs ready for her upcoming performances.  She still drifts, and because she’s now using the IMU FIFO code, I’m back to considering sensor drift due to temperature.  She performs best in the freezing cold with the large difference between cold ambient and her IMU operating temperature.  However I won’t be investigating this further within the timeframe of the performances.

Wrapping up warm

To overcome the reduced battery power in cold temperatures, I bought some 3mm thick adhesive neoprene foam mat.  I wrapped one of my batteries in it, and took Phoebe out to fly.  She behaved beautifully over several flights so I’ve popped the latest code up to GitHub.  Note there are no significant changes here, just temperature diagnostics and some tidying.

Neoprene wrap

Neoprene wrap

The battery is now held down by just a single silicone rubber band.  The tension in the band compresses the foam cover a little which keeps the battery locked in place, and the neoprene protects the battery from the frame on the underside – a couple of unexpected benefits.