clear all
clc
N=70;
t=0:0.5:35-0.5;
T=0.5;
a=0; %加速度
v=0.1; %初速度
y=ones(1,N);
y(1)=0; %初始位置
for n=2:N;
    y(n)=y(1)+v*(n-1)*T+1/2*a*((n-1)*T)^2;
end
figure(1);
plot(t,y,'*');
xlabel('時間');
ylabel('位置');
title('理想運動時位置曲線');

% 加白噪聲
a=3*randn(1,N);
s=y+a;
figure(2);
plot(t,s,'*');
xlabel('時間');
ylabel('位置');
title('加白噪聲后的位置曲線');

Y=zeros(2,N);
Y(:,1)=[0;0.2;0.1];
A=[1 T;0 1];  
H=[1 0 ];
C0=[0 0;0 1];
C=[C0 zeros(2,2*N-1)];
B=[T 1]';
% B=[1 1 1]'
Q=(0.25)^2; 
R=(0.25)^2; 

% 卡爾曼算法

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

figure(3) %位置的跟蹤曲線
for n=1:N
yp(n)=Y(1,n);
end
plot(t,yp,'*');
xlabel('時間');
ylabel('位置');
title('位置跟蹤曲線');

figure(4) %速度的跟蹤曲線
for n=1:N
yv(n)=Y(2,n);
end
plot(t,yv,'*');
xlabel('時間');
ylabel('速度');
title('速度跟蹤曲線');

figure(5) %加速度的跟蹤曲線
for n=1:N
ya(n)=Y(3,n);
end
plot(t,ya,'*');
xlabel('時間');
ylabel('加速度');
title('加速度跟蹤曲線');