function AHat = TOAAncUpd_pNsy(ANsy, pNsy, d, Q_d, Q_a, Q_p)
%
%  AHat = TOAAncUpd_pNsy(ANsy, pNsy, d, Q_d, Q_a, Q_p)
%
% This program realizes the algorithm for updating the noisy anchors position 
% using TOAs from the unknown source and a calibration emitter at perfect
% location.  The code is applicable for 2-D and 3-D localizations.
% Details of the algorithm can be found in K. C. Ho and
% Le Yang, "On the Use of a Calibration Emitter for Source Localization in 
% the Presence of Sensor Position Uncertainty," IEEE Trans. Signal
% Processing, Vol. 56, pp. 5758-5772, Dec. 2008.
%
% The program can be used for 2D(Dim=2) or 3D(Dim=3) localization.
%
% Input parameter list:
% ANsy:     KxM, noisy anchor positions, K is localization dimension.
% pNsy:     Kx1, noisy calibration emitter position.
% d:        Mx1, calibration range(TOA) measurements.
% Q_d:      MxM, covariance matrix of d.
% Q_a:      KMxKM, covariance matrix of ANsy.
% Q_p:      KxK, covariance matrix of pNsy.
%
% Output parameter list:
% AHat:     KxM, improved anchor positions.
%
%
% Le Yang, K. C. Ho      08-01-2009
%                        10-01-2010, revised
%
%       Copyright (C) 2009
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

M = size(ANsy,2);                      % Number of Anchors.
K = size(ANsy,1);                      % Dimension of the localization problem.

for m = 1 : M 
    hc(m) = d(m) - norm(pNsy - ANsy(:,m));
    Gc(m,:) = [zeros(1,K*(m-1)), (pNsy - ANsy(:,m))'/norm(pNsy - ANsy(:,m)), zeros(1,K*(M-m))];
    GcA(m,:) = [(pNsy - ANsy(:,m))/norm(pNsy - ANsy(:,m))];
end

cov_AHat = inv(inv(Q_a) + Gc' * inv(Q_d + GcA * Q_p * GcA') * Gc);   
Delta_AHat = cov_AHat * Gc' * inv(Q_d + GcA * Q_p * GcA') * hc';

AHat = reshape(reshape(ANsy,K*M,1) - Delta_AHat,K,M);  % Update anchor positions.
