function [QHat, qHat, tHat, AHat, pHat] = TOARBLAncErrCalSrc_MLE1(r, ANsy, C, pNsy, d, Q_r, Q_a, Q_d, Q_p, q_inial, t_inial)
%
%  [QHat, qHat, tHat, AHat, pHat] = TOARBLAncErrCalSrc_MLE1(r, ANsy, C, pNsy, d, Q_r, Q_a, Q_d, Q_p, q_inial, t_inial)
%
% This program implements the MLE1 iterative solution for rigid body
% localization using TOA measurements in the presence of anchor position 
% errors with a noisy calibration emitter.  The code is applicable for 2-D 
% and 3-D localizations.
% Details of the algorithm development can be found in
% Benjian Hao, K. C. Ho and Zan Li,  "Range based rigid body localization 
% with a calibration emitter for mitigating anchor position uncertainties," 
% IEEE Trans. Wireless Commun., vol. 18, no. 12, pp. 5734-5748, Dec. 2019.
%
% Input Parameter List:
% r:        MxN, range (TOA) measurements, M is the num. of anchors, N is the number of sensors.
% ANsy:     KxM, noisy anchor positions, K is localization dimension.
% C:        KxN, sensor positions local to the rigid body.
% pNsy:     Kx1, noisy calibration emitter position.
% d:        Mx1, range (TOA) measurements for the calibration emitter.
% Q_r:      MNxMN, covariance matrix of range measurements.
% Q_a:      KMxKM, covariance matrix of anchor positions.
% Q_d:      MxM, covariance matrix of calibration emitter range measurements.
% Q_p:      KxK, covariance matrix of calibration emitter position.
% q_inial:  Kx1, initial guess of Euler angles, in degrees.
% t_inial:  Kx1, initial guess of rigid body position.
%
% Output Parameter List:
% QHat:     KxK, rigid body rotation matrix.
% qHat:     Kx1, Euler angles, in degrees.
% tHat:     Kx1, ridid body position.
% AHat:     KxM, refined anchor positions.
% pHat:     Kx1, refined calibration position.
%
% Benjian Hao, K. C. Ho    12-31-2019
%
%       Copyright (C) 2019
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(ANsy,2);   % number of anchors
N = size(C,2);      % number of sensors
K = size(ANsy,1);   % number of anchors
pi = 3.1415926;

Theta = [r(:); ANsy(:); d; pNsy];
MaxTimes = 100;
q = q_inial;
t = t_inial;
a = ANsy;
p = pNsy;

q = q  * pi / 180;

