卡尔曼滤波matlab

% kalman algorithm ieration
for n=1:200
    i=(n-1)*2+1;
    K=C(:,i:i+1)*H'*inv(H*C(:,i:i+1)*H'+R);
    Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n));
    Y(:,n+1)=A*Y(:,n);
    C(:,i:i+1)=(eye(2,2)-K*H)*C(:,i:i+1);
    C(:,i+2:i+3)=A*C(:,i:i+1)*A'+B*Q*B';
end

% the diagram of position after filtering
figure(3)
t=0:0.1:20;
yp=Y(1,:);A
plot(t,yp,'+');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track after kalman filtering');

% the diagram of velocity after filtering
figure(4)
yv=Y(2,:);
plot(t,yv,'+');
xlabel('time');
ylabel('yv velocity');
title('the velocity caused by random acceleration');

% modeling data
% sample number N=200 points
% sample time T=0.1s

t=0:0.1:20-0.1;
T=0.1;            % sample time
v=0.5;            % initial velocity
y=ones(1,200);
y(1)=0;
for n=2:200;
    y(n)=y(n-1)+v*T;
end
figure(1);
plot(t,y,'-');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the initial track of movement');

% add white gauss noise
a=0.5*randn(1,200);
s=y+a;            %data+noise
figure(2);
plot(t,s,'+');
axis([0 20 0 20]);
xlabel('time');
ylabel('yp position');
title('the track of movement with noise');

save initial_track s