clc
clear all;
% 初始条件 %
N=150;
k=1:1:N;
x0=[0;0;10;0];
xa0=[15;6;12;150];
P0=[10,0,0,0;0,10,0,0;0,0,50,0;0,0,0,1];
Pa0=[10,0,0,0;0,10,0,0;0,0,50,0;0,0,0,1];
Q=[0,0,1.54,0;0,0,1.54,0;0,0,1.54,0;0,0,1.54,0];
H=eye(4);
R=[0.02,0,0,0;0,0.02,0,0;0,0,0.01,0;0,0,0,0.01];
fai=0;
faia=150*pi/180;
T=12;
v0=10;
v1=12;
% GREAT SYSTEM AND OBSERVATION NOISE %
w=sqrt(Q)*randn(4,N);
v=sqrt(R)*randn(4,N);
% GREAT STATE AND MEASUREMENT %
for k=1:N
if k==1
F(:,:,k)=[1,0,cos(fai)*T,0;0,1,sin(fai)*T,0;0,0,1,0;0,0,0,1];
Fa(:,:,k)=[1,0,cos(faia)*T,0;0,1,sin(faia)*T,0;0,0,1,0;0,0,0,1];
x(:,k)=F(:,:,k)*x0+w(:,k);
xa(:,k)=Fa(:,:,k)*xa0+w(:,k);
else
F(:,:,k)=[1,0,cos(fai)*T,v0*(k-1)^2*cos(fai)*T;0,1,sin(fai)*T,v0*(k-1)^2*cos(fai)*T;0,0,1,0;0,0,0,1];
Fa(:,:,k)=[1,0,cos(faia)*T,v1*(k-1)^2*cos(faia)*T;0,1,sin(faia)*T,v1*(k-1)^2*cos(faia)*T;0,0,1,0;0,0,0,1];
x(:,k)=F(:,:,k)*x(:,k-1)+w(:,k);
xa(:,k)=Fa(:,:,k)*xa(:,k-1)+w(:,k);
end
end
for k=1:N
z(:,k)=H*x(:,k)+v(:,k);
za(:,k)=H*xa(:,k)+v(:,k);
end
hold on
plot(x(1,:),x(2,:),'r',xa(1,:),xa(2,:),'g')
hold off
新手,文件名问题,为什么数值这么大呢
|
