function cost_MLE1 = TOARBLAncErrCalSrc_CostEval_MLE1(r, ANsy, d, pNsy, C, Q_r, Q_a, Q_d, Q_p, qHat, tHat, AHat, pHat)
%
%  cost_MLE1 = TOARBLAncErrCalSrc_CostEval_MLE1(r, ANsy, d, pNsy, C, Q_r, Q_a, Q_d, Q_p, qHat, tHat, aHat, pHat)
%
% This program evaluates the residual cost function of MLE1.  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.
% d:        Mx1, range (TOA) measurements for the calibration emitter.
% pNsy:     Kx1, noisy calibration emitter position.
% C:        KxN, sensor positions local to the rigid body.
% 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.
% qHat:     Kx1, Euler angle vector estimate from MLE1, in degrees.
% tHat:     Kx1, position estimate from MLE1.
% AHat:     KxM, anchor position estimates from MLE1.
% pHat:     Kx1, calibration position estimate from MLE1.
%
% Output Parameter List:
% cost_MLE1:    ML residual cost from MLE1.
%
% 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

if (K==2)
    theta = qHat(1);
    QHat = [cosd(theta) -sind(theta); sind(theta) cosd(theta)];
else
    alpha = qHat(1); Q_ap = [1 0 0; 0 cosd(alpha) -sind(alpha); 0 sind(alpha) cosd(alpha)];
    beta = qHat(2);  Q_bt = [cosd(beta) 0  sind(beta); 0  1 0; -sind(beta)  0 cosd(beta)];
    gamma = qHat(3); Q_gm = [cosd(gamma) -sind(gamma) 0; sind(gamma) cosd(gamma) 0; 0 0 1];
    QHat = Q_gm * Q_bt * Q_ap;
end;

for n = 1:N
    for m = 1:M
        r_calculated(m,n)  = norm(QHat * C(:,n) + tHat - AHat(:,m));
    end
end

for m = 1:M
    d_calculated(m)  = norm(pHat - AHat(:,m));
end

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];

Theta = [r(:); ANsy(:); d; pNsy];
Theta_Calculated = [r_calculated(:); AHat(:); d_calculated'; pHat];

cost_MLE1 = (Theta-Theta_Calculated)' * inv(R) * (Theta-Theta_Calculated);
