Hi,

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:

## Replies

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.

Thanks,

Robbie

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.

Best,

Robbie

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.

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.

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.