So for the last week or two I have been looking a bit at the performance of the Tau Labs altitude control and INS filter we have and the altitude control loop.
One issue I was suffering from is that the velocity estimate would be biased because of variations in the accelerometer bias. I switched to using a 16 state version of the INS written by Dale Schinstock that included an estimate of the bias terms for hte accels which improved things somewhat.
I'm still not happy with the performance It is a dual loop control - the position error sets a velocity desired which then runs through a PI controller.
So the first question I had was whether there was a problem in the estimation or in the control system. I grabbed some logs with my Freedom board and then also did some video processing. I swept a number of settings for hte baro variance and definitely if I assign to much noise to the barometer, then it doesn't track well enough. However, with the typical settings I use then it does a pretty good job of estimating the altitude. I made a video showing it with two values of the variance:
The plot on the right in some segments shows the segmented quad location from the video stream (i.e. ground truth)
One thing that struck me. When I tune it up to get a good estimate of position, that increases the noise in the velocity estimate. I feel like this ends up limiting my tuning parameters. I've also implemented the ardupilot complementary filter, (keep meaning to grab a video log of that too) and cannot tune it to the point I'm happy.
Anyway, the solution I'm considering is to apply some low pass filtering to the velocity estimate in the controller. Essentially the velocity control component is like a derivative of the position estimate, and this seems like a reasonable / sensible technique. However, I'd love to hear any comments from others here.
Discussion at new Tau Labs forum: http://forum.taulabs.org/viewtopic.php?f=17&t=27&start=20