yuanzhou1.m

转弯运动模型

clear
%圆周运动环境仿真 
g=30*pi/180;
v=300;
w=0.1
r=v/w;
x0=r*cos(g);
y0=r*sin(g);
 T=0.5;    %采样间隔
n=100;
 q=1;    %状态误差方差常数
 o=q*randn(n,2);      %  x方向上的噪声
 b=q*randn(n,2);      %  y方向上的噪声
 i=1:n;
 x2=r*cos(g+w*i*T)+o(i);
 y2=r*sin(g+w*i*T)+b(i);
 x1=r*cos(g+w*i*T);
 y1=r*sin(g+w*i*T);
 
%kalman滤波 
Z=[x1;y1];
A=[1 0 T*sin(w*T)/w T*(cos(w*T)-1)/w;0 1 T*(1-cos(w*T))/w T*sin(w*T)/w;0 0 cos(w*T) -sin(w*T);0 0 sin(w*T) cos(w*T)];
H=[1 0 0 0;0 1 0 0];
Q=[T^2*T^2/4 T^2*T^2/4 T^2*T/2 T*T^2/2 
   T^2*T^2/4 T^2*T^2/4 T*T^2/2 T*T^2/2 
   T^2*T/2 T*T^2/2 T^2 T^2
   T*T^2/2 T*T^2/2 T^2 T^2]*q;
L=[T^2/2 T^2/2 T T];
R=0.1.*eye(2);
i=0
X=[0 0 0 0]'
P=10.*eye(4);
I=eye(4);
for i=1:n 
     X=A*X;
     P=A*P*A'+L*Q*L';
     K=P*H'/(H*P*H'+R)
     i
     X=X+K*(Z(:,i)-H*X)
     P=(I-K*H)*P
     M(:,i)=X;
end
i=1:n;
t=i*T;
;
plot(x1,y1,'g-',M(1,:),M(2,:),'b:',x2,y2,'r--')
legend('观测曲线','滤波曲线','理论曲线')