I have been working on an improved altitude estimator for a while here but have been having issues with implementation.


The theory works fine but as seen above in the FFT of the accelerometer data, there is a significant ammount of vibration/noise in the sub 50Hz range.  This is due to out of balance props etc.  Needless to say, this proves to be very detrimental to estimating altitude and velocity changes when these vibrations are an order of magnitude greater than the actual accelerations felt by climbing and descending.  You cant really low pass filter or band pass filter this because you need everything below 60 Hz or so. 


So here is what I need from you guys:

Flight data that includes the following:

  • dt in ms between samples
  • phi,theta,psi
  • barometric altitude
  • sonar altitude (if available)
  • ax,ay,az

make sure you notate units etc.  The more information on the data the better.  Thanks for the help and I will post the results.  I'm really hoping to get some data from guys with slick clean vibe free quads. 





Views: 1694

3D Robotics
Comment by Chris Anderson on January 26, 2011 at 10:47pm
Can you advise us on how to collect that data? Is is just a matter of config settings (if so, which ones?) and downloading the datafile from the CLI (again, reminder: how?)

Comment by Randy on January 27, 2011 at 4:24am


     This is great stuff you're doing.  being able to smooth out the altitude using the accelerometer would be a real step forward.

     ..when you say phi, theta, psi, you mean roll, pitch yaw (perhaps not in that order).

Comment by Ryan Beall on January 27, 2011 at 4:58am
Chris, I have no clue. I have'nt touched the mega code at all. I just know that I've been getting the data from Jose.

Randy, thanks. Yeah. Phi theta psi are pitch roll yaw respectively.

Comment by Randy on January 27, 2011 at 6:28am

I know this is a bit laborious but unfortunately the NG code doesn't have great logging (Jason's ACM code it's done better) but it's not impossible.  This should work:

  • make the numerous code changes shown below
  • before their flight, connect to APM with serial monitor, with switch CLI set to the back
  • select "z" from the CLI menu to clear all logs, switch CLI forward again
  • have a nice flight
  • switch CLI back and connect with CLI again
  • select "d" from CLI to dump log contents to serial port.  cut and paste this into excel and save as CSV file.  there is always some errors in the data (like joined lines, characters missing) that needs to be cleaned up manually or ignored

code changse required:

1. manually open Sensors.pde and add the line in bold below around line 35:

void Read_adc_raw(void)
  //int temp;
  for (int i=0;i<6;i++)
    AN[i] = adc.Ch(sensors[i]);
  Log_Write_Sensor(AN[0], AN[1], AN[2], AN[3], AN[4], AN[5], press_baro_altitude);


2. open DCM.pde and around line 175 you should add the line in bold

void Euler_angles(void)


3. in Arducopter.h around line 431, turn on logging for the sonar (if you're using one).

#define LOG_RANGEFINDER 1       // Logs data from range finders 

Comment by robert bouwens on January 27, 2011 at 6:44am

extremely interesting - not just for altitude estimating.

a clearer view here helps much.

at the moment i'm looking deeply into roll and pitch.

i am prepping an indoor quad and will collect the data.

i hope to gather data real soon.

hmm, prob. doing it with jason's code ;-)

Comment by geir on January 27, 2011 at 12:23pm

I guss press_baro_altitude = press_alt??????


I've been trying to read this but i get strange values around (-7400). Is this wrong?

Comment by Bill Nesbitt on January 27, 2011 at 1:38pm



I think you will find that the problem is not so much noisy accelerometer data, but instead the craft's attitude estimates and other biases that contribute to errors in integration.  With a properly implemented low pass filter, you can easily smooth out rough acc data without loosing the essence of the actual acceleration trend.   However, the further off your attitude estimates are, the less accurate your derived Z acceleration will be.  The difference will show up as a constantly shifting bias.  There are other (not insignificant) factors that add to this apparent bias shift including sensor calibration (including temperature) and VRE (vibration rectification error.)  VRE is found to various degrees on all MEMs based accelerometers and I've seen as much as 0.4m/s/s on some.  The easiest way (assuming you can't improve the attitude estimates) is to create a filter which maintains state for altitude (vertical position), vertical velocity and a catch all of acc bias.  This can be done with a Kalman type filter or a "fixed gain" filter.  A fixed gain filter is the easiest and least computationally expensive option and can perform extremely well if properly tuned.

Comment by Ryan Beall on January 27, 2011 at 2:04pm

Bill, Thanks for the break down.  I have looked at all of these issues and done exactly what you have described.  I have even tried running it through and Extended Kalman and because there is just so much noise on the accelerometers in the lower freqs it has zero luck locking onto anything useful.  The real trick was the analysis of comparing the sonar to the baro and watching the attitude.  The attitude estimation may be off for sure but if you look at the power of the noise the RMS is over 0.5-0.7G at time for the X and Y axis when the quad is clearly not moving or changing attitude.  You can also find some validity in the attiude estimate by comparing the normalized sonar altitude to the actual raw sonar altitude.  They are both really close to the same and pitch and roll is stating really close to 0/0. 


So that is why I ended up doing to FFT analysis of the noise.  I wanted to filter out anything I was seeing that wasn't changes in vehicle posion.  However, there is clearly a huge vibe around 35Hz or so and therefore you cant really low pass it out because you would be low passing out the signal in that range also.  I say that with some confidence because I have tried.  The velocity estimate gets a little better but I would argue that it really is that accurate beings the error feedback is based on a cascaded state error making the solution an all or nothing type approach (unless you took the derivative of baro which I wanted to avoid and is not required based on my simulation with a less noisy aircraft)


Hope this helps


Comment by Bill Nesbitt on January 27, 2011 at 2:53pm



Again, assuming the noise is near zero mean (which I would agree might not be true - depends on the data collection process), proper exponential low pass filtering will not "loose" anything important from the underlying signal except bandwidth.  In fact, in this application, the lower bandwidth from the acc (to a point) the better.  Not sure how to explain myself more clearly, but what you care about is the integration of the acc readings to determine an actual acc over time.  How far off each reading is from one to the next (variation) is not as important (as long as it is understood.)  For example a craft that is not moving (sitting on a table) with readings coming in at 200Hz, simply keep a running total of them (no matter how wild they might be) and you will have a value which should be very close to zero.  A large portion (close to 100%) of the deviation of the integrated sum from zero will be the bias.  This can be done at smaller and smaller time steps by allocating a smaller portion of the deviation to bias at each step to gain resolution.  In the case where your table is replaced by a bubble of air during flight and observed by other sensors (GPS, barometer, ultrasonic range), the same thing would be done, but the observed error should be spread across more states.  The key is tuning - how much error to apply to the three states (pos, vel, bias.)  Even a Kalman filter (of any type) can perform very poorly unless well tuned values for process and measurement noise are determined.  Offline, I use a parameter estimation filter over a simulation running actual flight data to determine what the optimal values of fixed gain parameters could be to meet whatever criteria I require, like smoothness of transition or accuracy to a particular observation or any combination.  Actually, I am doing this in 3 dimensions with observation data from GPS & baro with very good results and very little CPU, but the same can be done in a single dimension.

Comment by Ryan Beall on January 27, 2011 at 3:42pm

We are saying the same thing basically. The problem is exactly that...The noise is extrememly non-Gaussian

-Hence the FFT having a huge spike at 35HZ and 6Hz


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

Join DIY Drones

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service