function compute_correspondences() taking in account of covariances(#10…

…) and function observation_to_landmark

Classes/Map.m

@@ -36,12 +36,7 @@
     % https://www.iri.upc.edu/people/jsola/JoanSola/objectes/curs_SLAM/SLAM2D/SLAM%20course.pdf
     function [z, H_x, R] = compute_innovation(map, robot, observation_vector)
 
-        P1, P2 = obj.compute_correspondences(robot, observation_vector)
-
-        if(length(P1) ~ length(P2))
-            error([ 'The number of correspondences is not the same for P1 and P2,',
-                    ' could not compute the innovation vector']);
-        end
+        [P1, P2] = obj.compute_correspondences(robot, observation_vector);
 
         % Initialization:
         dim_z   = 2 * length(P1);       % dimension of the observation vector
@@ -60,7 +55,7 @@
 
             % Compute the estimated observation (with jacobian) bases on the current robot pose 
             % estimation and the landmark position estimation
-            h, Jh_x_rob, Jh_x_land = robot.landmark_observation(landmark);
+            [h, Jh_x_rob, Jh_x_land] = robot.landmark_to_observation(landmark);
 
             % Fill the innovation vector, the Jacobian and the covariance matrix
             z(2*k-1:2*k)                            = observation.z - h;        % eq (19, 26)
@@ -86,37 +81,51 @@
     %   - the first observation is associated to the second landmark;
     %   - the third observation is associated to the fourth landmark;
     %   - the fourth observation is associated to the first landmark.
-    % The correspondence is computed by comparing the distance between the observation and the
-    % landmark. If the distance is lower than a threshold, then the observation is associated to the
-    % landmark.
+    % The correspondence is computed by comparing the distribution of the observations and the
+    % landmarks in the map, taking in account of the covariances.
+    % If 2 observations are associated to the same landmark, the best association is chosen
     function [P1, P2] = compute_correspondences(map, robot, observation_vector)
 
         % Initialization:
         P1 = [];
         P2 = [];
 
-        O = observation_vector;
-
-        landmark = map.landmark_vector();
-        [h, Jh_x_rob, Jh_x_land] = robot.landmark_observation(landmark);
-        L = h;
-
-        for i = 1:length(O)
-            for j = 1:length(L)
-                p1_obs = [O(2*i-1).z, O(2*i).z];
-                p2_land = [L(2*j-1), L(2*j)];
-                d = map.point_point_distance(p1_obs, p2_land);
-                if (d < 0.1)
+        landmarks = map.landmark_vector;
+
+        for i = 1:length(observation_vector)
+            absolute_observation = robot.observation_to_landmark(observation_vector(i));
+            for j = 1:length(landmarks)
+                pdf_land_j = mvnpdf(absolute_observation.x, landmarks(j).x, landmarks(j).P);
+                if pdf_land_j > 0.9
+                    index = find(P2==j);
+                    if size(index,2) > 0
+                        % If the landmark is already associated to another observation, then
+                        % we choose the best association (the one with the highest probability)
+                        pdf_land_index = mvnpdf(absolute_observation.x, landmarks(P2(index)).x, landmarks(P2(index)).P);
+                        if pdf_land_index > pdf_land_j
+                            continue;
+                        else
+                            P1(index) = [];
+                            P2(index) = [];
+
+                            disp('WARNING -> two observation are associated to the same landmark');
+
+                        end
+                    end
                     P1 = [P1; i];
                     P2 = [P2; j];
                 end
             end
         end
+
+        if(length(P1) ~ length(P2))
+            error([ 'The number of correspondences is not the same for P1 and P2,',
+                    ' could not compute the innovation vector']);
+        end
 
     end
 
 
-
     % Update
     function update_map(map, new_laserscan)
         %% TODO
@@ -142,11 +151,5 @@ function loop_closure(map, observation_vector)
 %
 % Here are defined auxiliary functions used in the public members or for other simpler computations
 
-    % overload of the point_point_distance function to work with vectors of points
-    function d = point_point_distance(map, p1, p2)
-        d = sqrt((p1(1) - p2(1))^2 + (p1(2) - p2(2))^2);
-    end
-
-
 end % methods
 end % Laserscan class

Classes/Robot.m

@@ -70,7 +70,7 @@
     % https://www.iri.upc.edu/people/jsola/JoanSola/objectes/curs_SLAM/SLAM2D/SLAM%20course.pdf
     % Projecting the absolute coordinate of a landmark into the reference frame of the robot.
     % The function return also the jacobians w.r.t. the state of the robot and the state of the landmark
-    function [h, Jh_x_robot, Jh_x_landmark] = landmark_observation(obj, landmark)
+    function [h, Jh_x_robot, Jh_x_landmark] = landmark_to_observation(obj, landmark)
 
         % landmark state
         x_land = landmark.x(1);
@@ -110,6 +110,44 @@
                          j21, j22];
 
     end
+
+    % Function that projects an observation with local coordinates --> into the absolute frame.
+    % To project the uncertainty in the global frame we have to add to the covariance of the observation
+    % the projection of the covariance of the robot into the global frame: 
+    % global covariance = R + H*P*H'. 
+    % Where R is the covariance of the observation, H is the jacobian of the transformation  w.r.t. the robot
+    % state and P is the covariance of the local observation.
+    % Absolute_observation is an object of type Landmark (but coming from an observation)
+    function absolute_observation = observation_to_landmark(obj, observation)
+        absolute_observation = Landmark();
+
+        % robot state
+        x_rob = obj.x(1);
+        y_rob = obj.x(2);
+        t_rob = obj.x(3);
+
+        % observation vector
+        xp = observation.x(1);
+        yp = observation.x(2);
+
+        % Transformation of reference frame from local (xp, yp) to global (x_land, y_land)
+        x_land = x_rob + xp * cos(t_rob) - yp * sin(t_rob) 
+        y_land = y_rob + xp * sin(t_rob) - yp * cos(t_rob)
+
+        % Jacobian w.r.t the robot state
+        j11 = 1;
+        j12 = 0;
+        j13 = - xp * sin(t_rob) - yp * cos(t_rob);
+        j21 = 0;
+        j22 = 1;
+        j23 = xp * cos(t_rob) + yp * sin(t_rob);
+        H = [j11, j12, j13;
+             j21, j22, j23];
+
+        absolute_observation.x = [x_land; y_land];
+        absolute_observation.P = observation.R + H*obj.P*H';
+
+    end
 
 
 %  ____       _            _         __  __                _                   

Scripts/EKF.m

@@ -28,7 +28,7 @@
 P_est = robot.P
 
 pos_robot = cell(N_laserscans);
-pos_robot = cell(N_laserscans);
+cov_robot = cell(N_laserscans);
 
 
 % Temporal cycle
@@ -45,10 +45,16 @@
   x_est(1:3) = robot.update_step(odometries(k))
   P_est = F_X*P*F_X' + F_N*N*F_N';
 
-  z, H_X = map.compute_innovation(robot, laserscans{k}.observations);
-
-  % Update
 
+  % Update
+  [z, H_X, R] = map.compute_innovation(robot, laserscans{k}.observations);
+  S = H_X*P_est*H_X' + R;
+  W = P_est*H_X/S;
+  x_est = x_est + W*z;
+  P_est = P_est - W*H_X*P_est;
+
+  pos_robot(end+1,:) = x_est;
+  cov_robot(end+1,:) = P_est;
 
 
 end