Kalman filter function

Other Hardware Robotics
1

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)).

//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.

2

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.

3

@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

4

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

Daniel