100KM

kalman filter for idiots

x_angle += dt * (gyro_angle - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;

y = acc_angle - x_angle;
S =P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;

x_angle += K_0 * y;
x_bias += K_1 * y;

P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;

x_angle = kalman filter state estimate
now was that realy so hard?
gyro_angle = gyro input in rad per sec
acc_angle = accel input in rad
dt=time since last loop
Q_angle= 0.01
Q_gyro= 0.03
R_angle = 0.7
x_angle = final estimate

Views: 2612

▶ Reply to This

Replies to This Discussion


3D RoboticsPermalink Reply by Chris Anderson on December 2, 2008 at 4:02pm
Very nice. Can you define the variables for us?

100KMPermalink Reply by crystal garris on December 2, 2008 at 4:40pm
ok .
the rest of the vari's are filled in by the filter itself. x_angle is in rad

3D RoboticsPermalink Reply by Chris Anderson on December 2, 2008 at 4:44pm
So the Ps and Ks are just temporary variables?

The output is a two-axis X and Y angle?

100KMPermalink Reply by crystal garris on December 2, 2008 at 5:43pm
yes on the Ps and Ks
single axis

T3Permalink Reply by Krzysztof Bosak on December 4, 2008 at 8:18am
Q_angle and Q_gyro are inverse of standard deviation?
R_angle is convergence factor, the smaller - the faster we converge from gyro towards accelerometer angle?
I see no problems for it to work with degrees... right?
Permalink Reply by Sergio O. on December 10, 2008 at 10:05pm
Hi!!, i implement this code in the Mc and i mounted on the airplane.... the x axis of the accelerometer is pointing to the airplane nose so... when the airplane starts to run the angle drifts as the ariplane accelerate... can i solve this problem with kalman filter o what i shoul use....

Pls!!! Help!!!

Thks.
Permalink Reply by hoopty on December 12, 2008 at 11:46am
Since I am a really big idiot, How can this be modified for x and y axis filtering?
Permalink Reply by Mathias on December 16, 2008 at 4:52pm
So I believe the title of this post aptly applies to me!! I pirated your code and matlab'ed it and let me say, WOW! So, what's the next step from here? I suppose that it is not as simple as running two independent loops for pitch and roll. Any further advice on how to proceed?
Permalink Reply by Mathias on December 16, 2008 at 4:55pm
Here's the code in Matlab for anyone interested.. Hope I implemented this correctly.

evalin('base','clc,clear')

dt = .01;
acc_angle = sin(.5*pi*(0:dt:10));
t = (0:dt:10);


for i = 1:length(acc_angle)-1
gyro_angle(i) = (acc_angle(i+1)-acc_angle(i))/dt;
end
acc_angle = acc_angle + rand(1,length(acc_angle))/5;
gyro_angle = gyro_angle + rand(1,length(gyro_angle))/5;

P11 = 0;
P10 = 0;
P01 = 0;
P00 = 0;

Q_angle= 0.01;
Q_gyro= 0.03;
R_angle = .7;

x_angle(2) = 0;
x_bias(2) = 0;


for i = 2:length(acc_angle)-1

P11(i) = P11(i-1) + Q_gyro*dt;
P10(i) = P10(i-1) - dt*P11(i-1);
P01(i) = P01(i-1) - dt*P11(i-1);
P00(i) = P00(i-1) - dt*(P10(i-1)+P01(i-1))+Q_angle*dt;
x_angle(i) = x_angle(i) + dt*(gyro_angle(i-1)-x_bias(i-1));

y(i) = acc_angle(i) - x_angle(i);
S(i) = P00(i) + R_angle;
K0(i) = P00(i)/S(i);
K1(i) = P10(i)/S(i);

x_angle(i+1) = x_angle(i)+K0(i)*y(i);
x_bias(i+1) = x_bias(i)+K1(i)*y(i);

P00(i) = P00(i) - K0(i) * P00(i);
P01(i) = P01(i) - K0(i) * P01(i);
P10(i) = P10(i) - K1(i) * P00(i);
P11(i) = P11(i) - K1(i) * P01(i);
end

plot(t,x_angle,'ro')
hold on
plot(t,acc_angle)
hold off
Permalink Reply by Wim De Wilde on December 19, 2008 at 12:03pm
I don't see the point of using a Kalman filter if you assume the noise variances to be constant. The Kalman gain coefficients K_0 and K_1 converge to constant values in this case, since they don't depend on any measurement. So instead you can reduce the filter of the (matlab-) example to :

x_angle(i) = x_angle(i) + dt*(gyro_angle(i-1)-x_bias(i-1));
y(i) = acc_angle(i)-x_angle(i);
x_angle(i+1) = x_angle(i)+0.0233*y(i);
x_bias(i+1) = x_bias(i)-0.0209*y(i);

You omit all P-stuff and the CPU intensive divisions by S that way. Moreover the gain doesn't need to converge.
I think Kalman filtering is only useful in this context if the variances are continuously monitored and evolving over time (for example, R_angle could be increased during accelerations, turns or at resonance frequencies of the engine). But this isn't that easy.
Permalink Reply by JC on December 19, 2008 at 5:17pm
I totally agree, once the K and P matrices convege they could be hard coded to streamline the computation. As you mentioned the ideal application would be to model the error (uncertainty) of the system and have the gains dynamically adjust accordingly.

100KMPermalink Reply by crystal garris on December 23, 2008 at 6:24pm
i disagree as the gyro bias drifts way to much especially after a power cycle temp change ect. ya you might get away with using the same K and P after the converge but i promise you they don't converge the same every time i boot the system as far as worrying about processor speed that might be a problem if you want to use a small old 8 bit cpu but that whole paradigm is being relegated to the past.

▶ Reply to Discussion

RSS

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service