First Estimate Jacobian (FEJ)经典论文中的EKF SLAM的error-state equation的推导过程,程序员大本营,技术文章内容聚合第一站。
本文的目标就是通过EKF算法同时估计出这些码的位置和机器人的位姿。 实验环境 要实现EKF-SLAM,最关键的就是建立运动模型和观测模型,将这两个模型直接带进EKF算法框架就是EKF-SLAM。EKF-SLAM算法使用扩展的状态空间:X=[xyθmx,1my,1mx,2my,2⋯mx,Nmy,N]T 前3项是机器人位姿,后2N项是 N个路标点的位置...