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.

Worth a Betta Bitta Butter

I suspect the low temperature increasingly unstable GPS tracking flights are partly due to the LiPo power (fixed with the hand / pocket digital warmers), and partly the accelerometer drift over time / temperature.  The problem with the latter is that gravity is recorded at the start of the flight and hence is fixed, meaning that accelerometer drift during the flight results in velocity and distance drift as the integration of (accelerometer – gravity) grows .  This is currently overcome by having second sources of velocity and distance (down-facing LiDAR + video) fused with the drifting IMU integrated (accelerometer – gravity) values.  The instability sets in over time as the IMU velocity and distance values drift increasingly over time but the LiDAR and video values don’t.  Ultimately these sources increasingly diverge and the instability ensues.

I remembered using a Butterworth IIR filter to extract gravity from the accelerometer dynamically 3 years ago to account for the accelerometer drift in cold temperatures, but it either failed or I got distracted by a better solution.

Yesterday, I tried again, but this time with a little better understanding: I now have a way to prime the filter without taking many seconds to do so.

Here’s the result: a stable hover without drift despite the LiDAR / video disabled – only the IMU status is in use:

Once the windy weather’s gone next week, I’ll be heading out into the next-door field again to fly with more GPS-waypoint intermediate targets, and this time, hopefully I’ll be able to complete the flight without the increasingly instability seen so far.

5 years later…

It was around Christmas 2012 that I started investigating an RPi drone, and the first post was at the end of January ’13.

5 years later, phase ‘one’ is all but done, barring all but the first as minor, mostly optional extras:

• Track down the GPS tracking instability – best guess is reduced LiPo power as the flight progress in near zero temperature conditions.
• Get Zoe working again – she’s been unused for a while – and perhaps, if possible, add GPS support although this may not be possible because she’s just a single CPU Pi0W
• Fuse the magnometer / gyrometer 3D readings to long term angle stability, particular yaw which has no backup long term sensor beyond the gyro.
• Add a level of yaw control such that ‘H’ always points the way she’s flying – currently she always points in the same direction she took off at.  I’ve tried this several times, and it’s always had a problem I couldn’t solve.  Third time lucky.
• Upgrade the operating systems to Raspbian Stretch with corresponding requirements for the I2C fix and network WAP / udhcpd / dnsmasq which currently means the OS is stuck with Jessie from the end of February 2017.
• Upgrade camera + lidar 10Hz sampling versus camera 320² pixels versus IMU 500Hz sampling to 20Hz, 480² pixels, 1kHz respectively.  However, every previous attempt to update one leads to the scheduling no longer able to process the others – I suspect I’ll need to wait for the Raspberry Pi B 4 or 5 for the increased performance.

Looking into the future…

• Implement (ironically named) SLAM  object mapping and avoidance with Sweep, ultimately aimed at maze nativation – just two problems here: no mazes wide enough for ‘H’ clearance, and the AI required to remember and react to explore only unexplored areas in the search for the center.
• Fuse GPS latitude, longitude and altitude / down-facing LiDAR + video / ΣΣ acceleration δt δt fusion for vertical + horizontal distance – this requires further connections between the various processes such that GPS talks to motion process which does the fusion.  It enables higher altitude flights where the LiDAR / Video can’t ‘see’ the ground – there are subtleties here swapping between GPS and Video / LiDAR depending whose working best at a given height above the ground based on an some fuzzy logic.
• Use down-facing camera for height and yaw as well as lateral motion – this is more a proof of concept, restrained by the need for much higher resolution videos which current aren’t possible with the RPi B3.
• Find a cold-fusion nuclear battery bank for flight from the Cotswolds, UK to Paris, France landing in Madrid, Spain or Barcelona, Catalonia!

These future aspirations are dreams unlike to become reality either to power supply, CPU performance or WiFi reach.  Although a solution to the WiFi range may be solvable now, the other need future technology, at least one of which my not be available within my lifetime :-).

Wishing you all a happy New Year and a great 2018!

Here we go round the Mulberry bush.

Yesterday it was a cold and frosty morning, but more critically, sunny with only a light breeze:

Thursday’s weather

So I managed to squeeze in another test flight, where the plan was to fly ‘H’ between 3 GPS wayspoint frisbee-markers from a takeoff in between all three.  As you can see, reality deviated significantly from the plan.

I’d made a minor change to the code such that the maximum horizontal speed was 1 m/s reducing proportionally as ‘H’ got less than 5 meters from the target e.g at 3m from the target, the speed is set to 0.66cm/s.  That worked well approaching the first red frisbee-marker after takeoff.  However the next phase, although heading in the right direction between red and orange frisbee-markers, was very unstable and ultimately overshot the orange frisbee-marker, so I killed the flight.  Here’s what the flight controller saw:

