PSO学习笔记（2）-代码

clc;clear all;close all;
%% 定位场景设置
s0(:,1)=[10e2;0];%观测站1的位置
s0(:,2)=[-10e2;0];%观测站2的位置
s0(:,3)=[0e2;8e2];%观测站3的位置
s0(:,4)=[15e2;0];%观测站4的位置
arfa=1/3*pi;
d=130e2;%目标到原点距离
p=d*[cos(arfa);sin(arfa);0];%目标位置
M=size(s0,2);
%% 相关参数设置
c=3e8;  %光速
model=2;%1:DPD,2:TDOA+FDOA两步法
c1=2;%学习因子
c2=2;%学习因子
Dimension=2;%粒子维度，和目标的维度一致
Size=20;%粒子群大小
Tmax=100;%最大迭代次数
Fun_Ub=[5e3;5e3];%位置上界,默认对应下界为-Fun_Ub
Xmax(1:Dimension)=p+Fun_Ub.*[1;1];%粒子位置上下界，即函数自变量的上下界
Xmin(1:Dimension)=p-Fun_Ub.*[1;1];
Velocity_max=Xmax-Xmin;%粒子最大速度，由目标位置的可行解范围（目标位置的上下界）决定
Vmax(1:Dimension)=Velocity_max;%粒子速度上下界
Vmin(1:Dimension)=-Velocity_max;

%% PSO粒子群初始化
[Position,Velocity]=Initial_position_velocity(Dimension,Size,Xmax,Xmin,Vmax,Vmin);%初始化粒子位置和速度
Pbest_position=Position;%粒子的历史最优位置，初始值为粒子的起始位置，存储每个粒子的历史最优位置
Gbest_position=zeros(Dimension,1);%全局最优的那个粒子所在位置，初始值认为是第1个粒子

% 两步法相关测量
sta_td=1e-9;%TDOA测量标准差
r1=norm(p-s0(:,1));
for m=1:M-1
    r(m)=norm(p-s0(:,m+1));
    tao(m)=(r(m)-r1)/c;
end
td=tao+sta_td^2*randn(1,M-1);%TDOA测量

%% PSO算法
for j=1:Size
    Pos=Position(:,j);%第j个粒子的位置
    fz(j)=Fitness_Function(Pos,s0,Dimension,td,model);%计算第j个粒子的适应值（）
end

[Gbest_Fitness,index]=min(fz);%求出所有适应值中最小的那个适应值，并获得该粒子的位置
Gbest_position=Position(:,index);%取最小适应值的那个粒子的位置，即I列

for itrtn=1:Tmax
    time(itrtn)=itrtn;
    Weight=1;
    r1=rand(1);
    r2=rand(1);
    for i=1:Size
        Velocity(:,i)=Weight*Velocity(:,i)+c1*r1*(Pbest_position(:,i)-Position(:,i))+c2*r2*(Gbest_position-Position(:,i));
    end
    
    %限制速度边界
    for i=1:Size
        for row=1:Dimension
            if Velocity(row,i)>Vmax(row)
                Veloctity(row,i)=Vmax(row);
            elseif Velocity(row,i)<Vmin(row)
                Veloctity(row,i)=Vmin(row);
            else
            end
        end
    end
    Position=Position+Velocity;
    %限制位置边界
    for i=1:Size
        for row=1:Dimension
            if Position(row,i)>Xmax(row)
                Position(row,i)=Xmax(row);
            elseif Position(row,i)<Xmin(row)
                Position(row,i)=Xmin(row);
            else      
            end
        end
    end
    
    
    for j=1:Size
        P_position=Position(:,j);%取一个粒子的位置   
        fitness_p(j)=Fitness_Function(P_position,s0,Dimension,td,model); 
        if fitness_p(j)< fz(j) %粒子的适应值比运动之前的适应值要好，更新原来的适应值 
            Pbest_position(:,j)=Position(:,j);     
            fz(j)=fitness_p(j);       
        end
        if fitness_p(j)<Gbest_Fitness  
            Gbest_Fitness=fitness_p(j);    
        end
    end
    [Gbest_Fitness_new,I]=min(fz);%更新后的所有粒子的适应值，取最小的那个，并求出其编号
    Best_fitness(itrtn)=Gbest_Fitness_new; %记录每一代的最好适应值  
    Gbest_position=Pbest_position(:,I);%最好适应值对应的个体所在位置
end
plot(time,Best_fitness);
xlabel('迭代的次数');ylabel('适应度值P_g');

 

子函数：

function [Position,Velocity] = Initial_position_velocity(Dimension,Size,Xmax,Xmin,Vmax,Vmin)
%% 初始化
for i=1:Dimension
    Position(i,:)=Xmin(i)+(Xmax(i)-Xmin(i))*randn(1,Size); % 产生合理范围内的随机位置，rand(1,Size)用于产生一行Size个随机数
    Velocity(i,:)=Vmin(i)+(Vmax(i)-Vmin(i))*randn(1,Size);
    
end

end

function Fitness=Fitness_Function(Pos,s0,Dimension,td,model)
%%适应值计算：
c=3e8;
M=size(s0,2);
switch model
    
    case 1% DPD
      %无
    case 2 %两步法%仅以TDOA为例
        r1=norm(Pos-s0(:,1));
        for m=1:M-1
            r(m)=norm(Pos-s0(:,m+1));
            tdd(m)=(r(m)-r1)/c;
        end
        cost=norm((tdd-td)*c);%误差
        Fitness=cost;    
end