Pardon? Too noisy, can’t hear you!

The difference between Chloe’s flight in the video, and the ones I tested the day before is the digital low pass filter, or to be more specific the accelerometer’s dlpf since the MPU-9250 provides independent filters for gyro and accelerometer data.

This isn’t the anti-aliasing filter which is applied prior to digital sampling of the analogue  accelerometer at 1kHz – the dlpf is aimed solely at getting the data you want, and not the noise you don’t.  I don’t know what the anti-aliasing filter is set to, but it must be less that 500Hz in the same way music is sampled at 44.1kHz even though what’s nominally audible is below 20kHz – there’s a high-order low-pass anti-aliasing filter just above 20kHz,

Anyway, for the previous flights, Chloe’s accelerometer filter was set to 184Hz on the basis that the higher the filter frequency, the lesser the delay in processing the filter (5.8ms according to the spec).  I dropped it to 92Hz (7.8ms delay), and the improvement was dramatic.

And that suggests the primary cause of my drift now is possibly noise: higher frequency vibrations from the motors (for example) are irrelevant to the flight speeds, but could cause errors in velocity integration because the pass band of the DLPF is set too high.

There’s clearly a balance however which is that if the DLPF is set too low (41, 20, 10 and 5Hz are the other options), while the noise will be filtered out, the risk of filtering out the good stuff increases as does the lag (11.8, 19.8, 35.7 and 66.9ms respectively) i.e. how out of date the filtered data is!  Perhaps that’s where Kalman steps in as it offers a level of prediction (so I’ve read) which could overcome this lag.  But for now, Kalman can continue sitting on the back-burner while I play with the dlpf values to find that magic number between noisy and necessary data samples.

Fly drive

I took Chloe out onto the drive – horizontal drift didn’t exist – a perfect vertical rise and fall.  There were only two flights; the first rose to 50cm but then descended slowly during the hover phase, landing gently even before descent phase of the flight plan has started.  The second flight also didn’t drift laterally, but in contrast, it rose to 2 meters during ascent phase, and continued to climb during the hover phase; at that point, I killed the flight.

There’s a few possible causes I can think of off the top of my head:

  •  the Butterworth filter needs turning as its filtering out of gravity is lagging behind sensor drift – this is my prime suspect
  • the complementary filter needs tuning as its merging of angle sources is lagging behind sensor drift – this is much less likely however, if only because it would affect lateral drift also
  • IMU temperature drift between the first flight and the second – again I’m doubtful as although it could affect hover height, there shouldn’t be vertical drift during hover phase – this is what’s pointing to the Butterworth lagging too.

Phoebe’s Playtime in the Park

I took Phoebe down to the local play park to try some extended flights without the risk of her crashing into solid objects – I’m on my last set of props and I really can’t afford to keep replacing them several times a month.

Anyway, net of the flights was she’s good for a few seconds as shown in yesterday’s video, but extending the hover time to 10s from yesterday’s 4 shown accelerating drift.

Best guess is longer term accuracy of angles using just integrated gyro reading, and I need to reintroduce the complementary filter to merge in the angles from the accelerometer to provide longer term stability.

Oh silly scope!

Here’s a capture from my ‘scope of the data ready interrupt pin.  I’ve configured it so that it goes high when there is data ready, and drops back low when the data is read by the code:

Data ready interrupt

Data ready interrupt

The display is 20ms wide, so there should be 20 pulses shown if everything was perfect.  The narrow pulses are perfect.  But there’s a couple interesting bits:

  1. There are several missing pulses most obviously between 14 and 18ms – that suggests the sensor is not consistently setting the interrupts pin high at the configured 1kHz sampling rate.
  2. There is one wide pulse between 6 and 9ms – that indicates an extended period between the sensor raising the interrupt pin high and the code reading the data – that could be because Linux / Raspian / Python is not real time, or because the motion processing thread was running, taking longer that 1ms – that prevents the sensors being read due to the Python GIL blocking threads.  That’s to be expected and there’s not a lot I can do without switching to another Python implementation – Cython or PyPy rather than CPython..

There’s nothing I can do about the gaps – that’s a sensor problem.  The code already attempts to compensate though by timing gaps between reads and uses that for integration,  which is then averaged on passing to the motion processing code periodically.  Still, it’s a worry gaps this large exist.

It’s reassuring that all the pulses shows are at 1ms intervals, so there’s no bogus spikes causing the I2C errors during flight.  The flight logs show only 1 I2C error which happened during warm up; to some extent that has lesser impact as the period is used to load up the Butterworth filters and at the end, extract take-off platform slope and resultant gravity distribution and calibration across the quad axes.

After all the tinkering I’ve been up to, I think it’s worth a flight just to see if anything is better as a result.

