🐛 Fix in calculating mu & sigma and Kalman gain in Fast SLAM correcti…

…on_step For mu & sigma, algo should be acessing the particular landmark by ID, I was only acessing the first landmark - index 1! For Kalman Gain, the algo should be using the jacobian Q not Q_t which is the sensor noise matrix!
conorhennessy · Aug 11, 2020 · d4d170c · d4d170c
1 parent 5291317
commit d4d170c
Show file tree

Hide file tree

Showing 332 changed files with 5 additions and 4 deletions.
diff --git a/s8_fastslam/octave/correction_step.m b/s8_fastslam/octave/correction_step.m
@@ -38,7 +38,7 @@
  robot_x = robot(1);
  robot_y = robot(2);
  robot_r = robot(3);
- particles(i).landmarks(1).mu = [ robot_x + z(j).range * cos(normalize_angle(z(j).bearing + robot_r));
+ particles(i).landmarks(l).mu = [ robot_x + z(j).range * cos(normalize_angle(z(j).bearing + robot_r));
  robot_y + z(j).range * sin(normalize_angle(z(j).bearing + robot_r))];
 
  % get the Jacobian with respect to the landmark position
@@ -60,19 +60,20 @@
  % This corresponds to line 14 of slide 30 of FAST SLAM lecture (12)
  Q = (H * particles(i).landmarks(l).sigma * H') + Q_t;
 
+
  % TODO: calculate the Kalman gain
  % This corresponds to line 15 of slide 30 of FAST SLAM lecture (12)
- K = particles(i).landmarks(l).sigma * H' * inv(Q_t);
+ K = particles(i).landmarks(l).sigma * H' * inv(Q);
 
  % TODO: compute the error between the z and expectedZ (remember to normalize the angle)'
  % error for range and bearing
  Z_diff = [z(j).range - expectedZ(1); normalize_angle(z(j).bearing - expectedZ(2))];
 
  % TODO: update the mean and covariance of the EKF for this landmark
  % mean, this corresponds to line 16 of slide 30 of FAST SLAM lecture (12)
- particles(i).landmarks(1).mu = particles(i).landmarks(1).mu + K * (Z_diff);
+ particles(i).landmarks(l).mu = particles(i).landmarks(l).mu + K * (Z_diff);
  % covariance, this corresponds to line 17 of slide 30 of FAST SLAM lecture (12)
- particles(i).landmarks(1).sigma = (eye(2) - K * H) * particles(i).landmarks(1).sigma;
+ particles(i).landmarks(l).sigma = (eye(2) - K * H) * particles(i).landmarks(l).sigma;
 
  % TODO: compute the likelihood of this observation, multiply with the former weight
  % to account for observing several features in one time step

diff --git a/s8_fastslam/plots/fast_slam.mp4 b/s8_fastslam/plots/fast_slam.mp4
diff --git a/s8_fastslam/plots/fastslam_001.png b/s8_fastslam/plots/fastslam_001.png
diff --git a/s8_fastslam/plots/fastslam_002.png b/s8_fastslam/plots/fastslam_002.png
diff --git a/s8_fastslam/plots/fastslam_003.png b/s8_fastslam/plots/fastslam_003.png
diff --git a/s8_fastslam/plots/fastslam_004.png b/s8_fastslam/plots/fastslam_004.png
diff --git a/s8_fastslam/plots/fastslam_005.png b/s8_fastslam/plots/fastslam_005.png
diff --git a/s8_fastslam/plots/fastslam_006.png b/s8_fastslam/plots/fastslam_006.png
diff --git a/s8_fastslam/plots/fastslam_007.png b/s8_fastslam/plots/fastslam_007.png
diff --git a/s8_fastslam/plots/fastslam_008.png b/s8_fastslam/plots/fastslam_008.png
diff --git a/s8_fastslam/plots/fastslam_009.png b/s8_fastslam/plots/fastslam_009.png
diff --git a/s8_fastslam/plots/fastslam_010.png b/s8_fastslam/plots/fastslam_010.png
diff --git a/s8_fastslam/plots/fastslam_011.png b/s8_fastslam/plots/fastslam_011.png
diff --git a/s8_fastslam/plots/fastslam_012.png b/s8_fastslam/plots/fastslam_012.png
diff --git a/s8_fastslam/plots/fastslam_013.png b/s8_fastslam/plots/fastslam_013.png
diff --git a/s8_fastslam/plots/fastslam_014.png b/s8_fastslam/plots/fastslam_014.png
diff --git a/s8_fastslam/plots/fastslam_015.png b/s8_fastslam/plots/fastslam_015.png
diff --git a/s8_fastslam/plots/fastslam_016.png b/s8_fastslam/plots/fastslam_016.png
diff --git a/s8_fastslam/plots/fastslam_017.png b/s8_fastslam/plots/fastslam_017.png
diff --git a/s8_fastslam/plots/fastslam_018.png b/s8_fastslam/plots/fastslam_018.png
diff --git a/s8_fastslam/plots/fastslam_019.png b/s8_fastslam/plots/fastslam_019.png
diff --git a/s8_fastslam/plots/fastslam_020.png b/s8_fastslam/plots/fastslam_020.png
diff --git a/s8_fastslam/plots/fastslam_021.png b/s8_fastslam/plots/fastslam_021.png
diff --git a/s8_fastslam/plots/fastslam_022.png b/s8_fastslam/plots/fastslam_022.png
diff --git a/s8_fastslam/plots/fastslam_023.png b/s8_fastslam/plots/fastslam_023.png
diff --git a/s8_fastslam/plots/fastslam_024.png b/s8_fastslam/plots/fastslam_024.png
diff --git a/s8_fastslam/plots/fastslam_025.png b/s8_fastslam/plots/fastslam_025.png
diff --git a/s8_fastslam/plots/fastslam_026.png b/s8_fastslam/plots/fastslam_026.png
diff --git a/s8_fastslam/plots/fastslam_027.png b/s8_fastslam/plots/fastslam_027.png
diff --git a/s8_fastslam/plots/fastslam_028.png b/s8_fastslam/plots/fastslam_028.png
diff --git a/s8_fastslam/plots/fastslam_029.png b/s8_fastslam/plots/fastslam_029.png
diff --git a/s8_fastslam/plots/fastslam_030.png b/s8_fastslam/plots/fastslam_030.png
diff --git a/s8_fastslam/plots/fastslam_031.png b/s8_fastslam/plots/fastslam_031.png
diff --git a/s8_fastslam/plots/fastslam_032.png b/s8_fastslam/plots/fastslam_032.png
diff --git a/s8_fastslam/plots/fastslam_033.png b/s8_fastslam/plots/fastslam_033.png
diff --git a/s8_fastslam/plots/fastslam_034.png b/s8_fastslam/plots/fastslam_034.png
diff --git a/s8_fastslam/plots/fastslam_035.png b/s8_fastslam/plots/fastslam_035.png
diff --git a/s8_fastslam/plots/fastslam_036.png b/s8_fastslam/plots/fastslam_036.png
diff --git a/s8_fastslam/plots/fastslam_037.png b/s8_fastslam/plots/fastslam_037.png
diff --git a/s8_fastslam/plots/fastslam_038.png b/s8_fastslam/plots/fastslam_038.png
diff --git a/s8_fastslam/plots/fastslam_039.png b/s8_fastslam/plots/fastslam_039.png
diff --git a/s8_fastslam/plots/fastslam_040.png b/s8_fastslam/plots/fastslam_040.png
diff --git a/s8_fastslam/plots/fastslam_041.png b/s8_fastslam/plots/fastslam_041.png
diff --git a/s8_fastslam/plots/fastslam_042.png b/s8_fastslam/plots/fastslam_042.png
diff --git a/s8_fastslam/plots/fastslam_043.png b/s8_fastslam/plots/fastslam_043.png
diff --git a/s8_fastslam/plots/fastslam_044.png b/s8_fastslam/plots/fastslam_044.png
diff --git a/s8_fastslam/plots/fastslam_045.png b/s8_fastslam/plots/fastslam_045.png
diff --git a/s8_fastslam/plots/fastslam_046.png b/s8_fastslam/plots/fastslam_046.png
diff --git a/s8_fastslam/plots/fastslam_047.png b/s8_fastslam/plots/fastslam_047.png
diff --git a/s8_fastslam/plots/fastslam_048.png b/s8_fastslam/plots/fastslam_048.png
diff --git a/s8_fastslam/plots/fastslam_049.png b/s8_fastslam/plots/fastslam_049.png
diff --git a/s8_fastslam/plots/fastslam_050.png b/s8_fastslam/plots/fastslam_050.png
diff --git a/s8_fastslam/plots/fastslam_051.png b/s8_fastslam/plots/fastslam_051.png
diff --git a/s8_fastslam/plots/fastslam_052.png b/s8_fastslam/plots/fastslam_052.png
diff --git a/s8_fastslam/plots/fastslam_053.png b/s8_fastslam/plots/fastslam_053.png
diff --git a/s8_fastslam/plots/fastslam_054.png b/s8_fastslam/plots/fastslam_054.png
diff --git a/s8_fastslam/plots/fastslam_055.png b/s8_fastslam/plots/fastslam_055.png
diff --git a/s8_fastslam/plots/fastslam_056.png b/s8_fastslam/plots/fastslam_056.png
diff --git a/s8_fastslam/plots/fastslam_057.png b/s8_fastslam/plots/fastslam_057.png
diff --git a/s8_fastslam/plots/fastslam_058.png b/s8_fastslam/plots/fastslam_058.png
diff --git a/s8_fastslam/plots/fastslam_059.png b/s8_fastslam/plots/fastslam_059.png
diff --git a/s8_fastslam/plots/fastslam_060.png b/s8_fastslam/plots/fastslam_060.png
diff --git a/s8_fastslam/plots/fastslam_061.png b/s8_fastslam/plots/fastslam_061.png
diff --git a/s8_fastslam/plots/fastslam_062.png b/s8_fastslam/plots/fastslam_062.png
diff --git a/s8_fastslam/plots/fastslam_063.png b/s8_fastslam/plots/fastslam_063.png
diff --git a/s8_fastslam/plots/fastslam_064.png b/s8_fastslam/plots/fastslam_064.png
diff --git a/s8_fastslam/plots/fastslam_065.png b/s8_fastslam/plots/fastslam_065.png
diff --git a/s8_fastslam/plots/fastslam_066.png b/s8_fastslam/plots/fastslam_066.png
diff --git a/s8_fastslam/plots/fastslam_067.png b/s8_fastslam/plots/fastslam_067.png
diff --git a/s8_fastslam/plots/fastslam_068.png b/s8_fastslam/plots/fastslam_068.png
diff --git a/s8_fastslam/plots/fastslam_069.png b/s8_fastslam/plots/fastslam_069.png
diff --git a/s8_fastslam/plots/fastslam_070.png b/s8_fastslam/plots/fastslam_070.png
diff --git a/s8_fastslam/plots/fastslam_071.png b/s8_fastslam/plots/fastslam_071.png
diff --git a/s8_fastslam/plots/fastslam_072.png b/s8_fastslam/plots/fastslam_072.png
diff --git a/s8_fastslam/plots/fastslam_073.png b/s8_fastslam/plots/fastslam_073.png
diff --git a/s8_fastslam/plots/fastslam_074.png b/s8_fastslam/plots/fastslam_074.png
diff --git a/s8_fastslam/plots/fastslam_075.png b/s8_fastslam/plots/fastslam_075.png
diff --git a/s8_fastslam/plots/fastslam_076.png b/s8_fastslam/plots/fastslam_076.png
diff --git a/s8_fastslam/plots/fastslam_077.png b/s8_fastslam/plots/fastslam_077.png
diff --git a/s8_fastslam/plots/fastslam_078.png b/s8_fastslam/plots/fastslam_078.png
diff --git a/s8_fastslam/plots/fastslam_079.png b/s8_fastslam/plots/fastslam_079.png
diff --git a/s8_fastslam/plots/fastslam_080.png b/s8_fastslam/plots/fastslam_080.png
diff --git a/s8_fastslam/plots/fastslam_081.png b/s8_fastslam/plots/fastslam_081.png
diff --git a/s8_fastslam/plots/fastslam_082.png b/s8_fastslam/plots/fastslam_082.png
diff --git a/s8_fastslam/plots/fastslam_083.png b/s8_fastslam/plots/fastslam_083.png
diff --git a/s8_fastslam/plots/fastslam_084.png b/s8_fastslam/plots/fastslam_084.png
diff --git a/s8_fastslam/plots/fastslam_085.png b/s8_fastslam/plots/fastslam_085.png
diff --git a/s8_fastslam/plots/fastslam_086.png b/s8_fastslam/plots/fastslam_086.png
diff --git a/s8_fastslam/plots/fastslam_087.png b/s8_fastslam/plots/fastslam_087.png
diff --git a/s8_fastslam/plots/fastslam_088.png b/s8_fastslam/plots/fastslam_088.png
diff --git a/s8_fastslam/plots/fastslam_089.png b/s8_fastslam/plots/fastslam_089.png
diff --git a/s8_fastslam/plots/fastslam_090.png b/s8_fastslam/plots/fastslam_090.png
diff --git a/s8_fastslam/plots/fastslam_091.png b/s8_fastslam/plots/fastslam_091.png
diff --git a/s8_fastslam/plots/fastslam_092.png b/s8_fastslam/plots/fastslam_092.png
diff --git a/s8_fastslam/plots/fastslam_093.png b/s8_fastslam/plots/fastslam_093.png
diff --git a/s8_fastslam/plots/fastslam_094.png b/s8_fastslam/plots/fastslam_094.png
diff --git a/s8_fastslam/plots/fastslam_095.png b/s8_fastslam/plots/fastslam_095.png
diff --git a/s8_fastslam/plots/fastslam_096.png b/s8_fastslam/plots/fastslam_096.png
diff --git a/s8_fastslam/plots/fastslam_097.png b/s8_fastslam/plots/fastslam_097.png
diff --git a/s8_fastslam/plots/fastslam_098.png b/s8_fastslam/plots/fastslam_098.png
diff --git a/s8_fastslam/plots/fastslam_099.png b/s8_fastslam/plots/fastslam_099.png
diff --git a/s8_fastslam/plots/fastslam_100.png b/s8_fastslam/plots/fastslam_100.png
diff --git a/s8_fastslam/plots/fastslam_101.png b/s8_fastslam/plots/fastslam_101.png
diff --git a/s8_fastslam/plots/fastslam_102.png b/s8_fastslam/plots/fastslam_102.png
diff --git a/s8_fastslam/plots/fastslam_103.png b/s8_fastslam/plots/fastslam_103.png
diff --git a/s8_fastslam/plots/fastslam_104.png b/s8_fastslam/plots/fastslam_104.png
diff --git a/s8_fastslam/plots/fastslam_105.png b/s8_fastslam/plots/fastslam_105.png
diff --git a/s8_fastslam/plots/fastslam_106.png b/s8_fastslam/plots/fastslam_106.png
diff --git a/s8_fastslam/plots/fastslam_107.png b/s8_fastslam/plots/fastslam_107.png
diff --git a/s8_fastslam/plots/fastslam_108.png b/s8_fastslam/plots/fastslam_108.png
diff --git a/s8_fastslam/plots/fastslam_109.png b/s8_fastslam/plots/fastslam_109.png
diff --git a/s8_fastslam/plots/fastslam_110.png b/s8_fastslam/plots/fastslam_110.png
diff --git a/s8_fastslam/plots/fastslam_111.png b/s8_fastslam/plots/fastslam_111.png
diff --git a/s8_fastslam/plots/fastslam_112.png b/s8_fastslam/plots/fastslam_112.png
diff --git a/s8_fastslam/plots/fastslam_113.png b/s8_fastslam/plots/fastslam_113.png
diff --git a/s8_fastslam/plots/fastslam_114.png b/s8_fastslam/plots/fastslam_114.png
diff --git a/s8_fastslam/plots/fastslam_115.png b/s8_fastslam/plots/fastslam_115.png
diff --git a/s8_fastslam/plots/fastslam_116.png b/s8_fastslam/plots/fastslam_116.png
diff --git a/s8_fastslam/plots/fastslam_117.png b/s8_fastslam/plots/fastslam_117.png
diff --git a/s8_fastslam/plots/fastslam_118.png b/s8_fastslam/plots/fastslam_118.png
diff --git a/s8_fastslam/plots/fastslam_119.png b/s8_fastslam/plots/fastslam_119.png
diff --git a/s8_fastslam/plots/fastslam_120.png b/s8_fastslam/plots/fastslam_120.png
diff --git a/s8_fastslam/plots/fastslam_121.png b/s8_fastslam/plots/fastslam_121.png
diff --git a/s8_fastslam/plots/fastslam_122.png b/s8_fastslam/plots/fastslam_122.png
diff --git a/s8_fastslam/plots/fastslam_123.png b/s8_fastslam/plots/fastslam_123.png
diff --git a/s8_fastslam/plots/fastslam_124.png b/s8_fastslam/plots/fastslam_124.png
diff --git a/s8_fastslam/plots/fastslam_125.png b/s8_fastslam/plots/fastslam_125.png
diff --git a/s8_fastslam/plots/fastslam_126.png b/s8_fastslam/plots/fastslam_126.png
diff --git a/s8_fastslam/plots/fastslam_127.png b/s8_fastslam/plots/fastslam_127.png
diff --git a/s8_fastslam/plots/fastslam_128.png b/s8_fastslam/plots/fastslam_128.png
diff --git a/s8_fastslam/plots/fastslam_129.png b/s8_fastslam/plots/fastslam_129.png
diff --git a/s8_fastslam/plots/fastslam_130.png b/s8_fastslam/plots/fastslam_130.png
diff --git a/s8_fastslam/plots/fastslam_131.png b/s8_fastslam/plots/fastslam_131.png
diff --git a/s8_fastslam/plots/fastslam_132.png b/s8_fastslam/plots/fastslam_132.png
diff --git a/s8_fastslam/plots/fastslam_133.png b/s8_fastslam/plots/fastslam_133.png
diff --git a/s8_fastslam/plots/fastslam_134.png b/s8_fastslam/plots/fastslam_134.png
diff --git a/s8_fastslam/plots/fastslam_135.png b/s8_fastslam/plots/fastslam_135.png
diff --git a/s8_fastslam/plots/fastslam_136.png b/s8_fastslam/plots/fastslam_136.png
diff --git a/s8_fastslam/plots/fastslam_137.png b/s8_fastslam/plots/fastslam_137.png
diff --git a/s8_fastslam/plots/fastslam_138.png b/s8_fastslam/plots/fastslam_138.png
diff --git a/s8_fastslam/plots/fastslam_139.png b/s8_fastslam/plots/fastslam_139.png
diff --git a/s8_fastslam/plots/fastslam_140.png b/s8_fastslam/plots/fastslam_140.png
diff --git a/s8_fastslam/plots/fastslam_141.png b/s8_fastslam/plots/fastslam_141.png
diff --git a/s8_fastslam/plots/fastslam_142.png b/s8_fastslam/plots/fastslam_142.png
diff --git a/s8_fastslam/plots/fastslam_143.png b/s8_fastslam/plots/fastslam_143.png
diff --git a/s8_fastslam/plots/fastslam_144.png b/s8_fastslam/plots/fastslam_144.png
diff --git a/s8_fastslam/plots/fastslam_145.png b/s8_fastslam/plots/fastslam_145.png
diff --git a/s8_fastslam/plots/fastslam_146.png b/s8_fastslam/plots/fastslam_146.png
diff --git a/s8_fastslam/plots/fastslam_147.png b/s8_fastslam/plots/fastslam_147.png
diff --git a/s8_fastslam/plots/fastslam_148.png b/s8_fastslam/plots/fastslam_148.png
diff --git a/s8_fastslam/plots/fastslam_149.png b/s8_fastslam/plots/fastslam_149.png
diff --git a/s8_fastslam/plots/fastslam_150.png b/s8_fastslam/plots/fastslam_150.png
diff --git a/s8_fastslam/plots/fastslam_151.png b/s8_fastslam/plots/fastslam_151.png
diff --git a/s8_fastslam/plots/fastslam_152.png b/s8_fastslam/plots/fastslam_152.png
diff --git a/s8_fastslam/plots/fastslam_153.png b/s8_fastslam/plots/fastslam_153.png
diff --git a/s8_fastslam/plots/fastslam_154.png b/s8_fastslam/plots/fastslam_154.png
diff --git a/s8_fastslam/plots/fastslam_155.png b/s8_fastslam/plots/fastslam_155.png
diff --git a/s8_fastslam/plots/fastslam_156.png b/s8_fastslam/plots/fastslam_156.png
diff --git a/s8_fastslam/plots/fastslam_157.png b/s8_fastslam/plots/fastslam_157.png
diff --git a/s8_fastslam/plots/fastslam_158.png b/s8_fastslam/plots/fastslam_158.png
diff --git a/s8_fastslam/plots/fastslam_159.png b/s8_fastslam/plots/fastslam_159.png
diff --git a/s8_fastslam/plots/fastslam_160.png b/s8_fastslam/plots/fastslam_160.png
diff --git a/s8_fastslam/plots/fastslam_161.png b/s8_fastslam/plots/fastslam_161.png
diff --git a/s8_fastslam/plots/fastslam_162.png b/s8_fastslam/plots/fastslam_162.png
diff --git a/s8_fastslam/plots/fastslam_163.png b/s8_fastslam/plots/fastslam_163.png
diff --git a/s8_fastslam/plots/fastslam_164.png b/s8_fastslam/plots/fastslam_164.png
diff --git a/s8_fastslam/plots/fastslam_165.png b/s8_fastslam/plots/fastslam_165.png
diff --git a/s8_fastslam/plots/fastslam_166.png b/s8_fastslam/plots/fastslam_166.png
diff --git a/s8_fastslam/plots/fastslam_167.png b/s8_fastslam/plots/fastslam_167.png
diff --git a/s8_fastslam/plots/fastslam_168.png b/s8_fastslam/plots/fastslam_168.png
diff --git a/s8_fastslam/plots/fastslam_169.png b/s8_fastslam/plots/fastslam_169.png
diff --git a/s8_fastslam/plots/fastslam_170.png b/s8_fastslam/plots/fastslam_170.png
diff --git a/s8_fastslam/plots/fastslam_171.png b/s8_fastslam/plots/fastslam_171.png
diff --git a/s8_fastslam/plots/fastslam_172.png b/s8_fastslam/plots/fastslam_172.png
diff --git a/s8_fastslam/plots/fastslam_173.png b/s8_fastslam/plots/fastslam_173.png
diff --git a/s8_fastslam/plots/fastslam_174.png b/s8_fastslam/plots/fastslam_174.png
diff --git a/s8_fastslam/plots/fastslam_175.png b/s8_fastslam/plots/fastslam_175.png
diff --git a/s8_fastslam/plots/fastslam_176.png b/s8_fastslam/plots/fastslam_176.png
diff --git a/s8_fastslam/plots/fastslam_177.png b/s8_fastslam/plots/fastslam_177.png
diff --git a/s8_fastslam/plots/fastslam_178.png b/s8_fastslam/plots/fastslam_178.png
diff --git a/s8_fastslam/plots/fastslam_179.png b/s8_fastslam/plots/fastslam_179.png
diff --git a/s8_fastslam/plots/fastslam_180.png b/s8_fastslam/plots/fastslam_180.png
diff --git a/s8_fastslam/plots/fastslam_181.png b/s8_fastslam/plots/fastslam_181.png
diff --git a/s8_fastslam/plots/fastslam_182.png b/s8_fastslam/plots/fastslam_182.png
diff --git a/s8_fastslam/plots/fastslam_183.png b/s8_fastslam/plots/fastslam_183.png
diff --git a/s8_fastslam/plots/fastslam_184.png b/s8_fastslam/plots/fastslam_184.png
diff --git a/s8_fastslam/plots/fastslam_185.png b/s8_fastslam/plots/fastslam_185.png
diff --git a/s8_fastslam/plots/fastslam_186.png b/s8_fastslam/plots/fastslam_186.png
diff --git a/s8_fastslam/plots/fastslam_187.png b/s8_fastslam/plots/fastslam_187.png
diff --git a/s8_fastslam/plots/fastslam_188.png b/s8_fastslam/plots/fastslam_188.png
diff --git a/s8_fastslam/plots/fastslam_189.png b/s8_fastslam/plots/fastslam_189.png
diff --git a/s8_fastslam/plots/fastslam_190.png b/s8_fastslam/plots/fastslam_190.png
diff --git a/s8_fastslam/plots/fastslam_191.png b/s8_fastslam/plots/fastslam_191.png
diff --git a/s8_fastslam/plots/fastslam_192.png b/s8_fastslam/plots/fastslam_192.png
diff --git a/s8_fastslam/plots/fastslam_193.png b/s8_fastslam/plots/fastslam_193.png
diff --git a/s8_fastslam/plots/fastslam_194.png b/s8_fastslam/plots/fastslam_194.png
diff --git a/s8_fastslam/plots/fastslam_195.png b/s8_fastslam/plots/fastslam_195.png
diff --git a/s8_fastslam/plots/fastslam_196.png b/s8_fastslam/plots/fastslam_196.png
diff --git a/s8_fastslam/plots/fastslam_197.png b/s8_fastslam/plots/fastslam_197.png
diff --git a/s8_fastslam/plots/fastslam_198.png b/s8_fastslam/plots/fastslam_198.png
diff --git a/s8_fastslam/plots/fastslam_199.png b/s8_fastslam/plots/fastslam_199.png
diff --git a/s8_fastslam/plots/fastslam_200.png b/s8_fastslam/plots/fastslam_200.png
diff --git a/s8_fastslam/plots/fastslam_201.png b/s8_fastslam/plots/fastslam_201.png
diff --git a/s8_fastslam/plots/fastslam_202.png b/s8_fastslam/plots/fastslam_202.png
diff --git a/s8_fastslam/plots/fastslam_203.png b/s8_fastslam/plots/fastslam_203.png
diff --git a/s8_fastslam/plots/fastslam_204.png b/s8_fastslam/plots/fastslam_204.png
diff --git a/s8_fastslam/plots/fastslam_205.png b/s8_fastslam/plots/fastslam_205.png
diff --git a/s8_fastslam/plots/fastslam_206.png b/s8_fastslam/plots/fastslam_206.png
diff --git a/s8_fastslam/plots/fastslam_207.png b/s8_fastslam/plots/fastslam_207.png
diff --git a/s8_fastslam/plots/fastslam_208.png b/s8_fastslam/plots/fastslam_208.png
diff --git a/s8_fastslam/plots/fastslam_209.png b/s8_fastslam/plots/fastslam_209.png
diff --git a/s8_fastslam/plots/fastslam_210.png b/s8_fastslam/plots/fastslam_210.png
diff --git a/s8_fastslam/plots/fastslam_211.png b/s8_fastslam/plots/fastslam_211.png
diff --git a/s8_fastslam/plots/fastslam_212.png b/s8_fastslam/plots/fastslam_212.png
diff --git a/s8_fastslam/plots/fastslam_213.png b/s8_fastslam/plots/fastslam_213.png
diff --git a/s8_fastslam/plots/fastslam_214.png b/s8_fastslam/plots/fastslam_214.png
diff --git a/s8_fastslam/plots/fastslam_215.png b/s8_fastslam/plots/fastslam_215.png
diff --git a/s8_fastslam/plots/fastslam_216.png b/s8_fastslam/plots/fastslam_216.png
diff --git a/s8_fastslam/plots/fastslam_217.png b/s8_fastslam/plots/fastslam_217.png
diff --git a/s8_fastslam/plots/fastslam_218.png b/s8_fastslam/plots/fastslam_218.png
diff --git a/s8_fastslam/plots/fastslam_219.png b/s8_fastslam/plots/fastslam_219.png
diff --git a/s8_fastslam/plots/fastslam_220.png b/s8_fastslam/plots/fastslam_220.png
diff --git a/s8_fastslam/plots/fastslam_221.png b/s8_fastslam/plots/fastslam_221.png
diff --git a/s8_fastslam/plots/fastslam_222.png b/s8_fastslam/plots/fastslam_222.png
diff --git a/s8_fastslam/plots/fastslam_223.png b/s8_fastslam/plots/fastslam_223.png
diff --git a/s8_fastslam/plots/fastslam_224.png b/s8_fastslam/plots/fastslam_224.png
diff --git a/s8_fastslam/plots/fastslam_225.png b/s8_fastslam/plots/fastslam_225.png
diff --git a/s8_fastslam/plots/fastslam_226.png b/s8_fastslam/plots/fastslam_226.png
diff --git a/s8_fastslam/plots/fastslam_227.png b/s8_fastslam/plots/fastslam_227.png
diff --git a/s8_fastslam/plots/fastslam_228.png b/s8_fastslam/plots/fastslam_228.png
diff --git a/s8_fastslam/plots/fastslam_229.png b/s8_fastslam/plots/fastslam_229.png
diff --git a/s8_fastslam/plots/fastslam_230.png b/s8_fastslam/plots/fastslam_230.png
diff --git a/s8_fastslam/plots/fastslam_231.png b/s8_fastslam/plots/fastslam_231.png
diff --git a/s8_fastslam/plots/fastslam_232.png b/s8_fastslam/plots/fastslam_232.png
diff --git a/s8_fastslam/plots/fastslam_233.png b/s8_fastslam/plots/fastslam_233.png
diff --git a/s8_fastslam/plots/fastslam_234.png b/s8_fastslam/plots/fastslam_234.png
diff --git a/s8_fastslam/plots/fastslam_235.png b/s8_fastslam/plots/fastslam_235.png
diff --git a/s8_fastslam/plots/fastslam_236.png b/s8_fastslam/plots/fastslam_236.png
diff --git a/s8_fastslam/plots/fastslam_237.png b/s8_fastslam/plots/fastslam_237.png
diff --git a/s8_fastslam/plots/fastslam_238.png b/s8_fastslam/plots/fastslam_238.png
diff --git a/s8_fastslam/plots/fastslam_239.png b/s8_fastslam/plots/fastslam_239.png
diff --git a/s8_fastslam/plots/fastslam_240.png b/s8_fastslam/plots/fastslam_240.png
diff --git a/s8_fastslam/plots/fastslam_241.png b/s8_fastslam/plots/fastslam_241.png
diff --git a/s8_fastslam/plots/fastslam_242.png b/s8_fastslam/plots/fastslam_242.png
diff --git a/s8_fastslam/plots/fastslam_243.png b/s8_fastslam/plots/fastslam_243.png
diff --git a/s8_fastslam/plots/fastslam_244.png b/s8_fastslam/plots/fastslam_244.png
diff --git a/s8_fastslam/plots/fastslam_245.png b/s8_fastslam/plots/fastslam_245.png
diff --git a/s8_fastslam/plots/fastslam_246.png b/s8_fastslam/plots/fastslam_246.png
diff --git a/s8_fastslam/plots/fastslam_247.png b/s8_fastslam/plots/fastslam_247.png
diff --git a/s8_fastslam/plots/fastslam_248.png b/s8_fastslam/plots/fastslam_248.png
diff --git a/s8_fastslam/plots/fastslam_249.png b/s8_fastslam/plots/fastslam_249.png
diff --git a/s8_fastslam/plots/fastslam_250.png b/s8_fastslam/plots/fastslam_250.png
diff --git a/s8_fastslam/plots/fastslam_251.png b/s8_fastslam/plots/fastslam_251.png
diff --git a/s8_fastslam/plots/fastslam_252.png b/s8_fastslam/plots/fastslam_252.png
diff --git a/s8_fastslam/plots/fastslam_253.png b/s8_fastslam/plots/fastslam_253.png
diff --git a/s8_fastslam/plots/fastslam_254.png b/s8_fastslam/plots/fastslam_254.png
diff --git a/s8_fastslam/plots/fastslam_255.png b/s8_fastslam/plots/fastslam_255.png
diff --git a/s8_fastslam/plots/fastslam_256.png b/s8_fastslam/plots/fastslam_256.png
diff --git a/s8_fastslam/plots/fastslam_257.png b/s8_fastslam/plots/fastslam_257.png
diff --git a/s8_fastslam/plots/fastslam_258.png b/s8_fastslam/plots/fastslam_258.png
diff --git a/s8_fastslam/plots/fastslam_259.png b/s8_fastslam/plots/fastslam_259.png
diff --git a/s8_fastslam/plots/fastslam_260.png b/s8_fastslam/plots/fastslam_260.png
diff --git a/s8_fastslam/plots/fastslam_261.png b/s8_fastslam/plots/fastslam_261.png
diff --git a/s8_fastslam/plots/fastslam_262.png b/s8_fastslam/plots/fastslam_262.png
diff --git a/s8_fastslam/plots/fastslam_263.png b/s8_fastslam/plots/fastslam_263.png
diff --git a/s8_fastslam/plots/fastslam_264.png b/s8_fastslam/plots/fastslam_264.png
diff --git a/s8_fastslam/plots/fastslam_265.png b/s8_fastslam/plots/fastslam_265.png
diff --git a/s8_fastslam/plots/fastslam_266.png b/s8_fastslam/plots/fastslam_266.png
diff --git a/s8_fastslam/plots/fastslam_267.png b/s8_fastslam/plots/fastslam_267.png
diff --git a/s8_fastslam/plots/fastslam_268.png b/s8_fastslam/plots/fastslam_268.png
diff --git a/s8_fastslam/plots/fastslam_269.png b/s8_fastslam/plots/fastslam_269.png
diff --git a/s8_fastslam/plots/fastslam_270.png b/s8_fastslam/plots/fastslam_270.png
diff --git a/s8_fastslam/plots/fastslam_271.png b/s8_fastslam/plots/fastslam_271.png
diff --git a/s8_fastslam/plots/fastslam_272.png b/s8_fastslam/plots/fastslam_272.png
diff --git a/s8_fastslam/plots/fastslam_273.png b/s8_fastslam/plots/fastslam_273.png
diff --git a/s8_fastslam/plots/fastslam_274.png b/s8_fastslam/plots/fastslam_274.png
diff --git a/s8_fastslam/plots/fastslam_275.png b/s8_fastslam/plots/fastslam_275.png
diff --git a/s8_fastslam/plots/fastslam_276.png b/s8_fastslam/plots/fastslam_276.png
diff --git a/s8_fastslam/plots/fastslam_277.png b/s8_fastslam/plots/fastslam_277.png
diff --git a/s8_fastslam/plots/fastslam_278.png b/s8_fastslam/plots/fastslam_278.png
diff --git a/s8_fastslam/plots/fastslam_279.png b/s8_fastslam/plots/fastslam_279.png
diff --git a/s8_fastslam/plots/fastslam_280.png b/s8_fastslam/plots/fastslam_280.png
diff --git a/s8_fastslam/plots/fastslam_281.png b/s8_fastslam/plots/fastslam_281.png
diff --git a/s8_fastslam/plots/fastslam_282.png b/s8_fastslam/plots/fastslam_282.png
diff --git a/s8_fastslam/plots/fastslam_283.png b/s8_fastslam/plots/fastslam_283.png
diff --git a/s8_fastslam/plots/fastslam_284.png b/s8_fastslam/plots/fastslam_284.png
diff --git a/s8_fastslam/plots/fastslam_285.png b/s8_fastslam/plots/fastslam_285.png
diff --git a/s8_fastslam/plots/fastslam_286.png b/s8_fastslam/plots/fastslam_286.png
diff --git a/s8_fastslam/plots/fastslam_287.png b/s8_fastslam/plots/fastslam_287.png
diff --git a/s8_fastslam/plots/fastslam_288.png b/s8_fastslam/plots/fastslam_288.png
diff --git a/s8_fastslam/plots/fastslam_289.png b/s8_fastslam/plots/fastslam_289.png
diff --git a/s8_fastslam/plots/fastslam_290.png b/s8_fastslam/plots/fastslam_290.png
diff --git a/s8_fastslam/plots/fastslam_291.png b/s8_fastslam/plots/fastslam_291.png
diff --git a/s8_fastslam/plots/fastslam_292.png b/s8_fastslam/plots/fastslam_292.png
diff --git a/s8_fastslam/plots/fastslam_293.png b/s8_fastslam/plots/fastslam_293.png
diff --git a/s8_fastslam/plots/fastslam_294.png b/s8_fastslam/plots/fastslam_294.png
diff --git a/s8_fastslam/plots/fastslam_295.png b/s8_fastslam/plots/fastslam_295.png
diff --git a/s8_fastslam/plots/fastslam_296.png b/s8_fastslam/plots/fastslam_296.png
diff --git a/s8_fastslam/plots/fastslam_297.png b/s8_fastslam/plots/fastslam_297.png
diff --git a/s8_fastslam/plots/fastslam_298.png b/s8_fastslam/plots/fastslam_298.png