Singer模型机动频率自适应调节算法-源码

clear all;close all;clc; T=1; N=300; a_max=50; pmax=0.1; M=100; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% X=zeros(3,N); x1=10000;vx1=300;ax1=0; X(:,1)=[x1;vx1;ax1];%%%%%%%%%%%%%%信号滤波值的初始值%%%%%%%%%%%% X_P=zeros(3,N);%%%%%%%%%%%%%%%%一步预测值%%%%%%%%%%%%%%%%%%%%%%% X_P(:,1)=[x1;vx1;ax1]; P_P=zeros(3,3,N);%%%%%%%%%一步预测协方差矩阵%……%%% P=zeros(3,3,N);%%%%协方差矩阵%%%%% P0=[5000 0 0 0 1000 0 0 0 100 ]; R=10^4; %%%%协方差矩阵初始值%%%%%%%%% P(:,:,1)=P0; P_P(:,:,1)=P0; H=[1 0 0]; y=track(); Z=y(1,:)+70*rand(1,N); for j=1:M EXa=zeros(1,2); XaS=zeros(1,15); a=1/60+zeros(1,N); hol5=5;%%加速度调整 for k=1:N-1 %%%%%%%%%%%%%%%%%%状态噪声的协方差%%%%%%%%%%%%%%%%%%%%%% q11(k)=(1-exp(-2*a(k)*T)+2*a(k)*T+2*a(k)^3*T^3/3-2*a(k)^2*T^2-4*a(k)*T*exp(-a(k)*T))/(2*a(k)^5); q12(k)=(exp(-2*a(k)*T)+1-2*exp(-a(k)*T)+2*a(k)*T*exp(-a(k)*T)-2*a(k)*T+a(k)^2*T^2)/(2*a(k)^4); q13(k)=(1-exp(-2*a(k)*T)-2*a(k)*T*exp(-a(k)*T))/(2*a(k)^3); q21(k)=q12(k); q22(k)=(4*exp(-a(k)*T)-3-exp(-2*a(k)*T)+2*a(k)*T)/(2*a(k)^3); q23(k)=(exp(-2*a(k)*T)+1-2*exp(-a(k)*T))/(2*a(k)^2); q31(k)=q13(k); q32(k)=q23(k); q33(k)=(1-exp(-2*a(k)*T))/(2*a(k)); %%%%%%%%%%%%%%%%%%%%%常系数矩阵%%%%%%%%%%%%%%%%%% U(:,:,k)=[T^2/2-(a(k)*T-1+exp(-a(k)*T))/a(k)^2;T-(1-exp(-a(k)*T))/a(k);1-exp(-a(k)*T)]; F(:,:,k)=[1 T (a(k)*T-1+exp(-a(k)*T))/a(k)^2 0 1 (1-exp(-a(k)*T))/a(k) 0 0 exp(-a(k)*T)]; X_P(:,k+1)=F(:,:,k)*X(:,k); ca=90^2/3*7/6; Q(:,:,k)=[2*a(k)*ca*q11(k) 2*a(k)*ca*q12(k) 2*a(k)*ca*q13(k) 2*a(k)*ca*q21(k) 2*a(k)*ca*q22(k) 2*a(k)*ca*q23(k) 2*a(k)*ca*q31(k) 2*a(k)*ca*q32(k) 2*a(k)*ca*q33(k)]; Q(:,:,k)=abs(Q(:,:,k)); %%====================调整因子==================== P_P(:,:,k+1)=F(:,:,k)*P(:,:,k)*F(:,:,k)'+Q(:,:,k); K(:,:,k+1)=P_P(:,:,k+1)*H'/(H*P_P(:,:,k+1)*H'+R); X(:,k+1)=X_P(:,k+1)+K(:,:,k+1)*(Z(k+1)-X_P(1,k+1)); da=abs(X(3,k+1)-X(3,k));%%当加速度未发生突变将当前加速度与上一时刻加速度加权减小波动 if da<hol5 X(3,k+1)=0.3*X(3,k)+0.7*X(3,k+1); end P(:,:,k+1)=(eye(3)-K(:,:,k+1)*H)*P_P(:,:,k+1); %%========================机动频率自适应调节7步增量均值============================= XaS(1)=XaS(2);XaS(2)=XaS(3);XaS(3)=XaS(4);XaS(4)=XaS(5);XaS(5)=XaS(6);XaS(6)=XaS(7); XaS(7)=XaS(8);XaS(8)=XaS(9);XaS(9)=XaS(10);XaS(10)=XaS(11);XaS(11)=XaS(12);XaS(12)=XaS(13);XaS(13)=XaS(14);XaS(14)=XaS(15);XaS(15)=X(3,k+1); EXa(1)=1/5*(XaS(1)+XaS(2)+XaS(3)+XaS(4)+XaS(5));%+XaS(6)+XaS(7)+XaS(8)+XaS(9)+XaS(10)+XaS(11)+XaS(12)+XaS(13)); EXa(2)=1/5*(XaS(11)+XaS(12)+XaS(13)+XaS(14)+XaS(15)); u=1*exp(-(EXa(2)-EXa(1))^2/100); if (EXa(2)-EXa(1))*u/(10*XaS(15))+1>0&&abs(XaS(15))>3 a(k+1)=-log((EXa(2)-EXa(1))*u/(10*XaS(15))+1); end if XaS(15)<-10 a(k+1)=1/200; end end X_monte(:,:,j)=X(:,:); end MSE=zeros(3,N); X1_filter=zeros(3,N); for j=1:M MSE=MSE+(X_monte(:,:,j)-y).^2; X1_filter=X1_filter+X_monte(:,:,j); end RMSE.x=sqrt(MSE(1,:)/M); RMSE.v=sqrt(MSE(2,:)/M); RMSE.a=sqrt(MSE(3,:)/M); X_filter.x=(X1_filter(1,:)/M); X_filter.v=(X1_filter(2,:)/M); X_filter.a=(X1_filter(3,:)/M); [com_filter1,comRMSE1]=Singer1(); [com_filter2,comRMSE2]=Singer2(); figure(1); plot(com_filter1.x,'b-.');hold on plot(com_filter2.x,'g--');hold on plot(X_filter.x,'r'); legend('true','a=1/20','a=1/80','self adap'); xlabel('T（s）'); ylabel('x(m)'); grid on; figure(2); plot(com_filter1.v,'b-.');hold on plot(com_filter2.v,'g--');hold on plot(X_filter.v,'r'); legend('true','a=1/20','a=1/80','self adap'); xlabel('T(s)'); ylabel('v(m/s)'); grid on; figure(3); plot(com_filter1.a,'b-.');hold on plot(com_filter2.a,'g--');hold on plot(X_filter.a,'r'); legend('true','a=1/20','a=1/80','self adap'); xlabel('T(s)'); ylabel('a(m/s^2'); grid on; figure(4) plot(comRMSE1.x,'b-.');hold on plot(comRMSE2.x,'g--');hold on plot(RMSE.x,'r');hold on legend('a=1/20','a=1/80','self adap'); xlabel('T(s)'); ylabel('RMSE(m)'); grid on; figure(5) plot(comRMSE1.v,'b-.');hold on plot(comRMSE2.v,'g--');hold on plot(RMSE.v,'r');hold on legend('a=1/20','a=1/80','self adap'); xlabel('T(s)'); ylabel('RMSE(m/s)'); grid on; figure(6) plot(comRMSE1.a,'b-.');hold on plot(comRMSE2.a,'g--');hold on plot(RMSE.a,'r');hold on legend('a=1/20','a=1/80','self adap'); xlabel('T(s)'); ylabel('RMSE(m/s^2)'); grid on;