目标定位基于matlab粒子滤波目标定位仿真含Matlab源码 129期

Posted 2022-01-18 紫极神光

一、获取代码方式

获取代码方式1：
完整代码已上传我的资源：【目标定位】基于matlab粒子滤波目标定位仿真【含Matlab源码 129期】

获取代码方式2：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

备注：订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、EKF算法简介

在SLAM问题解决方法中，EKF算法是比较常用的经典算法。机器人的自定位过程是一个非线性化的过程，EKF是为了解决Kalman滤波器不能应用于非线性系统而产生的，该滤波算法的主要过程是预测和更新。在预测和更新过程中，EKF算法对原有的系统方程和观测方程进行线性化并得到一个高估计的结果。如果系统中的非线性很弱，EKF也能给出很好的估计结果。

预测时，使用系统模型如下：

式中，z(k+1）表示的是第k+1步中获得的观测量，W （k+1）表示Kalman增益，其中还包括了前向估计在实际中的权重。

实现EKF-SLAM需要以下几方面内容：系统动态方程以及相应的参量，使用的传感器类型和观测方程，根据这些可以得到EKF的相应形式。二维平面中的EKF-SLAM需要知道机器人在X、Y方向的值，还需要知道二维平面下机器人头部朝向与X轴正方向的夹角。

三、部分源代码

clc;
clear;
N1=100;
thita=1*pi/4;
speed=20;
T=2.5;
point_Q2=[400 0;0 0.64*pi^2/180^2];
%Q1=[0.36 0;0 0];     %添加机动情形
%Q1=[0 0;0 0];        %不添加机动
l=[1/2*T^2 0;T 0;0 1/2*T^2;0 T];
N=200;
object_init_state=[2 speed*cos(thita) 3 speed*sin(thita)];    
for i=1:N1
    [real_data,view_data,real_v,point_view_data,Q1]=new_data_get(object_init_state,N,T,l);                   
    %前条件：目标的初始状态，目标跟踪次数已知
    %后条件：得到目标的观测矩阵，目标的真实值矩阵,真实速度
    [filter_data]=particle_filter(view_data,point_view_data,N,object_init_state,Q1,point_Q2,T,l,real_data);
    %前条件：目标的观测矩阵已知
    %后条件：得到目标的滤波矩阵和目标的各次增益
   
    [ME_temp_view_err_x(i,1:N),ME_temp_filter_err_x(i,1:N),ME_temp_view_err_y(i,1:N),ME_temp_filter_err_y(i,1:N)]=temp_err_save(real_data,...
    filter_data,view_data);
    %前条件：目标的真实值矩阵，目标的滤波矩阵，目标的观测矩阵已知
    %后条件：本次目标的观测误差，滤波误差得到存储
end
[ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y]=ME_err_count(ME_temp_view_err_x,ME_temp_filter_err_x,...
ME_temp_view_err_y,ME_temp_filter_err_y);
%前条件：目标的观测误差矩阵，目标的滤波误差矩阵已知
%后条件：得到目标的平均观测误差矩阵，平均滤波误差矩阵
[RMSE_final_view_err_x,RMSE_final_filter_err_x,RMSE_final_view_err_y,RMSE_final_filter_err_y]=RMSE_err_count(ME_temp_view_err_x,...
ME_temp_filter_err_x,ME_temp_view_err_y,ME_temp_filter_err_y,ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y);
%后条件：得到目标的均方观测误差矩阵，均方滤波误差矩阵
%show(ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y,final_filt_k1,final_filt_k2,N,real_data,view_data,filter_data,real_v);
show(ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y,RMSE_final_view_err_x,RMSE_final_filter_err_x,...
RMSE_final_view_err_y,RMSE_final_filter_err_y,N,real_data,view_data,filter_data,real_v);
%前条件：目标的平均观测误差矩阵，平均滤波误差矩阵已知
%后条件：作出目标的平均观测误差矩阵，平均滤波误差矩阵的仿真图
function [filter_data]=data_kalman_filter(view_data,point_view_data,N,object_init_state,Q1,point_Q2,T,l,real_data)
%定义初试滤波
filter_data0=object_init_state';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ch=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];               %定义状态转移矩阵
M=10;                                               %定义粒子数为10
M_out=10;
%h=[1 0 0 0;0 0 1 0];                               %观测转移矩阵
%观测误差是随时间变化的所以需计算观测误差
%cos_2_v_thita=(object_init_state(1,1)^2)/(object_init_state(1,1)^2+object_init_state(1,3)^2);
%sin_2_v_thita=1-cos_2_v_thita;
%R_2=(object_init_state(1,1)^2+object_init_state(1,3)^2);
%Q2=zeros(2,2);       
%Q2(1,1)=point_Q2(1,1)*cos_2_v_thita+R_2*sin_2_v_thita*point_Q2(2,2);
%Q2(1,2)=sqrt(cos_2_v_thita)*sqrt(sin_2_v_thita)*(point_Q2(1,1)-R_2*point_Q2(2,2));
%Q2(1,2)=0;
%Q2(2,1)=Q2(1,2);
%Q2(2,2)=point_Q2(1,1)*sin_2_v_thita+R_2*cos_2_v_thita*point_Q2(2,2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    
for i=1:M
    w1=sqrt(Q1(1,1))*randn;
    w2=sqrt(Q1(2,2))*randn;
    w=[w1;w2];
    x_part(:,i)=filter_data0+l*w;
end
 q0=ones(1,M)/M;   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%根据初始预测计算滤波值
%通过输入观测值进行卡尔曼滤波
for i=1:N
    for k=1:M
        w1=sqrt(Q1(1,1))*randn;
        w2=sqrt(Q1(2,2))*randn;
        w=[w1;w2];
        xpart_minus(:,k)=ch*x_part(:,k)+l*w;
        r=sqrt(xpart_minus(1,k)^2+xpart_minus(3,k)^2);
        if xpart_minus(1,k)>0 
           thita=atan(xpart_minus(3,k)/xpart_minus(1,k));
       else
           thita=atan(xpart_minus(3,k)/xpart_minus(1,k))+pi;
       end
        z_part=[r;thita];
        vhat=point_view_data(:,i)-z_part;                               %观测值和预测值的差
        if i==1
           q(i,k)=1/(2*pi*sqrt(point_Q2(1,1)*point_Q2(2,2)))*exp(-(vhat(1)^2/(2*point_Q2(1,1))+vhat(2)^2/(2*point_Q2(2,2))))*q0(k);  
       else
           q(i,k)=1/(2*pi*sqrt(point_Q2(1,1)*point_Q2(2,2)))*exp(-(vhat(1)^2/(2*point_Q2(1,1))+vhat(2)^2/(2*point_Q2(2,2))))*q(i-1,k); 
       end
    end
    qsum=sum(q(i,:));
    for k=1:M
        q(i,k)=q(i,k)/qsum;                                 %  归一化权值
    end
    %x_part=xpart_minus;
    qsum2=q(i,:)*q(i,:)';
    Nff=1/qsum2;
   
    if Nff<2/3*M
        u=rand/M_out; 
       
        for k=1:M           
            qtempsum=0;
            for j=1:M
                qtempsum=qtempsum+q(i,j);
                if qtempsum>= u
                   x_part(:,k) = xpart_minus(:,j);
                   u=u+1/M_out;
                   break; 
                end
           end
        end
        q(i,:)=ones(1,M)/M;
    else
        x_part=xpart_minus;
    end
    %重采样有问题
 
   x_filt=zeros(4,1);
   for k=1:M
       x_filt=q(k)*x_part(:,k)+x_filt;
   end
   filter_data(:,i)=x_filt;
 
    
end

四、运行结果

运行结果各图名称见实际运行的命名；

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.
[4]林志东.基于扩展卡尔曼滤波算法的SLAM问题分析[J].城市建筑. 2020,17(11)