Altitude estimation from barometer and accelerometer , fusion algoritm

Hi all, im developing a autopilot from drone (copter) application , for now the copter fly good and have a stabilization in all 3 axis computed from a imu with 3 axis accelerometer 3 axis gyroscope 3 axis magnetometer , but now the problem is to stabilize the drone in the aLtitude .

I have a barometer that give me the altitude estimation but this is not enought because barometer have some drift and the sample rate is not enought fast for have a good stabilization so i thought to fuse the accelerometer z axis with the barometer datas for have a clean and fast altitude estimation.

for get a dynamic z accelerometer data i have calculated a static gravity values from quaternion and i have subtract it from the accelerometer value (calibrated value). The problem is that i can not have a estimation on vertical position because accelerometer is really unstable with noise ( also after low pass filter ) and a offset generate a error in the velocity calculation and of course in the vertical position.

Someone had this problem ? what's the solution ?

It is not possible to calculate position using consumer-grade accelerometers; they are too noisy. More information here: http://www.chrobotics.com/library/accel-position-velocity

so is there no alternative ? i have seen in many autopilot they say to use barometer with z acceleration fusion with a kalman filter and get a usable altitude estimation value . maybe are they using only the acceleration ?

look these below

The noise isn’t the issue, its the drift you have to cancel. Kalman filter is the optimal
solution to any model estimation problem, but they are complex and you need to
sort out your variance matrices and so forth.

Easier is to use the altimeter to slowly derive the correction factor for the z-acceleration
Using a PID loop. Treat the correction factor to the IMU as the PID output, and the
difference between IMU and altimeter height estimates as the control input.

You make the PID coefficients small enough to keep its reponse slow and then that
averages out the error from the altimeter, and only have the short-term drift from
the IMU left (long term drift is cancelled out).

Remember to do all the height stuff in the global coordinate system, not the local one.

[ BTW you shouldn’t low-pass filter the acceleration vector, just integrate it twice (this
automatically low-pass filters but doesn’t lose information) ]

Extensive writeup:

http://home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf

I took a lot of math in college but not enough to grasp all of this.

The noise isn’t the issue, its the drift you have to cancel.

Why not post some code, and show us how it is done?

Basic integration:

#define MS 10   // lets say update every 10ms
float dt = MS / 1000.0 ;  // dt in seconds
unsigned long next_t = 0L ;
float velocity = 0.0 ;
float position = 0.0 ;
float correction = 0.0 ;

void loop ()
{
  if (milis () - next_t  >= MS)
  {
    next_t += MS ; // prepare for next time
    float measured_z_accel = ....... ;  // here get the (upwards) z component in m/s/s
    // now integrate twice, subtracting gravity and adding  the dynamic correction
    velocity += (measured_z_accel - 9.8 - correction) * dt ;
    position += velocity * dt ;

    altimeter_pos = ...... ; // get the altimeter reading
    correction = pid_loop (altimeter_pos - position, dt) ;  // run PID loop
  }
  ...
}

Well, that is a start!

The most difficult part is the estimation of the Z acceleration. As the CHRobotics link demonstrates (albeit with some numerical errors), errors in the estimated pitch and roll angles of the craft of just one degree lead to incorrect subtraction of the acceleration due to gravity, and to nonsensical position estimates within a minute or so. As a result of sensor noise, that level of error is the best you can currently expect from consumer grade IMUs.

I agree that proper weighting of the barometric and accelerometric contributions, in the iterative fashion suggested, should lead to a better estimate of the altitude.

so is there no alternative ?

Use GPS. Fusion of barometer and gps altitude. Forget about accelerometer.

GPS altitude is not all that good but better than nothing, and will enable compensation for variations in atmospheric pressure. Also the "noise" is not distributed in a proper random way.

Hi.

I am also very interested in this topic. Could anyone make some steps forward?
Anyone succeeding in combing barometer and accelerometer?

The readings I get from my barometer are noise but after applying a running average filter, that seems be OK. However, there still some open issues regarding accuracy and long term fluctuations. if I just could combine my current measurement with another eternal sensed data, that would greatly improve results.

Thank you. ciao
Marco

See this paper for a successful approach http://home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf

See this paper for a successful approach http://home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf

Hi.

Thank you!

Will go trough it and get back as soon as I will encounter some difficulties or questions will arise.

ciao
M.

