CS401 Homework 9 -Solved

 The first project shows how to use ekfSLAM for a reliable implementation of landmark Simultaneous Localization and Mapping (SLAM) using the Extended Kalman Filter (EKF) algorithm and maximum likelihood algorithm for data association. In this project, you create a landmark map of the immediate surroundings of a vehicle and simultaneously track the path of the vehicle. Generate a trajectory by moving the vehicle using the noisy control commands, and form the map using the landmarks it encounters along the path. Correct the vehicle trajectory and landmark estimates by observing the landmarks again. The second project shows the implementation of FastSLAM algorithm which is based on particle filters and belongs to the family of probabilistic SLAM approaches. It is used with feature-based maps (see gif above) or with occupancy grid maps.

 Simulation
 This is a simulation of EKF SLAM.

 -Black stars: landmarks

 -Green crosses: estimates of landmark positions

 -Black line: dead reckoning

 -Blue line: ground truth

 -Red line: EKF SLAM position estimation

 EKF SLAM models the SLAM problem in a single EKF where the modeled state is both the pose   and an array of landmarks   for n landmarks. The covariance between each of the positions and landmarks are also tracked.

 A single estimate of the pose is tracked over time, while the confidence in the pose is tracked by the covariance matrix P. P is a symmetric square matrix which each element in the matrix corresponding to the covariance between two parts of the system. For example,   represents the covariance between the belief of   and   and is equal to  .

 The state can be represented more concisely as following:

 Here the state simplifies to a combination of pose (  ) and map ( ). The covariance matrix becomes easier to understand and simply reads as the uncertainty of the robots pose ( ), the uncertainty of the map ( ), and the uncertainty of the robots pose with respect to the map and vice versa ( ).

 EKF SLAM algorithm walk through
 At each time step, the following is done:

 predict the new state using the control functions
update the belief in landmark positions based on the estimated state and measurements
 Predict
 Predict State update: The following equations describe the predicted motion model of the robot in case we provide only the control  , which are the linear and angular velocity respectively.

 Thus, the motion model is:

 Notice that while   is only defined by   and  , in the actual calculations, a   and   appear. These values represent the error between the given control inputs and the actual control inputs.

 As a result, the simulation is set up as the following. R represents the process noise which is added to the control inputs to simulate noise experienced in the real world. A set of truth values are computed from the raw control values while the values dead reckoning values incorporate the error into the estimation:

 The implementation of the motion model prediciton code is shown in "motionmodel" function of the provided code ekfslam.py. The "observation" function in ekf_slam.py shows how the simulation uses (or doesn’t use) the process noise Rsim to the find the ground truth and dead reckoning estimates of the pose.

 Update
 In the update phase, the observations of nearby landmarks are used to correct the location estimate. For every landmark observed, it is associated to a particular landmark in the known map. If no landmark exists in the position surrounding the landmark, it is taken as a NEW landmark. The distance threshold for how far a landmark must be from the next known landmark before its considered to be a new landmark is set by MDISTTH.

 With an observation associated to the appropriate landmark, the innovation can be calculated. Innovation ( ) is the difference between the observation and the observation that should have been made if the observation were made from the pose predicted in the predict stage:

 With the innovation calculated, the question becomes which to trust more - the observations or the predictions? To determine this, we calculate the Kalman Gain K - a percent of how much of the innovation to add to the prediction based on the uncertainty in the predict step and the update step:

 where H is the jacobian of the measurement function.

 The update is captured in the following:


 The observation step described here is outside the main EKF SLAM process and is primarily used as a method of driving the simulation. The observations function is in charge of calculating how the poses of the robots change and accumulate error over time, and the theoretical measurements that are expected as a result of each measurement.

 Observations are based on the TRUE position of the robot. Error in dead reckoning and control functions are passed along here as well. Please refer to the function "observation" in the code file ekf_slam.py

 Fast SLAM
 simulation
  
 This demo shows the example of landmark feature based FastSLAM using particle filters.

 The blue line is the true trajectory
The red line is the estimated trajectory
The red dots represent the distribution of particles
The black line represent dead reckoning tracjectory
The blue x is the observed and estimated landmarks
The black x is the true landmark
 As it is shown, the particle filter differs from EKF by representing the robot’s estimation through a set of particles. Each single particle has an independent belief, as it holds the pose   and an array of landmarks   for n landmarks.

 I.e. Each particle maintains a deterministic pose and n-EKFs for each landmark and update it with each measurement.

 The particles are initially drawn from a uniform distribution the represent the initial uncertainty. At each time step we do:

 Predict the pose for each particle by using control input u and the motion model (the landmarks are not updated).
Update the particles with observations z, where the weights are adjusted based on how likely the particle to have the correct pose given the sensor measurement
Resampling such that the particles with the largest weights survive and the unlikely ones with the lowest weights die out.
 Predict
 The predict step is the same as that of landmarks based EKF-SLAM. From the equations, we can see how the particles distribution evolves. To get the insight of the motion model change the value of and re-run the cells again. As R is the parameters that indicates how much we trust that the robot executed the motion commands. It is interesting to notice also that only motion will increase the uncertainty in the system as the particles start to spread out more. If observations are included the uncertainty will decrease and particles will converge to the correct estimate. Please refer to the functions "motionmodel" and "predictparticles" implementations in pf_slam.py.

 Update
 For the update step it is useful to observe a single particle and the effect of getting a new measurements on the weight of the particle. The weight of the particle is updated according to the following equation:

 Where,   is the computed weight,   is the measurement covariance,   is the actual measurment and   is the predicted measurement of particle i.

 To experiment this, a single particle is initialized then passed an initial measurement, which results in a relatively average weight. However, setting the particle coordinate to a wrong value to simulate wrong estimation will result in a very low weight. The lower the weight the less likely that this particle will be drawn during resampling and probably will die out.

 Resampling
 In the reseampling steps a new set of particles are chosen from the old set. This is done according to the weight of each particle.

 The following figure shows 100 particles distributed uniformly between [-0.5, 0.5] with the weights of each particle distributed according to a Gaussian funciton.

 Task 1
 The prediction, update and Jacobian functions are given, please implement the "fast_slam1" processing of EKF-SLAM and print the final experimental result using the provided visualization function.

 Task 2
 The basic functions such as prediction, update, Jacobian, weight calculation and resampling functions given, please implement the "fast_slam1" function of particle slam and print the final experimental result using the provided visualization function.

 Code instructions for tasks
 Note1 ekfslam.py give the implememtation framework of the task1, please complete the ekfslam function.

 Note2 pf_slam.py give the implememtation framework of the task2.