% NN = [6000 3000 1500 1000 700 500 450 400 300 200];
% NN=20000;
NN =[5000 2000 1000 800 600 400 200];
M = 200;
pf_res = []; ps_res =[];
nfig = 0;
for j=1:length(NN)
    [est_pf,mu,Sigma,z_rbpf,w,BBt,par,z_rbpf_pred_v] = ...
           frq_rbpf3(x,t,fs,tao,gm,prs,amp,NN(j),nfig,grnd);
    pf_res = [pf_res struct('est_pf',est_pf,'mu',mu,'Sigma',Sigma,...
            'z_rbpf',z_rbpf,'w',w)];
  
    [est_sm,est_arr_sm,est_sp_sm] = frq_rbps3(pf_res(j).mu,pf_res(j).Sigma,...
        pf_res(j).z_rbpf, pf_res(j).w,...
        BBt,par,z_rbpf_pred_v,t,fs,M,0);
    ps_res = [ps_res struct('m',est_sm,'m_arr',est_arr_sm,...
            'sp',est_sp_sm)];
end

pf_res_fn = [];
ps_res_fn = [];
for j=1:length(NN)
    pf_res_fn = [pf_res_fn, struct('est_pf',pf_res(j).est_pf)];
    ps_res_fn = [ps_res_fn, struct('m_arr',ps_res(j).m_arr)];
end

av = []; pv = []; fv = [];
for i=1:T
    if 1
        av(i) = var(pf_res(1).mu(i,:));
        pv(i) = var(pf_res(1).z_rbpf(1,i,:));
        fv(i) = var(pf_res(1).z_rbpf(2,i,:));
        nm = NN(1);
    else
        av(i) = var(ps_res(end).sp.mu(i,:));
        pv(i) = var(ps_res(end).sp.z(1,i,:));
        fv(i) = var(ps_res(end).sp.z(2,i,:));
        nm = M;
    end
end
   
pt = 10:81;
figure(2),subplot(311),plot(t(pt),sqrt(av(pt)/nm));
subplot(312),plot(t(pt),sqrt(pv(pt)/nm));
subplot(313),plot(t(pt),sqrt(fv(pt)/nm));

for j=1:length(NN)
figure(1),
subplot(311),hold off,plot(t,pf_res(j).est_pf.a*sign(pf_res(j).est_pf.a(end)))
title(['NN: ' ,num2str(NN(j))])
hold on,
subplot(312),hold off,plot(t,pf_res(j).est_pf.p),
hold on,
subplot(313),hold off,plot(t,pf_res(j).est_pf.f)
hold on,
subplot(311),plot(t,amp,'--')
hold on,
% plot(t,5*t.^2 +15,'--')
subplot(313),
plot(t,grnd,'--')
hold off
pause;
end

if 0
j = 1;
M = 400;
[est_sm,est_arr_sm] = frq_rbps3(pf_res(j).mu,pf_res(j).Sigma,...
    pf_res(j).z_rbpf, pf_res(j).w,...
    BBt,par,z_rbpf_pred_v,t,fs,M,0);
end

for j=1:size(est_arr_sm.a,2)
figure(1),
subplot(311),hold off,plot(t,est_arr_sm.a(:,j)*sign(est_arr_sm.a(end,j)))
title(['M: ' ,num2str(j*10)])
hold on,
subplot(312),hold off,plot(t,est_arr_sm.p(:,j)),
hold on,
subplot(313),hold off,plot(t,est_arr_sm.f(:,j))
hold on,
subplot(311),plot(t,amp,'--')
hold on,
% plot(t,5*t.^2 +15,'--')
subplot(313),
plot(t,grnd,'--')
hold off
pause;
end