kf+强跟踪+自适应.docVIP

一辆汽车沿标准400米跑道，以速度v=30Km/h做二维水平运动，车上安装有加速度传感器，分别感知汽车前后、左右运动的加速度。请设计过程噪声时变强跟踪自适应滤波器，估计汽车运动的瞬时速度v(t)和行走距离s(t)。 系统建模 系统方程： 整理得： 令 (1)卡拉曼滤波： (2)STF滤波 (3)自适应滤波 其中 Matlab程序 %=======================kalman=============================== clear %----------?ù±?2?êy------------- T=0.01; % 10ms L=400; % 400?×?üμà L1=85.96; % ?±????3¤?è Lr=36; % ?ú????°??? L2=114.04; % ?ú????3¤?è mV=30000/3600; % ??3μò?30km/h?è?ùDDê??￡è?1?·??è?ù￡?′ú±í???ù?ù?è?￡ N=L/mV/T; % 2é?ùμ?êy V=ones(1,N+1)*mV; % ?è?ù???ˉ￡????ù?Yêμ?ê·???Dèòa??±??ù?è?ú???￡ S=zeros(1,N+1);Ta=zeros(1,N+1);a1=zeros(1,N+1);a2=zeros(1,N+1); for i=2:N+1 S(i)=S(i-1)+V(i)*T; if S(i)L1 S(i)L1+L2 Ta(i)=(S(i)-S(i-1))/Lr; % μúò????ú???? elseif S(i)L1+L2+L1 S(i)L Ta(i)=(S(i)-S(i-1))/Lr; % μú?t???ú???? end a1(i)=(V(i)*cos(Ta(i))-V(i-1))/T; a2(i)=V(i)*sin(Ta(i))/T; end %---------------------kalman??2¨--------------------- Q1=1e-8; R1=1e-6; w1=normrnd(0,sqrt(Q1),1,N+1); v1=normrnd(0,sqrt(R1),1,N+1); a1(1)=0; A=[1 T T*T/2; 0 1 T; 0 0 1]; H=[0 0 1]; X1(1:3,1)=[0;mV;0]; za1=a1+v1; s1(1:3,1)=[0;mV;0]; p1=eye(3)*1e-8; for k=2:N+1; ss1(1:3,k)=A*s1(1:3,k-1); pp1=A*p1*A+Q1; K1=pp1*H/(H*pp1*H+R1); s1(1:3,k)=ss1(1:3,k)+K1*(za1(k)-H*ss1(1:3,k)); p1=(eye(3)-K1*H)*pp1; end t=1:N+1; figure(1); subplot(3,1,1) plot(t,s1(1,:),r,t,S,g); title(?·3ì); subplot(3,1,2) plot(t,s1(2,:),r,t,V,g); title(?ù?è); subplot(3,1,3) plot(t,s1(3,:),r,t,a1,g,t,za1,b); title(?ó?ù?è); %==========================stf================================= clear %----------?ù±?2?êy------------- T=0.01; % 10ms L=400; % 400?×?üμà L1=85.96; % ?±????3¤?è Lr=36; % ?ú????°??? L2=114.04; % ?ú????3¤?è mV=30000/3600; % ??3μò?30km/h?è?ùDDê??￡è?1?·??è?ù￡?′ú±í???ù?ù?è?￡ N=L/mV/T; % 2é?ùμ?êy V=ones(1,N+1)*mV; % ?è?ù???ˉ￡????ù?Yêμ?ê·???Dèòa??±??ù?è?ú???￡ S=zeros(1,N+1);Ta=zeros(1,N+1);a1=zeros(1,N+1);a2=zeros(1,N+1); for i=2:N+1 S(i)=S(i-1)+V(i)*T; if S(i)L1 S(i)L1+L2 Ta(i)=(S(i)-S(i-1))/L