【wsn定位問題】基于chan算法求解wsn定位問題
一、簡(jiǎn)介
Chan 算法是 TDOA 定位方法的一個(gè)很贊的 trick。但是很多方法一旦從學(xué)術(shù)的角度去看，就罩上了奇異的光環(huán)。TDOA，the time differnces of arrival，到達(dá)時(shí)間差。
Chan 算法1是非遞歸雙曲線方程組解法，具有解析表達(dá)式解。其主要的特點(diǎn)為在測(cè)量誤差服從理想高斯分布時(shí)，它的定位精度高、計(jì)算量小，并且可以通過增加基站數(shù)量來提高算法精度。該算法的推導(dǎo)的前提是基于測(cè)量誤差為零均值高斯隨機(jī)變量，對(duì)于實(shí)際環(huán)境中誤差較大的測(cè)量值，比如在有非視距誤差的環(huán)境下，該算法的性能會(huì)有顯著下降。Chan 算法在考慮二維的情況下，可分為 只有三個(gè) BS 參與定位 和 三個(gè)以上 BS 定位 兩種。
二、源代碼
function X = ChanAlgorithm(BSN, MSP, Radius, Noise)
%CHANALGORITHM 本函數(shù)用于實(shí)現(xiàn)無(wú)線定位中的CHAN算法
% ? ? ? ? ? ? ? - BSN ?為基站個(gè)數(shù)，3 < BSN <= 7；
% ? ? ? ? ? ? ? - MSP ?為移動(dòng)臺(tái)的初始位置, MSx, MSy均為[0,1]之間的數(shù)；
% ? ? ? ? ? ? ? ? ? ? ?特別要注意服務(wù)小區(qū)與MS之間的關(guān)系，MS的位置不能越界。
% ? ? ? ? ? ? ? - Noise 測(cè)距誤差方差。
% ? ? ? ? ? ? ? - R ? ?為小區(qū)半徑，單位(meter)；
% ? ? ? ? ? ? ? - X ? ?為移動(dòng)臺(tái)經(jīng)算法處理后的位置.
%See also: ChanAlgorithm.m
% ? 參數(shù)檢查：
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的估計(jì)值：
Za0 = inv(Ga'*inv(Q)*Ga)*Ga'*inv(Q)*h';
% 利用這個(gè)粗略估計(jì)值計(jì)算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結(jié)果：
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結(jié)果的協(xié)方差：
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結(jié)果：
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
?
