function [uQ,ut] =  AOARBL3D_SDR(Ain,Cin,thetam,phim,Qn,Loc)
% [uQ,ut] =  AOARBL3D_SDR(ao,c,thetav,phiv,Qn,Loc)
%
% Estimate the rotation matrix and the translation vector for the localization
% of a rigid body in 3-D using the semi-definite relaxation mathod
%
% Input:
%   Ain:    (3 x M), anchor positions in global coord, M = number of anchors
%   Cin     (3 x N), sensor positons in local coord., N = number of sensors
%   thetam: (N x M), azimuths of AOA measurements
%   phim:   (N x M), elevations of AOA measurements
%   Qn:     (2MN x 2MN), AOA covariance matrix
%   Loc:    scalar, Self-Localization (Loc=0), Localization(Loc=1)
%
% Output:
%   uQ:     (3 x 3), rotation matrix estimate
%   ut:     (3 x 1), translation vector estimate
%
% Reference:
% Y. Wang, G. Wang, S. Chen, K. C. Ho and L. Huang, "An investigation and
% solution of angle based rigid body localization," IEEE Trans. Signal
% Process., vol. 68, pp. 5457-5472, 2020.
%
% Yue Wang, K. C. Ho   07-18-2021
%
%       Copyright (C) 2020
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA
%       hod@missouri.edu
%

if Loc==0
    a=Ain; co=Cin; Cov=Qn;
else
    co=Ain; a=Cin; Cov=Qn;
end

K=size(co,1);    % localization dimension
N=size(co,2);   % number of sensors
M=size(a,2);  % number of anchors

