clear?all
clc
N=70;
t=0:0.5:35-0.5;
T=0.5;
a=0;?%加速度
v=0.1;?%初速度
y=ones（1N）;
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（ty‘*‘）;
xlabel（‘時間‘）;
ylabel（‘位置‘）;
title（‘理想運動時位置曲線‘）;

%?加白噪聲
a=3*randn（1N）;
s=y+a;
figure（2）;
plot（ts‘*‘）;
xlabel（‘時間‘）;
ylabel（‘位置‘）;
title（‘加白噪聲后的位置曲線‘）;

Y=zeros（2N）;
Y（:1）=[0;0.2;0.1];
A=[1?T;0?1];??
H=[1?0?];
C0=[0?0;0?1];
C=[C0?zeros（22*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;