///////////////////////////////////////////////////////////////////////
//   FILTRAGE DE SIGNAL NON LINEAIRE NON GAUSSIEN (dim=1)
// 
//   X(n)=a X(n-1)+ b espsilon(n)  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], ou sur [-L,L], ...
//   
//
//   Comparaisons entre :
//
//   1) Filtre de Kalman "entendu"
//   2) Filtres de Kalman en interaction
//   3) Systemes de particules en interaction de type genetique 
//      (copies en interaction du signal)
//   4) Lissage par arbre genealogique
//
// Remarque : Les lisseurs particulaires peuvent etre moins performants que
//les filtres optimaux. Ceci provient des pb de pertes d'ancêtres. 
//////////////////////////////////////////////////////////////////////////////


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 ');
a=input(' Parametre de derive du signal ? \n ') ;
p=input(' Frequence des sauts  ? \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 3) uniformes entre -L et L \n 4) poissonniennes d intensite lambda \n 5) Produit N(0,1) * poisson d intensite lambda\n '); 
if Choix==1 then sigmaw=input(' Variance des amplitudes  ? \n ');
elseif Choix==2 then L=input(' Amplitude des sauts ? \n ');
elseif Choix==3 then L=input(' Amplitude des sauts ? \n ');
elseif Choix==4 then lambda=input('Intensite des amplitudes des sauts ? \n ');
elseif Choix==5 then lambda=input('Intensite des amplitudes des sauts ? \n ');
end

m0=0;
sigma0=1;
x0=grand(1,1,'nor',m0,sigma0); 
X=[x0];
err0=m0+err;
b=1;
c=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); 
elseif Choix==3 then W=grand(1,T,'unf',-L,L);
elseif Choix==4 then W=grand(1,T,'poi',lambda);
elseif Choix==5 then W=grand(1,T,'nor',0,1).*grand(1,T,'poi',lambda);
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*X(t)+b*epsi(t)*W(t)];
end
Y=c*X+V;

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

/////////////////////////////////////////////////////////////////////////
//Lorsque les aleas du signal ne sont pas gaussiens 
//les filtres de Kalman ne sont plus "optimaux". 
//Pour les construire, l'on considere que les observations proviennent
//de signaux conduits par des v.a. gaussiennes de variance
// sigmaw^2(n)=E(epsilon(n)^2W(n)^2)
//
//Lorsque les amplitudes de sauts du signal ne sont pas gaussiens 
//les filtres de Kalman en interaction ne sont pas non plus "optimaux"!!!! 
//Pour les construire, l'on considere que les observations proviennent
//de signaux conduits par des sauts gaussiens de variance
// sigmaw^2(n)=E(W(n)^2)
////////////////////////////////////////////////////////////////////////

if Choix==2 then sigmaw=sqrt(L^2/3);
elseif Choix==3 then sigmaw=sqrt(4*L^2/3);
elseif Choix==4 then sigmaw=sqrt(lambda*(1+lambda));
elseif Choix==5 then sigmaw=sqrt(lambda*(1+lambda));
end

Rapport_signal_sur_bruit=(p*(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),a,b,c,p*sigmaw^2,sigmav^2); 
end

/////////////////////////////////////////////////////////
// Filtres de Kalman en interaction
// Chaque filtre évolue avec un jeu simulé de dates de sauts (espsi)
// Les filtres interagissent en fonction de la vraisemblance de
// ces dates en fonction des observations reçues.
////////////////////////////////////////////////////////
//Initialisation des prédicteurs et correcteurs (prédiction de la position Xp, 
//prédiction des variances d'erreurs Pp, variances d'erreurs Pc)
// Simulation des jeux de paramètres de pilotage des Kalman en interaction (epsi) 
/////////////////////////////////////////////////////////////////


Xp=err0*ones(N,T);
Pp=sigma0*ones(N,T);
Pc=zeros(N,T);
epsi=bool2s(rand(N,T,'uniform')<p);
Gain=zeros(N,T);
Moy=[];



