Improved Altitude Estimate

The barometric sensor alone in most cases provides noisy and drift prone measurements. I have been trying to come up with a simple way to implement a fixed gain filter to utilize the acceleration measurements from the accelerometers to help clean up the noise and also add a sanity check via sensor fusion.

The cool part about the filter is that it also estimates vertical velocity which is useful for altitude damping and control. The basic idea behind the filter is the Z or vertical component of the acceleration is derived using the body rotations to compare with the barometric measurement. The noise of the barometric sensor is rejected whenever the accelerometers disagree. The down side to this approach is it depends upon the accelerometers being less noisy than barometric sensor and also not being on a vibration intensive vehicle. The theory works but actual implementation is somewhat hardware dependant.

The real plus to this filter is the velocity estimate can be used for altitude damping and control. Instead of taking the derivative of the barometric sensor which is going to significantly increase the noise in the derivative term, you get a much cleaner vehicle state estimate with this approach.

The code is fairly simple but may require tuning based on units and hardware. I took the fixed gain approach to keep it simple and as a result looks like the following:

The thing to note here is these are all relative to the NED reference frame (ie: acc_z is not the body z acceleration it is the NED z acceleration)



E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Hello Mr. Ryan, I'm a student who are working on a quadrocopter project. I think your article is very amazing. However, I couldn't understand all of those 6 line of code. what kind of filter are you using to combine the data from accelerometer and barometer? I think it look some how similar to low pass filter, but it apparently not. is it ok if you provide me some further explanation? I think I couldn't apply it if I dont really understand what it is. Thanks a lot, Mr.

  • Developer
    I thought about writing a band pass filter or low passing it but after running a FFT on the data it was clear that the vibrations were all in the worst case low frequency range. It was quite clear that there was no way to get useful filter updates from the sensor because the noise was 3x the signal and right in the frequency range you'd expect to measure altitude changes.

    I tune it using matlab and post processing data.
  • I was thinking a low-pass on the accel might help with the noise issue.


    How did you arrive at these gains? Empirically?


    I don't (yet) have a quad either - but I'm working on it!


    - Roy

  • Developer
    This is a fixed gain approach. And yes I have tried this on jose's quad's data but he had really bad vibrations and the data proved to be unusable. I'd love to run the algorithm on better data but haven't received any. I don't personally have a quad so can't really generate my own except this sim data.
  • Has anyone tried this algorithm on a quadrotor?


    Brandon Wampler - any progress on a (constant gain) Kalman filter implementation?




  • Developer

    I just modeled White noise with no bias.  I might work up a bias tracker also.

  • Developer

    Ryan, did you model noise or offsets in the acceleration measurement.  I have a bit of both and am having a bad time getting decent results....


  • Ryan, I agree with your simple aproach, but don't anyone underestimate its power.

    The 4 simple equations, if combined with the the scalar valued steady-state Kalman gains, which only need to be calculated once, are the almost absolute optimal choice (the time varying Kalman gains would be the optimal). There is no better estimate in the world. Rudolf E. Kalman mathematically proved it when he derived the Kalman filter back in the 60's.

    Manually tweaking the gains will work well and is probably sufficient. Keep in mind there is a solution that is guaranteed to be the optimal combination if so desired.
  • Developer
    Your are crushing the point.... I have already written my full EKF deadreckoning also....

    The point is to keep it simple. 4 equations for better altitude hold or 6 matrix inversions and some more matrix addition and multiplication followed by a couple dials to tweak. 4 Equations is looking pretty good no? Bill's work is top notch! But way above most people's skill or debug level so hence the derivation of my 4 equations.
  • In MatrixPIlot for UDB there is the dead reckoning, a masterpiece written by Bill Premerlani.
    This already uses the fast data from the accelerometers combined with the slow noisy data from the gps.
    The accelerometers guess where the aircraft is and the gps slowly adjusts the errors. The gps data is already filtered.
This reply was deleted.