taking inspiration from a topic on this forum named "Kalman Filtered Nunchuck & Wii Motion Plus" , ive been trying to implement my own kalman filter. i took data from a 5dof imu, stored it in a text file and tried to implement the kalman filter. however im getting extremely bad readings. my output tends to rise continuously. has anyone else implemented the nunchuck code on an IMU? if so i would greatly appreciate it if you can verify if it is applicable for an IMU. thank you very much!
A basic Kalman filter is pretty straight forward. If your code looks correct: Where did the model for your plant come from? Is it derived correctly? double check your parameters and gains.
Also if you're using floating point math in an arduino then it might be taking you too long to compute the state estimate. Also, remember the discrete Kalman filter assumes a fixed period. If you are sampling in a loop that is not synchronized with a timer it could cause some error too.
thank you for taking the time to reply.
im trying to implement tilt angle estimation using accelerometer and gyroscope data. what i have is a single axis adxrs300 gyroscope and a 3-axis adxl335 accelerometer.
im basing my implementation on a topic in this forum titled "Kalman Filtered Nunchuck & Wii Motion Plus". below is a link to the work i have accomplished so far. so far im only trying to perform "offline" of any microcontroller and im using devc++.
below is a link to what i have done so far:
it reads data from sensordata.txt and outputs the angle in the textfile "output.txt". the sensordata included there is taken when the IMU is STATIONARY. i am however, getting all sorts of angles. anywhere ranging from 40 to 100s.
mr. mspguy, your ideas are very welcome. all you guys and gals are very welcome to pitch in too. thank you!
I found this thread:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1253202298
which I think is the continuation of the one you were referring to.
And there they are doing something different than the Kalman filter. The good new is its easier and you don't need to know the covariance matrix of your noise. The good news is that its a lot easier.
There is a pretty good powerpoint here
http://web.mit.edu/first/segway/segspecs.zip
in the file filter.pdf that has everything you need to know.
thank you very much for taking the time to look up several references and posts regarding this problem. i really appreciate your consideration.
ive read them both and they are really promising.
this may sound kind of brash, but is there any way though that i can appeal to you to continue helping me with the kalman filter implementation? many people in forums all over the internet crave for this. and since this project is not microcontroller-platform specific, people can adapt it. you would be helping not only me but also a lot of other people interested in this.
ive performed the following tests on the filter and weeded out what seems to be the error:
- printing the gyro output in degpersecond * dt -> angle estimate. this works out fine but
- printing out atan2(AccY , AccZ) results in huge erroneous angles
Right now I don't have a lot of time to dive into things too deeply 
I was thinking about writing some basic control systems and signal processing libraries for arduino since IMO everyone needs control systems, whether they know it or not ;).
Unfortunately I'm not really sure when I'd be able to get around to it. This last week I've been on vacation and have been using this forum to learn about the arduino since I'm stuck with some for my work. But come monday I'm going to be very busy
As far as your atan2(), can you post a few lines of AccY, AccZ and atan2(AccY,AccZ) values that you are seeing
good luck on your work and i look forward to seeing them!
im trying to estimate the roll angle and am using atan2 (AccY , AccZ). i take the sensor readings via a 10 bit ADC.
taking the accelerometer values when stationary, i get the following average readings for AccYaverage: 553 bits , AccZaverage: 643 bits.
i then subtract these values from the values i read when stationary / zero g.
AccY -= 553;
AccZ -= 643;
i am given to understand that accelerometers are sensitive to vibrations. i take all 3 readings from the accelerometer and put it in a data string with the format ^xacc.yacc.zacc^. some of the values i read when stationary are:
^547.555.642^ --- 63.43 deg
^548.555.642^ --- 63.43 deg
^549.556.642^ --- 71.53 deg
^548.555.643^ --- 90 deg
^548.555.642^ --- 116.5651 deg
^548.555.641^ --- 135 deg
^547.555.641^ --- 135 deg
^548.555.642^ --- 116.5651 deg
these are very large values and affect the corresponding output angle directly. i am stumped and dont know what to do.
here are some images of what i get when stationary:
- gyroscope output:
- accelerometer output:
- kalman filter output
as you can see instead of getting values around zero, i get weird values that are too high due to the accelerometer
edit: wait. no. disregard that!
hmm well note that
arctan(2) = 63.4349488 degrees
arctan(3) = 71.5650512 degrees
AccY and AccZ needs to be cast as floats
so, instead of
~~ ~~atan2(AccY,AccZ)~~ ~~
try
~~ ~~atan2((float)AccY,(float)AccZ)~~ ~~
nice catch there. it slipped my mind to mention that i did just as you said earlier. plots show the results from your suggestion
disregard my last post. the problem stems from the way you normalize. You assume that on the level you should see (0,0) what you really should see is 1g in the minus z (down) direction and 0 on the other axes
sorry for being a noob but what do you mean by i should "see" 1g? at stationary position? im hoping to see a roll value of 0 deg at stationary. i am not really so sure myself and any light you can shed on the matter is extremely helpful
sorry to break up what really should be one post into three, but I saw you were online so I wanted to get the post out quick before I confused you with the wrong information.
The way you should calibrate for x and y (assuming z is up down) stays the same, but Z should be calibrated with the sensor on 90deg on its side.
as a sanity check: sqrt(x^2+y^2+z^2) = C, a constant (modulo noise) for ALL orientations. This is because gravity is constant ( C=1g=9.8m/s^2).
sorry i didnt catch your last reply. had to run a few errands. im not really sure with the calibration. i am basing what i am doing on a datasheet (which im not too good at reading either) where i saw ZERO g BIAS LEVEL (RATIOMETRIC) heading. there are 3 values for the 3 axes and i went ahead and used them. so going by your suggestion:
- i am to tilt the sensor 90 deg to the side
- take the z-axis accelerometer reading
- use that for all my calculations of Acczfinal = Accreading - ACCat90deg?
your suggestion was extremely helpful. i would have never thought of that silly me.
doing the three steps you suggested and plotting the data for atan2 (atan2 ((float)AccZ , (float)AccY) * 180 /PI gave the following plot
i think that it is around 90 degrees due to the fact that [ atan2 (1,0) * 180 / PI] and [atan2(0,1) * 180 / PI] will give us 90 degrees. too bad though since what we want is zero degrees 
yep you don't want a zero g bias, you want a one g bias since you're looking for the direction that earth's gravity is pulling you. Zero g bias would be usefull if for example if your sensor was always level(never tilted) but you wanted to measure accelerations(like some one pushing on your robot)
For step 2 I'd take an average of some number of readings just to reject noise.
Choice of coordinates is arbitrary. But if you want 0degrees to be "down" then try
(atan2 ((float)[glow]AccY[/glow] , (float)[glow]AccZ[/glow]) * 180 /PI
thanks again for the suggestion. ill get on it as soon as i get a few winks. its 5 am here 
i was too excited that i waited for your reply
Lol. Now I feel bad for sleeping in so late this morning 
good day again! ive done a running average and this is what i came up with. it sure is a lot cleaner though our problem still remains.
did you swap AccY and AccZ?
hey there! thank god youre here. the above plot is for
atan2 (AccZAve , AccYAve);
this here below is for
atan2 (AccYAve , AccZAve)
That looks quite odd. Those two plots aren't the same data set are they?
Swapping Y and Z should rotate your coordinates 90deg and negating y or z should flip them.