///////////////////////////////////////////////////////////////////////
//   FILTRAGE DE SIGNAL NON LINEAIRE NON GAUSSIEN (dim=1)
// 
//   X(n)=(epsilon(n) a+(1-epsilon(n) b) X(n-1) +  W(n)   
//   Y(n)=c X(n)  +V(n)
//
//   V(n) iig gaussiennes, epsilon(n) iid Bernoulli \in {0,1},
//   W(n) iid gaussiennes, ou uniformes sur [0,L] <===Cas le + intéressant
//   
//
//   Comparaisons entre :
//
//   1) Filtre de Kalman "entendu"
//   2) Systemes de particules en interaction de type genetique 
//      (copies en interaction du signal)
//   3) Lissage par arbre genealogique
////////////////////////////////////////////////////////////////////////////////
//QQ valeurs : T=100, err=50, sigmav= de 1 a 10, x0=3, N=50 L= de 1 a 10
//             T=50           sigmav=0.5, 0.7  sigmaw=1 N=50        
//                         
//////////////////////////////////////////////////////

clear
stacksize(90000000)
T=input('Horizon temporel ? \n ') ;  
err=input('Erreur d initialisation des filtres ? \n ');
sigmav=input('Ecart type des perturbations de mesure ? \n ');
x0=input('Condition initiale du signal ? \n ');
N=input(' Nombre de particules ? \n ') ;
Choix=input('Choix des amplitudes de saut : \n 1) gaussiennes \n 2) uniformes entre 0 et L \n '); 
if Choix==1 then sigmaw=input(' Variance des bruits ? \n ');
elseif Choix==2 then L=input(' Amplitude des sauts  ? \n ');
end

m0=1/2;
sigma0=1;
//x0=grand(1,1,'nor',m0,sigma0); 
X=[x0];
err0=m0+err;
a=1;
b=0.1;
c=1;
p=.9;
//p=0.1;



//Choix des bruits et aleas des mesures et du signal
if Choix==1 then W=grand(1,T,'nor',0,sigmaw);
elseif Choix==2 then W=grand(1,T,'unf',0,L); 
end

V=grand(1,T+1,'nor',0,sigmav);
epsi=bool2s(rand(1,T,'uniform')<p);

///////////////////////////////////////
// Dynamique du signal et observations
//////////////////////////////////////



for t=1:T
X=[X,(a*epsi(t)+(1-epsi(t))*b)*X(:,t)+W(t)];
end
Y=c*X+V;

///////////////////////////////////////
// Filtre de Kalman-Bucy linéarisé 
///////////////////////////////////////
PKalm=err0*ones(1,T);
CKalm=err0*ones(1,T);
PRicc=(sigma0^2)*ones(1,T);
CRicc=(sigma0^2)*ones(1,T);


if Choix==2 then sigmaw=sqrt(L^2/3);
end

Rapport_signal_sur_bruit=((sigmaw^2))/(sigmav^2)

for t=2:T+1
[PKalm(t),PRicc(t),CKalm(t-1),CRicc(t-1)]=kalm(Y(t-1),PKalm(t-1),PRicc(t-1),(p*a+(1-p)*b),1,c,sigmaw^2,sigmav^2); 
end


 xset("window",1);
     xbasc();
     plot2d(1:T,X(1:T),5)
     plot2d(1:T,Y(1:T)/c,3)
     plot2d(1:T,CKalm(1:T),1)
     xtitle(['Filtre de Kalman (noir)'; 
     '(signal en rouge, observations(/c) en vert)'],'axe du temps','espace d etat')


//////////////////////////////////////////////////////////
// Filtres particulaires de type génétiques
////////////////////////////////////////////////////////

//Initialisations         
         
XIp=grand(N,T,'nor',err0,sigma0); //Positions


if Choix==1 then Wp=grand(N,T,'nor',0,sigmaw); //Jeux d'alèas 
elseif Choix==2 then Wp=grand(N,T,'unf',0,L); 
end

epsiI=bool2s(rand(N,T,'uniform')<p);

MoyI=[sum(XIp(:,1))/N]; //Estimateurs empiriques






for s=2:T
    //Etape de prédiction
    XIp(:,s)=(a*epsiI(:,s)+(1-epsiI(:,s))*b).*XIp(:,s-1)+Wp(:,s);
   


//Etape de correction/sélection
V=(Y(s)*ones(N,1)-c*XIp(:,s));
G=exp(-(V.*V)/(2*sigmav^2));
P=G/sum(G);
Gsup=sort(G);
G=G/Gsup(1); //(en cas de pb de division par 0)
P=G/sum(G);
F=cumsum(P);


E=rand(N,1,'uniform');
Ac=find(E<G);
NAc=length(Ac);
Rej=find(E>=G);
NRej=length(Rej);



Expo=-log(grand(1,NRej+1,'def'));
Texpo=cumsum(Expo);  
Vexp=Texpo/Texpo(NRej+1);



    Xmulti=ones(NRej,s);
    for i=1:NRej
        k=find(F>=Vexp(i));
	Xmulti(i,1:s)=XIp(k(1),1:s);
    end

    if NRej>0 then
       XIp(Rej,1:s)=Xmulti;
    end
    

//Evolution de l'arbre
     xset("window",2);
     xbasc();
     plot2d(1:s,XIp(:,1:s)')
     plot2d(1:s,X(1:s),5)
     xtitle(['Evolution de l arbre genealogique des filtres particulaires'; 
     '(signal en rouge)'],'axe du temps','espace d etat')

MoyI=[MoyI,sum(XIp(:,s))/N];
end


MoyIa=[];
for s=1:T
MoyIa=[MoyIa,sum(XIp(:,s))/N];
end

     xset("window",3);
     xbasc();
     plot2d(1:T,X(1:T),5)
     plot2d(1:T,MoyI(1:T),2)   
      plot2d(1:T,CKalm(1:T),1)
     xtitle(['Filtre de Kalman (noir) et moyennes empiriques des filtres particulaires (bleu)'; 
     '(signal en rouge)'],'axe du temps','espace d etat')
     
      xset("window",4);
     xbasc();
     plot2d(1:T,X(1:T),5)
     plot2d(1:T,MoyIa(1:T),2)   
      plot2d(1:T,CKalm(1:T),1)
      xtitle(['Filtre de Kalman (noir)';' et moyennes empiriques lissees des filtres particulaires (bleu)'; 
     '(signal en rouge)'],'axe du temps','espace d etat')