四机融合定位出图代码

% 定义目标点位置
x_target = 200;
y_target = 200;
%%%%%%%%%%引入电磁误差，表现在测量的位置方面，定义均值和标准差，这里设置为1m的电磁误差
mean_value = 0; % 均值为0
std_dev = 1;    % 标准差为1m
 % 定义误差样本的数量
num_samples = 100;
% % 生成随机高斯分布的电磁误差向量（这里是数组）
% errors1 = mean_value + std_dev * randn(num_samples, 1);
% errors2 = mean_value + std_dev * randn(num_samples, 1);
% 初始飞机位置
x_aircraft1 = 0;
y_aircraft1 = 0;
x_aircraft2 = 0;
y_aircraft2 = 50;

trajectory_target = [];
trajectory_target2 = [];
trajectory_aircraft1 = []; % 初始化空矩阵
 trajectory_aircraft2=[];

% 计算飞机到目标点的距离,在这里模拟电磁误差，加入0随机高斯分布，1m的电磁误差
distance_to_target1 = @(x, y) sqrt((x - x_target)^2 + (y - y_target)^2);
distance_to_target2 = @(x, y) sqrt((x - x_target)^2 + (y - y_target)^2);

% 计算飞机到目标点的角度
angle_to_target1 = @(x, y) atan2(y_target - y, x_target - x);
angle_to_target2 = @(x, y) atan2(y_target - y, x_target - x);
% 模拟飞机的运动和测量
for t = 1:100
    % 更新飞机位置（简单起见，假设飞机匀速运动）
    x_aircraft1 = x_aircraft1 +1 ;
    y_aircraft1 = y_aircraft1 +0.6; % 沿对角线运动，使其有一定的角度
    x_aircraft2 = x_aircraft2 +1;
    y_aircraft2 = y_aircraft2 +0.6;
       x_aircraft3 = x_aircraft2 +1;
    y_aircraft3 = y_aircraft2 +0.6;    
    x_aircraft4 = x_aircraft2 +1;
    y_aircraft4 = y_aircraft2 +0.6;
    %%%%%%%%%%%%%%%%%%%%%%%%%%5
    error_x = mean_value + std_dev * randn;
    error_y = mean_value + std_dev * randn;
    % 记录飞机位置
    trajectory_aircraft1 = [trajectory_aircraft1; x_aircraft1, y_aircraft1];
    trajectory_aircraft2 = [trajectory_aircraft2; x_aircraft2, y_aircraft2];

%     % 计算测量角度，用弧度表示
%     angle1= angle_to_target1(x_aircraft1, y_aircraft1);
%     angle2 = angle_to_target2(x_aircraft2, y_aircraft2);
%  % 计算测量角度，用弧度表示，并加入随机误差
%     angle1 = atan2(y_target - y_aircraft1, x_target - x_aircraft1) + (rand - 0.5) * deg2rad(1);
%     angle2 = atan2(y_target - y_aircraft2, x_target - x_aircraft2) + (rand - 0.5) * deg2rad(1); 
%    angle1 = atan2(y_target +error_y- y_aircraft1, x_target+error_x - x_aircraft1) ;
%     angle2 = atan2(y_target+error_y - y_aircraft2, x_target+error_x - x_aircraft2); 
    angle1 = atan2(y_target - y_aircraft1, x_target - x_aircraft1)+ (rand - 0.5) * deg2rad(1) ;
    angle2 = atan2(y_target - y_aircraft2, x_target - x_aircraft2) ; 
   
   % 计算目标的位置坐标的斜率
    m1 = tan(angle1);
    m2 = tan(angle2);
 
    x_position=(y_aircraft1- y_aircraft2-m1*x_aircraft1+m2*x_aircraft2)/(m2-m1);
    y_position=(y_aircraft1*m2-m1* y_aircraft2-m1*m2*x_aircraft1+m1*m2*x_aircraft2)/(m2-m1);
        

   trajectory_target=[trajectory_target; x_position,y_position];
   
  
    %计算目标的位置坐标
    % 显示结果
    fprintf('时刻 %d:\n', t);
    fprintf('飞机1的位置：(%d, %d)\n', x_aircraft1, y_aircraft1);
    fprintf('飞机2的位置：(%d, %d)\n', x_aircraft2, y_aircraft2);
    fprintf('飞机1测量到的角度：%.10f\n', rad2deg(angle1));
    fprintf('飞机2测量到的角度：%.10f\n\n', rad2deg(angle2));
 fprintf('m1：%.10f\n\n',m1);
      fprintf('m2：%.10f\n\n', m2);
     fprintf('目标X坐标：%.10f\n\n', x_position);
      fprintf('目标Y坐标：%.10f\n\n', y_position);
       fprintf('m2：%.10f\n\n', m2);
