//---------------------------------------------
function draw_simu(x,y,u,col)
   M=[-1  4   5   5  4 -1 -1  -1  ;  //Chassis
      -1 -1 -0.5 0.5 1  1 -1  -1  ;
       1  1  1    1  1  1  1   1  ];
   R=[cos(x(3)),-sin(x(3)),x(1);sin(x(3)),cos(x(3)),x(2);0 0 1];
   M=R*M;    
   xbasc();   ech=40;   isoview(-ech,ech,-ech,ech)
   xset("thickness",2);  xset("color",noir);
   xpoly(Aj(1,:),Aj(2,:));  //piscine
   xpoly(M(1,:),M(2,:),'lines',0)  //robot
   xset("color",col); 
   xpoly([x(1),x(1)+cos(x(3)+x(5))*y(1)],[x(2),x(2)+sin(x(3)+x(5))*y(1)]); //sonar
   MaN=[0 0;0 3*u(1)*x(4);1 1];  //motif accélération normale
   MaT=[0 3*u(2);0 0;1 1];       //motif accélération tangentielle
   MaN=R*MaN; xarrows(MaN(1,:),MaN(2,:),1,vert);
   MaT=R*MaT; xarrows(MaT(1,:),MaT(2,:),1,vert);
endfunction;
//---------------------------------------------
function v=f(x,u)  //x = (x,y,theta,v,alpha]
    v=[x(4)*cos(x(3));x(4)*sin(x(3));u(1);u(2);u(3) ];
endfunction;
//---------------------------------------------
function y=g(x)  //fonction d'observation
	 u=[cos(x(3)+x(5));sin(x(3)+x(5))];
  	m=[x(1);x(2)];
  	dist=%inf;
  	for j=1:size(Aj,2)-1
 	      a=Aj(:,j);
    	   b=Bj(:,j);
    	   if (det([a-m u])*det([b-m u]) <= 0)&(det([a-m b-a])/det([u b-a])>=0)
		       dist=min(det([a-m b-a])/det([u b-a]),dist);
      	 end
  	end
 	y=[dist;x(3);x(5)];
endfunction
//---------------------------------------------
function [D,Yd,Td,Ud]=simu(tmax)
   D=[];Yd=[];Td=[];Ud=[];
   x=[10;20;1.4;1.5;0];  // x=[x,y,theta,v,alpha]
   dt=0.01;  t=0;
   k0_old=int((x(3)+x(5))/(%pi/2));  t_old=0;
   for t=0:dt:tmax,
        u=[0.1*sin(0.21*t)+0.1;0;2];  //thetadot, vdot, alphadot
        y=g(x);    
	      D=[D,[x(3)+x(5);y(1)]];
	      k0=int((x(3)+x(5))/(%pi/2));
	      if (k0>k0_old),
    	      k0_old=k0;
	         Yd=[Yd,y]; Td=[Td,t-t_old];  Ud=[Ud,[u(2);x(4)*u(1)]];
	         t_old=t;
    	   end
 	      draw_simu(x,y,u,modulo(k0,4)+2);  //change de couleur à chaque quart de tour
	      x1=x+f(x,u)*dt;    x=x1;
   end;
endfunction;
//---------------------------------------------
function Draw_Ellipse(zhat,P); 
   s=0:0.05:2*%pi;
   z=zhat(1:2)*ones(s)+sqrtm(P(1:2,1:2))*[cos(s);sin(s)];
   xset('thickness',1);
   xset("color",magenta);
   xpoly(z(1,:),z(2,:));
endfunction
//---------------------------------------------
function kalman();
   zhat=[0;0;0;0];
   Pk=5000*eye(4,4);
   for k=1:size(Td,2);
      Draw_Ellipse(zhat,Pk); 
      dt=Td(k);
      distk=Yd(1,k); thetak=Yd(2,k); alphak=Yd(3,k); uk=Ud(:,k);
      Ak=[1 0 dt 0;0 1 0 dt; 0 0 1 0; 0 0 0 1];
      Bk=[0 0;0 0;-dt*sin(thetak), dt*cos(thetak);dt*cos(thetak), dt*sin(thetak)];
      select modulo(round((thetak+alphak)*2/%pi),4),
         case 0 then yk=[Rx-distk;0];   Ck=[1 0 0 0; 0 0 0 0]; //ping mur droit
         case 1 then yk=[0;Ry-distk];   Ck=[0 0 0 0; 0 1 0 0]; //ping mur fond
         case 2 then yk=[-Rx+distk;0];  Ck=[1 0 0 0; 0 0 0 0]; //ping mur gauche
         case 3 then yk=[0;-Ry+distk];  Ck=[0 0 0 0; 0 1 0 0]; //ping mur devant
      end
      Qy=0.5*eye(2,2);   Qx=0.1*eye(4,4);
      Kk=Pk*Ck'*inv(Qy+Ck*Pk*Ck');
      zhat=Ak*(zhat+Kk*(yk-Ck*zhat)) + Bk*uk;
      Pk=Ak'*(Pk-Kk*Ck*Pk)*Ak+Qx;
   end   
endfunction
//---------------------------------------------
//------- Programme principal  ----------------
//---------------------------------------------
rouge=5;bleu=2;blanc=8;vert=3;noir=0;magenta=6;
Rx=20;Ry=50;  //Rayons de la piscine
Aj=[-Rx -Rx  Rx  Rx -Rx ; -Ry  Ry  Ry -Ry -Ry];  //Segments [aj,bj]
Bj=[Aj(:,2:$),Aj(:,1)];                          //formant le tour de la piscine
[D,Yd,Td,Ud]=simu(20);   //Simulation
kalman(); xclick(); xdel();  //Localisation par filtrage de Kalman
plot2d(D(1,:),D(2,:)); xclick(); xdel();