-
Notifications
You must be signed in to change notification settings - Fork 1
/
AOA3DLocMPR_CCRLB.m
85 lines (77 loc) · 2.73 KB
/
AOA3DLocMPR_CCRLB.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
function [ CRLB ] = AOA3DLocMPR_CCRLB( srcLoc, senPos, varargin )
% [ CRLB ] = AOA3DLocMPR_CCRLB( srcLoc,senPos,Qa,Qs )
%
% Evaluate the CRLB for AOA localization in MPR using the Constrained CRLB
% formulation
%
% Input:
% srcLoc: (N x 1), source location (N = localization dimension)
% senPos: (N x M), sensor location (M = number of sensors)
% varargin:
% Qa: (2M x 2M), AOA covariance matrix
% Qs: (MN x MN), sensor position covariance matrix
%
% Output:
% CRLB: CRLB matrix
%
% Reference:
% Y. Sun, K. C. Ho, and Q. Wan, "Eigenspace solution for AOA localization
% in modified polar representation," IEEE Trans. Signal Process.,
% vol. 68, pp. 2256-2271, 2020.
%
% Yimao Sun, K. C. Ho 06-28-2020
%
% Copyright (C) 2020
% Computational Intelligence Signal Processing Laboratory
% University of Missouri
% Columbia, MO 65211, USA
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,M] = size(senPos);
thetaM = atan2(srcLoc(2)-senPos(2,:),srcLoc(1)-senPos(1,:))';
d = sqrt(sum((srcLoc(1:2)-senPos(1:2,:)).^2,1))';
r = sqrt(sum((srcLoc-senPos).^2,1))';
r0 = norm(srcLoc);
DrAng = zeros(2*M,N+1);
for i = 1:M
DrAng(i,:) = [-r0/d(i)^2*(srcLoc(2)-senPos(2,i)), ...
r0/d(i)^2*(srcLoc(1)-senPos(1,i)), ...
0, ...
r0/d(i)^2*(senPos(1,i)*(srcLoc(2))-srcLoc(1)*(senPos(2,i)))
];
DrAng(i+M,:) = [-r0/r(i)^2*(srcLoc(3)-senPos(3,i))*cos(thetaM(i)), ...
-r0/r(i)^2*(srcLoc(3)-senPos(3,i))*sin(thetaM(i)), ...
r0*d(i)/r(i)^2, ...
-r0/r(i)^2*(senPos(3,i))*d(i) + ...
r0/r(i)^2*(srcLoc(3)-senPos(3,i)) * ((senPos(1,i))*cos(thetaM(i))+(senPos(2,i))*sin(thetaM(i)))
];
end
L = length(varargin);
if L == 1
Q = varargin{1};
elseif L == 2
phiM = atan2(srcLoc(3)-senPos(3,:),sqrt(sum((srcLoc(1:2)-senPos(1:2,:)).^2,1)))';
Ba = diag([d;r])/r0;
C1 = zeros(M,M*N); C2 = zeros(M,M*N);
for m = 1:M
alpha1 = [sin(thetaM(m));-cos(thetaM(m));0];
C1(m,(1:N)+(m-1)*N) = -alpha1'/r0;
alpha2 = [cos(thetaM(m))*sin(phiM(m));sin(thetaM(m))*sin(phiM(m));-cos(phiM(m))];
C2(m,(1:N)+(m-1)*N) = -alpha2'/r0;
end
Ca = [C1;C2];
Q = varargin{1} + Ba\Ca*varargin{2}*Ca'/Ba;
else
error('Too many input.');
end
FIM = DrAng'/Q*DrAng;
psi = [srcLoc;1]/r0;
F = 2*psi'*diag([ones(1,N),0]);
U = null(F);
theta = atan2(srcLoc(2),srcLoc(1));
phi = atan2(srcLoc(3),norm(srcLoc(1:2),2));
D = [-sin(theta)/cos(phi), cos(theta)/cos(phi), 0, 0;
-cos(theta)*sin(phi), -sin(theta)*sin(phi), cos(phi), 0;
0, 0, 0, 1];
CRLB = D*U/(U'*FIM*U)*U'*D';