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)

Enjoy-

-Beall

## Comments

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.

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

Has anyone tried this algorithm on a quadrotor?

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

Thanks

Roy

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

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....

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.

http://diydrones.com/profiles/blogs/improved-windposition-ekf

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.

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.

Next