clc
clear
T=0.1;
F1=[1 T 0
0 1 0
0 0 0];
F2=[1 T 1/2*(T)^2
0 1 T
0 0 1];
H=[1 0 0];
x=zeros(3,200);
x(:,1)=[0 0.5 1];%初始值,速度为0.5,加速的为1
z=zeros(1,200);
for k=1:99
x(:,k+1)=F2*x(:,k);
end
for k=100:199
x(:,k+1)=F1*x(:,k);
end
v=wgn(1,200,0);
for i=1:200
z(i)=H*x(:,i)+v(i);
end
figure(1)
plot(x(1,:));
title('理论值');
figure(2)
plot(z);
title('量测值');
完整代码链接:https://pan.baidu.com/s/1MjJCIkfoiaiJvLQaXV0olw
提取码:asab