-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathTDOA_SUM_MPR.m
94 lines (83 loc) · 3.28 KB
/
TDOA_SUM_MPR.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
function [ varargout ] = TDOA_SUM_MPR( senPos, rd, varargin )
% [ varargout ] = TDOA_SUM_MPR( senPos, rd, varargin )
%
% Estimation of of the source position by MPR using successive unconstrained
% minimization, s1 not necessarily in the origin.
%
% Input:
% senPos: (Dim x M), postions of reciveing sensors, each column is a sensor position
% and the first column is the reference sensor location for TDOA.
% rd: ((M-1)x1), TDOA measurement vector.
% Q: ((M-1)x(M-1)), covariance matrix of TDOAs.
%
% Output:
% varargout: containing
% **Dim == 2:
% theta: (1x2), DOA estimation, including 1st & 2nd stage.
% g: (1x2), inverse-range (g) estimation, including 1st & 2nd stage.
% pos: (1x2), source position, only 2nd stage.
% **Dim == 3:
% theta: (1x2), azimuth estimation, including 1st & 2nd stage.
% phi: (1x2), elevation estimation, including 1st & 2nd stage.
% g: (1x2), inverse-range (g) estimation, including 1st & 2nd stage.
% pos: (1x3), source position, only 2nd stage.
%
% Reference: Y. Sun, K. C. Ho, G. Wang. J. Chen, Y. Yang, L. Chen, and Q. Wan,
% "Computationally attractive and location robust estimator for IoT device positioning,"
% IEEE Internet Things J., Nov. 2021.
%
% Yimao Sun and K. C. Ho 04-08-2022
%
% Copyright (C) 2022
% Computational Intelligence Signal Processing Laboratory
% University of Missouri
% Columbia, MO 65211, USA.
% hod@missouri.edu
%
[N,M] = size(senPos);
Qr = varargin{1};
Qs = 0;
if length(varargin) == 2
Qs = varargin{2};
end
h1 = -rd;
l = sqrt(sum((senPos(:,2:end) - senPos(:,1)).^2, 1))';
s_bar = (senPos(:,2:end)- senPos(:,1))./l';
% first stage
G1 = [(senPos(:,2:end)-senPos(:,1))', 0.5*(rd.^2-sum((senPos(:,2:end)-senPos(:,1))'.^2,2))];
Phi1 = (G1'/Qr*G1)\(G1')/Qr*h1; % initial estimation
b = 1 + l.^2*Phi1(end)^2 - 2*Phi1(end)*l.*(s_bar'*Phi1(1:N));
B1 = -diag(sqrt(b));
for i = 1:M-1
T(i,(1:N)+N*(i-1)) = (Phi1(1:N) - (senPos(:,i+1)-senPos(:,1))*Phi1(end))';
end
C1 = -T*kron([-ones(M-1,1),eye(M-1)],eye(N));
W1_inv = B1*Qr*B1 + C1*Qs*C1';
Phi20 = ((G1'/W1_inv*G1)\G1'/W1_inv*h1); % solution of 1st stage
Phi2 = sign(real(Phi20)).*abs(Phi20); % to keep it real
% second stage
B2 = diag([2*Phi2(1:N);1]);
h2 = [Phi2(1:N-1).^2;Phi2(N)^2-1;Phi2(end)];
G2 = [eye(N-1),zeros(N-1,1);-ones(1,N-1),0;zeros(1,N-1),1];
W2 = B2\(G1'/W1_inv*G1)/B2;
Psi0 = ((G2'*W2*G2)\(G2')*W2*h2); % solution of 2nd stage
Psi = sign(real(Psi0)).*abs(Psi0); % to keep it real
if N == 2
theta = atan2( abs(sqrt(1-Psi(1)))*sign(Phi2(2)), abs(sqrt(Psi(1)))*sign(Phi2(1)) );
g = Psi(2);
pos = [cos(theta);sin(theta)]/g + senPos(:,1);
varargout{1} = theta;
varargout{2} = g;
varargout{3} = pos;
elseif N == 3
theta = atan2( abs(sqrt(Psi(2)))*sign(Phi2(2)), abs(sqrt(Psi(1)))*sign(Phi2(1)) );
phi = atan2( abs(sqrt(1-sum(Psi(1:2))))*sign(Phi2(3)), abs(sqrt(sum(Psi(1:2)))) );
g = Psi(end);
pos = [cos(theta)*cos(phi);sin(theta)*cos(phi);sin(phi)]/g + senPos(:,1);
varargout{1} = theta;
varargout{2} = phi;
varargout{3} = g;
varargout{4} = pos;
else
error('Please check your input format of sensor positions');
end