kf_m.m

kalman滤波

e=0;
t=21/100000;
n=100;
a=[1,t,0.5*t^2,1/6*t^3,1/24*t^4,1/120*t^5,1/720*t^6,0;0,1,t,0.5*t^2,1/6*t^3,1/24*t^4,1/120*t^5,0;0,0,1,t,0.5*t^2,1/6*t^3,1/24*t^4,0;0,0,0,1,t,0.5*t^2,1/6*t^3,0;0,0,0,0,1,t,0.5*t^2,0;0,0,0,0,0,1,t,0;0,0,0,0,0,0,1,0;0,0,0,0,0,0,0,1];
c=[1,0,0,0,0,0,0,1];
r=0;
%p1=eye(8);
%p1=p1*100000;
p1=1.0e+005*[0.1363    0.0000    0.0000    0.0000    0.0000    0.0000    0.0000   -0.1363
    0.0000    0.0019    0.0006    0.0007    0.0000    0.0000    0.0000   -0.0000
    0.0000    0.0006    0.1107    0.0335    0.0019    0.0001    0.0000    0.0000
    0.0000    0.0007    0.0335    0.9360    0.0597    0.0025    0.0001   -0.0000
    0.0000    0.0000    0.0019    0.0597    1.0051    0.0629    0.0026   -0.0000
    0.0000    0.0000    0.0001    0.0025    0.0629    1.0052    0.0627   -0.0000
    0.0000    0.0000    0.0000    0.0001    0.0026    0.0627    1.0000    0.0000
   -0.1363   -0.0000    0.0000   -0.0000   -0.0000   -0.0000    0.0000    0.1363];
fp=fopen('901.dat','r');
gyro=fscanf(fp,'%f ');
fclose(fp);
for i=1:n
    e=e+gyro(i);
end
e=e/n;
s=e;
e=e-2.5;
x1=[2.5,0,0,0,0,0,0,e]';
for i=1:n
    r=r+(gyro(i)-s)^2;
end
r=r/n;
s1=0;
for i=1:length(gyro)
    s1=s1+t*(gyro(i)-s)*1000/22.2;
end
s1=s1*180/pi
%subplot(2,1,1);
%plot(gyro);
for i=1:length(gyro)
    if i>=2
        if abs(gyro(i)-gyro(i-1))>2*sqrt(r)*10000000
            a(1)=0;
            ge=gyro(i)-e;
        else
            a(1)=1;
            ge=0;
        end
        x1=a*x+[ge,0,0,0,0,0,0,0]';
        p1=a*p*a';
    end
    k=p1*(c')*((c*p1*c'+r)^(-1));
    x=x1+k*(gyro(i)-c*x1);
    p=p1-k*c*p1;
    y(i)=x(1);
    z(i)=c*x;
end
%subplot(2,1,2);
%plot(y);
ti=1:length(y);
plot(ti,gyro,ti,y,ti,z);
s2=0;
for i=1:length(y)
    s2=s2+t*(y(i)-2.5)*1000/22.2;
end
s2=s2*180/pi