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;

Views: 541

Comment by Nima K on March 8, 2010 at 9:43pm
It looks to me as though the error on alt_est is unbounded. How does that behave over a long period of time?

Comment by Ryan Beall on March 8, 2010 at 9:48pm
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).

Comment by Ryan Beall on March 8, 2010 at 9:52pm
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)
Comment by Tim Trueman on March 8, 2010 at 10:44pm
Haha +5 for mentioning Murphy's law

Comment by Jason Short on March 9, 2010 at 9:18am
That should really help the throttle control. Thanks!

Comment by Ryan Beall on March 9, 2010 at 8:25pm
@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.

Comment by Jason Short on July 23, 2010 at 11:42am
Hey Ryan,
Thanks for all your help. I have this implemented now on 2.6.3! It's working great.


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service