最小二乘求待定位点的位置（三维环境）|MATLAB

news2024/9/21 10:48:11

前言

之前发过三点法求待测点位置的程序讲解，哪个是二维的，见：基于伪逆的三点法距离求位置，MATLAB源代码（MATLAB函数）

这里给出三维情况下的函数和测试代码。对于函数，输入已知锚点的位置、待测点与各个锚点的距离，即可获得待测点的坐标。

运行结果

运行程序，可得到绘制的三维空间图如下：
请添加图片描述

图中，蓝色的点是待定位点的真实值，黄色点为通过算法求出来的位置。红色的点是已知的锚点。
锚点和待定位点的坐标在程序中可以修改：
在这里插入图片描述
测距误差也可以修改，通过设置标准差来更改距离误差的大小（统计特性）：

运行程序后，MATLAB的命令行窗口也会有如下结果输出：
请添加图片描述

程序源码（部分）

% 最小二乘求三维下的位置
% author:Evand
% 2024-7-19/Ver1
clear;clc;close all;
rng(0);
%% 主程序
point1 = [3,1,1]; %待定位点的坐标设置
baseP = [
    0,1,1;
    2,0,3;
    3,2,4;
    4,0,1]; %设置基站的坐标
r_err = 0.2; %设置测距误差（标准差）
R_calcu = sqrt(diag((point1-baseP)*(point1'-baseP')))+r_err*randn; %计算距离值+测距误差
[p_out] = triposition_3dim(R_calcu,baseP); %计算步骤【调用后面的定位函数】

%% 绘图
% 剩余代码，下载链接见：https://mbd.pub/o/bread/ZpiZmpht
figure;
plot3(point1(1),point1(2),point1(3),'o');