MATLAB基于遗传算法的二维TDOA定位

主函数

function [Global_best, Max_Generation] = main_program( MS,Noise, Cxmax, Cxmin,Cymax, Cymin)
PopSize =200;           % 染色体数目
Max_Generation =60;    % max.number of generation
Pc = 0.15;              % probability of crossover
Pm = 0.25;              % probability of mutation  
b = 6;                  % 确定对迭代数依赖程度的系统参数
glo_best = zeros(4,1);
Mu = 0;                 % 噪声的均值
M = 5;                  % 接受机的数目
generation = 0;

% 随机初始化种群
pop_arr = zeros(4,PopSize);
pop_arr(1:1:2, 1:1:PopSize) = rand( 2, PopSize ); 

[pop_arr, glo_best] = best( pop_arr, PopSize, glo_best, Cxmax, Cymax, Cxmin, Cymin, Noise,MS);
Global_best = zeros(4,Max_Generation);
while generation < Max_Generation    
    generation = generation + 1;        
    [pop_arr] = selection_crossover_mutation(pop_arr, PopSize, generation, Max_Generation, b, Pm,Pc, Cxmax, Cxmin,Cymax, Cymin);%执行选择，交叉，变异操作    
    [pop_arr, glo_best] = best( pop_arr, PopSize, glo_best, Cxmax, Cymax, Cxmin, Cymin, Noise,MS);%寻找全局最优染色体
    Global_best(1,generation) = Cxmin + glo_best(1,1) * (Cxmax - Cxmin);
    Global_best(2,generation) = Cymin + glo_best(2,1) * (Cymax - Cymin);
    Global_best(3,generation) = glo_best(3,1);
    Global_best(4,generation) = glo_best(4,1);
end

适应度函数

function [ind_pop_val, ind_pop_fit] = fit( x, y,  Cxmax, Cxmin,Cymax,Cymin, Noise,MS)
x = Cxmin + x * (Cxmax - Cxmin);%  X为pop_arr的第一行
y = Cymin + y * (Cymax - Cymin);%  Y为pop_arr的第二行 
% 计算个体的目标函数值 和 适应值， x 为传入的横坐标，y 为传入的纵坐标, 
% 求解似然函数的值
M = 5;      % 接受机的数目
% 五个接收机的坐标( 0， 0 ), ( 3*3^(1/2), 3 ), ( 3*3^(1/2), -3 ), ( -3*3^(1/2), -3  ), ( 0, -6 )
base_coor = [0, 0; 3*3^(1/2), 3; 3*3^(1/2), -3; -3*3^(1/2), -3;  0, -6 ];

% 由估计位置所得到的 源点到第 i 个接收机的距离
for i = 1:1:M
    R_est( i, 1 ) = ((x - base_coor( i, 1 ))^2 + (y - base_coor( i, 2 ))^2)^(1/2);
end

% 由估计位置所得到的 源点到第 i 个接收机的距离（除服务基站以外）    矩阵R
for i = 1:1:(M - 1)
    R( i, 1 ) = R_est(i+1,1);
end

% 由估计位置所得到的 源点服务基站的距离   矩阵R1
for i = 1:1:M - 1
    R1( i, 1) = R_est(1,1);
end                     
c = 3*10^5;                      % 单位km  光速
base_coor = [0, 0; 3*3^(1/2), 3; 3*3^(1/2), -3; -3*3^(1/2), -3;  0, -6 ];

% 由源点位置所得到的 源点到第 i 个接收机的距离
for i = 1:1:M
    R_all( i, 1 ) = ((MS(1) - base_coor( i, 1 ))^2 + (MS(2) - base_coor( i, 2 ))^2)^(1/2);
end

% 源点位置到第i 个接收机与到服务基站的距离差 R_dif_Noise （受到噪声的干扰 ）由直接得到的TDOA加上噪声所得
% 测量的过程中所受到的噪声干扰 NOISE
for i = 1:1:M-1
    R_dif(i, 1) = R_all(i + 1, 1) - R_all(1, 1);
end

for i = 1:1:M-1
    R_dif_Noise(i, 1) = R_dif(i,1) + c*Noise(i,1);
