【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

待完成

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

相关推荐
侃侃_天下4 小时前
最终的信号类
开发语言·c++·算法
echoarts4 小时前
Rayon Rust中的数据并行库入门教程
开发语言·其他·算法·rust
Aomnitrix4 小时前
知识管理新范式——cpolar+Wiki.js打造企业级分布式知识库
开发语言·javascript·分布式
每天回答3个问题5 小时前
UE5C++编译遇到MSB3073
开发语言·c++·ue5
伍哥的传说5 小时前
Vite Plugin PWA – 零配置构建现代渐进式Web应用
开发语言·前端·javascript·web app·pwa·service worker·workbox
小莞尔6 小时前
【51单片机】【protues仿真】 基于51单片机八路抢答器系统
c语言·开发语言·单片机·嵌入式硬件·51单片机
我是菜鸟0713号6 小时前
Qt 中 OPC UA 通讯实战
开发语言·qt
JCBP_6 小时前
QT(4)
开发语言·汇编·c++·qt·算法
Brookty6 小时前
【JavaEE】线程安全-内存可见性、指令全排序
java·开发语言·后端·java-ee·线程安全·内存可见性·指令重排序
百锦再6 小时前
[特殊字符] Python在CentOS系统执行深度指南
开发语言·python·plotly·django·centos·virtualenv·pygame