Are you worried about the drift caused by changes in weather? If so that drift can be handled by using a barometer on the ground to measure the changes and then use the difference in the atmospheric pressures to calculate the altitude difference between the ground station and the AV.

no that does not worry me. Swings in meteorological parameters, such as mean press, change over a time period which is greater then my flight time (battery charge). What I am interested in solving is, instead, the accuracy, reactivity and precision of the measured pressure data when fused with another sensor.

The barometer alone, won't serve much as that is very unreliable and prone to a wide range of ambient disturbances or measurm. errors. TO make a decent altitude hold I need and want something more robust, which the barometer alone cannot give me.

GPS altitude is probably worse than the barometer in the short term.

You probably need to fuse all three, accelerometer for timescales of a few seconds, barometer for seconds to
a minute, averaged GPS to correct long term drift.

Collect some graphs of the output data from a barometer and GPS in realistic conditions and look at
them?

I have a working Kalman Filter for altitude estimation from barometer and accelerometer data. It also estimates z axis acceleration bias.

At the moment, I’m using the LPS25HB barometer from ST, and over short time durations (< 5 minutes), it gives accuracy of around 100 mm (0.1 meters). Over longer durations, atmospheric pressure changes causes drift in the nominal value.

Has anyone been able to get better accuracy than that?

MM81:
The barometer alone, won't serve much as that is very unreliable and prone to a wide range of ambient disturbances or measurm. errors. TO make a decent altitude hold I need and want something more robust, which the barometer alone cannot give me.

The options appear limited obviously. Can only do so much. Eg..... the best we can do is to consider all the feasible option..... like, barometer, gps, accelerometer...... and maybe that's about it. If that's all we have, then that's the best we can do.

Chagrin:
Extensive writeup:

http://home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf

I took a lot of math in college but not enough to grasp all of this.

I think the only reason why most people don't grasp it - is because there appears to be never one source of information that 'fully' guides everybody from beginning to end.....with step-by-step examples of collecting various measurement data, collecting background noise data, then calculating covariance matrices from the data sets, then choosing starting values (starting estimates), and using the formulae and the obtained matrices/data to compute things like a projected/future values (such as 0.001 second ahead) using a 'kalman gain matrix'. A lot of sources explain the various matrix equations in wishy-washy ways as well. Can't blame people for not understanding it. There are at least a few internet sources, where the authors do provide enough detail to get people into the subject. But usually, it's either necessary to gather a whole bunch of sources together and figure it all out (including all the statistic mathematical manipulations/derivations) yourself, or ask somebody that already understands the area very well (to iron out uncertainties or questions).

The word 'fusion' used in kalman discussions is kind of wishy-washy in itself. But I guess they have to give a word for it heheh. I prefer to just call it information combining, or estimation based on gathering of information (with statistical considerations).

Also..... one more thing..... the equation at the top of page 14 at this link here ...

http://home.earthlink.net/~david.schultz/rnd/2004/KalmanApogeeII.pdf

has three vertical lines in the expression, plus three dots.

I think it means ... n = the number of states = 3, and the vertical lines are just separators for sub-matrices in-between, to be stitched together. And the pattern is:

{(Phi_T)^0}H_T {(Phi_T)^1}H_T {(Phi_T)^2}H_T

We stop at (Phi_T)^2 because that term represents (Phi_T)^(n-1) where n = 3

The "..." just means : keep building up the power of Phi_T until we get to a power index of n-1.

Update: I know what's going on now. The matrix is meant to be the 'observability matrix', so it involves the output matrix C... or in this case 'H', and also involves the state transition matrix A... in this case 'Phi_T'.

frenky81:
I have a barometer that give me the altitude estimation but this is not enought because barometer have some drift and the sample rate is not enought fast for have a good stabilization so i thought to fuse the accelerometer z axis with the barometer datas for have a clean and fast altitude estimation.

Firstly the accelerometer can do nothing at all about barometer drift, you need good GPS data to do that.
(Or good weather forecast!)

The Accelerometer can fill in the short term changes, with the barometer correcting for the accelerometer drift
(which rapidly gets unmanagably, but most barometers sample 1 or more times a second which should be enough I think).

For flight in practice an accelerometer alone won't do, you need a 6DoF IMU to get attitude-corrected accelerometer. It would work in a lift mechanism where the z-axis is accurately aligned at all times, but
not for a drone.