-
Notifications
You must be signed in to change notification settings - Fork 1
/
AOA3DLocMPR_CovEV.m
79 lines (65 loc) · 2.45 KB
/
AOA3DLocMPR_CovEV.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
function [ Cov ] = AOA3DLocMPR_CovEV( srcLoc, senPos, Qa, Qs )
% [ Cov ] = AOA3DLocMPR_CovEV( srcLoc, senPos, Qa, Qs )
%
% Evaluate the theoretical covariance matrix of the MPR source location
% estimate by the EV method using AOA measurements
%
% Input:
% srcLoc: (3 x 1), source location in Cartesian (localization dimension = 3)
% senPos: (3 x M), sensor location (M = number of sensors)
% Qa: (2M x 2M), AOA covariance matrix
% Qs: (3M x 3M), sensor position covariance matrix
%
% Output:
% Cov: (3 x 3), covariance matrix of source location estimate in MPR by EV
%
% 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 03-28-2021
%
% Copyright (C) 2020
% Computational Intelligence Signal Processing Laboratory
% University of Missouri
% Columbia, MO 65211, USA
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[N,M] = size(senPos);
go = 1/norm(srcLoc);
u0 = srcLoc/norm(srcLoc);
theta = atan2(u0(2),u0(1));
phi = atan2(u0(3),sqrt(u0(1)^2+u0(2)^2));
thetaM = atan2(srcLoc(2)-senPos(2,:),srcLoc(1)-senPos(1,:))';
phiM = atan2(srcLoc(3)-senPos(3,:),sqrt(sum((srcLoc(1:2)-senPos(1:2,:)).^2,1)))';
psi0 = [u0;go];
S = diag([ones(N,1);0]);
GA1 = [sin(thetaM),-cos(thetaM),zeros(M,1)];
Ga1 = -diag(GA1*(senPos));
GA2 = [cos(thetaM).*sin(phiM),sin(thetaM).*sin(phiM),-cos(phiM)];
Ga2 = -diag(GA2*(senPos));
G1 = [GA1,Ga1;GA2,Ga2];
b1 = sqrt(sum((u0(1:2)-go*(senPos(1:2,:))).^2,1))';
b2 = sqrt(sum((u0-go*(senPos)).^2,1))';
Ba = diag([b1;b2]);
for m = 1:M
thetaTmp = atan2(u0(2)-go*senPos(2,m), u0(1)-go*senPos(1,m));
phiTmp = atan2(u0(3)-go*senPos(3,m), norm(u0(1:2)-go*senPos(1:2,m),2));
alpha1 = [sin(thetaTmp);-cos(thetaTmp);0];
C1(m,(1:N)+(m-1)*N) = -alpha1'*go;
alpha2 = [cos(thetaTmp)*sin(phiTmp);sin(thetaTmp)*sin(phiTmp);-cos(phiTmp)];
C2(m,(1:N)+(m-1)*N) = -alpha2'*go;
end
Ca = [C1;C2];
W1 = inv(Ba*Qa*Ba' + Ca*Qs*Ca');
J = G1'*W1*G1;
kappa = S*psi0/norm(S*psi0);
U = null(kappa');
C = U/(U'*J*U)*U';
D1 = [-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];
Cov = D1*C*D1';
end