Height and vertical velocity Kalman filtering on MS5611 barometer

Height and vertical velocity from a MS5611 barometer

Steps towards vertical flight control

This isn't a post explaining how Kalman filters work, I assume some working knowledge.  Instead I wanted to record the process of developing a Kalman filter to derive both the height and the vertical velocity of a quadcopter using a MS5611 barometer, as it may be useful to others.  After recording some flight data on my quadcopter EEPROM, I then spat the output to a PC for post-processing and model training.  In Python I used a Kalman class to generate the following Kalman filter (Wikipedia: Kalman filter) based on the barometric height data.

This Kalman filter can be used to take the very noisy barometer data and, in theory, output the current height and vertical velocity.  This is particularly attractive as the results will be correct even when the quadcopter is tilted at an angle as the air pressure is not changing.  So how does it perform?  See below - the data is collected 250 times a second and records the barometric height above the ground.  Cyan represents this raw data and the resulting Kalman height (blue) and a simple complimentary filter (red) of the raw height are shown.

The filter works great for giving us an estimate of the height - but this isn't really such a great challenge.  As we can see, if all we wanted was the height then a complimentary filter would give us similar results for much less computation, i.e.

alpha = 0.96

filtered_height = alpha * filtered_height + (1-alpha) * barometer_height

where barometer height is derived directly from the air pressure, versus some reference ground pressure like so:

barometer_height = 100 * (pressure_ground_reference - pressure_measured) / 0.12

to give the barometer height in cm.  Note: I have derived this from the barometric formula, and it is valid to a good approximation over at least 100 m.

So what about the Kalman velocity?  The derived Kalman velocity is shown in the below figure, in the bottom pane (blue).  Noisy isn't it!  A complimentary filter (green) to smooth the Kalman velocity is shown for comparison.

So what can we do to get less noisy velocity data from our Kalman filter? We can't change the model, but we can change the co-variances associated with the raw data (in matrix R), and with the model (matrix Q).  With some tweaking of these co-variances it's possible to generate a much smoother filter, below. Note the height (red, top panel) and the Kalman velocity (blue, bottom panel) are a lot less noisy.  There is clearly a cost here though - a large lag has been introduced into the output.  I use the time between the maximum height and therefore velocity to be zero at around 4 seconds and the time taken for the kalman velocity to report as zero as a measure of this latency.

So, we have identified two conflicting variables here that we want to balance: the noise of the velocity filter and the sensitivity of the filter, i.e. the latency between a change in the raw data and the corresponding change in the state variables.  We can see that these are in tension - the more responsive the filter, the more raw data noise we'll have, and conversely the more smoothing and thus the larger the lag time we'll see.  So how do the measurement co-variance and the process co-variance interact to give what we'll consider an optimal balance?  Time to Science the fudge out of it!

Optimising co-variances

I should just say that there are probably good ways to do this... this is stuff I'm more or less making up as I go!  With that in mind - first, how can we quantify (1) the noise of the Kalman velocity and (2) the latency of the Kalman velocity?  We can easily use a high pass filter for the noise.  To quantify the latency, I compare a couple of points in the data when the velocity should be zero, against when the Kalman velocity does go to zero.  In order to handle the very noisy data I use a low pass filter to get the general shape.

The high and low pass filters were implemented in Python:

def lowpassfilter(array):

  f = []

  l = len(array)

  SAMPLE_RATE = 250.0

  CUTOFF = 2500.0

  RC = 1.0/(CUTOFF*2*3.14)

  dt = 1.0/SAMPLE_RATE

  alpha = RC/(RC + dt)

  f.append(array[0])

  for i in range(1,l):

    b = f[i-1] + (alpha*(array[i]-f[i-1]))

    f.append(b)

  return f

def highpassfilter(array):

  f = []

  l = len(array)

  SAMPLE_RATE = 250

  CUTOFF = 5

  RC = 1.0/(CUTOFF*2*3.14)

  dt = 1.0/SAMPLE_RATE

  alpha = RC/(RC + dt)

  f.append(array[0])

  for i in range(1,l):

    b = alpha * (f[i-1] + array[i] - array[i-1])

    f.append(b)

  return f

