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

三点法

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

函数作用

输入与知点(锚点)的距离、锚点坐标、权重,输出待定位点的坐标。

程序源码

matlab 复制代码
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

待完成

没有对输入的维度(锚点数量)做自适应,后续做了自适应以后上传新程序的链接

相关推荐
爱吃土豆的马铃薯ㅤㅤㅤㅤㅤㅤㅤㅤㅤ5 分钟前
如何使用Java WebSocket API实现客户端和服务器端的通信?
java·开发语言·websocket
Shartin11 分钟前
Can201-Introduction to Networking: Application Layer应用层
服务器·开发语言·php
共享家95271 小时前
linux_线程概念
linux·开发语言·jvm
apihz1 小时前
VM虚拟机全版本网盘+免费本地网络穿透端口映射实时同步动态家庭IP教程
android·服务器·开发语言·网络·数据库·网络协议·tcp/ip
tanyongxi662 小时前
C++ Map 和 Set 详解:从原理到实战应用
开发语言·c++
飒飒真编程2 小时前
C++类模板继承部分知识及测试代码
开发语言·c++·算法
博睿谷IT99_3 小时前
华为数据通信网络基础
开发语言·华为·php·华为认证
蓝桉(努力版)3 小时前
MATLAB可视化5:华夫图(饼图的平替可以表示种类的分布,附有完整代码详细讲解)(求个关注、点赞和收藏)(对配色不满意可以自己调节配色,附调色教程)
开发语言·数学建模·matlab·信息可视化·matlab可视化
艾莉丝努力练剑4 小时前
【C语言】学习过程教训与经验杂谈:思想准备、知识回顾(五)
c语言·开发语言·数据结构·学习·算法
云空4 小时前
《QtPy:Python与Qt的完美桥梁》
开发语言·python·qt·pyqt