I13 ekf slam

[nanwei1]

Files are updated in i13/code v1.0/ME597-6-EKFSLAM

Changes made:

• Functionalized measurement and motion models
• Linearized models were also functionalized
• The main difference from the previous version (ekf_slam) is that the map size is updated as new features are detected:
  • Initially the states and covariance only contain information about the vehicle (x,y,theta in this case)
  • As new features are observed, the estimate of this feature's state is augmented right below the vehicle states. The feature list, measurements, etc are rearranged accordingly
  • The covariance for the newly observed feature is set to a predefined value (S0mm=eye(2)), and the off-diagonal terms are set to 0
  • A line of text is printed to the Command Window, indicating how many features have been detected
• Added examples of vehicle running with different trajectories in different maps (ekf_slam_lineMap and ekf_slam_circularMap)
• The EKF function will be incorporated in the next round
• Note that just like the original code, this simulation fails from time to time as the linearization may not be accurate or the covariance matrix becomes not positive-definite. This code warns the user when the update in vehicle position is too large compared to input velocity but cannot deal with these failures. Any comments on how to fix this will be appreciated.
• Added example where features are placed further way from vehicle trajectory (ekf_slam_largerCircularMap) to show that linearization works better in this case (as illustrated by Mapping II slide 28). If you try these simulation enough times, you will see that this one fails less frequently than ekf_slam_circularMap. Below is a comparison of the estimates: (Features in a r=6m circle)

(Features in a r=12m circle)

[cbchau]

Suggest putting the plotting code in separate functions which can be used for multiple scripts.

[nanwei1]

• Combined all 4 examples into 1 file. The user will need to pick an example at the beginning of the code.
• An ekfSLAMplot function is used to replace the original ~40 lines for plotting
• In case if S (the covariance matrix) gets not positive definite, manually add a diagonal matrix to make it positive definite to keep the code running. Warn the use when this happens
• I considered using the ekf function, but the issue was that this ekf_slam code updates the size of its map and covariance matrix as new features are discovered. Initialization of newly discovered features is performed after the prediction step using the predicted robot states, and the size of mu and S is updated before the measurement update. Thus, I did not find a way to directly use the ekf function without much modification
• Range/bearing only measurement model was tested. I did not upload the code because it basically did not work. The problem began with initialization of features; with only 1 measurement, we needed to estimate the (x,y) position of a feature, which results in poor initialization that messed up the whole simulation