end

Deta_R = R_dif_Noise;    % Deta_R =R-R1+c*Niose为基站所测量的到达时间差（受到噪声的干扰）
% 所求似然函数的值
ind_pop_val = (Deta_R - R + R1)' * (Deta_R - R + R1);  
 %适应度函数
ind_pop_fit = 1 / ind_pop_val;

寻找最优染色体

function [pop_arr, glo_best] = best( pop_arr, PopSize, glo_best, Cxmax, Cymax, Cxmin, Cymin, Noise, MS)

for j = 1:1:PopSize
    x = pop_arr(1,j);
    y = pop_arr(2,j);    
    [ind_pop_val, ind_pop_fit] = fit( x, y,Cxmax, Cxmin,Cymax, Cymin, Noise,MS); 
    pop_arr(3, j) = ind_pop_val;% ind_pop_val目标函数值
    pop_arr(4, j) = ind_pop_fit;% ind_pop_fit适应度函数值
end

% 寻找此代中的最优染色体 best_ind_fit 和 最差染色体 worst_ind_fit
best_ind_fit = pop_arr(4, 1);
best_index = 1;
worst_ind_fit = pop_arr(4, 1);
worst_index = 1;

for j = 2:1:PopSize 
    if ( pop_arr(4, j) > best_ind_fit )
        best_ind_fit = pop_arr(4, j);      %此代中最优染色体
        best_index = j;
    else if ( pop_arr(4, j) < worst_ind_fit )
            worst_ind_fit = pop_arr(4, j); %此代中最差染色体
            worst_index = j;
        end
    end
end

% 寻找所有代中的全局最优染色体 glo_best

    for i = 1:1:4
        glo_best(i, 1) = pop_arr(i, best_index);
    end
glo_best(i, 1);
% 使用最优染色体 替代 最差染色体
for i = 1:1:4
    pop_arr(i, worst_index) = glo_best(i, 1);
end

执行选择，交叉，变异

function [pop_arr] = selection_crossover_mutation(pop_arr, PopSize, generation, Max_Generation, b, Pm,Pc, Cxmax, Cxmin,Cymax, Cymin)
%% 选择
% 根据轮盘选择法 产生 新一代群体 pop_arr
sum = 0.0;
for j = 1:1:PopSize
    sum = sum + pop_arr(4, j);
end

for j = 1:1:PopSize
    com_fitness(j) = pop_arr(4, j) / sum;%个体相对适应度
end

for j = 2:1:PopSize
    com_fitness(j) = com_fitness(j - 1) + com_fitness(j);
end

% selection operation
for j = 1:1:PopSize
    p = rand;
    index = 1;
    while  ((p > com_fitness(index)) && (index < PopSize))
        index = index + 1;
    end
    for i = 1:1:4
        new_pop_arr(i, j) = pop_arr(i, index);
    end
end

for i = 1:1:4
    for j = 1:1:PopSize
        pop_arr(i, j) = new_pop_arr(i, j);
    end
end
%% 交叉
% pop_arr 为进行了概率为 Pc 的均匀杂交所得到的
% 产生进行交叉运算的染色体对

% for j = 1:1:PopSize
%     index(j) = j;
% end
% 
% %交换码值
% for j = 1:1:PopSize
%     point = randsrc(1,1,0:1:(PopSize - j));
%     temp = index(j);
%     index(j) = index(point + j);
%     index(point + j) = temp;
% end

% 进行概率为 Pc 的 均匀算术杂交 
a =rand;
for j = 1:2:PopSize
    p = rand;
    if p < Pc
        for i = 1:1:2
            new_pop_arr(i, j) = a * pop_arr(i, j+1) + (1 - a) * pop_arr(i, j);
            new_pop_arr(i, j+1) = a * pop_arr(i, j); + (1 - a) * pop_arr(i, j+1);
        end
        pop_arr(i,j) = new_pop_arr(i,j);
        pop_arr(i,j+1) = new_pop_arr(i,j+1);
    end
