clear

clear
stacksize(90000000)
T=input('Horizon temporel ? \n ') ;  
err=input('Erreur d initialisation des filtres ? \n ') ;
sigmaw=input('Variance des bruits du signal  ? \n ') ;
sigmav=input('Variance des perturbations de mesure ? \n ') ;
a=input(' Parametre de derive du signal ? \n ') ;


m0=0;
sigma0=1;
x0=grand(1,1,'nor',m0,sigma0); 
err0=m0+err;
b=1;
c=1;







W=grand(1,T,'nor',0,sigmaw);
V=grand(1,T+1,'nor',0,sigmav);
///////////////////////////////////////
// Dynamique du signal et observations
//////////////////////////////////////
X=[x0];
for t=1:T
X=[X,a*X(t)+b*W(t)];
end
Y=c*X+V;


 xset("window",1);
     xbasc();
     plot2d(1:T,X(1:T),5)
     plot2d(1:T,Y(1:T)/c,3)
     xtitle(['Signal en rouge, et Observations(/c) en vert'],'axe du temps','espace d etat')

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

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,sigmaw^2,sigmav^2); 
end

///////////////////////////////////////
// Filtres de Kalman 
//////////////////////////////////////
Xp=err0*ones(1,T);
Pp=sigma0*ones(1,T);
Pc=zeros(1,T);
Gain=zeros(1,T);

for s=2:T+1

//Correction
    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);
    
//Prédiction    
    Xp(s)=a*Xc(s-1);
    Pp(s)=a*Pc(s-1)*a+(b.*b)*sigmaw^2;


end

     xset("window",2);
     xbasc();
     plot2d(1:T,X(1:T),5)
     plot2d(1:T,Y(1:T)/c,3)
     plot2d(1:T,CKalm(1:T),1)
     plot2d(1:T,Xc(1:T),1)
     xtitle(['Filtre de Kalman Scilab (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,Xc(1:T),1)
     xtitle(['Filtre de Kalman (noir)'; 
     '(signal en rouge, observations(/c) en vert)'],'axe du temps','espace d etat')
     
Diff=CKalm(1:T)-Xc(1:T)';
     
     
       xset("window",4);
     xbasc();
      plot2d(1:T,Diff,1)
     xtitle(['Difference entre ces deux filtres'],'','')