function [est,est_arr,est_sp] = frq_rbps3(mu,Sigma,z_rbpf,...
    w,BBt,par,z_rbpf_pred_v,t,fs,M,nfig)
% PURPOSE : RBPS for conditionally Gaussian frequency tracker
if nargin< 10, 
    M = 40;                     % Number of particles.
end
if nargin< 11, 
    nfig = 0;                     % Number of particles.
end

echo off;
flops(0);                  % Initialize timer for benchmarking
% =======================================================================
%              INITIALISATION AND PARAMETERS
% =======================================================================

% Here, we give you the choice to try three different types of
% resampling algorithms: multinomial (select 3), residual (1) and 
% deterministic (2). Note that the code for these O(N) algorithms is generic.
resamplingScheme = 2;    

% =======================================================================
%                              RBPF ESTIMATION
% =======================================================================

% INITIALISATION:
% ==============
n_z = size(z_rbpf,1);
[T,N] = size(mu);
z_rbps = ones(n_z,T,M);        % These are the particles for the estimate
                               % of z. Note that there's no need to store
                               % them for all t. We're only doing this to
                               % show you all the nice plots at the end.
mu_sm = rand(T,M);      % Kalman mean of x.
mu_pred = 5*randn(1,N); 
Sigma_sm = zeros(M,T);      % Kalman covariance of x.
Sigma_pred = zeros(1,N); 

nj = 10;
nm = floor(M/nj);
est_arr = struct('a',zeros(T,nj),'p',zeros(T,nj),...
    'f',zeros(T,nj),'t',zeros(nj,1));
est_sp = [];
for j=1:M
    outIndex = length(find(cumsum(w(T,:)')<rand))+1;
    % particle_ps(T,j) = xparticlePred_pf(T,outIndex); 
    z_rbps(:,T,j) = z_rbpf(:,T,outIndex); 
    mu_sm(T,j) = mu(T,outIndex);
    Sigma_sm(j,T) = Sigma(outIndex,T);
    s_rbps(T,j) = normrnd_mt(1,mu_sm(T,j),...
                   Sigma_sm(j,T));
    if mod(j,40)== 0, 
        fprintf('RBPS :  j = %i / %i k = %i / %i  \r',j,M,k,T); fprintf('\n');  
    end      

    for k=T-1:-1:1,   
        
        par.C = sin(z_rbps(1,k+1,j));
        % Kalman prediction:
        mu_pred = mu(k,:); 
        Sigma_pred = Sigma(:,k+1)'+BBt; 
        
         % Evaluate importance weights.
         ws(k,:) = log(w(k,:))+ ...
         normlogpdf(s_rbps(k+1,j),mu_pred,sqrt(Sigma_pred))+...
         normlogpdf(z_rbps(2,k+1,j),reshape(z_rbpf(2,k,:),1,N),...
         sqrt(z_rbpf_pred_v(2,2)));
        
         ws(k,:) = exp(ws(k,:)-logsumexp(ws(k,:),2));       % Normalise the weights.
        
         if abs(sum(ws(k,:),2)-1)>1e-7, 
             warning('ws(k,:) ~= 1');
         end
         % SELECTION STEP:
         % ===============      
          outIndex = length(find(cumsum(ws(k,:)')<rand))+1;
          z_rbps(:,k,j) = z_rbpf(:,k,outIndex); 
          
          mu_tp = mu(k,outIndex);
          Sigma_tp = Sigma(outIndex,k);
          Sigma_pred_tp = Sigma_pred(outIndex);
          
          % Kalman smoothing update          
          S = Sigma_tp*par.A'*inv(Sigma_pred_tp);
          mu_sm(k,j) = mu_tp+S*(mu_sm(k+1,j)-par.A*mu_tp);
          Sigma_sm(j,k) = Sigma_tp+S^2*(Sigma_sm(j,k+1)-Sigma_pred_tp);
          
          if Sigma_sm(j,k) >0
              s_rbps(k,j) = normrnd_mt(1,mu_sm(k,j),Sigma_sm(j,k));
          else
              s_rbps(k,j) = mu_sm(k,j);
          end
     end % End of k loop over data.

    % =======================================================================
    %                          SUMMARIES AND PLOTS
    % =======================================================================

    if mod(j,nj)== 0,
       p_plot_rbps = zeros(T,j,1);
       pos = 1:j;
       for k=1:T
           p_plot_rbps(k,:,1) = z_rbps(1,k,pos);
       end

       f_plot_rbps = zeros(T,j,1);
       for k=1:T
           f_plot_rbps(k,:,1) = z_rbps(2,k,pos);
        end

       a_mean_rbps = mean(mu_sm(:,pos),2);
       p_mean_rbps = zeros(T,1);
       f_mean_rbps = zeros(T,1);
       for k=1:1
          for g=1:T,
            if 1
                p_mean_rbps(g,k) = mean(p_plot_rbps(g,:,k),2);
                f_mean_rbps(g,k) = mean(f_plot_rbps(g,:,k),2);
            else
            % p_mean_rbps(g,k) = (j-1)*
            end
          end
      end;
    
      z = j/nj;
      est_arr.a(:,z) = a_mean_rbps;
      est_arr.p(:,z) = p_mean_rbps;
      est_arr.f(:,z) = f_mean_rbps;
      est_arr.t(z) = flops;
  end
end;   % End of j loop over samples.

est = struct('a',a_mean_rbps,...
    'p',p_mean_rbps,'f',f_mean_rbps,'t',flops);
est_sp = struct('mu',mu_sm, 'z',z_rbps);

if nfig == 1
    
figure(1),
subplot(311),
for k=1:1
    plot(t,sign(a_mean_rbps(end,k))*a_mean_rbps(:,k));
    hold on;
end
subplot(312),
for k=1:1
    plot(t,p_mean_rbps(:,k));
    hold on;
end
subplot(313),
for k=1:1
    plot(t,f_mean_rbps(:,k));
    hold on;
end

Tst = 10;
if 0
L1_error_rbps = norm(f_mean_rbps(Tst:end)-4,1)/(T-Tst+1);
disp(' ');
disp('Time averaged L1 errors ');
disp('-----------------------------');
disp(' ');
disp(['RBps    = ' num2str(L1_error_rbps)]);
disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['RBps    = ' num2str(time_rbps)]);
disp(' ');
end

figure(2)
clf;
domain = zeros(N,1);
range = zeros(N,1);
thex=[0:0.5:fs/2];
hold on
ylabel('j','fontsize',15)
zlabel('Pr(f_t|y_{1:t})','fontsize',15)
xlabel('f_t','fontsize',15)
for k=1:1
    for j=Tst:1:T,
%      [range,domain]=hist(f_plot_rbps(j,:,k)',floor(N/5));
      [range,domain]=hist(f_plot_rbps(j,:,k)',thex);
      waterfall(domain,j,range/sum(range));
      hold on;
    end;
end
view(-30,80);
rotate3d on;
a=get(gca);
set(gca,'ygrid','off');
title('RBps')

figure(3)
clf;
domain = zeros(N,1);
range = zeros(N,1);
% thex=[0.5:0.05:n_z+.5];
hold on
ylabel('t','fontsize',15)
zlabel('Pr(p_t|y_{1:t})','fontsize',15)
xlabel('p_t','fontsize',15)
for j=Tst:1:T,
  [range,domain]=hist(p_plot_rbps(j,:,1)',floor(N/5));
  waterfall(domain,j,range/sum(range))
end;
view(-30,80);
rotate3d on;
a=get(gca);
set(gca,'ygrid','off');
title('RBps')

end

