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: 9592

Reply to This

Replies to This Discussion

Hello Tom, 


Did you use any book to assist you with the quadrotor simulator? If so, which book? 


I've been looking for a simulator which allows me to use different control strategies and also simulate way point, follow a predefined path, and obstacle avoidance... but I haven found any... so I started thinking that maybe I'd have to do it myself, but honestly I have no knowledge of how to make 3D simulations in matlab nor have any idea of how you designed that quadrotor body for your simulation...


So I'd like to know how and where to start.

I very much like BYU's work on quadrotors, I think it's very well suited for beginners. I used this. You don't have to start with 3D simulations in Matlab (Simulink, actually), you can start with a simple model of dynamics and kinematics which will print the state each simulation step. When this works, read about Simulink 3D Visualisation / Simulation - you need a separate toolbox for that. The model I made is a simple VRML model (VRML was superseded by X3D, but Simulink still uses VRML).

About quadrotor simulation, I think I'd now consider using ROS / Player / Stage instead, so read about this as well. Go to answers.ros.org and www.ros.org/wiki/ and take a longer read. Try to dig in the mailing-list's archives. I guess you'll find something suitable. There are some bridging possibilities from ROS to Matlab and Octave if you needed it.


Good luck :).



Thanks for the info and suggestions, I'll do that and see how it goes...

Hi Tom,

I´m currently working on a similar project, and I´m stuck with the animation. I´m using the Virtual Reality Toolbox to animate, but the thing i´m missing is the VRML model of the quadrotor, and since I have the MacOS version of the toolboxes, I don´t have the VRML Builder to draw it myself. I´ve been searching everywhere, but can´t find a .wrl model that will work in Matlab.


It would be great if you could pass me the one you have, which looks very nice.




Hope that's it (I use Linux now, cannot check).

Good luck :).


Thank you very much Tom!! That works perfectly well!





Hi everyone,
I've set up an animation of a quadrotor using the Virtual Reality Animation objects, but am having trouble with viewing the orientation changes of the quad.

To be more specific: I have a simulator going, where I calculate from rigid body dynamics the trajectory of it in time. This is, I have x, y, z, phi, theta, psi vs. time. I put these in a 'Array6DoF' structure, and associate this to the node corresponding to the quadrotor. When pressing play, I can see the translation in all three dimensions, but don't see the changes in the orientation.

Not sure what the problem could be. Maybe I have to add one Transform in the .wrl for each of the rotational degrees of freedom, or maybe there is some Field I need to set in order to allow the node to rotate.

Any help is most welcome.


Reply to Discussion



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