【定位问题】基于matlab三维chan算法求解室内定位问题【含Matlab源码 580期】
作者:互联网
一、简介
Chan 算法是 TDOA 定位方法的一个很赞的 trick。但是很多方法一旦从学术的角度去看,就罩上了奇异的光环。TDOA,the time differnces of arrival,到达时间差。
Chan 算法1是非递归双曲线方程组解法,具有解析表达式解。其主要的特点为在测量误差服从理想高斯分布时,它的定位精度高、计算量小,并且可以通过增加基站数量来提高算法精度。该算法的推导的前提是基于测量误差为零均值高斯随机变量,对于实际环境中误差较大的测量值,比如在有非视距误差的环境下,该算法的性能会有显著下降。Chan 算法在考虑二维的情况下,可分为 只有三个 BS 参与定位 和 三个以上 BS 定位 两种。
二、源代码
function X = ChanAlgorithm(BSN, MSP, Radius, Noise)
%CHANALGORITHM 本函数用于实现无线定位中的CHAN算法
% - BSN 为基站个数,3 < BSN <= 7;
% - MSP 为移动台的初始位置, MSx, MSy均为[0,1]之间的数;
% 特别要注意服务小区与MS之间的关系,MS的位置不能越界。
% - Noise 测距误差方差。
% - R 为小区半径,单位(meter);
% - X 为移动台经算法处理后的位置.
%See also: ChanAlgorithm.m
% 参数检查:
if nargout>1,
error('Too many output arguments.');
end
if nargin<2 | nargin>4,
error('Wrong number of input arguments.');
end
% 算法开始:
BS = Radius*NetworkTop(BSN);
MS = Radius*MSP;
% 噪声功率:
Q = eye(BSN-1);
% 第一次LS:
% Ri
K1 = 0;
for i = 1: BSN,
R0(i) = sqrt((BS(1,i) - MS(1))^2 + (BS(2,i) - MS(2))^2);
end
for i = 1: BSN-1,
R(i) = R0(i+1) - R0(1) + Noise*randn(1);
K(i) = BS(1,i+1)^2 + BS(2,i+1)^2;
end
% Ga
for i = 1: BSN-1,
Ga(i,1) = -BS(1, i+1);
Ga(i,2) = -BS(2, i+1);
Ga(i,3) = -R(i);
end
% h
for i = 1: BSN-1,
h(i) = 0.5*(R(i)^2 - K(i) + K1);
end
% 由(14b)给出B的估计值:
Za0 = inv(Ga'*inv(Q)*Ga)*Ga'*inv(Q)*h';
% 利用这个粗略估计值计算B:
B = eye(BSN-1);
for i = 1: BSN-1,
B(i,i) = sqrt((BS(1,i+1) - Za0(1))^2 + (BS(2,i+1) - Za0(2))^2);
end
% FI:
FI = B*Q*B;
% 第一次LS结果:
Za1 = inv(Ga'*inv(FI)*Ga)*Ga'*inv(FI)*h';
if Za1(3) < 0,
Za1(3) = abs(Za1(3));
% Za1(3) = 0;
end
%***************************************************************
% 第二次LS:
% 第一次LS结果的协方差:
CovZa = inv(Ga'*inv(FI)*Ga);
% sB:
sB = eye(3);
for i = 1: 3,
sB(i,i) = Za1(i);
end
% sFI:
sFI = 4*sB*CovZa*sB;
% sGa:
sGa = [1, 0; 0, 1; 1, 1];
% sh
sh = [Za1(1)^2; Za1(2)^2; Za1(3)^2];
% 第二次LS结果:
Za2 = inv(sGa'*inv(sFI)*sGa)*sGa'*inv(sFI)*sh;
% Za = sqrt(abs(Za2));
Za = sqrt(Za2);
% 输出:
% if Za1(1) < 0,
% out1 = -Za(1);
% else
% out1 = Za(1);
% end
% if Za2(1) < 0,
% out2 = -Za(2);
% else
% out2 = Za(2);
% end
三、运行结果
四、备注
版本:2014a
完整代码或代写加1564658423
标签:end,580,定位问题,inv,BSN,源码,Ga,BS,Za1 来源: https://www.cnblogs.com/homeofmatlab/p/14932914.html