Orientation without kalman filter.

hey. Ive studied the kalman filter for a long time. I need it to fuse sensor readings together to estimate orientation on a quadcopter. Since the kalman filters i hard to undestand and(concidering I have e 328p arduino duemlinaove) it takes a lot of processing.

So, today, i sat down and thought about other ways to solve this problem. the orientation problem.
I think ive figured it out. i have tested it with a breakout from sparkfun, and it seems to work. if anyone has any comments, feel free. all critizism is good.


take the gyro rate and integrate it. this will drift.
use atan2 to get a angle from accelerometers. this will vary with vibrations and accelerations(process noice).

use the angle from the accelerometers to reset the angle from the gyro, when the change in the accelerometer angle is negligible.
you dont only reset that, you also say that the “true” angle, is that angle. so, when the accelerometer gets noisy again, your gyro readings wont have drifted so much.

when the accelerometer angle is noisy, you put your trust in the gyro angle. easy as shit.

the code is tuned for my use, and you might want to change it concering the orientation of the breakout combo.

long time = 0;
long oldtime = 0;
int dt;

double gyrorate = 0;
double gyroangle = 0;
double accangle = 0;
double angle = 0;
double accangledot = 0;
double accangleold = 0;

void setup(){

void loop(){
  time = millis();
  dt = time-oldtime;
  oldtime = time;
  double vinkel = getangle(analogRead(2), analogRead(1), analogRead(0));
  Serial.print("   ");


double getangle(int xread,int yread,int gyrorate){
  gyrorate = gyrorate-500;
  gyrorate = -(gyrorate*1024/600)*(3.14/180 *1/1000);
  gyroangle = gyroangle + gyrorate * dt;
  accangle = atan2(yread-512, -(xread-512));
  accangledot = (accangle - accangleold)/dt;
  double accanglecovariance = accangledot*accangledot;
  if(accanglecovariance <= 0.005/dt){
    angle = accangle;
    gyroangle = accangle;
    angle = gyroangle;

 return angle;


I really like your idea of handling drift and noise, it is quite easy to understand.

Have you got an actual quad platform to test it on?

And are your intentions to make it be able to log its coordinates and maybe even receive coordinates and go there?

I'm into making a serious quad (been for years actually), then checked out for gyros/accelerometers and got hit in the face by the price. Then I read about interfacing Wii-sensors on the AeroQuad forum (arduino based quad) and it seems to work really well: http://aeroquad.com/showthread.php?12-Low-cost-6DOF-for-this-project/page4 (the thread where it is discussed)


I have a quad actually. Yours might be serious, mine is just plain hazardous. check out www.danielaaroe.blogg.no for more. i havent updated in a while, so its a bit out of date. the hardware is finished now, and i'm just awaiting one ESC to get it flying. i use RF to send angle references. coordinates will come later when i know the rest is working. if i want it to fly for a long time, i'll need more batteries. i only have 6000mah atm.(that will keep me flying for like...a moment with 3500W).(yeah... almost 4000 watts. i like it.)

concernining the price of gyros and acc, i undestand what you mean :stuck_out_tongue:
a quad is really pricy, so just save up some cash and but the best. the orientation is the most important :stuck_out_tongue:



since earlier today, i can now control 2 axis, and recieve angle references from another arduino using RF. 250meter(750 feet) should be enough range for now. if anyone have any experience with GSM, please speak up :smiley:

Well, for the moment I am just screwing around collecting info to make it reality. Played with an old X-Ufo before and made a static control board for it (RC - to - Xufo), but nothing more atm.

I am thinking of having Sharp distance sensors as collision detectors for indoor flying for example on the real thing.

May I ask why the extreme torque/lifting power is needed (I read your blog, seems to be a stable platform btw)? How heavy camera are you planning to fly around with?

Hmm… Your project is really interesting! You need to post more about it and maybe some video?

if i want it to fly for a long time, i'll need more batteries.

Given what I've seen of your project (impressive), and that you say it is already "hazardous" (LOL - but true!) - you might want to look into nitro engines for thrust; you would likely get much better time aloft, equal or greater lifting power - but control won't be easy. You would need servos to control the throttles, and some kind of optical encoders on the shafts to measure RPM, coupled with a PID feedback loop.

Hmm - now that I think about it, it seems rather complicated; that isn't even taking into account the fuel system - you would want a central tank, but how do you set things up to keep from starving engines on one side of the craft vs the other side as it tilts (you would probably want to ask a an R/C plane/helicopter enthusiast about that unless you have the experience).

All in all, an interesting project and something I have wanted to try since I first saw the Dragonflyer videos several years ago. Too many other projects on the backlog though!

Keep us up-to-date on your project - sounds like fun!


im with Honk i would like to see more details about the build and maybe a video.


why do I need it... why would you have 500 bhp in a car. just for fun. also, it is usually better to have to much, than to little. power that is.
I should post a video. but i dont have a camera yet. e mobile camera video would just be annoying.


it seems you put some thought into this while writing. i like ideas, but i think a nitro Quad would be way more expensive and more problematic to build and program(due to extra noise filtering aso).

the best way to do this, i think, would be to use ONE oversized motor, with 4 drive shafts, one to each arm, powering 4 variable pitch propellers. so that you would adjust the lift on each arm with pitch instead of RPM. not to easy to accomplish i think.


the build is interesting, but in no way important. mount them together. make the angles straight. thats it :smiley: all unforseen problem you're gonna have to solve on the go.
the video is hard to make happen due to lack of camera. As soon as i get it up in the air i'll get a camera :smiley:

Fucking shit. the shit wont fucking work.

the "filter" needs sudden changes to work. if the changes come over time, it wont do shit. it responds very well to vibrations and such, but long time steady movements it cant tell is noise.


if you are making a fucking balancing robot, use it. if not, fuck it.

i'm pissed. I'm going to need the kalman filter after all. if anyone have a kalman filter that I can use(angle + angular rate fusion) please let me know. I can adapt it and tune it myself.

Some info I found on a Kalman filter implementation on the web is here: Arduino code for IMU Guide algorithm. Using a 5DOF IMU (accelerometer and gyroscope combo) – Starlino Electronics (and the theoretical background is explained in a background article)

Now this is an 'inclinometer', so it will give you two angles ('pitch' and 'roll') that tell you how 'horizontal' your quadcopter is. The software will eliminate the values for speed in any direction, and it won't tell you the 'yaw'.

Estimating the speed is relatively simple and should consider only the accelerometer's values, but I don't think you need to know the speed in the first place.

'yaw' (rotation around the vertical axis) can only be calculated if you add a third gyro, but then it should be a piece of cake. I guess keeping your quadcopter level is your first goal.

Thinking of it: the accelerometer could be used to estimate the rate of rotation in the horizontal plane too, as long as the angular speed is high enough. Centriputal/centrifuginal forces will exitade the outputs x and y if you place the IMU away from the center of rotation. This is risky calculation, as it will only work if the copter is hovering at the same position and the only movement is the rotation in the xy plane... But it would be worth trying to place your IMU out of the center of the copter.

just as a tip : have you seen the work on navigation with mouse chips ? optical mouses are DSP's with a tiny camera which extract motion information from what they see.
I have seen an article about a flying wing with 7 sensors able to do free-flight. cannot find it back though.
however, check out this Insect-Inspired Optical-Flow Navigation Sensors - Tech Briefs
Blimpduino seems to do optical navigation too...

I'm not sure if it avoids Kalman filters...