AOA三维二机定位模型(笔记）

t = linspace(0, 10, 100); % 从0到10，等间隔取100个时间点
% 设置参数
v = 100; % 飞机的速度
delta_t = 1; % 时间间隔，修改为与之前运动目标相同的时间步长
total_time = 100; % 总时间，修改为与之前运动目标相同的总时间
initial_separation = 500; % 初始间距
% 计算飞机的位置
time = 0:delta_t:total_time;
position_target=[time * 120;time * 100;time * 80];
position_1 = [time * v;time * 80;time * 60];
position_2 = [initial_separation+time * v; time * 80;time * 60];
% % 绘制目标和两个基站在三维空间中的轨迹
figure;
set(gcf,'color','white');%白色
plot3(position_target(1,:), position_target(2,:), position_target(3,:), 'r-', 'LineWidth', 2);hold on;
plot3(position_1(1,:), position_1(2,:), position_1(3,:), 'b-', 'LineWidth', 2);hold on;
plot3(position_2(1,:), position_2(2,:), position_2(3,:), 'g-', 'LineWidth', 2);grid on;
xlabel('X'); ylabel('Y'); zlabel('Z');
title('Trajectories of Target and Stations in 3D Space');
grid on;
axis equal; % 设置坐标轴的刻度间距相等
xlim([0, 1500]); % 设置 X 轴的范围
ylim([0, 1500]); % 设置 Y 轴的范围
zlim([0, 1500]); % 设置 Z 轴的范围

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 绘制飞机的轨迹图
figure;
plot3(position_target(1,:), position_target(2,:), position_target(3,:), 'r-', 'LineWidth', 2);
grid on;
xlabel('X');
ylabel('Y');
zlabel('Z');
title('Aircraft Trajectory (Uniform Acceleration)');
%%%%%%%%%%%%%%%%%%%%某一点

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%选择某一特定时刻
 time_index = 50;

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

% 绘制目标和两个飞机在该时刻的位置
figure;
set(gcf,'color','white');%白色
% 绘制位置点
scatter3(target_position(1), target_position(2), target_position(3), 100, 'MarkerFaceColor', 'r', 'MarkerEdgeColor', 'k');
hold on;
scatter3(aircraft1_position(1), aircraft1_position(2), aircraft1_position(3), 100, 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k');
scatter3(aircraft2_position(1), aircraft2_position(2), aircraft2_position(3), 100, 'MarkerFaceColor', 'g', 'MarkerEdgeColor', 'k');
%绘制连接线
plot3([target_position(1), aircraft1_position(1)], [target_position(2), aircraft1_position(2)], [target_position(3), aircraft1_position(3)], '--b');
plot3([target_position(1), aircraft2_position(1)], [target_position(2), aircraft2_position(2)], [target_position(3), aircraft2_position(3)], '--g');
plot3([aircraft1_position(1), aircraft2_position(1)], [aircraft1_position(2), aircraft2_position(2)], [aircraft1_position(3), aircraft2_position(3)], '--k');
% 设置图例
legend('目标', '飞机1', '飞机2');

% 设置坐标轴标签
xlabel('X');
ylabel('Y');
zlabel('Z');

% 设置标题
title('目标和飞机位置');

% 显示网格
grid on;

text(target_position(1), target_position(2), target_position(3), sprintf('(%.1f, %.1f, %.1f)', target_position(1), target_position(2), target_position(3)));
text(aircraft1_position(1), aircraft1_position(2), aircraft1_position(3), sprintf('(%.1f, %.1f, %.1f)', aircraft1_position(1), aircraft1_position(2), aircraft1_position(3)));
text(aircraft2_position(1), aircraft2_position(2), aircraft2_position(3), sprintf('(%.1f, %.1f, %.1f)', aircraft2_position(1), aircraft2_position(2), aircraft2_position(3)));

% 飞机1和飞机2的位置已经在之前的代码中计算并得到了 aircraft1_position 和 aircraft2_position

% 飞机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)); % 方位角

% 计算飞机2的俯仰角和方位角
theta2 = atan(relative_vector2(3) / sqrt(relative_vector2(1)^2 + relative_vector2(2)^2)); % 俯仰角
phi2 = atan2(relative_vector2(2), relative_vector2(1)); % 方位角

% 将弧度转换为度数
theta1_deg = rad2deg(theta1);
phi1_deg = rad2deg(phi1);
theta2_deg = rad2deg(theta2);
phi2_deg = rad2deg(phi2);

% 打印结果
disp('飞机1的俯仰角和方位角：');
fprintf('俯仰角：%.2f°\n', theta1_deg);
fprintf('方位角：%.2f°\n', phi1_deg);

disp('飞机2的俯仰角和方位角：');
fprintf('俯仰角：%.2f°\n', theta2_deg);
fprintf('方位角：%.2f°\n', phi2_deg);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算目标的位置
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的长度公式输入有误需要修正
% AM = AB * (tan(deg2rad(b_deg)) / (tan(deg2rad(a_deg)) + tan(deg2rad(b_deg)))) * cos(deg2rad(a_deg));
 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)];

% 显示结果
disp(['AM的长度为：', num2str(AM)]);
disp('目标相对于飞机1的位置：');
% disp(['X向量： [', num2str(X'), ']']);
disp(['X向量： [', num2str(X','%0.15f '), ']']);

figure;
set(gcf,'color','white');%白色