Butterworth, 2nd order, 0.1Hz cutoff, 71Hz sampling

Zoë with 2nd order, 0.1Hz cut-off and 71Hz sampling butterworth IIR filter from Andy Baker on Vimeo.

Currently the code which calculates the Butterworth coefficients and implements the filter only covers 2nd order – I need to do some more reading / understanding to go higher.  I suspect 4th order at 0.2Hz might be better.  Cut-off and sampling frequencies are already configurable.

The accelerating drift to the right suggests calibration errors rather than a take-off problem which normally leads to a fixed velocity drift. Calibration cube out again, I think.

Butterworth coefficient ‘C’ code

I have a nasty feeling the server used to calculate the Butterworth filter coefficients may have been overloaded – if I enter values there and hit “Submit” I just get “500 – Internal Server Error” back.

So I went hunting and found a site with ‘C’ code to calculate the coefficients, and the code is simple enough to rewrite in Python allowing easy testing with different order, cut-off and sampling frequency of the filter.  It’s under GPL so I’ve posted a version here:

/*
 *                            COPYRIGHT
 *
 *  Copyright (C) 2014 Exstrom Laboratories LLC
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  A copy of the GNU General Public License is available on the internet at:
 *  http://www.gnu.org/copyleft/gpl.html
 *
 *  or you can write to:
 *
 *  The Free Software Foundation, Inc.
 *  675 Mass Ave
 *  Cambridge, MA 02139, USA
 *
 *  Exstrom Laboratories LLC contact:
 *  stefan(AT)exstrom.com
 *
 *  Exstrom Laboratories LLC
 *  Longmont, CO 80503, USA
 *
 */

#include <stdlib.h> 
#include <stdio.h>
#include <string.h>
#include <math.h>

// Compile: gcc -lm -o bwlpf bwlpf.c

int main( int argc, char *argv[] )
{
  if(argc < 4)
  {
      printf("Usage: %s n s f\n", argv[0]);
      printf("Butterworth lowpass filter.\n");
      printf("  n = filter order 2,4,6,...\n");
      printf("  s = sampling frequency\n");
      printf("  f = half power frequency\n");
      return(-1);
  }

  int i, n = (int)strtol(argv[1], NULL, 10);
  n = n/2;
  long double s = strtod(argv[2], NULL);
  long double f = strtod(argv[3], NULL);
  long double a = tan(M_PI*f/s);
  long double a2 = a*a;
  long double r;
  long double *A = (long double *)malloc(n*sizeof(long double));
  long double *d1 = (long double *)malloc(n*sizeof(long double));
  long double *d2 = (long double *)malloc(n*sizeof(long double));

  for(i=0; i<n; ++i){
    r = sin(M_PI*(2.0*i+1.0)/(4.0*n));
    s = a2 + 2.0*a*r + 1.0;
    A[i] = a2/s;
    d1[i] = 2.0*(1-a2)/s;
    d2[i] = -(a2 - 2.0*a*r + 1.0)/s;

    printf("%Lf\n", 1/A[i]);
    printf("%Lf\n", d1[i]);
    printf("%Lf\n", d2[i]);
  }
  return(0);
}

It compiles on my Raspberry Pi using the instructions in the code, and produces the same constants the York code did. So next step is to pythonize it.

P.S. A test flight this morning showed promising signs, but I think I need my Butterworth cut-off to be lower (0.1Hz instead of the 0.5Hz I flew).

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.

The worm that turned*

I’ve started to consider adding a barometer, even though it’s anathema to my core target of using just accelerometer and gyro (because consensus is it cannot be done but as yet, I’ve not found a single explanation why not).  But if I rigidly stick with just the accelerometer and gyro I have a nasty feeling the project will just grind to a halt and I’ll start getting bored.  I don’t do bored at all well.

What’s been holding me adding the barometer up is the pin spacing on my chosen IMU replacement for the MPU6050.  There will be an improved version coming in the spring with the pin layout I need for 0.1″ pin spacing, but spring is a very long time away on my getting bored scale.  Whereas I could just stop waiting for the world to conform to my needs, and instead, choose to fit in with the rest of the world; in other words, produce a new Raspberry Beret for Zoë that has PCB holes specifically designed to take the current IMU.

The changes are trivial, and I’ve already have a draft layout of the new PCB and components; I’ll be talking to Ragworm tomorrow about a quote.

In the meantime, I’ll start looking at FIR / IIR filters (thanks Phil!) to separate gravity from net-acceleration to cope with the sensor drift.

I also have a niggle that there must be an underlying cause for the sensor drift, and the prime candidate has to be temperature, so I’ll probably do some experimentation there too.

*The worm that turned