For now, lanmarks are all initialized the same in 3D-FastSLAM
Init. FastSLAM
added linear and angular speed to Ground Truth log to check if MORSE is doing something wrong.
Cleaning the repository.
Cleaned.
Init of FastSLAM finished. Repo is clean
and uncommented...
Moved BESE, Utils&FastSLAM to QT5.1, wouldn't compile otherwise. Removed accidentally the *.txt files to define constraints in the BESE, and they were not under VCS...
changed updatestate to updateOrientation/position
Particle also need to know the pinger variance.
Map was initialized both in the particle contructor and initParticles. Only kept the last one.
Changed FastSLAM constructor to allow different resampling algorithms and methods.
Made the weight depending on the previous ones.
Give the choise for the weighting method (will be able to test the difference between the two). Incorporating the different resampling methods and strategy choice.
finished the map update method
removed quartil resampling for percentil resampling.
Made the robot pos. accessible from the KF
Minor aesthetics changes
Wrote jacobian of the observation model in the KFs
Completed EKF equations