EKF-UKF-PF三种算法的比较matlab

%%%EKFUKFPF三种算法对比clccloseallclear;%tic;x=0.1;%初始状态x_estimate=1;%状态的估计e_x_estimate=x_estimate;%EKF的初始估计u_x_estimate=x_estimate;%UKF的初始估计p_x_estimate=x_estimate;%PF的初始估计Q=10;%input('请输入过程噪声方差Q的值:');%过程状态协方差R=1;%input('请输入测量噪声方差R的值:');%测量噪声协方差P=5;%初始估计方差e_P=P;%UKF方差u_P=P;%UKF方差pf_P=P;%PF方差tf=50;%模拟长度x_array=[x];%真实值数组e_x_estimate_array=[e_x_estimate];%EKF最优估计值数组u_x_estimate_array=[u_x_estimate];%UKF最优估计值数组p_x_estimate_array=[p_x_estimate];%PF最优估计值数组u_k=1;%微调参数u_symmetry_number=4;%对称的点的个数u_total_number=2*u_symmetry_number+1;%总的采样点的个数linear=0.5;N=500;%粒子滤波的粒子数closeall;%粒子滤波初始N个粒子fori=1:Np_xpart(i)=p_x_estimate+sqrt(pf_P)*randn;endfork=1:tf%模拟系统x=linear*x+(25*x/(1+x^2))+8*cos(1.2*(k-1))+sqrt(Q)*randn;%状态值y=(x^2/20)+sqrt(R)*randn;%观测值%扩展卡尔曼滤波器%进行估计第一阶段的估计e_x_estimate_1=linear*e_x_estimate+25*e_x_estimate/(1+e_x_estimate^2)+8*cos(1.2*(k-1));e_y_estimate=(e_x_estimate_1)^2/20;%这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的第24行的y也是观测值不过是由加了噪声的真实值得到的%相关矩阵e_A=linear+25*(1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵e_H=e_x_estimate_1/10;%观测矩阵%估计的误差e_p_estimate=e_A*e_P*e_A'+Q;%扩展卡尔曼增益e_K=e_p_estimate*e_H'/(e_H*e_p_estimate*e_H'+R);%进行估计值的更新第二阶段e_x_estimate_2=e_x_estimate_1+e_K*(y-e_y_estimate);%更新后的估计值的误差e_p_estimate_update=e_p_estimate-e_K*e_H*e_p_estimate;%进入下一次迭代的参数变化e_P=e_p_estimate_update;e_x_estimate=e_x_estimate_2;%粒子滤波器%粒子滤波器fori=1:Np_xpartminus(i)=0.5*p_xpart(i)+25*p_xpart(i)/(1+p_xpart(i)^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn;%这个式子比下面一行的效果好%xpartminus(i)=0.5*xpart(i)+25*xpart(i)/(1+xpart(i)^2)+8*cos(1.2*(k-1));p_ypart=p_xpartminus(i)^2/20;%预测值p_vhat=y-p_ypart;%观测和预测的差p_q(i)=(1/sqrt(R)/sqrt(2*pi))*exp(-p_vhat^2/2/R);%各个粒子的权值end%平均每一个估计的可能性p_qsum=sum(p_q);fori=1:Np_q(i)=p_q(i)/p_qsum;%各个粒子进行权值归一化end%重采样权重大的粒子多采点，权重小的粒子少采点,相当于每一次都进行重采样；fori=1:Np_u=rand;p_qtempsum=0;forj=1:Np_qtempsum=p_qtempsum+p_q(j);ifp_qtempsum=p_up_xpart(i)=p_xpartminus(j);%在这里xpart(i)实现循环赋值；终于找到了这里！！！break;endendendp_x_estimate=mean(p_xpart);%p_x_estimate=0;%fori=1:N%p_x_estimate=p_x_estimate+p_q(i)*p_xpart(i);%end%不敏卡尔曼滤波器%采样点的选取存在x(i)u_x_par=u_x_estimate;fori=2:(u_symmetry_number+1)u_x_par(i,:)=u_x_estimate+sqrt((u_symmetry_number+u_k)*u_P);endfori=(u_symmetry_number+2):u_total_numberu_x_par(i,:)=u_x_estimate-sqrt((u_symmetry_number+u_k)*u_P);end%计算权值u_w_1=u_k/(u_symmetry_number+u_k);u_w_N1=1/(2*(u_symmetry_number+u_k));%把这些粒子通过传递方程得到下一个状态fori=1:u_total_numberu_x_par(i)=0.5*u_x_par(i)+25*u_x_par(i)/(1+u_x_par(i)^2)+8*cos(1.2*(k-1));end%传递后的均值和方差u_x_next=u_w_1*u_x_par(1);fori=2:u_total_numberu_x_next=u_x_next+u_w_N1*u_x_par(i);endu_p_next=Q+u_w_1*(u_x_par(1)-u_x_next)*(u_x_par(1)-u_x_next)';fori=2:u_total_numberu_p_next=u_p_next+u_w_N1*(u_x_par(i)-u_x_next)*(u_x_par(i)-u_x_next)';end%%对传递后的均值和方差进行采样产生粒子存在y(i)%u_y_2obser(1)=u_x_next;%fori=2:(u_symmetry_number+1)%u_y_2obser(i,:)=u_x_next+sqrt((u_symmetry_number+k)*u_p_next);%end%fori=(u_symmetry_number+2):u_total_number%u_y_2obser(i,:)=u_x_next-sqrt((u_symmetry_number+u_k)*u_p_next);%end%另外存在y_2obser(i)中；fori=1:u_total_numberu_y_2obser(i,:)=u_x_par(i);end%通过观测方程得到一系列的粒子fori=1:u_total_numberu_y_2obser(i)=u_y_2obser(i)^2/20;end%通过观测方程后的均值y_obseu_y_obse=u_w_1*u_y_2obser(1);fori=2:u_total_numberu_y_obse=u_y_obse+u_w_N1*u_y_2obser(i);end%Pzz测量方差矩阵u_pzz=R+u_w_1*(u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';fori=2:u_total_numberu_pzz=u_pzz+u_w_N1*(u_y_2obser(i)-u_y_obse)*(u_y_2obser(i)-u_y_obse)';end%Pxz状态向量与测量值的协方差矩阵u_pxz=u_w_1*(u_x_par(1)-u_x_next)*(u_y_2obser(1)-u_y_obse)';fori=2:u_total_numberu_pxz=u_pxz+u_w_N1*(u_x_par(i)-u_x_next)*(u_y_2obser(i)-u_y_obse)';end%卡尔曼增益u_K=u_pxz/u_pzz;%估计量的更新u_x_next_optimal=u_x_next+u_K*(y-u_y_obse);%第一步的估计值+修正值；u_x_estimate=u_x_next_optimal;%方差的更新u_p_next_update=u_p_next-u_K*u_pzz*u_K';u_P=u_p_next_update;%进行画图程序x_array=[x_array,x];e_x_estimate_array=[e_x_estimate_array,e_x_estimate];p_x_estimate_array=[p_x_estimate_array,p_x_estimate];u_x_estimate_array=[u_x_estimate_array,u_x_estimate];e_error(k,:)=abs(x_array(k)-e_x_estimate_array(k));p_error(k,:)=abs(x_array(k)-p_x_estimate_array(k));u_error(k,:)=abs(x_array(k)-u_x_estimate_array(k));endt=0:tf;figure;plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g--',t,u_x_estimate_array,'b:');set(gca,'FontSize',10);set(gcf,'color','White');xlabel('时间步长');%lable---label我的神ylabel('状态');legend('真实值','EKF估计值','PF估计值','UKF估计值');figure;plot(t,x_array,'k.',t,p_x_estimate_array,'g--',t,p_x_estimate_array-1.96*sqrt(P),'r:',t,p_x_estimate_array+1.96*sqrt(P),'r:');set(gca,'FontSize',10);set(gcf,'color','White');xlabel('时间步长');%lable---label我的神ylabel('状态');legend('真实值','PF估计值','95%置信区间');%rootmeansquare平均值的平方根e_xrms=sqrt((norm(x_array-e_x_estimate_array)^2)/tf);disp(['EKF估计误差均方值=',num2str(e_xrms)]);p_xrms=sqrt((norm(x_array-p_x_estimate_array)^2)/tf);disp(['PF估计误差均方值=',num2str(p_xrms)]);u_xrms=sqrt((norm(x_array-u_x_estimate_array)^2)/tf);disp(['UKF估计误差均方值=',num2str(u_xrms)]);%plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');%legend('EKF估计值误差','PF估计值误差','UKF估计值误差');t=1:tf;figure;plot(t,e_error,'r-',t,p_error,'g--',t,u_err