Arduino Forum upgrade scheduled for Monday, October 20th, 11am-4pm (CEST). Sorry for the inconvenience!
Pages: [1]   Go Down
Author Topic: Kalman filter function  (Read 4344 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Newbie
*
Karma: 0
Posts: 19
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Ive made a kalman filter function based on Tom Pyckes tutorial. Read the tutorial first, it will be easier to understand the function. The function lets you use the kalmanfilter on multiple axis. It works great, but youre gonna have to tune it to your application and IMU's(EDIT: to tune the filter you just change the values Sz and Sw. in the code you see Sz as 0.01 and Sw as 0.0000001 and 0.0000003(se Toms tutorial for explanation of these)).

Code:

//Kalman
double x[2][2];
double xv[2][2];
double p[2][4];
double pv[2][4];
double k[2][2];
double Inn[2];
double s[2];
double Sz = 0.01; // was 17.2 for dt =10. Sz is lower the higher delta time.


double KalmanFilter(int a, double y, double u){
  
x[a][1] = u+x[a][1]-dt*x[a][2];
x[a][2] = x[a][2];


Inn[a] = y-x[a][1];

s[a] = p[a][1]+Sz;

k[a][1] = (p[a][1]-dt*p[a][3])/s[a];
k[a][2] = p[a][3]/s[a];

xv[a][1] = x[a][1]+k[a][1]*(y-x[a][1]);
xv[a][2] = x[a][2]+k[a][2]*(y-x[a][1]);

x[a][1] = xv[a][1];
x[a][2] = xv[a][2];


pv[a][1] = 0.0000003+p[a][1]-k[a][1]*p[a][1]+dt*k[a][1]*p[a][2]-dt*p[a][3]-dt*(p[a][2]-dt*p[a][4]);
pv[a][2] = p[a][2]-k[a][1]*p[a][2]-dt*p[a][4];
pv[a][3] = dt*k[a][2]*p[a][2]+p[a][3]-dt*p[a][4]-k[a][2]*p[a][1];
pv[a][4] = 0.00000001-k[a][2]*p[a][2]+p[a][4];


p[a][1] = pv[a][1];
p[a][2] = pv[a][2];  
p[a][3] = pv[a][3];
p[a][4] = pv[a][4];

return x[a][1];

}


on top, you see the variables declared as field. you might declare them inside the function frame but make sure to make then "volatile". Kalman filter is a statistical optimal estimator so you need the previous values to make new ones.

a is the axis number(if you only have one axis, it would be "a=0")
y is the angle from the accelerometers
u is the gyro rate

In addtition to these you need a cycle time variable which Ive called dt.

Feel free to use it out of the box.
« Last Edit: February 19, 2011, 01:04:00 pm by danielaaroe » Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 19
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Here is the link to Tom's tutorial. lots of great stuff on his site.

http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

EDIT:

It would be great if anyone who uses it give some feedback or comment on it to make it easier for the next to impelement in their code.
« Last Edit: February 18, 2011, 06:12:02 pm by danielaaroe » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 1
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

@danielaaroe do you have any inf. about how i chose the dt valu to make suitable readings . do you use it in balncing robots. thanks
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 19
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi.
Uhm... "dt" is the delta time value. ie. the cycle time. its determined by the time the program uses to run the program one cycle. a good way to determine it is:

int time, oldTime, dt = 0; //declare once

//put in loop()
oldTime = time;
time = millis();
dt = time - oldTime;

Ive used it in a balancing robot app that is not yet ready. Ive put it on hold due to temporary lack of interest.

Its very important that you tune the filter properly for it to work. I did this by logging gyrorate, accelerometer angle and dt on the computer(using serial monitor and copy paste). then i wrote the filter in matlab and ran it multiple times while tuning the parameters in between. when you plot it to graph it is much easier to see the effect of the filter.

hope this helps! if not, try to reformulate the question and i'll try to answer again smiley-razz

Daniel

Logged

Pages: [1]   Go Up
Arduino Forum upgrade scheduled for Monday, October 20th, 11am-4pm (CEST). Sorry for the inconvenience!
Jump to: