function [iW]=DSF3DLoc_weight(f,fc,S,c,p,v,Qn,Qfc,Qs,wghIdc)
% % % function iW=DSF3DLoc_weight(f,fc,S,c,p,v,Qn,Qfc,Qs,wghIdc)
% iW=weight(f,fc,S,c,p,v,Qn,Qfc,Qs,c,wghIdc)
%
% Weight generation for an algorithm in estimating the object position 
% and velocity.
%
% Input:
% f:      (M x 1), DSF measurement vector, M=num. of sensors.
% fc:     scalar, center frequency.
% S:      (3 x M), sensor poition matrix.
% c:      scalar, signal propagation speed.
% p:      (3 x 1), object position.
% v:      (3 x 1), object velocity.
% Qn:     (M x M), covariance matrix of DSF measurements.
% Qfc:    scalar, variance of center frequency.
% Qs:     (3M x 3M), covariance matrix of sensor positions.
% wghIdc: scalar, weighting matrix indicator for different mix of errors
%         wghIdc=0   No weighting matrix
%         wghIdc=1   weighting matrix for measurement errors only
%         wghIdc=2   weighting matrix for measurement and sensor pos. errors
%         wghIdc=3   weighting matrix for measurement and source freq. errors
%         otherwise  Full weighting matrix
%
% Output:
% iW:  (MN x MN), inverse of weighting matrix.
%
% Reference: M. H. Ahmed, K. C. Ho, and G. Wang, "3-D target localization 
% and motion analysis based on Doppler shifted frequencies," IEEE Trans. 
% Aerosp. Electron. Syst., vol. 58, no. 2, pp. 815-833, Apr. 2022.
% 
% M. H. Ahmed, K. C. Ho     10-28-2022
%
%       Copyright (C) 2022
%       Computational Intelligence Signal Processing Laboratory
%       University of Missouri
%       Columbia, MO 65211, USA.
%       hod@missouri.edu
%

% tp=0;
% 
S=S';
M=length(S);
Df=-f/fc;
r=c*(1-f/fc);

dn=sqrt(sum((repmat(p',M,1)-S).^2,2));
Ds=zeros(M,3*M);
for i=1:M
    rho=(p-S(i,:)')/dn(i);
    Ds(i,3*i-2:3*i)=-1/(c*dn(i))*fc*v'*(eye(3)-rho*rho');
end

B1=2*c/fc*diag(r.*dn.^2);
if wghIdc==0
    iW=eye(M);
elseif wghIdc==1
    iW=B1*(Qn)*B1';
elseif wghIdc==2
    iW=B1*(Ds*(Qs)*Ds'+Qn)*B1';
elseif wghIdc==3
    iW=B1*(Df*Qfc*Df'+Qn)*B1';
    iQn=inv(Qn); tp=iQn-iQn*Df*inv(inv(Qfc)+Df'*iQn*Df)*Df'*iQn;
%     tp=inv(B1')*iQn*inv(B1);
else
    iW=B1*(Ds*(Qs)*Ds'+Df*Qfc*Df'+Qn)*B1';
end
iW=iW/max(iW(:));
end