RTF
TAKEOFF
HOVER
GPS: WHERE AM I?
GPS TARGET 6m -69o
GPS TARGET 6m -69o
GPS TARGET 5m -70o
GPS TARGET 4m -71o
GPS TARGET 4m -73o
GPS TARGET 3m -75o
GPS TARGET 2m -78o
GPS TARGET 2m -82o
GPS TARGET 1m -86o
GPS TARGET 1m -90o
GPS TARGET 8m 85o
GPS TARGET 8m 85o
GPS TARGET 7m 85o
GPS TARGET 6m 84o
GPS TARGET 4m 86o
GPS TARGET 2m 93o
GPS TARGET 2m -117o
Flight time 20.242111

The green text is from takeoff to the red frisbee-marker.  The yellow section shows a good heading towards the second frisbee-marker.  It started at the right speed of 1m/s while more than 5m away, but the red lines show velocity increasing to 2, 2 and 4 m/s as H got closer to the orange frisbee-marker.  On the plus side, she knew she’d overshot the target and had been told to double back at the point I killed the flight -check the angles at the end of the red lines.

Time to go bug hunting – it’s hopefully just a stupid crass typo on my part.  Luckily, the kids go back to school on Tuesday, and the weather forecast so far is looking good that morning:

Tuesday’s weather forecast

I hope by Tuesday I’ll also have worked out how to get better video quality from the Mavic!

P.S. No crass bug found, so my second best guestimation is that the LiPo cooled to below optimal performance temperature; this has the same effect as seen, and I had set the heater elements on the lowest level. The plan for the next flight is identical to before, but with the heaters on high and with full logging enabled in case this also fails and I need to diagnose in detail the source of the problem from the lateral velocity targets.

Perfect, nearly!

With a minor tweak to the u-blox NEO-M8T GPS receiver configuration, ‘H’ headed off in the right direction, making subtle changes of direction as she got closer to the pre-recorded GPS target point, and when within one meter, hovered briefly before landing. Only down side was the LiPo was at under 40% by then (it started at 48%), and that seems to be the point there’s not enough power for a stable descent, hence the very chaotic landing.

This is probably the last test for 2017 due both to the weather forecast and the chaos of Christmas.  See you all in the New Year – have a great holiday break!

P.S. The latest code has been updated on GitHub.

Same shᴉt, different day.

I went to the neighbouring field that the farmer isn’t using because they are digging for gravel in 90% of it, still leaving a sizeable 10% right next to my house; again tried GPS tracking, and again overshat (overshotted? overshooted? overshot?) significantly.  She started 13 meters away from the orange frisbee based on the GPS position of both, correlating nicely with the video.  She’s facing almost exactly away from the target as passed from the autopilot and logged by the motion process:

GPS TARGET 13m 173o

The target is in a NNE direction by gut feel and confirmed by the GPS log stats:

GPS tracking

GPS thought it had travelled about 5.3 meters when in fact this was more than 16 meters, based both on the video and on the GPS itself showing 33 samples at 1Hz with ‘H’ programmed to fly at 0.5 m/s.

Now what’s interesting here are the spacing between the dots on the graph. Since each dot is one second apart, this give the speed the GPS thought it was moving:

GPS speed

As already mentioned, ‘H’ is flying at a constant and stable 0.5m/s, confirmed by the video, but GPS ‘speed’ climbed and has not yet reached the stable 0.5m/s.

As a result of all the above, I finally I have a clue of why ‘H’ keeps overshooting her GPS target: it’s like the NEO-M8T algorithm uses some form of low pass filter, which gives brilliant accuracy for a stable location (i.e. the waypoints and at takeoff), but when moving, each new reading is fused with historic readings, causing significant lag.

The NEO-M8T has a vast amount of config parameters accessible via its u-center app.  Time for me to explore the options.

P.S. A post to the u-blox forum quickly yielded the solution: the UBX-CFG-NAV5 can be set via the u-center app (or other means, I’m sure).  By default, the NEO-M8T algorithm uses a “stationary” model, but there are many other options:

u-center

I’ll try “portable” model first based on their descriptions of the different models in their NEO-M8T spec, section 7, followed by “pedestrian” and finally “Automotive”.

P.P.S. The “portable” model worked perfectly as shown by the next post.  There was one additional config change to make which is in the “CFG” section: changes need to be saved in all possible options so the update to “portable” survived after reboot.

Winter Wondering

Worth reminding yourself of yesterday’s down-facing Mavic video of the flight first.

This graph is made from the raw GPS data from the NEO-M8T – no processing by me other than saving the results to file:

GPS waypoints and flight

The grey line is the 3 preset waypoints: orange, red and purple correspond to the same coloured frisbees you can see in the down-facing video from yesterday.  The GPS waypoints on the graph are a very plausible match with their places in the video, both in location of each and the distance and direction between them .

The blue line is Hermione recorded live as she flew to the orange waypoint.

Here are the problems:

1. Hermione took off from the purple frisbee in real life.  As she took off, she determined her GPS takeoff point dynamically.  This GPS point in the graph is about 4m away from purple waypoint(s) on the graph.
2. In the video, you can see she flew in the right direction towards and beyond the orange frisbee.  In contrast, she thought from her GPS tracking that she was only ½ way to the orange waypoint, hence the real-world overshoot, and my termination of the flight.
3. Hermione was travelling at 1m/s and the video back this up.  However the intermediate GPS locations she read suggest more like 0.3m/s based on the fact GPS updates happen at 1Hz.

