%  Notes
%  When resampling, distribution of points > point weighting
%  If no resampling, look at the point weighting.
%  noise driving the PF motion update is required!


% Plant Eqs.   SISO 

plantno=2;

if plantno==1,
    dt=.01;
    A=-.1;
    B=1;
    C=1;
    D=[0];
    sysc=ss(A,B,C,D);
    sysd=c2d(sysc,dt,'zoh');
    [Phi,Gam,C,D]=ssdata(sysd);
    sigmaw=5;
    sigmav=.1;       %note: sigmav too small-->big gain-->estimator roots too fast                  
    Q=sigmaw^2;
    R=sigmav^2;
    RI=inv(R);
    npts=500;

elseif plantno==2,
    dt=.01;
    zeta=0.7;
    wn=1;
    A=[0 1; -wn^2 -2*zeta*wn];
    B=[0;1];
    C=[1 0];
    D=[0];
    sysc=ss(A,B,C,D);
    sysd=c2d(sysc,dt,'zoh');
    [Phi,Gam,C,D]=ssdata(sysd);
    sigmaw=5/3;
    sigmav=.1*2;       %note: sigmav too small-->big gain-->estimator roots too fast                  
    Q=sigmaw^2;
    R=sigmav^2;
    RI=inv(R);
    npts=2000;
end




% process noise w=N(0,sigma_w);  
% meas noise v=N(0,sigma_v);
sigmaw=5;
sigmav=1;       %note: sigmav too small-->big gain-->estimator roots too fast                  
Q=sigmaw^2;
R=sigmav^2;
RI=inv(R);

[nstates,ncontrols]=size(Gam);



%xxhat=xx;
%P=eye(2);
%M=1*eye(2);                         %%%%%?

