DOA of a scanning RF emitter based on "Passive Localization of Scanning Emitter"

Hello Everyone

I have been trying trying to code a script for determining position of a scanning RF emitter with a fixed scan rate.

The code is based on the method described in "H. Hmam and K. Dogancay, "Passive Localization of Scanning Emitters," in IEEE Transactions on Aerospace and Electronic Systems, vol. 46, no. 2, pp. 944-951, April 2010, doi: 10.1109/TAES.2010.5461671."

The methodology of the algorithm is descirbed by

The script covers the grid search initialization algorithm for finding the scan time estimate and is implemented as follows

%% Defining the initial conditions
c = 299792000; % speed of light [m/s]
M=3; % number of satellites
N=1; % number of emitters
h_sat=5e5; % satellite height [m]
sig_p=1; % standard deviation of satellite posittion error
sig_t=0.0005; % standart deviation of time measurement error
sat_scale=3e3; % scaling factor for inter-satellite distanddce
em_scale=3e6; % scaling factor for emitter-satellite distanddce
pe=[50000;10000]; % position for the RF emitter
ps1 = zeros(2,M); % defining the position of the satellites with receivers
for i=1:M
    ps1(:,i) = [(i-2)*sat_scale; em_scale];
end

scan_rate=80; % the true emitter radar scanning rate in [deg/s]
scan_grid=30:1:150; % determining a grid of estimation scan points for the algorithm [deg/s]
m_scan=length(scan_grid);
%% Determining the scan time of arrival
toa_true=zeros(M,1); t_h=zeros(M,1); scant=zeros(M,1); a_h=zeros(M,1); betak1=zeros(M-1,1); alpha_dum=zeros(M,N);
% determing the satellite-to-emitter angles
for i=1:M
    dummy = ps1(:,i) - pe;
    dummy_deg = atan2d(dummy(2),dummy(1)); % determining the rotation angle from the emitter to receivers 
    alpha_dum(i)=dummy_deg + (dummy_deg < 0)*360; % angles are constraine dto be within 2 pi
end
% sorting the angles in order of time of arrival
[alpha,Ida]=sort(alpha_dum);
ps=ps1(:,Ida); % rearangement of the receiver positions
% adding the standard deviation of the satellite receiver positions
for i=1:M
    ps(:,i)=[ps(1,i) + randn *sig_p; ps(2,i) + randn *sig_p];
    dummy = pe -   ps(:,i);
    toa_true(i) = norm(dummy) / c;
    a_h(i) = alpha(i);
end
% adding the standard deviation of the time measurement error
scant(1) = toa_true(1) + randn * sig_t; t_h(1) = scant(1);
for i=2:M
    scant(i) = t_h(i) + ((alpha(i) - alpha(1)) / scan_rate);
    betak1(i-1) = scan_rate * (abs(scant(i) - scant(1)));
    t_h(i) = scant(i) + randn * sig_t;
end
scan_toa=t_h;
%% Grid Search Initialization Algorithm
X_k=zeros(m_scan,3); bt=zeros(M,1); B_mat=zeros(M,4); K_nls=zeros(m_scan,1); g_wp=zeros(m_scan,M-1); bt2=zeros(M,1);
for i=1:m_scan
    w_k = scan_grid(i); % scan rate estimates
    for j=2:M  % generate the angle values to be used for the B-matrix ,[eq. 14] 
        bt(j) = w_k * (scan_toa(j) - scan_toa(1)); 
    end
    
    B_mat(1,:) = [0, -1, -ps(2,1), ps(1,1)]; % composing the B-matrix ,[eq. 15]
    for ii=2:M
        B_mat(ii,:) = [sind(bt(ii)), -cosd(bt(ii)), ps(1,ii) * sind(bt(ii))...
            - ps(2,ii) * cosd(bt(ii)), ps(1,ii) * cosd(bt(ii)) + ps(2,ii) * sind(bt(ii))];
    end
    
    [U_B,S_B,V_B] = svd(B_mat); % svd decomposition of the B matrix
    
    W_b = V_B(:,4) / norm(V_B(3:4,4)); % Generate the W matrix [eq. 20]
    
    theta1=atan2d((W_b(3)),(W_b(4))); % detemrine the angle estimate by bearing angle [eq. 21a]
    theta=theta1 + (theta1 < 0)*360;
    thlist(i) = theta;
    
    R_theta=[cosd(theta), sind(theta); -sind(theta), cosd(theta)]; % The  Rotation matrix for the angle [eq. 17]
    pk = -R_theta * W_b(1:2); % The position estimate [eq. 21b]
   
    A_p=zeros(M-1,1); % The A matrix with respect to p [eq. 3]
    for ij=1:M-1
        A_p(ij) = acosd(((ps(:,ij) - pk)' * (ps(:,ij+1) - pk))/...
            (norm(ps(:,ij) - pk) * (norm(ps(:,ij+1) - pk))));
    end
    
    Alist(i)=norm(A_p);
    g_wp = (1/w_k) * A_p; % the true scan time vector g_wp [eq. 2]
    meas_err = zeros(M-1,1); % The error vector for the measurement
    
    for jj=1:M-1
        meas_err(jj) = randn * sig_t;  % measurement error vector defined from meas err. std dev.
    end
    
    tau = g_wp + meas_err; % The scan time measurement vector
    
    Sigma = diag(ones(M-1,1)) * (sig_t^2 + sig_t^2) + diag(ones(M-2,1),1)... 
        * (-sig_t^2) + diag(ones(M-2,1),-1) * (-sig_t^2); % The measurement error covariance matrix [eq. 5]
   
    Q=inv(Sigma);
    scan_est=(A_p' * Q * A_p)/(A_p'*Q*tau); % the scan rate estimate [eq. 8]
    X_k(i,:)=[scan_est,pk'];
    pkx(i)=pk(1); pky(i)=pk(2);
    
    Gnls1 = (A_p' * Q * tau); 
    Gnls2 = (A_p' * Q * A_p);
    K_nls(i)= (Gnls1)^2/(Gnls2); % The cosdt function [eq. 10]
    Gnls1v(i)=Gnls1; Gnls2v(i)=Gnls2;    
end

% find the maximum value of the Knls cost function
[M_temp,I_temp]=max(K_nls);
scan_grid(I_temp);
X0_est=X_k(I_temp,:)';

However the algorithm does not converge at a correct scan time estimate