AOA三维出图代码

t = linspace(0, 10, 100); % 从0到10，等间隔取100个时间点
% 设置参数
target_position_new =[];
v = 100; % 飞机的速度
delta_t = 1; % 时间间隔，修改为与之前运动目标相同的时间步长
total_time = 100; % 总时间，修改为与之前运动目标相同的总时间
% 计算飞机的位置
time = 0:delta_t:total_time;
position_target=[time * 120;time * 100;time * 80];
position_1 = [time * v;time * 80;time * 60];
position_2 = [500+time * v; time * 80;time * 60];
target_trajectory = zeros(3, numel(t));
new_target_trajectory = zeros(3, numel(t));
%%%%% 绘制飞机的轨迹图
 for time_index = 1:numel(t)

% 提取目标和两个飞机在该时刻的位置
target_position = position_target(:, time_index);
aircraft1_position = position_1(:, time_index);
aircraft2_position = position_2(:, time_index);

% 飞机1相对目标位置向量
relative_vector1 = target_position - aircraft1_position;
% 飞机2相对目标位置向量
relative_vector2 = target_position - aircraft2_position;
% 计算飞机1的俯仰角和方位角
theta1 = atan(relative_vector1(3) / sqrt(relative_vector1(1)^2 + relative_vector1(2)^2)); % 俯仰角
phi1 = atan2(relative_vector1(2), relative_vector1(1)); % 方位角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%加入测角误差
theta1 = theta1 + deg2rad(randn()); % 添加随机扰动
phi1 = phi1 + deg2rad(randn()); % 添加随机扰动
% 计算飞机2的俯仰角和方位角
theta2 = atan(relative_vector2(3) / sqrt(relative_vector2(1)^2 + relative_vector2(2)^2)); % 俯仰角
phi2 = atan2(relative_vector2(2), relative_vector2(1)); % 方位角
%%%%%%%%%%%%%%%%%加入测角误差
theta2 = theta2 + deg2rad(randn()); % 添加随机扰动
phi2 = phi2 + deg2rad(randn()); % 添加随机扰动
% 将弧度转换为度数
theta1_deg = rad2deg(theta1);
phi1_deg = rad2deg(phi1);
theta2_deg = rad2deg(theta2);
phi2_deg = rad2deg(phi2);

AB = 500; % AB的长度
% 计算AB向量
vector1_vector2 = aircraft2_position - aircraft1_position;
% 计算AM和BM的单位向量
Ma = [cosd(theta1_deg)*cosd(phi1_deg); cosd(theta1_deg)*sind(phi1_deg); sind(theta1_deg)];
Mb = [cosd(theta2_deg)*cosd(phi2_deg); cosd(theta2_deg)*sind(phi2_deg); sind(theta2_deg)];

% 计算AB向量与AM、BM单位向量的点积
dot_product_a = dot(vector1_vector2, Ma);
dot_product_b = dot(vector1_vector2, Mb);

% 计算AB向量与AM、BM单位向量的夹角（确保除零错误）
if AB ~= 0
    a = acos(dot_product_a / AB);
    b = acos(-dot_product_b / AB);
else
    disp('Error: AB的长度为零！');
    return;
end

% 转换为度数
a_deg = rad2deg(a);
b_deg = rad2deg(b);

% % 输出角度
% disp(['角度a为：', num2str(a_deg)]);
% disp(['角度b为：', num2str(b_deg)]);

% 计算AM的长度
 AM = AB * (tan(deg2rad(b_deg)) / (tan(deg2rad(a_deg)) + tan(deg2rad(b_deg)))) /cos(deg2rad(a_deg));
% AM=1697.41;
% 计算向量X
X = AM * [cosd(theta1_deg)*cosd(phi1_deg); cosd(theta1_deg)*sind(phi1_deg); sind(theta1_deg)];
%计算x方向误差
     target_position_new = aircraft1_position + X;
     new_target_trajectory(:, time_index) = target_position_new;
     %计算x方向误差，Y,Z
    x_error(time_index) = target_position_new(1) - target_position(1);
    y_error(time_index) = target_position_new(2) - target_position(2);
    z_error(time_index) = target_position_new(3) - target_position(3);
    
 end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 figure;
set(gcf,'color','white');%白色
 x=1:1:100;%x轴上的数据，第一个值代表数据开始，第二个值代表间隔，第三个值代表终止
 a=x_error; %a数据y值
 plot(x,a,'b-','LineWidth', 2); %线性，颜色，标记
% ylim([-1e-13, 1e-13]); % 设置 y轴范围，无测角误差下使用
 ylim([-100, 100]);
xlim([1, 100]); % 设置 x 轴范围为 1 到 100
xticks(1:2:100); % 设置 x 轴刻度为每秒一个刻度
xlabel('时间t')  %x轴坐标描述
ylabel('X轴误差') %y轴坐标描述
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
set(gcf,'color','white');%白色
 x=1:1:100;%x轴上的数据，第一个值代表数据开始，第二个值代表间隔，第三个值代表终止
 b=y_error; %a数据y值
 plot(x,b,'b-','LineWidth', 2); %线性，颜色，标记
  ylim([-100, 100]);
% ylim([-1e-13, 1e-13]); % 设置 y轴范围，无测角误差下使用
xlim([1, 100]); % 设置 x 轴范围为 1 到 100
xticks(1:2:100); % 设置 x 轴刻度为每秒一个刻度
xlabel('时间')  %x轴坐标描述
ylabel('Y轴误差') %y轴坐标描述
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
set(gcf,'color','white');%白色
 x=1:1:100;%x轴上的数据，第一个值代表数据开始，第二个值代表间隔，第三个值代表终止
 c=z_error; %a数据y值
 plot(x,c,'b-','LineWidth', 2); %线性，颜色，标记
 ylim([-100, 100]);
%  ylim([-1e-13, 1e-13]); % 设置 y轴范围，无测角误差下使用
xlim([1, 100]); % 设置 x 轴范围为 1 到 100
xticks(1:2:100); % 设置 x 轴刻度为每秒一个刻度
xlabel('时间')  %x轴坐标描述
ylabel('Z轴误差') %y轴坐标描述