function [sHat] = TOALoc_pNsy(r, AHat, pNsy, Q_r, Q_a, Q_d, Q_p)
%
%  [SHat] = TOALoc_pNsy(r, A, pNsy, Q_r, Q_a, Q_d, Q_p)
%
% This program evaluates the sensor position in rigid body localization using 
% TOA measurements, in the presence of anchor and calibration emitter position
% errors.  The code is applicable for 2-D and 3-D localizations.
% Details of the algorithm development can be found in
% B. Hao, K. C. Ho, and Z. Li, "Range Based Rigid Body Localization With a
% Calibration Emitter for Mitigating Anchor Position Uncertainties," IEEE 
% Trans. Wireless Commun., vol. xx, no. xx, pp. xx-xx, xx. 20xx.
%
% Input Parameter List:
% r:        MxN, range (TOA) measurements, M is the num. of anchors, N is the number of sensors.
% AHat:     KxM, anchor position estimates from calibration emitter, K is localization dimension.
% pNsy:     Kx1, noisy calibration emitter position.
% 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.
%
% Output Parameter List:
% sHat:     Kx1, estimated sensor position in the global frame.
%
%
% Benjian Hao, K. C. Ho    12-31-2019
%
%       Copyright (C) 2016
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

RptCnt = 3;     % number of repetitions in Stage-2 to recompute W1

M = size(AHat,2);                      % Number of anchors.
dim = size(AHat,1);                      % Dimension of localization
P = sum(AHat.^2)';

%=========== construct related vector and matrix ============
h1 = r.^2-P;
G1 = [-2*AHat', 1*ones(M,1)];
%============= first step ===================================
W1 = inv(Q_r);          
phi1 = inv(G1'*W1*G1)*G1'*W1*h1;
s = phi1(1 : dim);
rHat=r;

for times = 1 : max(1,RptCnt)                       % Iterate to update W1.
    B1 = 2*diag(rHat);
    for m = 1 : M
        D1(m,:) = 2 * [zeros(1,dim*(m-1)), (s-AHat(:,m))',zeros(1,dim*(M-m))];
        Gc(m,:) = [zeros(1,dim*(m-1)), (pNsy - AHat(:,m))'/norm(pNsy - AHat(:,m)), zeros(1,dim*(M-m))];
        GcA(m,:) = [(pNsy - AHat(:,m))/norm(pNsy - AHat(:,m))];
    end;
    cov_AHat = inv(inv(Q_a) + Gc' * inv(Q_d + GcA * Q_p * GcA') * Gc);  
    W1 = inv(B1 * Q_r * B1' + D1 * cov_AHat * D1');
    phi1 = inv(G1' * W1 * G1) * G1' * W1 * h1;
    s = phi1(1 : dim);
    rHat=sqrt(sum((repmat(s,1,M)-AHat).^2));
end;        
cov_phi1 = inv(G1' * W1 * G1);

u1p = phi1;
%========== second step =====================================
h2 = [u1p(1:end-1).^2;u1p(end)];
G2 = [eye(dim);ones(1,dim)];
B2 = 2*diag([u1p(1:end-1);1/2]);
W2 = inv(B2') * inv(cov_phi1) * inv(B2);

u2 = inv(G2' * W2 * G2) * G2' * W2 * h2;
%=========== mapping ========================================
sHat = sign(u1p(1:dim)).*sqrt(abs(u2));
%============================================================
%
% if u1(end) < 0 || min(u2) < 0
%     SourceLocation = u1(1:length(u2));
% end
