*Keywords: Sensor fushion, altitude estimation, barometer, accelerometer, complementary filter*

As part of my ongoing quadcopter flight controller project I had to find a realiable way to estimate altitude in order to get an “Altitude-Hold” functionality. I solved this by using data from the barometer and accelerometer by fusing it together using a complmentary filter. The result is the picture below, followed by a brief explanation of my approach. As can be seen, the filtered result depend short time on the drifting accelerometer, while it long term is dependent on the correcting, but noisy baromter.

The data provided by the barometer differs about ± 50cm, which is not good enough for altitude estimation of a quadcopter. In order to fix this, the z-axis from the accelerometer can be used. Firstly, the gravity has to be substracted so the acceleration is zero at rest. To do this, we must calculate the orientation of the gravitational vector using the pitch and roll angles. This will allow the quadcopter to read zero g’s on the z-axis independent on orientation, as long as it’s at rest.

Plotting the corrected data it can be seen that both sensors are rather noisy, which is why I applied a simple low-pass filter to both signals.

typedef struct{ float input; float output_last; float output; float coeff; // lower value, lower damping float dt; // delta time } lowpass_t; void low_pass(lowpass_t *temp) { const float damping = temp->dt/(temp->coeff + temp->dt); temp->output = temp->output_last + damping*(temp->input - temp->output_last); temp->output_last = temp->output; } |

The unit of measurement from the barometer is already position. But the unit of the accelerometer is, shocking, acceleration. To be able to fuse these two signals together their units must match. While the temporal integral of acceleration is velocity, the second integral is position, which is what we’re interested in. Unfortunately, all accelerometers has an internal offset error, which will cause the velocity calculation error to rise linearly with time. A position calculation will have error rising quadratically with time.

The complementary filter will be useful here. It is used to fuse the velocity calculated from the integral of acceleration together with the velocity calculated from the derivative of the altitude estimation from the barometer.

typedef struct{ float vel; float acc; float alt; float dt; } altitude_t; altitude_t alt_baro; // Derive altitude and get velocity in cm/s static float last_alt; alt_baro.vel = (alt_baro.alt - last_alt) / alt_baro.dt; last_alt = alt_baro.alt; |

altitude_t alt_tot; // Velocity = initial velocity + (acceleration * time) float vel_temp = alt_tot.vel + alt_tot.acc * alt_tot.dt; // Calculate velocity using complimentary filter static float vel_coeff = 0.999f; alt_tot.vel = vel_coeff * vel_temp + (1 - vel_coeff) * alt_baro.vel; |

When the velocity is corrected, finally the same procedure can be used for altitude estimation:

// Position = initial position + (initial velocity * time) + (1/2 * acceleration * (time^2)) float alt_temp = alt_tot.alt + (vel_temp* alt_tot.dt) + (0.5f * alt_tot.acc * alt_tot.dt * alt_tot.dt); // Calculate altitude with complimentary filter static float alt_coeff = 0.998f; alt_tot.alt = alt_coeff * alt_temp + (1 - alt_coeff) * alt_baro.alt; |

The result (first picture) is a fused signal almost as smooth as the altitude estimation from the accelerometer, but with the long term correction from the barometer. This can surely be tuned and improved, and I might update the post when progress has been made.

The full source code can be found here, but has probably been improved since this post was written.