end
x_target = 200;
y_target = 200;
% 初始飞机位置
x_aircraft11= 50;
y_aircraft11= 0;
x_aircraft22 = 100;
y_aircraft22 = 50;
trajectory_target2 = [];
trajectory_aircraft11 = []; % 初始化空矩阵
 trajectory_aircraft22=[];
% 计算飞机到目标点的距离
distance_to_target1 = @(x, y) sqrt((x - x_target)^2 + (y - y_target)^2);
distance_to_target2 = @(x, y) sqrt((x - x_target)^2 + (y - y_target)^2);
 
% 计算飞机到目标点的角度
angle_to_target11 = @(x, y) atan2(y_target - y, x_target - x);
angle_to_target22 = @(x, y) atan2(y_target - y, x_target - x);
% 模拟飞机的运动和测量
for t = 1:100
    % 更新飞机位置（简单起见，假设飞机匀速运动）
    x_aircraft11 = x_aircraft11 +1 ;
    y_aircraft11 = y_aircraft11 +0.6; % 沿对角线运动，使其有一定的角度
    x_aircraft22 = x_aircraft22 +1;
    y_aircraft22 = y_aircraft22 +0.6;
    
    % 记录飞机位置
    trajectory_aircraft11 = [trajectory_aircraft11; x_aircraft11, y_aircraft11];
    trajectory_aircraft22 = [trajectory_aircraft22; x_aircraft22, y_aircraft22];
    
%     % 计算测量角度，用弧度表示
%     angle1= angle_to_target1(x_aircraft1, y_aircraft1);
%     angle2 = angle_to_target2(x_aircraft2, y_aircraft2);
   % 计算测量角度，用弧度表示，并加入随机误差
%     angle1 = atan2(y_target - y_aircraft1, x_target - x_aircraft1) + (rand - 0.5) * deg2rad(1);
%     angle2 = atan2(y_target - y_aircraft2, x_target - x_aircraft2) + (rand - 0.5) * deg2rad(1); 
    angle11= atan2(y_target - y_aircraft11, x_target - x_aircraft11) ;
    angle22 = atan2(y_target - y_aircraft22, x_target - x_aircraft22)+ (rand - 0.5) * deg2rad(1); 
   % 计算目标的位置坐标的斜率
    m11 = tan(angle11);
    m22 = tan(angle22);
 
    xx_position=(y_aircraft11- y_aircraft22-m11*x_aircraft11+m22*x_aircraft22)/(m22-m11);
    yy_position=(y_aircraft11*m22-m11* y_aircraft22-m11*m22*x_aircraft11+m11*m22*x_aircraft22)/(m22-m11);
   trajectory_target2=[trajectory_target2; xx_position,yy_position];
    
    %计算目标的位置坐标
    % 显示结果
    fprintf('时刻 %d:\n', t);
    fprintf('飞机1的位置：(%d, %d)\n', x_aircraft11, y_aircraft11);
    fprintf('飞机2的位置：(%d, %d)\n', x_aircraft22, y_aircraft22);
    fprintf('飞机1测量到的角度：%.10f\n', rad2deg(angle11));
    fprintf('飞机2测量到的角度：%.10f\n\n', rad2deg(angle22));
 fprintf('m1：%.10f\n\n',m11);
      fprintf('m2：%.10f\n\n', m22);
     fprintf('目标Xx坐标：%.10f\n\n', xx_position);
      fprintf('目标Yy坐标：%.10f\n\n', yy_position);
end
% 假设所有飞机具有相同的权重，这里简化为等权重情况
% 假设有两架飞机，我们给它们分配权重
w1 = 0.3; % 飞机1的权重
w2 = 0.7; % 飞机2的权重

% 初始化融合后的目标位置矩阵
combined_trajectory = zeros(100, 2); % 假设有100个时间步长

