diff --git a/matlab_simulation/01-examples_lecture/mr-6-ekfslam/ekf_slam.m b/matlab_simulation/01-examples_lecture/mr-6-ekfslam/ekf_slam.m
index 9cf3fbd..5e22d47 100644
--- a/matlab_simulation/01-examples_lecture/mr-6-ekfslam/ekf_slam.m
+++ b/matlab_simulation/01-examples_lecture/mr-6-ekfslam/ekf_slam.m
@@ -170,7 +170,7 @@
 
             % Linearization
             % Predicted range
-            Ht = range_bearing_meas_linearized_model2(mu,i);
+            Ht = range_bearing_meas_linearized_model(mu,i);
 
             % Measurement update
             K = S*Ht'* inv(Ht*S*Ht'+Qi);
@@ -209,4 +209,4 @@
 
 if (makemovie) 
     close(vidObj); 
-end
\ No newline at end of file
+end
diff --git a/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model.m b/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model.m
index 3e6c136..52cc821 100644
--- a/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model.m
+++ b/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model.m
@@ -4,11 +4,11 @@
             dx = mu(3+2*(i-1)+1)-mu(1);
             dy = mu(3+2*i)-mu(2);
             rp = sqrt((dx)^2+(dy)^2);
-            
+
             N=length(mu);
-            Fi = zeros(5,N);
-            Fi(1:3,1:3) = eye(3);
-            Fi(4:5,3+2*(i-1)+1:3+2*i) = eye(2);
-            Ht = [ -dx/rp,   -dy/rp,    0,  dx/rp,   dy/rp;
-                    dy/rp^2, -dx/rp^2, -1, -dy/rp^2, dx/rp^2]*Fi;
+            Ht = zeros(2,N);
+            Ht(1:2,1:3) = [ -dx/rp,     -dy/rp,     0;
+                             dy/rp^2,   -dx/rp^2,  -1 ];
+            Ht(1:2, 3+2*(i-1)+1:3+2*i) = [  dx/rp,   dy/rp;
+                                           -dy/rp^2, dx/rp^2 ];
 end
diff --git a/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model2.m b/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model2.m
deleted file mode 100644
index b871ab0..0000000
--- a/matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model2.m
+++ /dev/null
@@ -1,16 +0,0 @@
-function Ht = range_bearing_meas_linearized_model2(mu,i)
-    %% range_bearing_meas_linearized_model(mu,i)
-    % linearized measurement model of range and bearing
-    % can be found P.22 of Mapping II slides
-    dx = mu(3+2*(i-1)+1)-mu(1);
-    dy = mu(3+2*i)-mu(2);
-    rp = sqrt((dx)^2+(dy)^2);
-
-    N=length(mu);
-    Fi = zeros(5,N);
-    Fi(1:3,1:3) = eye(3);
-    Fi(4:5,3+2*(i-1)+1:3+2*i) = eye(2);
-    % Multiplying Ht by Fi maps Ht into the correct space
-    Ht = [ -dx/rp,     -dy/rp,     0,   dx/rp,   dy/rp;
-            dy/rp^2,   -dx/rp^2,  -1,  -dy/rp^2, dx/rp^2 ] * Fi;
-end