Singer模型自适应_机动_机动频率自适应调节_自适应跟踪_singer模型算法_singer模型_

%%%%%%%%%%%%%%%%%%%%%本程序为Singer模型机动频率自适应调节算法%%%%%%%%% %%%%%%%%%%%%%采用5个十步均值平均调节机动频率%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%采用正太分布曲线作为隶属度函数调节加速度扰动对机动频率调整的灵敏度%%%%%%%%%%%% function [X_filter,RMSE]=CSa8() 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; end