for tt = 1:MaxTimes
    dd_dpsi = zeros(M,(2*K-3)+K);
    dd_da = zeros(K*M,M);
    dd_dp = zeros(K,M);
    for m = 1:M
        d_calculated(m)  = norm(p - a(:,m));
        dd_dp(:,m) = -1/d_calculated(m) * (a(:,m) - p);
        dd_da(:,m) =  1/d_calculated(m) * [zeros(1,K*(m-1)),(a(:,m)-p)',zeros(1,K*(M-m))]';
    end
    
    if (K==2)
        theta = q(1);
        Q = [cos(theta) -sin(theta); sin(theta) cos(theta)];
        dQ = [-sin(theta) -cos(theta); cos(theta) -sin(theta)];
    else
        alpha = q(1); Q_ap = [1 0 0; 0 cos(alpha) -sin(alpha); 0 sin(alpha) cos(alpha)];
        dQ_ap = [0 0 0; 0 -sin(alpha) -cos(alpha); 0 cos(alpha) -sin(alpha)];
        beta = q(2);  Q_bt = [cos(beta) 0  sin(beta); 0  1 0; -sin(beta)  0 cos(beta)];
        dQ_bt = [-sin(beta) 0  cos(beta); 0  0 0; -cos(beta)  0 -sin(beta)];
        gamma = q(3); Q_gm = [cos(gamma) -sin(gamma) 0; sin(gamma) cos(gamma) 0; 0 0 1];
        dQ_gm = [-sin(gamma) -cos(gamma) 0; cos(gamma) -sin(gamma) 0; 0 0 0];
        Q = Q_gm * Q_bt * Q_ap;
    end;
    
    dr_dpsi = zeros(M*N,(2*K-3)+K);
    dr_da = zeros(M*N,K*M);
    dr_dp = zeros(M*N,K);
    
    if (K==2)
        for n = 1:N
            for m = 1:M
                r_calculated(m,n)  = norm(Q * C(:,n) + t - a(:,m));
                dr_dtheta(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m))' * dQ * C(:,n);
                dr_dt(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m));
                dr_dpsi(M*(n-1)+m,:) = [dr_dtheta(M*(n-1)+m,:), dr_dt(M*(n-1)+m,:)];
                dr_da(M*(n-1)+m,:) = 1/r_calculated(m,n) * [zeros(1, K * (m-1)),(a(:,m) - Q * C(:,n) - t)',zeros(1, K * (M-m))];
            end
        end
    else
        for n = 1:N
            for m = 1:M
                r_calculated(m,n)  = norm(Q * C(:,n) + t - a(:,m));
                dr_dalpha(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m))' * Q_gm * Q_bt * dQ_ap * C(:,n);
                dr_dbeta(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m))' * Q_gm * dQ_bt * Q_ap * C(:,n);
                dr_dgamma(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m))' * dQ_gm * Q_bt * Q_ap * C(:,n);
                dr_dt(M*(n-1)+m,:) =  1/r_calculated(m,n) * (Q * C(:,n) + t - a(:,m));
                dr_dpsi(M*(n-1)+m,:) = [dr_dalpha(M*(n-1)+m,:), dr_dbeta(M*(n-1)+m,:), dr_dgamma(M*(n-1)+m,:), dr_dt(M*(n-1)+m,:)];
                dr_da(M*(n-1)+m,:) = 1/r_calculated(m,n) * [zeros(1, K * (m-1)),(a(:,m) - Q * C(:,n) - t)',zeros(1, K * (M-m))];
            end
        end
    end;
    
    da_dpsi = zeros(K*M,(2*K-3)+K);
    da_da = eye(K*M,K*M);
    da_dp = zeros(K*M,K);
    
    dp_dpsi = zeros(K,(2*K-3)+K);
    dp_da = zeros(K,K*M);
    dp_dp = eye(K,K);
    
    G = [dr_dpsi, dr_da, dr_dp; da_dpsi, da_da, da_dp; dd_dpsi, dd_da', dd_dp'; dp_dpsi, dp_da, dp_dp];
    
    phi = [q;t;a(:);p];
    Theta_Calculated = [r_calculated(:); a(:); d_calculated'; p];
    
    R = [Q_r, zeros(M*N,K*M), zeros(M*N,M), zeros(M*N,K); zeros(K*M,M*N), Q_a, zeros(K*M,M), zeros(K*M,K); zeros(M,M*N), zeros(M,K*M), Q_d, zeros(M,K); zeros(K,M*N), zeros(K,K*M), zeros(K,M), Q_p];
    
    det_phi = cond(G' * inv(R) * G);
    det_R = cond(R);
    
    if det_phi > 10^14 | det_R > 10^14
        break;
    else
        R_inv = inv(R);
        phi_inv = inv(G' * R_inv * G);
        phi_k1 = phi + phi_inv * G' * R_inv * (Theta - Theta_Calculated);
        phi_diff = norm(phi_k1 - phi);
        if phi_diff > 10^(-10)
            phi = phi_k1;
            q = phi(1:(2*K-3));
            t = phi((2*K-3)+1:(2*K-3)+K);
            a = reshape(phi((2*K-3)+K+1:(2*K-3)+K+K*M),K,M);
            p = phi((2*K-3)+K+K*M+1:end);
        else
            break;
        end
    end
    
end

if (K==2)
    theta = q(1);
    Q = [cos(theta) -sin(theta); sin(theta) cos(theta)];
    theta = atan2d(Q(2,1),Q(1,1));
    q = [theta];
else
    alpha = q(1); Q_ap = [1 0 0; 0 cos(alpha) -sin(alpha); 0 sin(alpha) cos(alpha)]; dQ_1 = [0 0 0; 0 -sin(alpha) -cos(alpha); 0 cos(alpha) -sin(alpha)];
    beta = q(2);  Q_bt = [cos(beta) 0  sin(beta); 0  1 0; -sin(beta)  0 cos(beta)]; dQ_2 = [-sin(beta) 0  cos(beta); 0  0 0; -cos(beta)  0 -sin(beta)];
    gamma = q(3); Q_gm = [cos(gamma) -sin(gamma) 0; sin(gamma) cos(gamma) 0; 0 0 1]; dQ_3 = [-sin(gamma) -cos(gamma) 0; cos(gamma) -sin(gamma) 0; 0 0 0];
    Q = Q_gm * Q_bt * Q_ap;
    alpha = atan2d(Q(3,2),Q(3,3));
    gamma = atan2d(Q(2,1),Q(1,1));
    beta = atan2d(-Q(3,1),sqrt(Q(3,3)^2+Q(3,2)^2));
    q = [alpha beta gamma]';
end;

QHat=Q; qHat=q; tHat=t; AHat=a; pHat=p;

