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)
Enjoy-
-Beall
-
Developer Comment by Randy on December 6, 2010 at 7:15am -
Looks great.
I'll bet you need to be very careful setting up the accelerometer values to be sure that you don't see a change in your Z acceleration as you tilt.
-
Comment by Matthew Coleman on December 6, 2010 at 9:16am -
State estimation
Noisy input signal
Fusing different input signals
Smells like a Kalman filter to me.
$0.02 from Matt
-
Developer Comment by Ryan Beall on December 6, 2010 at 10:07am -
@Randy - Yeah you have to rotate the body accelerations into the North East Down fram. The code above assumes that the acc_z is the acceleration that has already been rotated.
@Matt - yeah this would be a simple Kalman to write but I was really looking for a simple way to help out the arducopter guys and this is a very low math intensive approach. No matrix math needed as would be required in a Kalman. I guess because it is so simple (ie one state) you could probably do all of the matrix math by hand and simplify it, but the code would still be longer than 4 lines! If I were to actually use this I'd prolly go the Kalman route if processor power wasn't an issue.
-
Comment by Matthew Coleman on December 6, 2010 at 11:05am
-
Fair enough.
Your example would not adjust for the barometer thermal drift.
Have you considered using the gps to correct barometer offsets in the same way that the barometer corrects for accelerometer drift?
-
Comment by Tom Yochum on December 6, 2010 at 11:13am -
I am with Matt on this. Find a clever way to blend the GPS altitude in as well and you have yourself a real winner!
What kind of error modeling did you use for the barometric sensor in the simulation? Random noise or something more complicated.
Tom
-
Developer Comment by Ryan Beall on December 6, 2010 at 11:31am -
Yeah I've written something similar using gps. To use nothing more than gps you can just replace bar_alt with gps_alt. For a blended version you could lowpass both and constrain the Bari drift with gps but use bar_alt in the filter for higher bandwidth. Not too hard.
As far as the modeled noise on accelerometers and barometric sensor: they are both gaussian of approximate value if the sensors used by the ardupilot mega.
-
Comment by Matthew Coleman on December 6, 2010 at 1:42pm
-
A problem is barometer gain error.
Say you are at altitude and make a fast descent.
The barometer gain error will appear as an offset.
How do you know if that is gps error or barometric error?
Someone else must have been through this before.
-
Comment by Brandon Wampler on December 6, 2010 at 1:51pm -
This has the same form as a two state (vertical position and velocity) with a single measurement (barometric altitude) steady-state Kalman filter. You can pre-compute the steady-state Kalman gains using the noise properties of your sensors and the Algebraic Ricatti equation. This way you just replace your gains with the steady-state Kalman gains and avoid matrix math and inversion. This approach would also make it easy to add another measurement such as GPS altitude and fuse the IMU, GPS, and barometer together in a Gaussian-ly correct method. I'd be happy to work on this after the semester ends but maybe you guys can get a jump start on it.
-
Comment by Matthew Coleman on December 6, 2010 at 3:12pm
-
Brandon,
This kind of makes sense. My Kalman knowledge is not deep so I will have to take your word for it on the maths techniques.
I still get the feeling someone must have done this already. I really dislike re-inventing stuff.
-
Comment by Brandon Wampler on December 6, 2010 at 4:28pm
-
There shouldn't be too much re-inventing, Ryan's equations are spot on with GPS measurement just needing another addition term (for example: ... + k_*_gps(h_est - gps_alt).
The new part is how you choose the gains which are based on the state propagation equations and the amount of noise expected in each sensor which is hardware specific. Fortunately I would expect most ArduPilot Mega sensor boards to have about the same noise so the gains could be hard coded once calculated. Matlab has a function kalman() (http://www.mathworks.com/products/control/demos.html?file=/products...) and Scilab has a function lqe() (http://www.scilab.org/product/man/lqe.html) to calculate these gains.
You can also throw you're barometer offset into the state and estimate it as well. I'd have to write out state-space equations, but I think it may still remain a steady-state problem.
This stuff sounds like fun to work on, but I need to get graduation out of the way first. Let me know if anyone has any questions tackling this.
Contests
Season Two of the Trust Time Trial (T3) Contest has now begun. The third round was a reliablilty/aerial photography round for both planes and copters, which is now closed. Stay tuned for the next round, beginning soon.A list of all T3 contests is here.
Top Content
Groups
-
DIYDrones - CNC Project
61 members
-
Moderators
57 members
-
ArduPilot (legacy) User …
95 members
-
ArduCopter Evolution Team
108 members
-
ArduCopter User Group
617 members
© 2012 Created by Chris Anderson.
You need to be a member of DIY Drones to add comments!
Join DIY Drones