clc;
clear;
close all;
% 实际极坐标数据（有误差）
name = ['FY00'; 'FY01'; 'FY02'; 'FY03'; 'FY04'; 'FY05'; 'FY06'; 'FY07'; 'FY08'; 'FY09'];
S = [
0, 0; % 基站1
100, 0; % 基站2
98, 40.10; % 基站3
112, 80.21; % 未知节点4
105, 119.75; % 未知节点5
98, 159.86; % 未知节点6
112, 199.96; % 未知节点7
105, 240.07; % 未知节点8
98, 280.17; % 未知节点9
112, 320.28; % 未知节点10
];
% 理想极坐标数据（无误差）
S_ideal = [
0, 0;
100, 0;
100, 40;
100, 80;
100, 120;
100, 160;
100, 200;
100, 240;
100, 280;
100, 320;
];
% 转换为直角坐标系
S1 = zeros(size(S, 1), 2);
S1_ideal = zeros(size(S_ideal, 1), 2);
for i = 1:size(S, 1)
S1(i, :) = Zzjzb(S(i, :));
S1_ideal(i, :) = Zzjzb(S_ideal(i, :));
end
% 初始化历史坐标记录
max_iter = 5;
history = zeros(max_iter, size(S1, 1), size(S1, 2));
history(1, :, :) = S1;
% 迭代优化
for iter = 1:max_iter
for k = 4:10 % 对未知节点进行优化
% 使用基站1和基站2进行角度匹配
sender1 = S1(1, :);
sender2 = S1(2, :);
% 获取理想角度
ideal_loc = S1_ideal(k, :);
angle_ideal = [angle(ideal_loc, sender1, sender2), angle(ideal_loc, sender2, sender1)];
% 使用角度匹配算法更新位置
S1(k, :) = location(S1(k, :), sender1, sender2, angle_ideal, S1_ideal(k, :));
end
% 记录当前迭代坐标
history(iter, :, :) = S1;
end
% 显示最终所有飞机的坐标
fprintf('直角坐标 (x, y) 和极坐标 (r, theta)\n');
fprintf('----------------------------------------------\n');
fprintf('Name x y r theta(deg)\n');
fprintf('----------------------------------------------\n');
for i = 1:size(S1, 1)
x = S1(i, 1);
y = S1(i, 2);
[theta, r] = cart2pol(x, y);
theta_deg = rad2deg(theta);
fprintf('%s %.4f %.4f %.4f %.4f\n', name(i, :), x, y, r, theta_deg);
end
% 绘制极坐标图（圆周图）
figure;
polaraxesHandle = polaraxes;
hold on;
for i = 1:size(S1, 1)
[theta, r] = cart2pol(S1(i, 1), S1(i, 2));
polarplot(theta, r, 'o', 'MarkerFaceColor', 'b', 'MarkerEdgeColor', 'k');
text(theta, r, name(i, :), 'HorizontalAlignment', 'right', 'VerticalAlignment', 'middle', 'FontSize', 10);
end
title('All UAVs in Polar Coordinates (Circle Plot)');
hold off;
% 显示所有迭代图像在一张图中
figure;
for iter = 1:max_iter
subplot(2, 3, iter); % 假设最多6次迭代
Huatu(squeeze(history(iter, :, :)), name, ['Iteration ', num2str(iter)]);
end
sgtitle('Position Iteration Process');
% 显示最终误差
for i = 1:10
disp([name(i, :), ' 误差: ', num2str(distance(S1(i, :), S1_ideal(i, :)))]);
end
%% 子函数定义
% 极坐标转直角坐标
function cart = Zzjzb(polar)
r = polar(1);
theta = deg2rad(polar(2));
cart = [r * cos(theta), r * sin(theta)];
end
% 角度计算
function a = angle(ver, tra1, tra2)
b = distance(tra1, ver);
c = distance(tra2, ver);
d = 2 * b * c;
if d == 0
a = 0;
return;
end
cos_a = (b^2 + c^2 - distance(tra1, tra2)^2) / d;
a = rad2deg(acos(max(min(cos_a, 1), -1))); % 防止浮点误差
end
% 成本函数
function cost = cost_sq(angle1, angle2)
cost = sum((angle1 - angle2).^2);
end
% 距离计算
function d = distance(p1, p2)
d = sqrt((p1(1) - p2(1))^2 + (p1(2) - p2(2))^2);
end
% 高级可视化函数
function Huatu(loc, name, titleStr)
x = loc(:, 1);
y = loc(:, 2);
scatter(x, y, 75, 'filled');
for i = 1:size(loc, 1)
text(x(i), y(i), name(i, :), ...
'HorizontalAlignment', 'center', ...
'VerticalAlignment', 'bottom', ...
'FontSize', 10);
end
title(titleStr);
xlabel('X');
ylabel('Y');
grid on;
axis equal;
end
% 用角度确定坐标
function result = location(loc_real, sender1, sender2, angle_real, ideal_loc)
% 生成搜索网格
distance_max = 8;
distance_num = 160;
x = linspace(ideal_loc(1) - distance_max, ideal_loc(1) + distance_max, distance_num);
y = linspace(ideal_loc(2) - distance_max, ideal_loc(2) + distance_max, distance_num);
min_cost = inf;
best_point = loc_real;
% 遍历搜索最优位置
for i = 1:length(x)
for j = 1:length(y)
loc_calculate = [x(i), y(j)];
angle_calculate = [angle(loc_calculate, sender1, sender2), angle(loc_calculate, sender2, sender1)];
cost_result = cost_sq(angle_calculate, angle_real);
if cost_result < min_cost
min_cost = cost_result;
best_point = loc_calculate;
end
end
end
result = best_point;
end
所有绘图删除，仅保留或添加调整前和调整后的对比图
最新发布
设计一个Location类表示直角坐标系的点，创建A（3象限）、B（2象限）两个对象，利用成员和友元函数计算它们间的距离。输出格式包含坐标值及两点间距离。
最低0.47元/天 解锁文章
扫一扫
7516
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
