function EKFLocalization
    clear; close all;


    % Map configuration
    Size = 50;
    NumLandmarks = 10;
    Map=CreateMap(NumLandmarks, Size);  % Create map of size [Size*2 Size*2]

    %mode = 'one_landmark';
    %mode = 'one_landmark_in_fov';
    mode = 'landmarks_in_fov';

    % Sensor characterization
    SigmaR = 1; % Standard deviation of the range
    SigmaB = 0.7; % Standard deviation of the bearing
    Q = diag([SigmaR^2 SigmaB^2]); % Cov matrix
    fov = pi/2;        % fov = 2*alpha
    max_range = 1 * Size;  % maximum sensor measurement range

    % Robot base characterization
    SigmaX = 0.8; % Standard deviation in the x axis
    SigmaY = 0.8; % Standard deviation in the y axis
    SigmaTheta = 0.1; % Bearing standar deviation
    R = diag([SigmaX^2 SigmaY^2 SigmaTheta^2]); % Cov matrix

    % Initialization of poses
    x = [-Size+Size/3 -Size+Size/3 pi/2]';      % Ideal robot pose
    xTrue = [-Size+Size/3 -Size+Size/3 pi/2]';  % Real robot pose
    xEst = [-Size+Size/3 -Size+Size/3 pi/2]';   % Estimated robot pose by EKF
    sEst = zeros(3,3);                          % Uncertainty of estimated robot pose

    % Drawings
    plot(Map(1,:),Map(2,:),'sc');
    axis([-Size-5 Size+5 -Size-5 Size+5]);
    hold on;
    DrawRobot(x,'r');
    DrawRobot(xTrue,'b');
    DrawRobot(xEst,'g');
    PlotEllipse(xEst,sEst,4,'g');

    nSteps = 20; % Number of motions
    turning = 5; % Number of motions before turning (square path)

    u = [(2*Size-2*Size/3)/turning;0;0]; % Control action

    pause;

    % Let's go!
    for (k = 1:nSteps-3) % Main loop

        u(3) = 0;
        if mod(k,turning) == 0 % Turn?
            u(3) = -pi/2;
        end

        x = tcomp(x,u);             % New pose without noise
        noise = sqrt(R)*randn(3,1);     % Generate noise
        noisy_u = tcomp(u,noise);   % Apply noise to the control action
        xTrue = tcomp(xTrue,noisy_u);   % New noisy pose (real robot pose)

        % Get sensor observation/s
        %

        % Prediction (done for all steps)
        xMed = tcomp(xEst,u);
        incertidumbreXmed = J1(xEst, u)*sEst *J1(xEst, u)' + J2(xEst, u)*R *J2(xEst, u)';

        if(strcmp(mode, 'one_landmark') == 1)

            [z, l] = getRandomObservationFromPose(xTrue, Map, Q);
            line([xTrue(1), Map(1,l)], [xTrue(2), Map(2,l)], 'Color', 'red');

            lxy = [Map(1,l) Map(2,l)];
            K = incertidumbreXmed*getObsJac(xMed,lxy)' * inv(getObsJac(xMed, lxy) * incertidumbreXmed *getObsJac(xMed, lxy)' + Q);
            zH = [z(1)-(getDistance(xMed, lxy)) ; z(2)-(getBearing(xMed,lxy))];
            xEst = xMed + K * zH;
            sEst = (eye(3) - K * getObsJac(xMed, lxy))*incertidumbreXmed;
        end



        if(strcmp(mode,'one_landmark_in_fov') == 1)
            restrictedMap = getObservationsInsideFOV(xTrue,Map,fov,max_range, Q);
            [z, l] = getRandomObservationFromPose(xTrue,restrictedMap,Q);
            if(l > 0)
                line([xTrue(1), restrictedMap(1,l)], [xTrue(2), restrictedMap(2,l)], 'Color', 'red');
                %
                % EKF Localization
                %
                lxy = [restrictedMap(1,l) restrictedMap(2,l)];



                % Correction
                K = incertidumbreXmed*getObsJac(xMed,lxy)' * inv(getObsJac(xMed, lxy) * incertidumbreXmed *getObsJac(xMed, lxy)' + sqrt(Q));
                zH = [z(1)-(getDistance(xMed, lxy)) ; z(2)-(getBearing(xMed,lxy))];

                xEst = xMed + K * zH;
                sEst = (eye(3) - K * getObsJac(xMed, lxy))*incertidumbreXmed;

            else
                xEst = xMed;
                sEst = incertidumbreXmed;
            end
        end
        if(strcmp(mode,'landmarks_in_fov') == 1)

            restrictedMap = getObservationsInsideFOV(xTrue,Map,fov,max_range, Q);
            [z, l] = getKRandomObservationsFromPose(xTrue,restrictedMap,Q);
            if(l(1) > 0)
                newMap = [];
                for r=1:length(l)
                    newMap = [newMap ; [restrictedMap(1,l(r)) restrictedMap(2,l(r))]];
                    line([xTrue(1), restrictedMap(1,l(r))], [xTrue(2), restrictedMap(2,l(r))], 'Color', 'red');
                end
                %
                % EKF Localization
                %
                newMap = newMap';
                klands = length(l);

                % Correction
                K1 = incertidumbreXmed*getKObsJac(xMed,newMap)';
                %Create a replicated Q
                Qrep = eye(2*klands);
                for kk=1:(2*klands)
                    if (mod(kk,2) == 0)
                        Qrep(kk,kk) = Q(1,1);
                    else
                        Qrep(kk,kk) = Q(2,2);
                    end
                end

                K2 = inv(getKObsJac(xMed, newMap) * incertidumbreXmed *getKObsJac(xMed, newMap)' + Qrep);
                K = K1 * K2;

                zH = [];
                newMap
                for kk=1:klands
                    zHone = [0 0]';

                    zHone(1) = z(kk,1)-(getDistance(xMed, [newMap(1,kk) newMap(2,kk)])) ;
                    zHone(2) = z(kk,2)-(getBearing(xMed, [newMap(1,kk) newMap(2,kk)]));
                    zH = [zH ; zHone];
                end


                xEst = xMed + K * zH;
                sEst = (eye(3) - K * getKObsJac(xMed, newMap))*incertidumbreXmed;

            else
                xEst = xMed;
                sEst = incertidumbreXmed;
            end

        end
        %line([xTrue(1), Map(1,l)], [xTrue(2), Map(2,l)], 'Color', 'g');



        % Drawings
        DrawRobot(x,'r');
        %if(strcmp(mode, 'one_landmark')==0)drawFOV(xTrue,fov,max_range,'b'); end
        DrawRobot(xTrue,'b');
        DrawRobot(xEst,'g');
        PlotEllipse(xEst,sEst,4,'g');

        pause;

    end

end % main

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Create Map

function Map=CreateMap(NumLandmarks, Size)

Map=Size*2*rand(2,NumLandmarks)-Size;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%get a Random observation (1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%landmark)

function [z,landmark] = getRandomObservationFromPose(x,Map,Q)
    z = [0 0]; %Initially we have no observation and no landmark
    landmark = -1;
    if (size(Map,2) > 0) %if there is any landmark in the map
        landmark = round(abs(rand)*(size(Map,2)-1))+1; %choose one random landmark
        d = getDistance([Map(1,landmark) Map(2,landmark)], x) + randn * sqrt(Q(1,1)); %get the distance from the pose to the observed landmark
        ang = getBearing([Map(1,landmark) Map(2,landmark)], x) + randn * sqrt(Q(2,2)); %get the bearing
        z = [d ang]; %return it as a vector
    end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%get a Random observation (K
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%landmarks

function [z,landmarks] = getKRandomObservationsFromPose(x,Map,Q)
    z = [];
    landmarks = [];
    if (size(Map,2) > 0)
        for i=1:size(Map,2)
            if(rand <= 0.75) %Only take 75percent of the landmarks
                d = getDistance([Map(1,i) Map(2,i)], x) + randn * Q(1,1);
                ang = getBearing([Map(1,i) Map(2,i)], x) + randn * Q(2,2);
                z = [z ; [d ang]];
                landmarks = [landmarks ; i];
            end
        end
    end
    if (length(landmarks) == 0)
        landmarks = -1;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%get Jacobians for the 1-landmark
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%detection

function jH = getObsJac(xPred,Landmark)
    r = sqrt((Landmark(1)-xPred(1))^2 + ((Landmark(2)-xPred(2))^2));
    a = -(Landmark(1) - xPred(1))/r;
    b = -(Landmark(2) - xPred(2))/r;
    c = (Landmark(2) - xPred(2))/(r^2);
    d = -(Landmark(1) - xPred(1))/(r^2);
    jH = [ a b 0 ; c d -1];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%get Jacobians for the K-landmarks
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%detection

function jH = getKObsJac(xPred,resMap)
    jH = [];
    for i=1:size(resMap,2)
        Landmark = [resMap(1,i) resMap(2,i)];
        r = sqrt((Landmark(1)-xPred(1))^2 + ((Landmark(2)-xPred(2))^2));
        a = -(Landmark(1) - xPred(1))/r;
        b = -(Landmark(2) - xPred(2))/r;
        c = (Landmark(2) - xPred(2))/(r^2);
        d = -(Landmark(1) - xPred(1))/(r^2);
        jTemp = [ a b 0 ; c d -1];
        jH = [jH ; jTemp];
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%getObservations for 1-Landmark
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%detection

function MapFOV = getObservationsInsideFOV(x,Map,fov,max_range, Q)
    %Check which landmarks are in range
    MapFOV = [;];
    for i=1:size(Map,2)
        d = getDistance([Map(1,i) Map(2,i)], x);
        ang = (getBearing([Map(1,i) Map(2,i)], x));
        %line([x(1), Map(1,i)], [x(2), Map(2,i)], 'Color', 'magenta');
        if (d <= max_range)
            if((ang - x(3)) < fov/2 && (ang-x(3)) >= -fov/2)
                MapFOV = [ MapFOV [Map(1,i) ; Map(2,i)]];
            end
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%getDistance given cartesian
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%coordinate points

function d = getDistance(p1, p2)
    d = (sqrt((p1(1) - p2(1))^2 + ((p1(2) - p2(2))^2)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%get bearing between two points p1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%and p2

function ang = getBearing(p1, p2)
    ang = atan2(p1(2) - p2(2), p1(1) - p2(1));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Draw Field of View

function h = drawFOV(x,fov,max_range,c)

if nargin< 4; c = 'b'; end

alpha = fov/2;
angles = -alpha:0.01:alpha;
nAngles = size(angles,2);
arc_points = zeros(2,nAngles);

for i=1:nAngles
arc_points(1,i) =  max_range*cos(angles(i));
arc_points(2,i) =  max_range*sin(angles(i));

aux_point = tcomp(x,[arc_points(1,i);arc_points(2,i);1]);
arc_points(:,i) = aux_point(1:2);
end

    h = plot([x(1) arc_points(1,:) x(1)],[x(2) arc_points(2,:) x(2)],c);

end