By applying the high and low pass filter separately to the original Kalman velocity, it is possible to extract a measure of the noise and the general 'shape' of the Kalman velocity, without the noise.  In the below figure, the Kalman velocity straight out of the filter is shown.  In the bottom panel, a high pass filter (blue) gives a measure of the noise of the velocity and the low pass filter (green) extracts the lower frequency events (I'm calling this the 'shape' of the signal but I'm sure there's a better term).

It is then possible to throw the recorded flight data through the Kalman filter and change the Q and R covariances in a systematic way in a grid and plot both the RMSD of the noise, and a measure of the latency.  If nothing else, these plots look really pretty :)

I think this has worked quite well.  I can easily calculate an RMSD for the noise, and for the low pass filtered data I can programatically calculate the latency at a couple of identified time points to generate a "latency score". For various values of the co-variances (Q and R) I can now calculate both a measure of the noise and the lag of the filter, as shown in the figure below.  These plots are in line with our expectations.  The top left Q/R combination shows a high noise (red) in the left hand plot but a low latency score (blue) in the right hand plot, and vice versa for the bottom right corner.  Interestingly we can now pick out values of Q and R that give us what we want, a balance between low noise and responsiveness.

For the record the y-axis on the above plots corresponds to the variance (sigma2a):

and x-axis corresponds to the variance of the barometer measurement (sigma2z):

What is the 'optimal' pair of values here?  That somewhat depends on a decision for me to make - do I value responsiveness more than noise?  I think at the moment I will forego some latency in order to gain better signal to noise.

Addendum:  Seeing the low pass filter at work makes me wonder whether I can tolerate more noise in the output Kalman filter by passing the velocity through a low pass filter before using it?  The low pass filter itself may introduce a lag but this might be enough countered by choosing a responsive enough Kalman filter in the first place.  

Porting Kalman filter to the Atmega328/Arduino Nano:

Because the Kalman filter we're using is fairly simple - a single barometer measurement and small 2x2 matrices, it is quite straightforward to calculate the matrix inverses and transpositions by hand where necessary to calculate implement the Kalman filter without needing a matrix library. 

First we setup a few variables:

void KalmanPosVel(void);

void initKalmanPosVel(void);

struct quad_properties{

// State variables

float height;

float kalmanvel_z;

// Measurements

float baro_height;

};

struct quad_properties quadprops;

struct matrix2x2{

float m11;

float m21;

float m12;

float m22;

};

struct matrix2x2 current_prob;

void initKalmanPosVel(void)

{

current_prob.m11 = 1;

current_prob.m21 = 0;

current_prob.m12 = 0;

current_prob.m22 = 1;

}

quadprops.baro_height should be set as the height derived from raw barometer reading every time we have a new reading.  We are then ready to run the Kalman filter every program loop, by calling KalmanPosVel().  Note var_acc and R11 are equivalent to the y-axis and x-axis values respectively in the 2d plots above.  Please experiment with these values yourself - I have not finalised them myself yet!

#define timeslice 0.004 # 250 Hz

#define var_acc 1

void KalmanPosVel(void)

{

const float Q11=var_acc*0.25*(timeslice*timeslice*timeslice*timeslice),Q12=var_acc*0.5*(timeslice*timeslice*timeslice),Q21=var_acc*0.5*(timeslice*timeslice*timeslice),Q22=var_acc*(timeslice*timeslice);

const float R11=0.008;

float ps1,ps2,opt;

float pp11,pp12,pp21,pp22;

float inn, ic, kg1, kg2;

ps1 = quadprops.height  + timeslice * quadprops.kalmanvel_z;

ps2 = quadprops.kalmanvel_z;

opt = timeslice*current_prob.m22;

pp12 = current_prob.m12 + opt + Q12;

pp21 = current_prob.m21 + opt;

pp11 = current_prob.m11 + timeslice * (current_prob.m12 + pp21) + Q11;

pp21 += Q21;

pp22 = current_prob.m22 + Q22;

inn = quadprops.baro_height - ps1;

ic = pp11 + R11;

kg1 = pp11/ic;

kg2 = pp21/ic;

quadprops.height = ps1 + kg1 * inn;

quadprops.kalmanvel_z = ps2 + kg2 * inn;

opt = 1-kg1;

current_prob.m11 = pp11 * opt;

current_prob.m12 = pp12 * opt;

current_prob.m21 = pp21 - pp11 * kg2;

current_prob.m22 = pp22 - pp12 * kg2;

}

