三点法
通过测距,使用最小二乘法定位,用于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
待完成
没有对输入的维度(锚点数量)做自适应,后续做了自适应以后上传新程序的链接