|||
原理图片:
源码:
%**********************************************************************
%功能: 利用Bancroft算法实现2维和3维定位结果及测量偏差
%作者:何成文(中科院测地所)20180520
%参考文献:
% [1].一种全球定位系统接收机单点定位的新算法及数值试验[J].
%参数说明:
% L:基站坐标-m基站*n坐标
% d:对应基站的距离观测值-列向量
% m:2维定位 or 3维定位
% P:未知点解算结果-列向量
% b:观测值偏差-列向量
% 代码调试成功!20180520
%**********************************************************************
function [P,b]=Bancroft_cwhe(L,d,m)
N=length(L(:,1)); %基站数目!
if m==2
h=zeros(3,1);
M=diag([1 1 -1]);
L=L(:,1:2);
B=[L d]; %参考文献[1]中公式(8)的实现!
tau=ones(N,1); %参考文献[1]中公式(8)的实现!
aerfa=zeros(N,1); %参考文献[1]中公式(8)的实现!
for i=1:N
aerfa(i,1)=L(i,1)*L(i,1)+L(i,2)*L(i,2)-d(i,1)*d(i,1);
end
aerfa=aerfa/2;
wuyon=inv(B'*B)*B'; %参考文献[1]中公式(12)的实现!
A=lorentz_bancro(wuyon*tau,wuyon*tau); %参考文献[1]中公式(12)的实现!
wuyon2=lorentz_bancro(wuyon*tau,wuyon*aerfa);
BB=2*[wuyon2-1]; %参考文献[1]中公式(12)的实现!
C=lorentz_bancro(wuyon*aerfa,wuyon*tau); %参考文献[1]中公式(12)的实现!
Nmuda1=[-BB+sqrt(BB^2-4*A*C)]/(2*A);
Nmuda2=[-BB-sqrt(BB^2-4*A*C)]/(2*A);
Nmuda=max([Nmuda1 Nmuda2]);
h=M*inv(B'*B)*(Nmuda1*B'*tau+B'*aerfa);%获取定位结果和偏差
P=[h(1) h(2)]';
b=h(3);
elseif m==3 %三维定位!
h=zeros(4,1);
M=diag([1 1 1 -1]);
L=L(:,1:3);
B=[L d]; %参考文献[1]中公式(8)的实现!
tau=ones(N,1); %参考文献[1]中公式(8)的实现!
aerfa=zeros(N,1); %参考文献[1]中公式(8)的实现!
for i=1:N
aerfa(i,1)=L(i,1)*L(i,1)+L(i,2)*L(i,2)-d(i,1)*d(i,1);
end
aerfa=aerfa/2;
wuyon=inv(B'*B)*B'; %参考文献[1]中公式(12)的实现!
A=lorentz_bancro(wuyon*tau,wuyon*tau); %参考文献[1]中公式(12)的实现!
BB=2*[lorentz_bancro(wuyon*tau,wuyon*aerfa)-1];%参考文献[1]中公式(12)的实现!
C=lorentz_bancro(wuyon*aerfa,wuyon*tau); %参考文献[1]中公式(12)的实现!
Nmuda1=[-BB+sqrt(BB^2-4*A*C)]/(2*A);
Nmuda2=[-BB-sqrt(BB^2-4*A*C)]/(2*A);
Nmuda=max([Nmuda1 Nmuda2]);
h=M*inv(B'*B)*(Nmuda*B'*tau+B'*aerfa); %获取定位结果和偏差
P=[h(1) h(2) h(3)]';
b=h(4);
end
%*******************************************代码调试成功!20180520
结论:应用于UWB室内定位技术方面,精度不高!
PDF文档下载链接:026-Bancroft算法原理20180520.pdf
Archiver|手机版|科学网 ( 京ICP备07017567号-12 )
GMT+8, 2024-10-19 21:57
Powered by ScienceNet.cn
Copyright © 2007- 中国科学报社