No Baro altitude filter for ArduPilot

I wrote down a napkin sketch of an altitude filter a while back and finally got around to testing it in the simulator. I designed it to improve the sensed altitude of the ardupilot. It is based on a fixed gain observer model and shows very promising results. The idea is to use the "lagless" airspeed sensor to give you a less noisy lagless altitude estimate. Enjoy!

K_alt = -0.01;
avg_aoa = 0.52*(pi/180); //alpha = 0.52 deg on the average
gamma = theta - avg_aoa;

alt_est = alt_est + Vair*sin(gamma)*dt;
alt_error = alt_est - GPS_alt;

alt_est += K_alt*alt_error;

E-mail me when people leave their comments –

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

Join diydrones


  • Developer
    Hey Ryan,
    Thanks for all your help. I have this implemented now on 2.6.3! It's working great.
  • Developer
    @Michael, yeah that would make sense. You could also just constrain the ammount of error the filter sees. I think the proper implementation would just be to keep the gain low anyway and rely more on the airspeed/pitch setup than the gps. Use the GPS just to move the offset over a couple of minutes or so. Also, this is predicated on the fact that airspeed is not that noisy and is also TAS not CAS. Both of which are not true so I would probably lean the filter more towards 50/50% and look for more of a 20 second time constant and use it primarily to reduce the majority of the noise but mostly the latency lag of the gps.
  • Developer
    That should really help the throttle control. Thanks!
  • Haha +5 for mentioning Murphy's law
  • Developer
    With that being said, the code posted is psudo code. I would make it a VGO (variable gain observer) basing the deweighting on theta. For high values of pitch turn the filter off (excess of 45deg etc). It just makes sense to do that. And like you said contrain the error to 100 feet or something. I just left the gain low and kept pitch +-15 deg. Asuming the aircraft is under an autopilot anyway you don't really need the adhoc logic to catch the blowups. (but always assume Murphy's law and add it in anyway)
  • Developer
    fine, it will always "search" for what ever the average altitude the gps is outputting. It is a stable filter, meaning the error will always come back to the gps (assuming you keep the gain within reason).
  • It looks to me as though the error on alt_est is unbounded. How does that behave over a long period of time?
This reply was deleted.