From 82a147ad026862f59d3f41b3618a8e0b10ddcb40 Mon Sep 17 00:00:00 2001 From: idlebear <11653808+idlebear@users.noreply.github.com> Date: Thu, 5 Apr 2018 21:44:21 -0400 Subject: [PATCH 1/3] Removed '2' from range_bearing_meas_linearized_model A holdover from when I messed up the original pull request. Cleaning up while I'm passing through... --- matlab_simulation/01-examples_lecture/mr-6-ekfslam/ekf_slam.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From 8da1d68c5a9cfaebae6411fad8f3c6ff30cb1a04 Mon Sep 17 00:00:00 2001 From: idlebear <11653808+idlebear@users.noreply.github.com> Date: Thu, 5 Apr 2018 21:47:37 -0400 Subject: [PATCH 2/3] Removed Fi translation matrix Some were finding the Fi matrix multiplication confusion and not helpful in understanding the solution --- .../ekf_slam/range_bearing_meas_linearized_model.m | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) 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 From fd009b61df082d971b771f48474db13a522f509c Mon Sep 17 00:00:00 2001 From: idlebear <11653808+idlebear@users.noreply.github.com> Date: Thu, 5 Apr 2018 21:49:31 -0400 Subject: [PATCH 3/3] Remove extra file The '2' version of the linearized measurement model was kept by accident when the two versions were merged. Cleaning up. --- .../range_bearing_meas_linearized_model2.m | 16 ---------------- 1 file changed, 16 deletions(-) delete mode 100644 matlab_simulation/06-mapping/ekf_slam/range_bearing_meas_linearized_model2.m 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