Extended Kalman Filter design for Attitude Estimation of a Quadrotor (with Acc + Gyro only)



I've been stumbling across this issue for a pretty long time now and I think the right time has come to ask you guys for help. I've written a simple quadrotor simulation in Matlab / Simulink basing on Randal Beard's papers (a short video below - yes, the controller is unstable just now which is fine with me at the moment).

I've simulated the dynamics and kinematics, programmed a PID controller, added simulated IMU readings and am working on state estimation. As I'm generally heading for an indoor quadrotor setting, I'm not interested in GPS / Magneto readings. I've also read in many places, it should be possible to estimate roll and pitch angles of a VTOL aircraft basing just on gyroscope and accelerometer readings with an Extended Kalman Filter.

I'm using the following algorithm to drive the time update

which works just fine. I simulated the gyroscope readings as follows:

With my simulated gyroscope readings I get following estimates, without any state correction just yet (angles in rad, time in seconds, estimates in green, blue are the true values). The drift due to simulated unbalanced gyro bias of 0.05 rad/s (see above).

Up to that point everything seems fine. My simulated accelerometer readings are (with T being the total propellers' thrust, m the quadrotor's mass):

What should my state correction equation look like, to get decent results? I've read, for a VTOL not performing aggresive manouvers, one could assume the following which should work well enough to obtain smooth pitch and roll control:

but according to my results I'm afraid it just doesn't. I've played around with different covariance matrices for process and measurement noise, but never getting satisfactory results. The best I could obtain is around this one:

Any hints?

Views: 9578

Reply to This

Replies to This Discussion

Hey Tom,


A chapter in my MS thesis deals with this very topic.  I'm one of Randy Beard's grad students at BYU.  My thesis will be published really soon - I defended it last week and will be submitting it tomorrow.  I'm not sure if there are any specific rules for distribution before it is officially published... but once it is out, I'd be happy to send it to you.


In case you care to know, I think that my thesis should put to rest any debate about accelerometer-based estimation on quadrotors - there has been a long-standing debate whether they are even useful on a rotorcraft here on DIY Drones and elsewhere.

One more note - if you haven't figured it out yet, your model of the accelerometer outputs will make them impossible to use for state estimation.  If ax and ay are always zero, you can't measure the attitude with the accels and your updates will be wrong.


In reality, the ax and ay outputs aren't zero, but near hover without wind they are very close to zero.  In fact, the accels measure scaled versions of the body-frame velocities (weird, huh?).  If you want high accuracy, you can't assume that the accels are measuring gravity.  If accuracy doesn't matter, on the other hand, then you can probably get away with it.

Thanks, I'd really like to read your thesis once you're free to send it over.

I know assuming ax and ay are always zero is wrong, but my Matlab model for accelerometer readings is simulating it this way too. So in my case programming the state correction any different wouldn't make sense, I guess. I just thought this assumption can work for me in my simulation. But maybe this simulation model's assumption and the following correction model are exactly what's causing problems and I should make both more precise.

Sure thing, I'd be happy to send it over.


If the update step is working properly (in spite of the accelerometer model), then your estimator results should almost always underestimate the actual angle.  This should make sense - if the x and y axis outputs are always zero or close to it, then the accelerometer-based angle estimate will always be close to zero.  This will, in turn, cause your angle estimates to be pulled consistently (and incorrectly) toward zero.


If the blue line in the second plot is the angle estimate and green is truth, then that is probably exactly what is happening.  Lowering your process noise covariance can help with this problem, but it will make the estimator more sensitive to non-zero gyro biases.  If the blue line is truth, on the other hand, then there is something else going on...

One other thing...


If you are using -g*cos(phi)*cos(theta) in your derivation of the filter, then it isn't observable.  The angles phi and theta are indistinguishable using only the z-axis measurement.  In any event, I'm not sure that model is a good one.  The z-axis accel won't ever measure gravity - only the thrust from the rotors.

Actually blue are real values and green are estimated values. The PID controller is unstable, so the vehicle is not really hovering, but "swaying" and its altitude is oscillatory as well. So those peaks in the estimate are happening when the quadrotor is accelerating in the z-direction (as I'm only using the z-axis for roll and pitch correction). So I guess having ax and ay readings simulated and then considered in the model could actually fix that. If I get it right.

Ah, that makes sense.  Yeah, the z-axis should just measure thrust.  I don't think you can observe pitch or roll from the z-axis accelerometer measurement.  In fact, with the ax = 0 and ay = 0 model, it can't be done at all.


I'll post a link to my thesis as soon as it is available.  It should help, I think.

Ok, thanks in advance.


I'd be interested in a copy of your thesis as well, if you don't mind.


Caleb & Tom - have you seen this paper: http://cas.ensmp.fr/~petit/forFC/PhM/2010/MartiS2010ICRA.pdf (The True Role of Accelerometer Feedback in Quadrotor Control)?




Sure thing.


I actually use that paper in my thesis to motivate the model that I use.  I think my thesis is complementary to that paper - Martin, et al. does not provide many details about the nature of the angle estimate errors that occur when a bad dynamic model is used for state estimation.  My thesis provides both simulated and experimental data, in addition to a fairly intuitive model to describe accelerometer/rotorcraft behavior.

Looking forward to seeing you work.




I started a new topic to post my thesis and the link provided by Roy together:




Hopefully this helps answer people's questions in the future.

Reply to Discussion


© 2020   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service