%npts=500 ;
uout=zeros(1,npts);
xPlantout=zeros(npts,nstates);
zout=zeros(1,npts);
tout=zeros(1,npts);
w1out=zeros(1,npts);
vout=zeros(1,npts);
xhatKFout=zeros(npts,nstates);
xbarKFout=zeros(npts,nstates);
sigmaxhatout=zeros(npts,nstates);
sigmaxbarout=zeros(npts,nstates);
sigmaPFout=zeros(npts,nstates);
Neff=zeros(1,npts);
xhatPFout=zeros(npts,nstates);
xhatPFout1=zeros(npts,nstates);
xhatPFout2=zeros(npts,nstates);
iresample=zeros(1,npts);



 
xPlant=ones(nstates,1);  % ic on plant
xPlant=zeros(nstates,1);  % ic on plant
t=-dt;
for i=1:npts
  t=t+dt;
  tout(i)=t;

  % enter a time step  
  %u=1;
  u = sin(2*pi*.1*t);
  
  %w1= normrnd(0,sigmaw);      % update the plant
  w1= sigmaw*randn;      % update the plant
  xPlant=Phi*xPlant+Gam*(u+w1);
  
  %v = normrnd(0,sigmav);
  v = sigmav*randn;
  z=C*xPlant +v;                  % make measurement
  
  w1out(i)=w1;
  vout(i)=v;
  xPlantout(i,:)=xPlant';
  zout(i)=z;
  
  
  % KF
  if i==1
    M=eye(nstates);
    xbarKF=zeros(nstates,1);
  end
  
  P=inv(inv(M)+C'*RI*C);      % P and K could be calculated at end of time step
  sigmaxhatout(i,:)=sqrt(diag(P));
  
  K=P*C'*RI;

  xhatKF=xbarKF+K*(z-C*xbarKF);  % measurement update
  xhatKFout(i,:)=xhatKF';
  
  xbarKF=Phi*xhatKF+Gam*(u);      
  M=Phi*P*Phi'+Gam*Q*Gam';
  xbarKFout(i,:)=xbarKF';
  sigmaxbarout(i,:)=sqrt(diag(M));
  
  
  %PF  
  if i==1,
    %  to start, assume all initial ic's =0 except for x1
    nx1=100;
    x1min=-5;
    x1max=5;
    x1vals=x1min:(x1max-x1min)/nx1:x1max;
    nparticles=max(size(x1vals));
    
    xparticles=zeros(nparticles,nstates);
    xparticles(:,1)=x1vals';

    wpart=ones(1,nparticles)/nparticles;
    %kresamp=10;   %  if you want to force resampling at fixed intervals
    %kk=0;
    
  end
  
  for jj=1:nparticles
    xpart=xparticles(jj,:)';
    %w2=normrnd(0,sigmaw);
    w2=sigmaw*randn;
    %w2=0;
    xpart=Phi*xpart+Gam*(u+w2);             
    zpart=C*xpart;
    % Likelihood function
    %w=(1/sqrt(sigmav^2))*exp(-.5*(zpart-z)^2/sigmav^2);
    w=(1/sqrt(2*pi*sigmav^2))*exp(-.5*(zpart-z)^2/sigmav^2);
    % Weight the particle
    wpart(jj)=wpart(jj)*w;
    xparticles(jj,:)=xpart';
  end  
  %Vector of all particle weights
  %Normalize the weights to unity.
  wpart=wpart/sum(wpart);

  % Check particle variancies to see if any is small enough to throw away.
  Neff(i)=1/sum(wpart.*wpart);  % N=Neff(1)?
  
  % .75 determines when to resample
  if Neff(i)<.75*nparticles
    iresample(i)=1;
    xparticles=resampLV(xparticles,wpart);
    wpart=ones(1,nparticles)/nparticles;
  end
  
  if( 0 )
    figure(4)
    plot(xparticles(:,1),wpart,'.');
    %axis([x1min x1max 0 max(wpart)])
    grid
    pause(0.1)
  end

  for jj=1:nstates
    xhatPFout(i,jj)=sum(xparticles(:,jj).*wpart');    %  should be a weighted mean?
    sigmaPFout(i,jj)=sqrt(sum((xparticles(:,jj).*xparticles(:,jj)).*wpart')  - xhatPFout(i,jj)^2);
  end

end

%meanxhat=mean(xhatout)                 xhat is the mean  xx? but not const
%vv=xhatout-meanxhat*ones(1,npts);
%sigmaxhat=sqrt(mean(vv.*vv))

if(~exist('position'))
  position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end
fontsize = 20;

figure(1)
plot(tout,xPlantout(:,1),'k',tout,zout,'r',tout,xhatKFout(:,1),'b',tout,xhatKFout(:,1)+sigmaxhatout(:,1),'g',tout,xhatKFout(:,1)-sigmaxhatout(:,1),'g')
title('state#1: Plant(k) KF(b) KF bounds(g)')  
set(gcf,'position',position)
%set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
grid on

figure(2)
plot(tout,sigmaxhatout(:,1),tout,sigmaPFout(:,1))
title('sigma of state#1 KF(b) PF(r)')
set(gcf,'position',position)
%set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
grid on

figure(4)
plot(xparticles(:,1),wpart,'.');
title('particle range and weighting')
%axis([x1min x1max 0 2])
grid
set(gcf,'position',position)
%set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
grid on

%figure(5)
%fo=fit(xpart',wpart','gauss1');
%sigmaPF=fo.c1
%meanPF=fo.b1
val=-1;
count1=0;
count2=0;
for i=1:npts
    if iresample(i)==1; val=-val;end
    if val>0 
            count1=count1+1;
            tout1(count1)=tout(i);
            xhatPFout1(count1)=xhatPFout(i);
    else
            count2=count2+1;
            tout2(count2)=tout(i);
            xhatPFout2(count2)=xhatPFout(i);
    end
end

figure(6)
plot(tout,xPlantout(:,1),'k',tout,xhatKFout(:,1),'b',tout1(1:count1),xhatPFout1(1:count1,1),'.r',tout2(1:count2),xhatPFout2(1:count2,1),'.g')
title('state#1:  Plant(k) KF(b) PF(r)')
set(gcf,'position',position)
%set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
grid on

figure(7)
nbin=10;
[yhist,binlocation]=hist(xparticles(:,1),nbin);
plot(binlocation,yhist,'*')
title('histogram of particle distribution (state#1)')
set(gcf,'position',position)
%set(gca,'fontsize',fontsize,'fontweigh',fontweight)
set(gca,'fontsize',fontsize);
set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
grid on


 

    
