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