EKF-SLAM-on-Manifold
This is a joint work by Teng Zhang, Kanzhi Wu, Shoudong Huang and Gamini Dissanayake.
The document is the paper "Convergence and Consistency for A 3D Invariant-EKF SLAM". Fortunately, this paper has been accepted by Robotics and Automation Letters.
This repository is for code release only! Under construction This repository is for R-EKF(right_ekf_3d), FEJ-EKF(f_ekf_3d) and T-EKF(ekf_3d) code ONLY
Simulation settings can be tuned into "./datagen_3d": (1) simulated trajectory: gen_trajectory.m (2) noise level, number of landmarks, FOV: config.m
If you want to test a simple case: for example, (1) run "./datagen_3d/gen_data.m" to generate "data.mat"(all measurements and ground truth) (2) copy this "data.mat" into the foler "./right_ekf_3d.m" or "./f_ekf_3d.m" or "./ekf_3d.m" (3) run "REKF_SLAM.m" or "FEKF_SLAM.m" or "EKF_SLAM.m" and get the simulation test
If you want to test a simple test for three filters: for example, (1) run "./comparison_3d.m" and the comparsion will be displayed
If you want to perform Monte Carlo test for three filters: for example, (1) run "./multi_comparison_3d.m" and the comparsion will be displayed