for s=2:T

    //Etape de prédiction des filtres de Kalman en interaction
    Gain(:,s-1)=(Pp(:,s-1)*c).*(c*Pp(:,s-1)*c+sigmav^2)^(-1);
    Xc(:,s-1)=Xp(:,s-1)+Gain(:,s-1).*(Y(s-1)-c*Xp(:,s-1));
    Pc(:,s-1)=(1-Gain(:,s-1)*c).*Pp(:,s-1);
    Xp(:,s)=a*Xc(:,s-1);
    Pp(:,s)=a*Pc(:,s-1)*a+((b*epsi(:,s)).*(b*epsi(:,s)))*sigmaw^2;


//Etape de Sélection des filtres les plus vraisemblables
V=(Y(s)*ones(N,1)-c*Xp(:,s));
G=exp(-(V.*V).*((2*(sigmav+c*Pp(:,s)*c))^(-2))+((Y(s)*ones(N,1)).*(Y(s)*ones(N,1))/(2*sigmav)));
Gsup=sort(G);
G=G/Gsup(1);
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);
Pmulti=ones(NRej,s);
Xcmulti=ones(NRej,s-1);
Pcmulti=ones(NRej,s-1);

//Boucle de sélection "longue"
//i=1;
//k=1;
//         while i<=NRej,      
//             if Vexp(i)<=F(k) then Xmulti=[Xmulti;Xp(k,1:s)]; 
//                                   Pmulti=[Pmulti;Pp(k,1:s)];
//                                   Xcmulti=[Xcmulti;Xc(k,1:s-1)]; 
//                                   Pcmulti=[Pcmulti;Pc(k,1:s-1)];
//                                       i=i+1;
//                                       else k=k+1;
//            end                
//        end

//Boucle de sélection plus rapide          
   
  for i=1:NRej
        k=find(F>=Vexp(i));
	Xmulti(i,1:s)=Xp(k(1),1:s);
	Pmulti(i,1:s)=Pp(k(1),1:s);
	Xcmulti(i,1:s-1)=Xc(k(1),1:s-1);
	Pcmulti(i,1:s-1)=Pc(k(1),1:s-1);
    end


         
if NRej>0 then
     Xp(Rej,1:s)=Xmulti;
     Pp(Rej,1:s)=Pmulti;
     Xc(Rej,1:s-1)=Xcmulti;
     Pc(Rej,1:s-1)=Pcmulti;
     
     
end

  
    

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

//Moyennes empiriques corrigées
Moy=[Moy,sum(Xc(:,s-1))/N];

end


//Moyennes empiriques lissées
Moya=[];
for s=1:T-1
Moya=[Moya,sum(Xc(:,s))/N];
end



     xset("window",2);
     xbasc();
     plot2d(1:T-1,X(1:T-1),5)
     plot2d(1:T-1,Y(1:T-1)/c,3)
     plot2d(1:T-1,Moy,2) 
     plot2d(1:T-1,Moya,1) 
     xtitle(['Moyennes empiriques des filtres Kalman en interaction (lissee en noir)';
     '(signal en rouge, observations(/c) en vert)'],'axe du temps','espace d etat')


     xset("window",3);
     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')

     
     xset("window",4);
     xbasc();
     plot2d(2:T,Moy',2)
     plot2d(2:T,CKalm(1:T-1),1)
     plot2d(2:T,X(1:T-1),5)
     xtitle(['Filtre de Kalman (noir)';' et moyennes empiriques des filtres Kalman en interaction (bleu)'; 
     '(signal en rouge)'],'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); 
elseif Choix==3 then Wp=grand(N,T,'unf',-L,L); 
elseif Choix==4 then Wp=grand(N,T,'poi',lambda);
elseif Choix==5 then Wp=grand(N,T,'nor',0,1).*grand(N,T,'poi',lambda);
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*XIp(:,s-1)+b*epsiI(:,s).*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);
//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",5);
     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",6);
     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",7);
     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')