Tau Labs Altitude control loop architecture and INS tuning

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


Views: 2403

Comment by Crashpilot1000 on March 15, 2014 at 4:57am

Hi! To get better rawdata you can use a spikefilter on raw baro data. It adds no to minimal lag but gets rid of those nasty barospikes a great deal and can therfor minimize further filtering needs. I once proposed it here: http://diydrones.com/forum/topics/perhaps-useful-spikefilter?page=1 . I still use it in my althold code (and some part of GPS) and I think it's practically useful. Just saying.

Comment by Bill Nesbitt on March 15, 2014 at 7:20am

With the optical ground truth data, you have enough information to replay the flight in a ground simulation and compare and score the results against reality.  This could be used to create an iterative goal seeking function aimed at driving the error towards zero by manipulating all filter process and measurement variances.  In the end you will find that you can trust the ACC data to a very high degree if they are being biased correctly by the filter.  Integration will then provide very smooth velocity estimates.

Comment by James Cotton on March 15, 2014 at 9:38am

@Crashpilot1000: nice, you actually rediscovered the median filter, and it is really good for exactly what you described: throwing outliers out.

In my video, that lower left plot you can see a faint white line, especially in the first section. That is the raw baro data median filtered with a window of 21 samples. It ends up introducing the amount of lag as the window size. One problem with implementing that on a flight controller is that sort is actually a relatively expensive operation (in computer science it is typically one of the prototype algorithms to study the O() of various algorithms), and to run it on a time series you need to perform a sort for each new sample. It's probably still doable. The real question becomes if you have a lot of outliers when you have the raw data at the full sampling rate. From my brief glances, it looks like the data is fairly gaussian, at which point some cheaper filters can get a long way. In this case, in principle the INS is trying to do the same thing - which is the yellow curve in the lower left plot. Compare how well it tracks the white (median filter) line in the first video clip versus second and you will see it starts to do better.

Comment by James Cotton on March 15, 2014 at 9:46am

@Bill Nesbitt: thanks. I've had less success than you I think in terms of trusting the accel, but given the performance I've seen in your videos, it is probably time to revisit that and see why. Do you apply much or any vibration damping? I tend to rigidly mount my flight controllers.

So I rescaled the video output to match fairly well with the INS output. In terms of position, they are doing quite well. However, the velocity shows the noise I described in the previous video. Interestingly, you can really see the latency in the velocity estimate versus the ground truth - probably as a result of the fact it is being driven by the baro instead of the accelerometer.

Now in the first clip, where the baro variance was lower, which should make it trust the accel more and approximate integration, the position itself was not tracking well - and specifically looked like it was lower amplitude than the real movement. I need to make 100% sure there isn't some scaling issue (e.g. incorrect dt) happening that makes accels not drive enough change in velocity.

Time to do what you suggest: drag out my code for rerunning the sensor data through the filter on the PC and optimize some parameters. Do you have a good recommendation for formal ways to do that. I've seen two approaches: optimization procedure on the params themselves to maximize the likelihood of the observed data, and including the params as augmented state variables and running the EKF that way. It isn't something I've found good literature on.

Comment by Gary McCray on March 15, 2014 at 10:10am

Hi James,

I think the reason you can't trust your accels is the rigid mounting of the flight controller.

Interestingly, you also see some vibration effect on the baro as well.

As a general solution a small square of Kyosho Zeal at each corner of the flight controller will reign in vibrations satisfactorily for most airframes and applications.

If you are uncomfortable with the zeal's adhesive for retention by itself either a rubber band or a loose velcro tie can be used.

There are other vibration damping / isolation products, but so far none have yielded as consistent results (for flight controllers) as Kyosho Zeal.

I definitely recommend you revisit the vibration isolation thing as otherwise you will be chasing a lot of vibration related phenomena instead of the underlying issues you are looking for.

It is also a good idea to provide a "strain relief loop" in all wiring connected to the flight control board as well to minimize vibration transferred through the wire bundle.

This wiki page might help: http://copter.ardupilot.com/wiki/vibration-damping/

Best Regards,


Comment by Bill Nesbitt on March 15, 2014 at 10:11am

No, I use only hard mount.  Vibration insulation will tend to create another rotational body somewhat independent of the frame.  I've found that ACC noise does not really hurt the integration if your timstep is short enough.  What does hurt is a shifting bias caused by VRE.  You will see a shift in ACC bias between when the craft is static on the ground vs vibrating in the air during flight.  How much will depend on the sensors and the amplitude and frequency of the vibration.

Adding another large set of parameter to try to estimate in real time will probably not work out very well. There will be too many more parameters than there are observations to drive it.  Pre-estimating variances on the ground will likely be your best bet.

You may need to add noise terms to the process update step of your Kalman filter to add some cross variance isolation and account for measurement time differences like the lag of the realized baro reading vs the ACC data.  In my filters, I do include these terms as additional states to be estimated with the rest of the system, but their initial state is again, pre-calculated.

Comment by Jason Short on March 15, 2014 at 10:28am

Virtually every professional AP system uses some sort of dampening of the accelerometers. 

Comment by Gary McCray on March 15, 2014 at 10:39am

A thin piece of gel pad under a relatively light flight controller causes negligible lag.

Its job is to mitigate high frequency low amplitude vibrations which it does very well, but its density is such that it moves very little under normal flying maneuvers.

Short of full stop acrobatics, it's "lag" is not meaningful at all, but its capability for mitigating high frequency low amplitude vibration is excellent, please look at actual use comparisons on wiki page referenced above.

On multicopters, accels are of severely limited use without sufficient vibration damping.

Comment by Bill Nesbitt on March 15, 2014 at 10:44am

There is a difference between dampening and isolation.  There is nothing wrong with rigidly attaching a hunk of lead to the IMU.  But isolating the IMU from the craft's frame is counter productive.  My point is that neither is actually necessary, especially if one takes the time to try to reasonably reduce vibrations at their source.

BTW: What's your definition of a professional AP?  And, why do you think they would they know best?

Comment by James Cotton on March 15, 2014 at 12:08pm

I think everyone would agree getting noise out of the accels would be is useful, and if it starts to not track the frame that is bad. The performance I've seen from Bill's AutoQuad navigation videos and the fact he doesn't use any additional damping indicates it can be done. I think his point about sampling rate is pretty key - I know we run at a minimum of a couple hundred Hz and at least in terms of attitude estimation do not need any damping. I've had pretty fair navigation performance without it too.

We did, interestingly, find it was necessary to apply some LPF to the accels before attitude estimation with the complementary filter. That surprised me since it should behave somewhat like an LPF itself. Going through the math though, it was apparent that if you take gaussian noise (independent on x and z), then a atan2 transform, then filter the output results in a bias in the attitude. However, if you filter and then run atan2 you reduce the bias in the estimated orientation. So it really becomes the interplay of the noise and your estimator if you can handle it well.

My goal is always to try and make our code work in a rigid system if possible (although reasonably balanced frame) because it makes it much easier for people to mount and use. Then if people take more care and add damping and  it works even better - great. For what it's worth, almost all of my navigation videos are without any damping and the performance is pretty good.

I had the scaling wrong in that last plot, looks like the INS is underestimating the velocity:

So yeah, time to work on the accels.


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