www.gusucode.com > 基于机动目标跟踪课题的整个算法matlab程序 > sinc222.m
function main() %产生观测数据 total=3*60;%总的时间长度 global T;%采样周期 T=1; N=total/T;%数据长度 a=50; var_rx=100; var_ry=100; X=[];%观测数据 X_ideal=[];%理想数据 for i=1:N [rx,ry]=track(i*T,20); X_ideal=[X_ideal,[rx;ry]]; rx=rx+var_rx*randn(1,1); ry=ry+var_ry*randn(1,1); X=[X,[rx;ry]]; end X_filter=zeros(size(X));%滤波后数据 X_mean=X_filter;%蒙特卡洛平均数据 Error_var=zeros(size(X)); M=10;%蒙特卡洛仿真次数 for iCount=1:M X_filter=Trace(X); X_mean=X_mean+X_filter; Error_var=Error_var+(X_ideal-X_filter).^2; end X_mean=X_mean/M; Error_var=Error_var/M; Error_mean=X_ideal-X_mean;%误差均值 Error_var=sqrt(Error_mean.^2); for iCount=1:99 Error_var=Error_var+sqrt((X_ideal-X_filter).^2-Error_mean.^2); end set(gca,'FontSize',12); set(gcf,'Color','White'); plot(X(1,:),X(2,:),X_mean(1,:),X_mean(2,:)); xlabel('X(米)'),ylabel('Y(米)'); axis equal; legend('真实轨迹','滤波轨迹'); figure; k=1:N; set(gca,'FontSize',12); set(gcf,'Color','White'); subplot(2,1,1),plot(k,Error_var(1,:)/100);title('x方向误差平均标准值');xlabel('采样次数'),ylabel('RMSE(米)'); subplot(2,1,2),plot(k,Error_var(2,:)/100);title('y方向误差平均标准值');xlabel('采样次数'),ylabel('RMSE(米)'); %理想航迹方程 function [x,y]=track(t,a) % t:时间 % x:横轴位移 % y:纵轴位移 % a:转弯处加速度 % r:初始位置 % v:初始速度 r=[0,0]'; v=300+randn(1,1); w=a/v;%角速度 t1=pi/w; t2=t1+pi/w; D=v^2/a*2;%圆周运动直径 if t<=0 x=0,y=0; elseif t>0&&t<=t1 angel=t*w; x=D/2-D/2*cos(angel); y=D/2*sin(angel); elseif t>t1&&t<=t2 angel=(t-t1)*w; x=(3-cos(angel))*D/2; y=-D*sin(angel); else x=D+D+v*(t-t2); y=0; end function R=Trace(X) %飞行器跟踪模拟 % X:观测数据 % R:输出坐标 %观测时间间隔 global T; %观测矩阵 H=[1,0,0,0,0;... 0,1,0,0,0]; %位移测量误差 var_rx=100; var_ry=100; var_rx2=var_rx^2; var_ry2=var_ry^2; %观测噪声协方差矩阵 C=[var_rx2,0;... 0,var_ry2]; %状态噪声协方差矩阵 var_v=30; var_a=5; var_v2=var_v^2; var_a2=var_a^2; Q=zeros(5,5); Q(4,4)=var_v2; Q(5,5)=var_a2; %初始状态 s0=[0,0,0,300,0]'; %Kalman滤波跟踪 N=size(X,2);%观测数据长度 s=s0; a=@traverse; M=Q; Xplus=[];%修正后的航迹 for icurrent=1:N [s,M]=Karlman(s,M,X(:,icurrent),a,Q,C,H); Xplus=[Xplus;(s(1:2))']; end R=Xplus'; function s_estimate=traverse(s) %状态方程 global T; s_estimate=zeros(5,1); s_estimate(1)=s(1)+s(4)*cos(s(3))*T; s_estimate(2)=s(2)-s(4)*sin(s(3))*T; s_estimate(3)=s(3)+(s(5)/s(4))*T; s_estimate(4)=s(4); s_estimate(5)=s(5); function [s,M]=Karlman(s_forward,M_forward,X,a,Q,C,H) %卡尔曼滤波 %参数说明 % X--观测数据矢量 % A--状态矩阵 % Q--状态噪声协方差 % C--观测噪声协方差 % h--观测方程句柄 % s--输出数据矢量 % s_foward--前次输出矢量 % M--前次预测矩阵 global T; %预测 s=feval(a,s_forward); %状态转换矩阵 A=[1,2*s(4)*sin(s(3))*T+9000,2*s(4)*cos(s(3))*T*(-s(4)*sin(s(3))*T-4500),2*sin(s(3))*T*(-s(4)*sin(s(3))*T-4500),0;... 1/(2*sqrt(4500-s(4)*cos(s(3))*T)),1,-s(4)*sin(s(3))*T/(2*sqrt(4500-s(4)*cos(s(3))*T)),cos(s(3))*s(4)/(2*sqrt(4500-s(4)*cos(s(3))*T)),0;... sin(s(3))/(s(4)*T),cos(s(3))/(s(4)*T),1,-s(5)*T/(s(4))^2,T/s(4);... 0,0,0,1,0;... 0,0,0,0,1]; %最小预测MSE矩阵 M=M_forward; M=A*M*A'+Q; %协方差的进一步预测 %卡尔曼增益矩阵 K=M*H'*inv(C+H*M*H'); %修正(状态更新方程) s=s+K*(X-H*s); %最小MSE矩阵(协方差更新方程) I=eye(5); M=[I-K*H]*M*[I+K*H]'-K*C*K';