end
%% 变异
% 对 pop_arr 进行非均匀变异,使用论文中给定的变异算子
for j = 1:1:PopSize
    p = rand;
    if p < Pm
        p1 = rand;
        p2 = rand;
        pp = rand;
               % 当随机数为0时
               if pp <= 0.5            
                 pop_arr(1, j) = pop_arr(1, j) + p1 * (Cxmax - pop_arr(1, j)) * (1 - generation / Max_Generation)^b;
                 pop_arr(2, j) = pop_arr(2, j) + p2 * (Cymax - pop_arr(2, j)) * (1 - generation / Max_Generation)^b;                
               %当随机数为1时
                    else 
                   pop_arr(1, j) = pop_arr(1, j) + p1 * (pop_arr(1, j) - Cxmin) * (1 - generation / Max_Generation)^b;
                   pop_arr(2, j) = pop_arr(2, j) + p2 * (pop_arr(2, j) - Cymin) * (1 - generation / Max_Generation)^b;
                end       
                   if pop_arr(1, j)>Cxmax,
                     pop_arr(1, j)=Cxmax;
                    elseif pop_arr(1, j)Cymax,
                     pop_arr(2, j)=Cymax;
                   elseif pop_arr(2, j)

function [gsrv1, gsrv2] = gngauss(m, sgma)
%   [gsrv1, gsrv2] = gngauss(m, sgma)
%   [gsrv1, gsrv2] = gngauss(sgma)
%   [gsrv1, gsrv2] = gngauss
%       GNGAUSS generates two independent Gaussian random variables with
%       mean m and standard deviation sgma. If one of the input arguments
%       is missing, it takes the mean as 0.
%       If neither the mean nor the variance is given, it generates two
%       standard Gaussian random variables.

if nargin == 0
    m = 0; 
    sgma = 1;
elseif nargin == 1
    sgma = m; 
    m = 0;
end

u = rand;                                   % a uniform random variable in(0,1)
z = sgma * (sqrt(2 * log(1 / (1 - u))));    % a Rayleigh distributed random variable
u = rand;                                   % another uniform random variable in(0,1)
gsrv1 = m + z * cos(2 * pi * u);
gsrv2 = m + z * sin(2 * pi * u);

clear;
clc;
account_test =100;
Counter_Size=account_test;
GLOBAL_BEST = zeros(4,account_test);
MV1=zeros(2,6);
MSE1=zeros(1,6);
M=5;
k=1;
MS = [1,2.5];
c = 3*10^5;                      % 单位km

for Noise_db = -18:2:-8                 % 信道所受到的噪声干扰
    Sigma = 10^(Noise_db./10)/c; 
    
    for m = 1:1:Counter_Size%%%%%%%%%%%%%%%%%%%%cishu 
        for i = 1:1:M-1
            Noise(i, 1) = gngauss(Sigma);
        end
 

   Cxmax = 2*sqrt(3);
   Cxmin = -2*sqrt(3); 
   Cymax = 3;
   Cymin = -3;  
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%    GA   
   [Global_best, Max_Generation] = main_program(MS,Noise,Cxmax, Cxmin,Cymax, Cymin);
   for i = 1:1:4
        GLOBAL_BEST(i, m) = Global_best(i, Max_Generation);
   end
   GLOBAL_BEST2(:, m)=GLOBAL_BEST(:, m);
% 计算试验所得的平均估计坐标MV
    end
    
    MV = [0.0; 0.0];
    for i = 1:1:2
        for j = 1:1:account_test
            MV(i) = MV(i) + GLOBAL_BEST2(i, j);
        end
    end
 
    MV = MV/account_test;    % 计算GA所得的均值MV
    MV1(1:1:2,k)=MV;

    MSE = 0.0;
    for j = 1:1:account_test
        MSE = MSE + (GLOBAL_BEST2(1, j) - MS(1))^2 + (GLOBAL_BEST2(2, j) - MS(2))^2;
    end
    MSE  = MSE/account_test;  % 计算GA所得的均方误差MSE
    MSE1(1,k)=MSE;
    k=k+1;
    if k==7
       break;
    end
end

MV1
MSE1

plot(-14:2:-4,MSE1,'bo-.')
xlabel('10lg(cσ)/dB')
ylabel('均方误差MSE/km')
legend('GA算法')

代码运行结果