How to use more than two targets using MFSK waveform?

Hi. I am trying to simulate multiple targets range and speed estimation using fmcw radar. I have come accross MFSK (https://www.mathworks.com/help/phased/examples/simultaneous-range-and-speed-estimation-using-mfsk-waveform.html) example that correctly gives results of 2 targets. I tried to follow the procedure and changed to detection of 4 targets. But I was getting 5 range and velocity values and all were not correct. Can someone tell the issue?

So far I have changed code to modeling of 4 (instead of 2) targets, added subplots. I was getting detection of 4 targets before CA-CFAR application that means it detected 4 targets. But after it, gives 5 targets and with incorrect values.

CODES:

This is the main file running. mySetup.m and mySimulate.m are given below. I changed targets in mySetup.m to 4; and added subplots in main file.

rng(2015);

[fmcwwaveform,target,tgtmotion,channel,transmitter,receiver,...
    sensormotion,c,fc,lambda,fs,maxbeatfreq] = mySetup;
Nsweep = 2;
xr = mySimulate(Nsweep,fmcwwaveform,sensormotion,tgtmotion,...
    transmitter,channel,target,receiver);
dfactor = ceil(fs/maxbeatfreq)/2;
fs_d = fs/dfactor;
fbu_rng = rootmusic(decimate(xr(:,1),dfactor),2,fs_d);
fbd_rng = rootmusic(decimate(xr(:,2),dfactor),2,fs_d);
sweep_slope = fmcwwaveform.SweepBandwidth/fmcwwaveform.SweepTime;
rng_est = beat2range([fbu_rng fbd_rng;fbu_rng flipud(fbd_rng)],...
    sweep_slope,c)
mfskwaveform = phased.MFSKWaveform(...
    'SampleRate',151e6,...
    'SweepBandwidth',150e6,...
    'StepTime',2e-6,...
    'StepsPerSweep',1024,...
    'FrequencyOffset',-294e3,...
    'OutputFormat','Sweeps',...
    'NumSweeps',1);
numsamp_step = round(mfskwaveform.SampleRate*mfskwaveform.StepTime);
sig_display = mfskwaveform();
spectrogram(sig_display(1:8192),kaiser(3*numsamp_step,100),...
    ceil(2*numsamp_step),linspace(0,4e6,2048),mfskwaveform.SampleRate,...
    'yaxis','reassigned','minthreshold',-60)
Nsweep = 1;
release(channel);
channel.SampleRate = mfskwaveform.SampleRate;
release(receiver);
receiver.SampleRate = mfskwaveform.SampleRate;

xr = helperFMCWSimulate(Nsweep,mfskwaveform,sensormotion,tgtmotion,...
    transmitter,channel,target,receiver);

x_dechirp = reshape(xr(numsamp_step:numsamp_step:end),4,[]).';
fs_dechirp = 1/(2*mfskwaveform.StepTime);

xf_dechirp = fft(x_dechirp);
num_xf_samp = size(xf_dechirp,1);
beatfreq_vec = (0:num_xf_samp-1).'/num_xf_samp*fs_dechirp;

clf;
subplot(411),plot(beatfreq_vec/1e3,abs(xf_dechirp(:,1)));grid on;
ylabel('Magnitude');
title('Frequency spectrum for sweep 1');
subplot(412),plot(beatfreq_vec/1e3,abs(xf_dechirp(:,2)));grid on;
ylabel('Magnitude');
title('Frequency spectrum for sweep 2');
xlabel('Frequency (kHz)')
subplot(413),plot(beatfreq_vec/1e3,abs(xf_dechirp(:,3)));grid on;
ylabel('Magnitude');
title('Frequency spectrum for sweep 3');
xlabel('Frequency (kHz)')
subplot(414),plot(beatfreq_vec/1e3,abs(xf_dechirp(:,4)));grid on;
ylabel('Magnitude');
title('Frequency spectrum for sweep 4');
xlabel('Frequency (kHz)')

cfar = phased.CFARDetector('ProbabilityFalseAlarm',1e-2,...
    'NumTrainingCells',16);

peakidx = cfar(abs(xf_dechirp(:,1)),1:num_xf_samp);

Fbeat = beatfreq_vec(peakidx);
phi = angle(xf_dechirp(peakidx,2))-angle(xf_dechirp(peakidx,1));

sweep_slope = mfskwaveform.SweepBandwidth/...
    (mfskwaveform.StepsPerSweep*mfskwaveform.StepTime);
temp = ...
    [1 sweep_slope;mfskwaveform.StepTime mfskwaveform.FrequencyOffset]\...
    [Fbeat phi/(2*pi)].';

r_est = c*temp(2,:)/2
v_est = lambda*temp(1,:)/(-2)

%% mySetup.m %%

function [fmcwwaveform,target,tgtmotion,channel,transmitter,receiver,...
    sensormotion,c,fc,lambda,fs,fr_max] = mySetup

% System parameter
fc = 77e9;      % operating frequency
c = 3e8;        % propagation speed
lambda = c/fc;  % wavelength

tm = 0.001;     % sweep time

range_res = 1;              % range resolution
bw = range2bw(range_res,c); % bandwidth
sweep_slope = bw/tm;        % sweep slope

range_max = 200;
fr_max = range2beat(range_max,sweep_slope,c);

v_max = 230*1000/3600;
fd_max = speed2dop(2*v_max,lambda);

fb_max = fr_max+fd_max;


fs = max(2*fb_max,bw);


fmcwwaveform = phased.FMCWWaveform('SweepTime',tm,'SweepBandwidth',bw,...
    'SampleRate',fs,'SweepDirection','Triangle');

car_dist = 50;
car_speed = 96*1000/3600;
car_rcs = db2pow(min(10*log10(car_dist)+5,20));
car2_dist = 60;
car2_speed = 69*1000/3600;
car2_rcs = db2pow(min(10*log10(car_dist)+5,20));


truck_dist = 55;
truck_speed = -70*1000/3600;
truck_rcs = db2pow(min(10*log10(truck_dist)+5,20));

truck2_dist = 65;
truck2_speed = -40*1000/3600;
truck2_rcs = db2pow(min(10*log10(truck2_dist)+5,20));

tgtpos = [[car_dist;0;0],[car2_dist;0;0],[truck_dist;0;0],[truck2_dist;0;0]];
tgtvel = [[car_speed;0;0],[car2_speed;0;0],[truck_speed;0;0],[truck2_speed;0;0]];
tgtmotion = phased.Platform('InitialPosition',tgtpos,'Velocity',tgtvel);

tgtrcs = [car_rcs,car2_rcs,truck_rcs,truck2_rcs];
target = phased.RadarTarget('MeanRCS',tgtrcs,'PropagationSpeed',c,...
    'OperatingFrequency',fc);

channel = phased.FreeSpace('PropagationSpeed',c,...
    'OperatingFrequency',fc,'SampleRate',fs,'TwoWayPropagation',true);

ant_aperture = 6.06e-4;                         % in square meter
ant_gain = aperture2gain(ant_aperture,lambda);  % in dB

tx_ppower = db2pow(5)*1e-3;                     % in watts
tx_gain = 9+ant_gain;                           % in dB

rx_gain = 15+ant_gain;                          % in dB
rx_nf = 4.5;                                    % in dB

transmitter = phased.Transmitter('PeakPower',tx_ppower,'Gain',tx_gain);
receiver = phased.ReceiverPreamp('Gain',rx_gain,'NoiseFigure',rx_nf,...
    'SampleRate',fs);

radar_speed = 60*1000/3600;
sensormotion = phased.Platform('Velocity',[radar_speed;0;0]);


%% mySimulate.m %%

function [xr,xr_unmixed] = mySimulate(Nsweep,waveform,...
    radarmotion,carmotion,transmitter,channel,cartarget,receiver)
% This function helperFMCWSimulate is only in support of FMCWExample. It
% may be removed in a future release.

%   RSWEEP =
%   helperFMCWSimulate(NSWEEP,WAVEFORM,RADARMOTION,CARMOTION,TRANSMITTER,
%   CHANNEL,CARTARGET,RECEIVER) returns the simulated sweep train RSWEEP. 
%
%   The input parameters are:
%       NSWEEP:             number of sweeps
%       WAVEFORM:           waveform object
%       RADARMOTION:        platform object for the radar
%       CARMOTION:          platform object for target car
%       TRANSMITTER:        transmitter object
%       CHANNEL:            propagation channel object
%       CARTARGET:          target car object
%       RECEIVER:           receiver object
%
%   The rows of RSWEEP represent fast time and its columns represent slow
%   time (pulses). When the pulse transmitter uses staggered PRFs, the
%   length of the fast time sequences is determined by the highest PRF.

%   Copyright 2010-2016 The MathWorks, Inc.

release(waveform);
release(radarmotion);
release(transmitter);
release(receiver);
release(carmotion);
release(channel);
release(cartarget);

if isa(waveform,'phased.MFSKWaveform')
    sweeptime = waveform.StepTime*waveform.StepsPerSweep;
else
    sweeptime = waveform.SweepTime;
end
Nsamp = round(waveform.SampleRate*sweeptime);

xr = complex(zeros(Nsamp,Nsweep));
xr_unmixed = xr;

Ntgt = numel(cartarget.MeanRCS);
for m = 1:Nsweep
    % Update radar and target positions
    [radar_pos,radar_vel] = radarmotion(sweeptime);
    [tgt_pos,tgt_vel] = carmotion(sweeptime);

    % Transmit FMCW waveform
    sig = waveform();
    txsig = transmitter(sig);

    % Propagate the signal and reflect off the target
    rxsig = complex(zeros(Nsamp,Ntgt));
    for n = 1:Ntgt
        rxsig(:,n) = channel(txsig,radar_pos,tgt_pos(:,n),radar_vel,tgt_vel(:,n));
    end
    rxsig = cartarget(rxsig);
    
    % Dechirp the received radar return
    rxsig = receiver(sum(rxsig,2));
    xd = dechirp(rxsig,sig);
    xr_unmixed(:,m) = rxsig;
    xr(:,m) = xd;
end