c=co-co(:,1)*ones(1,N);
h=zeros(2*M*N,1);
for j=1:N
    for i=1:M
        gt(:,M*(j-1)+i)=[sin(thetam(M*(j-1)+i)) -cos(thetam(M*(j-1)+i)) 0];
        gp(:,M*(j-1)+i)=[cos(thetam(M*(j-1)+i))*sin(phim(M*(j-1)+i)) sin(thetam(M*(j-1)+i))*sin(phim(M*(j-1)+i)) -cos(phim(M*(j-1)+i))];
        zti(M*(j-1)+i,:)=[zeros(1,K*(i-1)),gt(:,M*(j-1)+i)',zeros(1,K*(M-i))];
        zpi(M*(j-1)+i,:)=[zeros(1,K*(i-1)),gp(:,M*(j-1)+i)',zeros(1,K*(M-i))];
        h(M*(j-1)+i)=zti(M*(j-1)+i,:)*reshape(c(:,j)*ones(1,M),K*M,1);
        h(M*N+M*(j-1)+i)=zpi(M*(j-1)+i,:)*reshape(c(:,j)*ones(1,M),K*M,1);
    end
end
Z=[zti;zpi];
n1=12;
M1=zeros(n1,n1);
M2=zeros(n1,n1);
M3=zeros(n1,n1);
M4=zeros(n1,n1);
M5=zeros(n1,n1);
M6=zeros(n1,n1);
M1(1,1)=1;M1(2,2)=1;M1(3,3)=1;
M2(4,4)=1;M2(5,5)=1;M2(6,6)=1;
M3(7,7)=1;M3(8,8)=1;M3(9,9)=1;
M4(1,4)=1;M4(2,5)=1;M4(3,6)=1;M4(4,1)=1;M4(5,2)=1;M4(6,3)=1;
M5(1,7)=1;M5(2,8)=1;M5(3,9)=1;M5(7,1)=1;M5(8,2)=1;M5(9,3)=1;
M6(4,7)=1;M6(5,8)=1;M6(6,9)=1;M6(7,4)=1;M6(8,5)=1;M6(9,6)=1;
M7=zeros(n1,n1);
M8=zeros(n1,n1);
M9=zeros(n1,n1);
M10=zeros(n1,n1);
M11=zeros(n1,n1);
M12=zeros(n1,n1);
M7(1,1)=1;M7(4,4)=1;M7(7,7)=1;
M8(2,2)=1;M8(5,5)=1;M8(8,8)=1;
M9(3,3)=1;M9(6,6)=1;M9(9,9)=1;
M10(1,2)=1;M10(4,5)=1;M10(7,8)=1;M10(2,1)=1;M10(5,4)=1;M10(8,7)=1;
M11(1,3)=1;M11(4,6)=1;M11(7,9)=1;M11(3,1)=1;M11(6,4)=1;M11(9,7)=1;
M12(2,3)=1;M12(5,6)=1;M12(8,9)=1;M12(3,2)=1;M12(6,5)=1;M12(9,8)=1;
F=[];
for i=1:M
    F=[F;kron(a(:,i)',eye(K)),eye(K)];
end

cvx_solver sdpt3
cvx_begin quiet;
cvx_precision high;
variable V1(n1,n1) symmetric;
variable v1(n1,1);
variable err;
minimize(err);
subject to
trace(F'*Z'*Z*F*V1)-2*h'*Z*F*v1+h'*h<=err;
1-trace(M1*V1)==0;
1-trace(M2*V1)==0;
1-trace(M3*V1)==0;
trace(M4*V1)==0;
trace(M5*V1)==0;
trace(M6*V1)==0;
1-trace(M7*V1)==0;
1-trace(M8*V1)==0;
1-trace(M9*V1)==0;
trace(M10*V1)==0;
trace(M11*V1)==0;
trace(M12*V1)==0;
[1 v1'; v1 V1]==hermitian_semidefinite(n1+1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end))*sqrt(D(IX(end),IX(end)));
u2=-u1;
detR1=det(reshape(u1(1:9),3,3));
if detR1>0
    u=u1;
else
    u=u2;
end
if N>1
    u=v1;
end

B=zeros(2*M*N);
for j=1:N
    for i=1:M
        btn(M*(j-1)+i)=[cos(thetam(M*(j-1)+i)),sin(thetam(M*(j-1)+i)),0]*F(3*(i-1)+1:3*i,:)*(u-[zeros(9,1);c(:,j)]);
        bpw(M*(j-1)+i)=[cos(thetam(M*(j-1)+i))*cos(phim(M*(j-1)+i)),sin(thetam(M*(j-1)+i))*cos(phim(M*(j-1)+i)),sin(phim(M*(j-1)+i))]*F(3*(i-1)+1:3*i,:)*(u-[zeros(9,1);c(:,j)]);
    end
end
btw=zeros(M*N,1);
bpn=zeros(M*N,1);
B=[diag(btn),diag(btw);diag(bpn),diag(bpw)];
P1=(B*Qn*B');
W1=inv(P1);
P=(B*Qn*B');
W=pinv(size(P,1)*P);
W = 10*W/max(max(W));

clear V1 v1 err;
cvx_solver sdpt3
cvx_begin quiet;
cvx_precision high;
variable V1(n1,n1) symmetric;
variable v1(n1,1);
variable err;
minimize(err);
subject to
trace(F'*Z'*W*Z*F*V1)-2*h'*W*Z*F*v1+h'*W*h<=err;
1-trace(M1*V1)==0;
1-trace(M2*V1)==0;
1-trace(M3*V1)==0;
trace(M4*V1)==0;
trace(M5*V1)==0;
trace(M6*V1)==0;
1-trace(M7*V1)==0;
1-trace(M8*V1)==0;
1-trace(M9*V1)==0;
trace(M10*V1)==0;
trace(M11*V1)==0;
trace(M12*V1)==0;
[1 v1'; v1 V1]==hermitian_semidefinite(n1+1);
cvx_end;
[V,D]=eig(V1);
[d,IX]=sort(sum(D));
u1=V(:,IX(end))*sqrt(D(IX(end),IX(end)));
u2=-u1;
detR1=det(reshape(u1(1:9),3,3));
if detR1>0
    u=u1;
else
    u=u2;
end
if N>1
    u=v1;
end
uQ=reshape(u(1:9),3,3);
ut=u(10:12)+co(:,1);

% -------------------------------------------------------------------
