CT模型机动目标圆周运动跟踪

源程序： % modeling data % sample number N=200 points % sample time T=0.1s t=0:0.1:20-0.1; T=0.1; % sample time y=ones(6,200);y y(1)=0; R=4000;w=0.05;v=w*R;acc=w*w*R; for n=2:200; r=R*n*w*T; y(1,n)=r*cos(n*w*T/2); y(4,n)=r*sin(n*w*T/2); y(3,n)=acc*cos(n*w*T+pi/2); y(6,n)=acc*sin(n*w*T+pi/2); y(2,n)=v*cos(n*w*T); y(5,n)=v*sin(n*w*T); end % add white gauss noise a=20*randn(6,200); s=y+a; %data+noise % kalman filtering % y:initial data,s:data with noise Y=zeros(6,200); Y0=[0;0;0;0;0;0]; Y(:,1)=Y0; H=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; B=[(T)^2/2 0; T 0; 0 0; 0 (T)^2/2; 0 T; 0 0]; p=ones(6); A=[1 sin(w*T)/w 0 0 -(1-cos(w*T))/w 0; 0 cos(w*T) 0 0 -sin(w*T) 0; 0 0 0 0 0 0; 0 (1-cos(w*T))/w 0 1 sin(w*T)/w 0; 0 sin(w*T) 0 0 cos(w*T) 0; 0 0 0 0 0 0]; U=[T.^2/2; T; 1]; C0=[0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1]; C=[C0 zeros(6,6*199)]; Q=(0.25)^2; R=(0.25)^2; a=[1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1];R=R*a; % kalman algorithm ieration for n=1:199 i=(n-1)*6+1; K=C(:,i:i+5)*H'*inv(H*C(:,i:i+5)*H'+R); Y(:,n)=Y(:,n)+K*(s(:,n)-H*Y(:,n)); Y(:,n+1)=A*Y(:,n); C(:,i:i+5)=(eye(6,6)-K*H)*C(:,i:i+5); C(:,i+6:i+11)=A*C(:,i:i+5)*A'+B*Q*B'; end %画X-Y位移图像% figure(1) plot(y(1,:),y(4,:),'r-',s(1,:),s(4,:),'b-',Y(1,:),Y(4,:),'k-');grid legend('the initial track of movement','the track of movement with noise','the track after kalman filtering'); xlabel('time'); ylabel('position'); title('CT model filtering'); %画X-Y速度图像% figure(2) plot(t,sqrt(y(2,:).^2+y(5,:).^2),'r-',t,sqrt(s(2,:).^2+s(5,:).^2),'b-',t,sqrt(Y(2,:).^2+Y(5,:).^2),'k-');grid legend('the initial track of movement','the track of movement with noise','the track after kalman filtering'); xlabel('time'); ylabel('velocity'); title('CT model filtering'); %画X-Y加速度图像% figure(3) plot(t,sqrt(y(3,:).^2+y(6,:).^2),'r-',t,sqrt(s(3,:).^2+s(6,:).^2),'b-',t,sqrt(Y(3,:).^2+Y(6,:).^2),'k-');grid legend('the initial track of movement','the track of movement with noise','the track after kalman filtering'); xlabel('time'); ylabel('加速度'); title('CT model filtering');