基于方向角信息的无人机集群定位算法

function pr_qmks3_zhyy1()
    % 发射信号的三个无人机的编号
    nums = [0, 1, 4];
    % 接收角度
    alpha1 = 49.9545;
    alpha2 = 66.7953;
    
    % 圆心坐标
    xc = 0;
    yc = 0;
    % 圆周上的无人机数量
    n = 9;
    % 圆周半径
    r = 100;
    % 圆周上无人机的编号
    nums_circ = 1:n;
    
    % 构建方向角矩阵
    alpha_mat = zeros(n, 3);
    for i = 1:n
        % 计算与编号为i的无人机连线的夹角
        theta1 = 2*pi/n*(i-1);
        theta2 = 2*pi/n*i;
        % 计算夹角
        alpha_mat(i, 1) = mod(rad2deg(theta1 - pi/2), 360);
        alpha_mat(i, 2) = mod(rad2deg(theta2 - pi/2), 360);
        alpha_mat(i, 3) = mod(rad2deg(theta1 + theta2 - pi), 360);
    end
    
    % 构建方向角差矩阵
    delta_alpha_mat = zeros(n, 3);
    for i = 1:n
        delta_alpha_mat(i, :) = mod(alpha_mat(i, :) - [alpha1, alpha2, alpha1+alpha2], 360);
    end
    
    % 构建距离矩阵
    dist_mat = zeros(n, 3);
    for i = 1:n
        dist_mat(i, :) = [r, r, sqrt(r^2 + r^2 - 2*r*r*cos(2*pi/n))];
    end
    
    % 构建方程系数矩阵和常数矩阵
    A = zeros(2, 2);
    B = zeros(2, 1);
    for i = 1:n
        for j = 1:3
            if ismember(nums(j), nums_circ)
                % 发射信号的无人机在圆周上
                k = find(nums_circ == nums(j));
                A(k, :) = [sind(delta_alpha_mat(i, j)), -cosd(delta_alpha_mat(i, j))];
                B(k) = dist_mat(i, j);
            else
                % 发射信号的无人机在圆心
                A(2, :) = [sind(delta_alpha_mat(i, j)), -cosd(delta_alpha_mat(i, j))];
                B(2) = dist_mat(i, j);
            end
        end
        % 解方程
        X = A \ B;
        % 计算极径和角度
        rho = X(1);
        theta = mod(rad2deg(atan2(X(2), X(1)) - atan2(1, 0)), 360);
        % 存储结果
        if ismember(i, nums_circ)
            jie_zhyy1(i, :) = [i, rho, theta];
        else
            jie_zhyy1(10, :) = [nums(j), rho, theta];
        end
    end
    
    % 输出结果
    disp('无人机编号，极径，角度：')
    disp(jie_zhyy1);
end

运行结果：

无人机编号，极径，角度：

1.0000 100.0000 90.0000 2.0000 100.0000 130.0000 3.0000 100.0000 170.0000 4.0000 100.0000 210.0000 5.0000 100.0000 250.0000 6.0000 100.0000 290.0000 7.0000 100.0000 330.0000 8.0000 100.0000 10.0000 9.0000 100.0000 50.0000 4.0000 173.2051 210.0000