kalman

代码说明：

  实现了1D,2D,3D空间自由度下的运动参数的扩展Kalman滤波算法，文章2008 IROS Visual SLAM for 3D Large-Scale Seabed Acquisition Employing Underwater Vehicles的具体实现。(a novel technique to align partial 3D reconstructions of the seabed acquired by a stereo camera mounted on an autonomous underwater vehicle. Vehicle localization and seabed mapping is performed simultaneously by means of an Extended Kalman Filter. Passive landmarks are detected on the images and characterized considering 2D and 3D features. Landmarks are re-observed while the robot is navigating and data association becomes easier but robust. Once the survey is completed, vehicle trajectory is smoothed by a Rauch-Tung-Striebel filter obtaining an even better alignment of the 3D views and yet a large-scale acquisition of the seabed.)