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
re. in our flight manual
You talking about a big, thick, blue NATOPS manual?
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.
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.
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.
-
1
-
2
-
3
of 3 Next