% 遍历所有时间步长
for t = 1:100
    % 获取飞机1和飞机2在时刻t的目标位置
    x1 = trajectory_target(t, 1);
    y1 = trajectory_target(t, 2);
    x2 = trajectory_target2(t, 1);
    y2 = trajectory_target2(t, 2);
    
    % 计算加权平均位置
    combined_x = w1 * x1 + w2 * x2;
    combined_y = w1 * y1 + w2 * y2;
    
    % 将加权平均位置存储在融合后的轨迹矩阵中
    combined_trajectory(t, :) = [combined_x; combined_y];
end

% 显示融合后的目标位置的一个样本
fprintf('在时刻 %d，融合后的目标位置是：(%.10f, %.10f)\n', 1, combined_trajectory(1, 1), combined_trajectory(1, 2));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 假设trajectory_target和trajectory_target2分别存储了飞机1和飞机2的测量结果

% 绘制融合后的目标位置的散点图
figure; % 创建一个新的图形窗口
 set(gcf,'color','white');%白色
 % 假设combined_trajectory矩阵已经被正确计算并填充了融合后的位置数据

% 创建图形窗口
figure;
set(gcf,'color','white');%白色
% 绘制融合后的轨迹，使用红色圆圈标记
plot(combined_trajectory(:, 1), combined_trajectory(:, 2), 'r*', 'MarkerSize', 4,'LineWidth', 2);
% plot(combined_x, combined_y, 'bo', 'MarkerSize', 8,'LineWidth', 2); % 使用蓝色圆圈标记融合后的位置
hold on; % 保持当前图形，以便在同一图形上绘制其他数据
 plot(trajectory_target(:,1), trajectory_target(:,2), 'b.', 'MarkerSize', 6); % 飞机1的测量结果
 plot(trajectory_target2(:,1), trajectory_target2(:,2), 'g.', 'MarkerSize', 6); % 飞机2的测量结果
% 添加图形的标题和轴标签
title('数据融合后的散点图');
xlabel('X坐标');
ylabel('Y坐标');

% 添加图例
legend('融合后位置', '飞机1、2测量结果', '飞机3、4测量结果');

% 显示网格
grid on;
% 可选：设置图形的尺寸和分辨率
set(gcf, 'Position', [100, 100, 800, 600]); % [x, y, width, height]


