HRDPD Doppler code

% HRDPD - Doppler
% Reference: High Resolution Localization of Narrowband Radio Emitters Based on Doppler Frequency Shifts
% written by Tom Tirer
clear; clc;

simulate_sensors_motion = 1;
use_sensors_motion_in_estimation = 0; % see: "An interesting observation..." in Section 4.1. Example 1.

L = 4; % number of sensors (receivers)
N = 128; % number of used time samples per interval
N_skip = 3;
N_toltal = N*N_skip+100; % number of time samples per interval
J = 3; % number of frequency bands (snapshots)
K = 10; % number of intervals, i.e. different locations of the sensors
SNR = 30; % [dB]
Q = 2; % number of emitters
fc = 2.5*10^9; % 2.5 GHz
fmax = 10*10^3;
fs = fmax;
fs_air = fmax*20;
N_air = N_toltal*40;
T_air = N_air/fs_air;

x0 = [-1.5,1.5]; % true x coordinate of emitters [km]
y0 = [0,0]; % true y coordinate of emitters [km]

x_sensors = zeros(L,K);
y_sensors = zeros(L,K);
vx_sensors = zeros(L,K);
vy_sensors = zeros(L,K);

radius = 22; % [km]
angles = linspace(0,360-360/K,K);
angles0 = [0,atand(1/radius),0,-atand(1/radius)]';
angles0 = angles0(1:L);
angles = repmat(angles,L,1) + repmat(angles0,1,K);
radii_sensors = repmat([radius-0.5,radius,radius+0.5,radius]',1,K);
radii_sensors = radii_sensors(1:L,:);
v_tangent = repmat([300,300,300,300]',1,K); % [m/sec]
v_tangent = v_tangent(1:L,:);
x_sensors = 0 + radii_sensors.*cosd(angles);
y_sensors = 0 + radii_sensors.*sind(angles);
vx_sensors = v_tangent.*sind(-angles);
vy_sensors = v_tangent.*cosd(angles);
if simulate_sensors_motion
% more accurate observations
angular_range = T_air * v_tangent./radii_sensors;
angles_phy = zeros(L,K,N_air);
for l=1:1:L
for k=1:1:K
angles_phy(l,k,:) = angles(l,k) + linspace(-angular_range(l,k)/2,angular_range(l,k)/2,N_air);
end
end
x_sensors_phy = 0 + repmat(radii_sensors,1,1,N_air).*cosd(angles_phy);
y_sensors_phy = 0 + repmat(radii_sensors,1,1,N_air).*sind(angles_phy);
vx_sensors_phy = repmat(v_tangent,1,1,N_air).*sind(-angles_phy);
vy_sensors_phy = repmat(v_tangent,1,1,N_air).*cosd(angles_phy);
end

C = 3*10^8; % m/sec

x0 = x0(1:Q);
y0 = y0(1:Q);

% create filters for DPD-HR
filterBank = cell(J,1);
[B,A] = cheby1(3,0.5,0.7);
filterBank{1} = [B;A];
[B,A] = cheby1(3,0.5,[0.15 0.85]);
filterBank{2} = [B;A];
[B,A] = cheby1(3,0.5,0.3,'high');
filterBank{3} = [B;A];

% create signals
signal_air = zeros(N_air,Q,K);
for k=1:1:K
for q=1:1:Q
temp = (randn(N_air,1)+1i*randn(N_air,1))/sqrt(2);
[B,A] = cheby1(10,0.5,fs/fs_air);
signal_air(:,q,k) = filter(B,A,temp); % narrowband signal
end
end

% take into account the delays between receivers
tau0_history = zeros(Q,L,K);
debug_max_shift = zeros(Q,K);
signal_air_delayed = cell(Q,K,L);
for k=1:1:K
for q=1:1:Q
for l=1:1:L
tau0 = (1/C) * 1000 * ((x0(q)-x_sensors(l,k)).^2+(y0(q)-y_sensors(l,k)).^2).^0.5;
tau0_history(q,l,k) = tau0;
tau0 = tau0 - tau0_history(q,1,k);
tau0_samples = tau0*fs_air;
debug_max_shift(q,k) = max(abs([debug_max_shift(q,k),tau0_samples]));

yyy_fft = fft(signal_air(:,q,k));
yyy_fft_exp = yyy_fft .* (exp(-1i*2*pi*tau0_samples/N_air*[0:1:(N_air/2-1),-N_air/2:1:-1])).';
yyyi = ifft(yyy_fft_exp);
signal_air_delayed{q,k,l} = yyyi;
end
end
end

X = zeros(N_toltal,L,K);
fd0_history = zeros(Q,L,K);
fac = 10^(SNR/20);

% create measurements
signal_unfilt = zeros(N_toltal,L,Q,K);
norm_fac = zeros(Q,K);
for k=1:1:K
for l=1:1:L
channel_phi = 2*pi*rand(Q,1) * 1;
channel_mag = 1+0.1*randn(Q,1);
alpha = channel_mag.*exp(1i*channel_phi); % same alpha during each interval (per sensor per emitter)

for q=1:1:Q
if ~simulate_sensors_motion
v_lk = [vx_sensors(l,k); vy_sensors(l,k)];
p0 = [x0(q);y0(q)];
p_lk = [x_sensors(l,k); y_sensors(l,k)];
mu = v_lk'*(p0-p_lk) / norm(p0-p_lk) / C;
else
v_lk = squeeze([vx_sensors_phy(l,k,:); vy_sensors_phy(l,k,:)]); % 2 x N_air
p0 = [x0(q);y0(q)];
p_lk = squeeze([x_sensors_phy(l,k,:); y_sensors_phy(l,k,:)]); % 2 x N_air
mu = sum(v_lk.*(repmat(p0,1,N_air)-p_lk),1) ./ sqrt(sum((repmat(p0,1,N_air)-p_lk).^2,1)) / C;
end
fd0 = fc*mu; % real doppler frequency
fd0_history(q,l,k) = mean(fd0);

if ~simulate_sensors_motion % for backward compatability
sampleOffset = 100; % keep sampleOffset > max(tau0)
signal_unfilt(:,l,q,k) = signal_air_delayed{q,k,l}(sampleOffset*2:fs_air/fs:sampleOffset*2+(N_toltal-1)*fs_air/fs);
else
signal_unfilt(:,l,q,k) = signal_air_delayed{q,k,l}(N_air/2-(N_toltal/2-1)*fs_air/fs :fs_air/fs: N_air/2+(N_toltal/2)*fs_air/fs);
end
if l==1
norm_fac(q,k) = norm(signal_unfilt(:,1,q,k));
end
signal_unfilt(:,l,q,k) = signal_unfilt(:,l,q,k)/norm_fac(q,k)*sqrt(N_toltal);

if ~simulate_sensors_motion
A_lk_vect = exp(1i*2*pi*fd0/fs).^(0:1:N_toltal-1).';
else
A_lk_vect = exp(1i*2*pi*cumsum(fd0)/fs_air); A_lk_vect = A_lk_vect(N_air/2-(N_toltal/2-1)*fs_air/fs :fs_air/fs: N_air/2+(N_toltal/2)*fs_air/fs);
end
           X(:,l,k) = X(:,l,k) + fac * alpha(q) * signal_unfilt(:,l,q,k) .* A_lk_vect(:);
end

noise = (randn(N_toltal,1) + 1i*randn(N_toltal,1))./sqrt(2); %noise
       X(:,l,k) = X(:,l,k) + noise;
end
end

x_candidate = [-2.5:0.125:2.5];
y_candidate = [-2.5:0.125:2.5];

Q_ML = zeros(length(y_candidate),length(x_candidate));
Q_HR = zeros(length(y_candidate),length(x_candidate));

r_unfiltered = X(50:N_skip:50+(N-1)*N_skip,:,:); %(N,L,K);
r_unfiltered = permute(r_unfiltered,[2 1 3]); %(L,N,K)
r = zeros(L,J,N,K);

% filter the observations for DPD-HR
for k=1:1:K
for l=1:1:L
for j=1:1:J
B = filterBank{j}(1,:);
A = filterBank{j}(2,:);
temp = filter(B,A,X(:,l,k));
r(l,j,:,k) = temp(50:N_skip:50+(N-1)*N_skip);
end
end
end

R_tilde = zeros(L,L,N,K);
R_hat = zeros(L,L,N,K);
inv_R_hat = zeros(L,L,N,K);

tic

for k=1:1:K % invert all R_hat beforehand
for n=1:1:N
R_kn = r(:,:,n,k) * r(:,:,n,k)' / J;
R_hat(:,:,n,k) = R_kn;
inv_R_hat(:,:,n,k) = inv(R_hat(:,:,n,k)+diag(0.001*ones(1,L))); % adding diagonal loading
end
% do not filter the observations for ML (use the full frequency band)
for n=1:1:N
R_kn = r_unfiltered(:,n,k)*r_unfiltered(:,n,k)';
R_tilde(:,:,n,k) = R_kn;
end
end

for x_ind=1:1:length(x_candidate)
for y_ind=1:1:length(y_candidate)

x = x_candidate(x_ind);
y = y_candidate(y_ind);
D_ML = 0;
D_HR = 0;

for k=1:1:K

disp(['x_ind=',num2str(x_ind),', y_ind=',num2str(y_ind),', k=',num2str(k)]);

D_ML_k = zeros(L,L);
D_HR_k = zeros(L,L);
lamda_kn = zeros(L);
lamda_kn_phy = zeros(L,N);
for l=1:1:L
if ~use_sensors_motion_in_estimation
v_lk = [vx_sensors(l,k); vy_sensors(l,k)];
p = [x;y];
p_lk = [x_sensors(l,k); y_sensors(l,k)];
mu = v_lk'*(p-p_lk) / norm(p-p_lk) / C;
fd = fc*mu;
lamda_kn(l,l) = exp(1i*2*pi*fd/(fs/N_skip));
else
v_lk = squeeze([vx_sensors_phy(l,k,:); vy_sensors_phy(l,k,:)]); % 2 x N_air
p = [x;y];
p_lk = squeeze([x_sensors_phy(l,k,:); y_sensors_phy(l,k,:)]); % 2 x N_air
mu = sum(v_lk.*(repmat(p,1,N_air)-p_lk),1) ./ sqrt(sum((repmat(p,1,N_air)-p_lk).^2,1)) / C;
fd = fc*mu;
temp = exp(1i*2*pi*cumsum(fd)/fs_air);
temp = temp(N_air/2-(N_toltal/2-1)*fs_air/fs :fs_air/fs: N_air/2+(N_toltal/2)*fs_air/fs);
lamda_kn_phy(l,:) = temp(50:N_skip:50+(N-1)*N_skip);
end
end

if ~use_sensors_motion_in_estimation
% n=0 ==> Lambda = eye(L)
D_ML_k = D_ML_k + R_tilde(:,:,1,k);
D_HR_k = D_HR_k + inv_R_hat(:,:,1,k);
for n=2:1:N
Lambda = lamda_kn^(n-1);
D_ML_k = D_ML_k + Lambda'*R_tilde(:,:,n,k)*Lambda;
D_HR_k = D_HR_k + Lambda'*inv_R_hat(:,:,n,k)*Lambda;
end
else
for n=1:1:N
Lambda = diag(lamda_kn_phy(:,n));
D_ML_k = D_ML_k + Lambda'*R_tilde(:,:,n,k)*Lambda;
D_HR_k = D_HR_k + Lambda'*inv_R_hat(:,:,n,k)*Lambda;
end
end
D_ML = D_ML + max(real(eig(D_ML_k)));
D_HR = D_HR + min(real(eig(D_HR_k)));
end

Q_ML(y_ind,x_ind) = real(D_ML);
Q_HR(y_ind,x_ind) = real(inv(D_HR));

end
end

toc

figure; imagesc(x_candidate,y_candidate,Q_ML); title('Q_S_M_L'); set(gca,'YDir','normal'); colormap jet;

figure; imagesc(x_candidate,y_candidate,Q_HR); title('Q_H_R'); set(gca,'YDir','normal'); colormap jet;