【MATLAB函数】三点法定位+权重

MATLAB卡尔曼

于 2024-03-13 20:30:50 发布

阅读量741

点赞数 16

分类专栏：定位与导航文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/callmeup/article/details/136690584

版权

定位与导航专栏收录该内容

11 篇文章 0 订阅

订阅专栏

本文介绍了如何通过最小二乘法和权重调整，利用三点法在UWB、WIFI和Zigbee的定位中计算待定位点的坐标。提供了函数源码，展示了如何根据锚点距离、坐标和权重进行坐标求解，但未提及输入维度的自适应处理。后续版本将包括此改进。

摘要由CSDN通过智能技术生成

三点法

通过测距，使用最小二乘法定位，用于UWB、WIFI、zigbee的定位等。

函数作用

输入与知点（锚点）的距离、锚点坐标、权重，输出待定位点的坐标。

程序源码

function [p_out] = triposition_weight(R_calcu,baseP,varargin)
% p = [5,5;10,9;15,12;20,22;25,3]; %real location
if size(varargin,1) == 0
    W = 0.5*eye(3);
else
    W = cell2mat(varargin);
end
[baseX_,baseY_,baseZ_] = deal(baseP(:,1),baseP(:,2),baseP(:,3));
% baseY_ = baseP(:,2);
% baseZ_ = baseP(:,3);
H = [
    baseX_(2)-baseX_(1),baseY_(2)-baseY_(1);
    baseX_(3)-baseX_(1),baseY_(3)-baseY_(1);
    baseX_(4)-baseX_(1),baseY_(4)-baseY_(1)];
for i=1
    % HX=a
    a = 0.5*[
        baseX_(2).^2+baseY_(2).^2-R_calcu(i,2).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(i,1).^2;
        baseX_(3).^2+baseY_(3).^2-R_calcu(i,3).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(i,1).^2;
        baseX_(4).^2+baseY_(4).^2-R_calcu(i,4).^2-baseX_(1).^2-baseY_(1).^2+R_calcu(i,1).^2];
    %     p_out(i,:) = (pinv(H)*a)'; %伪逆求法
%         (H'*H)^(-1)*H'*a;
        X(:,i) = (H'*W*H)^(-1)*H'*W*a; %左逆求法

end
p_out = X';
end