figure;
set(gcf,'color','white');%白色
plot(trajectory_aircraft1(:, 1), trajectory_aircraft1(:, 2), '-', 'LineWidth', 1.5, 'MarkerSize', 2);
hold on;
plot(trajectory_aircraft2(:, 1), trajectory_aircraft2(:, 2), '-', 'LineWidth', 1.5, 'MarkerSize', 2);
plot(trajectory_aircraft11(:, 1), trajectory_aircraft11(:, 2), '-', 'LineWidth', 1.5, 'MarkerSize', 2);
hold on;
plot(trajectory_aircraft22(:, 1), trajectory_aircraft22(:, 2), '-', 'LineWidth', 1.5, 'MarkerSize', 2);
plot(x_target, y_target, 'rx', 'MarkerSize', 10, 'LineWidth', 2);
xlabel('X轴');
ylabel('Y轴');
title('飞机运动轨迹和目标点位置');
legend('飞机A', '飞机B','飞机C','飞机D', '目标点');
grid on;
% 固定坐标轴范围
xlim([0, 150]); % X轴范围从0到15
% figure;
% set(gcf,'color','white');%白色
%  % 画出计算出的目标位置点
% plot(trajectory_target(:, 1), trajectory_target(:, 2), 'g.', 'MarkerSize', 6, 'LineWidth', 1.5); % 目标点1，使用绿色圆圈标记
% hold on; % 确保新的点能够添加到已有的图上
% plot(trajectory_target2(:, 1), trajectory_target2(:, 2), 'm.', 'MarkerSize', 6, 'LineWidth', 1.5); % 目标点2，使用洋红色圆圈标记
% 
% % 可选：添加文本标签到目标点上
% text(trajectory_target(:, 1), trajectory_target(:, 2), 'T1', 'FontSize', 10, 'Color', 'g');
% text(trajectory_target2(:, 1), trajectory_target2(:, 2), 'T2', 'FontSize', 10, 'Color', 'm');
% 
% hold off; % 完成所有绘图后，关闭hold on状态
% %  
% x = 200;
% y = 200;
% figure;
% set(gcf,'color','white');%白色
%  
% plot(trajectory_target(:, 1), trajectory_target(:, 2), 'gO', 'LineWidth', 1.7, 'MarkerSize', 1.7);
% hold on;
% plot(x, y, 'rx', 'MarkerSize', 10, 'LineWidth', 2);hold on;
% % 固定坐标轴范围
% xlim([0, 150]); % X轴范围从0到15
% ylim([0, 150]); % Y轴范围从0到15
%  
%  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % 时间
% time = 1:100;
% % 目标 X 和 Y 方向位置随时间的变化
% target_x = trajectory_target(:, 1);
% target_y = trajectory_target(:, 2);
% x_e=target_x-200;
% y_e=target_y-200;
% % 假设 time, x_e, y_e 已经定义并包含相应数据
% 
% % 创建一个新的图形窗口
% figure;
% set(gcf, 'color', 'white'); % 设置图形窗口背景为白色
% 
% % 上半部分：绘制X方向位置随时间的变化
% subplot(2, 1, 1); % 创建2行1列的子图布局，并定位到第1个子图
% plot(time, x_e, 'b-', 'LineWidth', 1.5); % 绘制X方向位置
% title('目标 X 方向误差随时间变化');
% xlabel('时间');
% ylabel('目标 X 位置');
% xticks(1:2:100); % 设置x轴刻度为每秒一个刻度
% ylim([-1e-13, 1e-13]); % 设置y轴范围为10的负13次方到10的正13次方
% grid on; % 显示网格
% 
% % 下半部分：绘制Y方向位置随时间的变化
% subplot(2, 1, 2); % 定位到第2个子图
% plot(time, y_e, 'r-', 'LineWidth', 1.5); % 绘制Y方向位置
% title('目标 Y 方向误差随时间变化');
% xlabel('时间');
% ylabel('目标 Y 位置');
% xticks(1:2:100); % 设置x轴刻度为每秒一个刻度
% ylim([-1e-13, 1e-13]); % 设置y轴范围为10的负13次方到10的正13次方
% % ylim([-20, 20]); % 设置y轴范围为10的负13次方到10的正13次方
% grid on; % 显示网格
% 
% % 调整子图间距
% subplot(2, 1, 1);
% set(gca, 'Position', [0.08 0.58 0.88 0.35]); % 上图位置和大小
% subplot(2, 1, 2);
% set(gca, 'Position', [0.08 0.07 0.88 0.35]); % 下图位置和大小
% % % 画出 X 方向位置随时间的折线图
% figure;
% set(gcf,'color','white');%白色
%  
% plot(time, x_e, 'b-', 'LineWidth', 1.5);
% title('目标 X 方向位置随时间变化');
% xlabel('时间');
% ylabel('目标 X 位置');
%  xticks(1:2:100); % 设置 x 轴刻度为每秒一个刻度
% ylim([-1e-13, 1e-13]); % 设置 y 轴范围为 10 的负 13 次方到 10grid on;
%  
% % 画出 Y 方向位置随时间的折线图
% figure;
% set(gcf,'color','white');%白色
% plot(time, y_e, 'r-', 'LineWidth', 1.5);
% 
% title('目标 Y 方向位置随时间变化');
% xlabel('时间');
% ylabel('目标 Y 位置');
%  xticks(1:2:100); % 设置 x 轴刻度为每秒一个刻度
%  ylim([-1e-13, 1e-13]); % 设置 y 轴范围为 10 的负 13 次方到 10grid on;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% grid on;% 定义误差半径
% radius = 1; % 这是一个示例值，你可以根据实际情况调整
% 
% % 计算目标实际位置的圆概率误差
% mean_target_position = mean(trajectory_target); % 目标位置均值
% var_target_position = var(trajectory_target);   % 目标位置方差
% 
% % 计算误差范围
% error_radius = sqrt(var_target_position(1)^2 + var_target_position(2)^2);
% 
% % 绘制目标实际位置的圆概率误差
% figure;
% hold on;
% plot(trajectory_target(:, 1), trajectory_target(:, 2), 'gO', 'LineWidth', 1.7, 'MarkerSize', 1.7);
% plot(mean_target_position(1), mean_target_position(2), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
% circle(mean_target_position(1), mean_target_position(2), error_radius + radius, 'r'); % 绘制误差范围圆
% xlabel('X 位置');
% ylabel('Y 位置');
% title('目标实际位置及其圆概率误差');
% legend('目标实际位置', '均值', '圆概率误差');
% grid on;
% hold off;