Both for presetting the waypoints, and during the flight itself, 9 satellites were in use.

While the difference in take-off location of 4m is just about tolerable as a fixed offset, the fact the GPS points suggest 0.3m/s (compared with the real, correct 1m/s) is not and I have absolutely no idea why the NEO-M8T is doing this.

Winter wonderland?

Yes, based upon my walk to the next village shop along a bridleway this morning:

Winter wonderland 1

Winter wonderland 2

Sadly no, based on flying Hermione in the park shortly after I got back from my walk:

GPS gave 15 samples corresponding well with the lateral flight time.  However the stats showed it still had 4 meters to reach the first waypoint when I aborted the flight.  I’m not clear as to the cause of this; perhaps how GPS prerecorded the waypoints verses how it then flew towards them?

On the plus point, she

• flew well at 1m/s despite the reduced contrast for the down-facing video
• was heading in the right direction.

On the downside, due to weather, start of kids’ school holidays, and Christmas, this may turn out to be my last flight in 2017!

Speedy Gonzales

Currently, all speeds, both horizontally and vertically are set to 0.3m/s for the sake of safety in enclosed arenas like indoors and the walled back garden.  The down side is that in the park with the GPS waypoints perhaps 20m apart, it takes a very long time, often over a minute between waypoints, wearing out the batteries in a few flights.

The limitation other than safety is to ensure the down-facing video can track the difference between frames, which means there needs to be a significant overlap between consecutive frames.

The video runs at 10Hz*.  The RPi camera angle of view (AOV) is 48.8°.  With the camera 1m off the ground (the standard set throughout all flights)**, 48.8° corresponds to 80cm horizontal distance (2 x 1m * tan (AOV / 2)). Assuming there needs to be a 90% overlap between frames to get accurate video macro-block vectors, every 0.1s, Hermione can move up to 8cm (10%) or 0.80m/s compared to the current 0.3m/s.  I’ll be trying this out on the GPS tracking flights in the park tomorrow.

*10Hz seems to be about the highest frequency for the video that the macro-block processing can handle without causing other sensor processing to overflow – specifically the IMU FIFO.

**1 meter height is for the sake of safety, and because the video 320² pixels macro-blocks can resolve distance accurately on grass and gravel.  Doubling the height requires quadrupling the video frame size to 640² to get the same resolution required for grass / gravel, and once again, the processing time required will cause IMU FIFO overflowing.

P.S. The weather isn’t as good as I’d hoped to do the GPS tracking flights in the park yet, but I did take Hermione into the back garden this morning to test her increased horizontal velocity changes; she happily ran at 1m/s over the grass, so that will be the new speed used for the much longer distance GPS flights to reduce Hermione’s flight time and hence her and the DJI Mavic’s battery drain.

Grass mowing followup

Although she wasn’t running full stats, she always logs some core details.  These show that she probably had around 1.5m to reach the target.  At 1m, she moves onto the next target, or lands if there are no more left.  She was probably only a few seconds away from this happening when she flipped.  This is in line with my speculation that the mowing simply was due to the main LiPo no longer having enough power to keep her up in the air.  Post flight checking shows it at 38% which (after 5 years experience) is where flights always go wobbly. It was contact with the ground that triggered the lawn mowing flip.

She took off and hovered pointing roughly upwards in the video, and the GPS had her heading approximately 60° anticlockwire i.e. as Z gyrometer defines the angles in the main motion processing:

RTF
TAKEOFF
HOVER
GPS: WHERE AM I?
GPS TARGET 8m 58o
GPS TARGET 8m 58o
GPS TARGET 8m 58o
GPS TARGET 8m 58o
GPS TARGET 8m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 58o
GPS TARGET 7m 57o
GPS TARGET 7m 58o
GPS TARGET 6m 58o
GPS TARGET 6m 58o
GPS TARGET 6m 57o
GPS TARGET 6m 57o
GPS TARGET 6m 57o
GPS TARGET 6m 56o
GPS TARGET 6m 56o
GPS TARGET 5m 56o
GPS TARGET 5m 56o
GPS TARGET 5m 57o
GPS TARGET 5m 56o
GPS TARGET 5m 57o
GPS TARGET 4m 57o
GPS TARGET 4m 56o
GPS TARGET 4m 55o
GPS TARGET 4m 54o
GPS TARGET 4m 53o
GPS TARGET 3m 52o
GPS TARGET 3m 50o
GPS TARGET 3m 52o
GPS TARGET 3m 53o
GPS TARGET 3m 56o
GPS TARGET 2m 61o
GPS TARGET 2m 62o
GPS TARGET 2m 63o
GPS TARGET 2m 65o
GPS TARGET 2m 66o
GPS TARGET 2m 68o
Flight time 44.65

Next time and weather available is Monday, so I’ll be taking here out again with a set of fully charged batteries to do the same again.  Third time lucky.