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

《EKF-UKF-PF三种算法的比较matlab.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、%%%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_

2、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;%总的采

3、样点的个数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

4、*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_

5、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;%粒子滤波器%粒子滤

6、波器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)/

7、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);%在这

8、里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%不敏