function out = EKF(log,filter)
% out: cell que contiene, en la primera celda, una
% matriz Nx3 donde cada fila se corresponde con una pose del robot, y en la
% segunda celda, a su vez, otro cell, que contiene en cada celda la matriz
% de covarianza de la pose correspondiente a ese paso
% Variables
% log: matriz con las lecturas del robot
% filter: Si se hace o no la corrección
 
r = 0.062/2; % Radio de las ruedas del bot
rk = 0.1^2; % Varianza asociada a la incertidumbre del sensor
var = (0.05/2)^2; % Varianza de la posición inicial
map = [0 0 1 0; 0 0 0 1.09; 1 0 1 1.09; 0 1.09 1 1.09]; % mapa
 
% Crea la matriz de pose y el cell de matrices de covarianzas, S
pose = zeros(length(log),3);
S = cell(length(log), 1);
 
% Establece el valor inicial de ambos
pose(1,:) = [0.25 0.25 pi/2];
S{1} = [var 0 0; 0 var 0; 0 0 (deg2rad(5)/2)^2];
 
% Genera los valores iniciales para la odometria
t1=log(1,1);
left1=log(1,2);
right1=log(1,3);
 
for i=2:1:length(log)
    % Guarda los valores anteriores para la odometria
    t0 = t1;
    left0 = left1;  
    right0 = right1;
   
    % Obtiene los nuevos valores para la odometria
    t1=log(i,1);
    left1=log(i,2);
    right1=log(i,3);
   
    % Obtiene la nueva pose
    pose(i,:) = odometry(pose(i-1,:),t0,left0,right0,t1,left1,right1);
   
    % Obtiene el jacobiano, Gk
    theta = pose(i,3);
    incr=deg2rad(right1-right0)*r;
    incl=deg2rad(left1-left0)*r;
    Gk = [1 0 -((incl+incr)/2)*sin(theta);0 1 ((incr+incl)/2)*cos(theta); 0 0 1];
    % Gk = [dg1/x dg1/y dg1/theta; dg2/x dg2/y dg2/theta; dg3/x dg3/y dg3/theta]

	% Varianza de los incrementos 
    sigmaXX = (0.05*(pose(i,1)-pose(i-1,1)))^2;
    sigmaYY = (0.05*(pose(i,2)-pose(i-1,2)))^2;
    sigmaTT = (0.573*(pose(i,3)-pose(i-1,3)))^2;
    % Calcula la matriz que introduce la intertidumbre del movimiento, Qk
    Qk = [sigmaXX 0 0; 0 sigmaYY 0; 0 0 sigmaTT];
   
    % Recalcula la matriz de covarianza tras realizar un movimiento
    S{i} = Gk*S{i-1}*(Gk.')+Qk;
 
    % Se predice la medida del sensor
    [ok,Hk,mu_zk] = eq3(map,pose(i,:),Tsonar2univ(pose(i,:),-0.063,-0.045, pi/2));
   
    if filter && ok
        % Calcula la incertidumbre de la observación
        Szk = Hk*S{i}*Hk.'+rk; % rk = varianza de la medida del sensor
        Spok = S{i}*Hk.';
        Kk = Spok*inv(Szk);
        % Corrige la pose y la matriz de covarianza
        pose(i,:) = pose(i,:)+(Kk.')*(log(i,4)*0.01-mu_zk);
        S{i} = S{i}-Kk*Hk*S{i};
    end
end
 
out = {pose, S};
 
end