Demo entry 6035043

hi

   

Submitted by anonymous on Sep 20, 2016 at 00:30
Language: Matlab. Code size: 892 Bytes.

function pos = posLSaux(prevSortedDist, sortedBS_POS)

global DIMENSION;

C_SPEED = 3e8;
nBS_TDoA = length(prevSortedDist);
dimenstion = DIMENSION; %input(3,1); % 2

shortestPOS = sortedBS_POS(1, 1:dimenstion);
shortestDist = prevSortedDist(1);

BS_TDOA_NUM = nBS_TDoA - 1;
BS_TDOA_POS = sortedBS_POS(2:end, 1:dimenstion);

D = repmat(shortestPOS, [BS_TDOA_NUM, 1]) - BS_TDOA_POS;

% ki = xi^2 + yi^2 + zi^2
% P1 = k1 - ki
tmp = (BS_TDOA_POS.^2)';
P1 =  sum(shortestPOS.^2) - sum(tmp)';

% P2 = delta i1: BSi to shortestBS
P2 = (prevSortedDist(2:end) - shortestDist)';

% D[x y z]' = 1/2{P1 + P2^2 + 2*shortestDist*P2}
P = 0.5 * (P1 + P2.^2 + 2 * shortestDist * P2);

% weight = I or diag(Cti) or diag(C(ti - t1));
%W = eye(BS_TDOA_NUM);
%W = diag(1./abs(P2.^2));
W = diag(1./((C_SPEED * prevSortedDist(2:BS_TDOA_NUM+1)).^2));

pos = pinv(D'*W*D)*D'*W*P;

This snippet took 0.00 seconds to highlight.

Back to the Entry List or Home.

Delete this entry (admin only).