And that's it - I have of course fully validated this code against the output produced by the matrix.

Conclusions

Hopefully this has given some insight into a height-velocity Kalman filter just using barometric pressure.  It has been a useful learning exercise for me!

One caveat of this simple Kalman filter is that there is an issue with the rotors perturbing the measured air pressure as the drone is near the ground, fooling the filter into thinking there is a change in height where there is none.  For our purposes we will ignore this close-to-ground effect for now.  Creating a more complex Kalman filter using the accelerometer readings could be done in the future to solve this problem (see Part 2 here!), but the larger we make the matrices involved, the more computationally expensive it will become to manipulate them - particularly to find the matrix inverse.  We would also need to rotate the accelerometer vectors into the World frame. I am interested in what performance we can squeeze out of the 8-bit Atmega328 in our 250 Hz loop!  A more complex Kalman filter may require a switch to fixed-point integer maths.  I have also been exploring the possibility of using Savitzky-Golay filtering on a complimentary filtered barometer height, which I hope to post on soon.

Height and vertical velocity Kalman filtering on MS5611 barometer: part 2

References:

https://www.quora.com/Whats-the-C-coding-for-a-low-pass-filter

https://stackoverflow.com/questions/13882038/implementing-simple-high-and-low-pass-filters-in-c

UPDATE 25/11/2017
To help others I have deposited my Python code for generating the 2D figures below.  The code uses the Python MATLAB library matplotlib as well as the python libraries numpy and scipy.

import scipy.interpolate
import pylab
import math
import numpy

# Setup some ranges for R and Q

# X-axis - R- measurement covariance
R_min = 0.0001
R_max = 1
R_n = 10

# Y-axis - Q - process covariance
var_min =0.001
var_max = 10
var_n = 10

logvarmin = math.log10(var_min)
logvarmax = math.log10(var_max)
logvarrange = logvarmax-logvarmin
logRmin = math.log10(R_min)
logRmax = math.log10(R_max)
logRrange = logRmax-logRmin

Raxis=numpy.zeros((1, R_n*var_n))
Vaxis=numpy.zeros((1, R_n*var_n))
z = numpy.zeros((1,R_n*var_n))

# Now run through the ranges for R and Q, calculate the property desired (i.e. noise or latency) and store in array "z"

for ri in range(0, R_n):
  r = logRmin+(ri*logRrange/(R_n-1))
  r10 = 10**r
   
  for vi in range(0,var_n):
    v = logvarmin+(vi*logvarrange/(var_n-1))
    v10 = 10**v

    index = (var_n*ri)+vi
    
    Raxis[0][index] = r # use exponents for now....
    Vaxis[0][index] = v

   # calculate your Kalman filter code here, using r10 and v10 as your R and Q variances
   # and work out what property of the filter you want to plot (i.e. noise or latency).

    z[0][index]= what_to_plot_goes_here!

# Now make the 2d plot

xi, yi = numpy.linspace(Raxis.min(), Raxis.max(), 100), numpy.linspace(Vaxis.min(), Vaxis.max(), 100)
xi, yi = numpy.meshgrid(xi, yi)

pylab.figure(1)

rbf = scipy.interpolate.Rbf(Raxis, Vaxis, z, function='linear')
zi = rbf(xi, yi)
        
pylab.imshow(zi, vmin=z.min(), vmax=z.max(), origin='lower',
             extent=[Raxis.min(), Raxis.max(), Vaxis.min(), Vaxis.max()])

pylab.xlabel('log(Measurement covariance, R)')
pylab.ylabel('log(Process covariance, Q)')

pylab.title('RMSD noise of Kalman filter')
pylab.rcParams.update({'font.size': 20})

pylab.scatter(Raxis, Vaxis, c=z)
pylab.colorbar()
pylab.show()