✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍2D SLAM 技术致力于同时确定机器人在二维空间中的位置和构建周围环境的地图。在机器人导航领域它是实现自主移动的核心技术使机器人能够在未知环境中探索并找到路径。在自动驾驶领域2D SLAM 为车辆提供高精度的定位和地图信息辅助车辆做出决策。在 2D SLAM 过程中传感器如激光雷达、摄像头等会产生大量含有噪声的数据。滤波算法的作用是对这些数据进行处理通过融合传感器信息来估计机器人的位姿以及地图特征从而实现准确的定位和地图构建。不同的滤波算法在处理非线性系统和噪声方面各有特点对 2D SLAM 系统的性能影响显著。因此深入了解和对比不同滤波算法在 2D SLAM 中的应用具有重要意义。扩展卡尔曼滤波EKF基本原理扩展卡尔曼滤波EKF是一种广泛应用于非线性系统状态估计的算法。由于实际中的 2D SLAM 系统通常是非线性的EKF 通过对非线性方程进行线性化近似来处理此类问题。四种滤波算法原理Ideal EKF理想扩展卡尔曼滤波Ideal EKF 假设系统模型和测量完全准确不存在任何噪声和误差。在这种理想情况下EKF 能够完美地跟踪系统状态变化准确估计机器人位姿和地图信息。虽然在现实世界中这种理想条件几乎不可能实现但 Ideal EKF 作为一种理论参考模型有助于我们清晰地理解 EKF 的基本原理和潜在的最佳性能表现为评估其他实际应用的滤波算法提供了一个理论上限。Standard EKF标准扩展卡尔曼滤波Standard EKF 是实际应用中常用的算法它充分考虑了系统噪声和测量噪声的影响。在 2D SLAM 中机器人的运动模型如里程计模型和传感器观测模型如激光雷达测量模型通常是非线性的。Standard EKF 通过计算 Jacobian 矩阵对这些非线性方程进行线性化处理以此来近似估计系统状态。然而这种线性化近似不可避免地引入了误差尤其是在系统状态变化较大或非线性程度较高的情况下线性化误差可能会逐渐累积导致估计精度下降。尽管如此在许多实际场景中Standard EKF 仍能提供较为可靠的状态估计。FEJ - EKFFirst - Estimate - Jacobian EKF首估计雅可比扩展卡尔曼滤波FEJ - EKF 为了简化计算过程在整个估计过程中使用初始线性化点来计算 Jacobian 矩阵即首估计雅可比矩阵。这样做的好处是减少了每次迭代中计算 Jacobian 矩阵的次数从而降低了计算量。然而由于使用固定的首估计雅可比矩阵当系统状态发生较大变化时该矩阵可能无法准确反映系统的非线性特性导致估计误差逐渐积累影响最终的估计精度。因此FEJ - EKF 在系统状态变化较为平缓的场景下可能表现良好但在复杂多变的 2D SLAM 环境中其性能可能受到限制。OC - EKF基于可观测性约束的扩展卡尔曼滤波OC - EKF 是在 Standard EKF 基础上发展而来的一种改进算法它引入了可观测性分析。在 2D SLAM 中系统的某些状态可能在特定情况下难以准确观测这会影响滤波算法的估计精度和稳定性。OC - EKF 通过构建可观测性矩阵来评估系统状态的可观测性该矩阵反映了系统状态与观测值之间的关系。依据可观测性条件OC - EKF 对滤波过程进行约束例如当某些状态的可观测性较差时适当调整卡尔曼增益减少这些状态对估计结果的影响或者采取其他措施来增强对这些状态的观测。通过这种方式OC - EKF 能够提高估计精度增强滤波稳定性尤其在处理 2D SLAM 中复杂的非线性问题和不确定的观测环境时展现出更好的性能。算法对比意义在 2D SLAM 应用中对比这四种滤波算法Ideal EKF、Standard EKF、FEJ - EKF、OC - EKF具有重要意义。不同的算法在估计精度、计算效率和稳定性等方面各有优劣。例如Ideal EKF 提供了理论上的最佳性能参考Standard EKF 是常用的实际算法但存在线性化误差FEJ - EKF 牺牲一定精度换取计算效率OC - EKF 则在提高精度和稳定性方面有独特优势。通过对比这些算法在不同场景下的性能表现我们可以根据具体的应用需求和硬件条件选择最适合的滤波算法从而优化 2D SLAM 系统的性能实现更准确的定位和地图构建。总结四种滤波算法Ideal EKF、Standard EKF、FEJ - EKF、OC - EKF在 2D SLAM 中各有其背景原理和特点。Ideal EKF 作为理论参考展现了 EKF 的理想性能Standard EKF 是实际应用基础FEJ - EKF 在计算效率上有所侧重OC - EKF 通过可观测性约束提升了精度和稳定性。理解这些算法的原理以及对比它们的性能对于优化 2D SLAM 系统推动机器人导航和自动驾驶等相关领域的发展具有关键作用。⛳️ 运行结果 部分代码close all% % setupfor nr 1%1:nRunsfigureplot(xR_true(1,:,nr), xR_true(2,:,nr), b-.,Linewidth,1), hold onplot(xL_true(1,:,nr), xL_true(2,:,nr), b*,Linewidth,1)title(Robot trajectory and landmarks)legend(True Trajectory,True Landmarks)xlabel(x (m),FontWeight,bold), ylabel(y (m),FontWeight,bold)axis equalprint(-dpng,setup_sim)end% % robot % %start 1; incr 5;% Robot NEESfigure, hold onplot([start:incr:nSteps],neesR_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],neesR_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],neesR_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],neesR_avg_ocekf_1(start:incr:end),k--,Linewidth,2);xlabel(Time (sec),FontWeight,bold), ylabel(Robot pose NEES,FontWeight,bold)legend( Ideal-EKF,Std-EKF,FEJ-EKF,OC-EKF)%,OC-EKF3,Robocentric)print(-dpng,slam_robot_nees_oc3_sim)% Robot RMSEfiguresubplot(2,1,1), hold onplot([start:incr:nSteps],rmsRp_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],rmsRp_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],rmsRp_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],rmsRp_avg_ocekf_1(start:incr:end),k--,Linewidth,2);ylabel(Position RMSE (m),FontWeight,bold)legend( Ideal-EKF,Std-EKF,FEJ-EKF,OC-EKF)%,OC-EKF3,Robocentric) %#ok*LEGINTPARsubplot(2,1,2), hold onplot([start:incr:nSteps],rmsRth_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],rmsRth_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],rmsRth_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],rmsRth_avg_ocekf_1(start:incr:end),k--,Linewidth,2);xlabel(Time (sec),FontWeight,bold),ylabel(Heading RMSE (rad),FontWeight,bold)print(-dpng,slam_robot_rms_oc3_sim)NEES_Robot [mean(neesR_avg_id(1:end)),mean(neesR_avg_std(1:end)), mean(neesR_avg_fej(1:end)), mean(neesR_avg_ocekf_1(1:end)) ]RMS_Position [mean(rmsRp_avg_id(1:end)),mean(rmsRp_avg_std(1:end)), mean(rmsRp_avg_fej(1:end)), mean(rmsRp_avg_ocekf_1(1:end)) ]RMS_Heading [mean(rmsRth_avg_id(1:end)),mean(rmsRth_avg_std(1:end)), mean(rmsRth_avg_fej(1:end)), mean(rmsRth_avg_ocekf_1(1:end))]% % landmarks % %% NEESfigurebar([neesL_avg_id,...neesL_avg_std,...neesL_avg_fej,...neesL_avg_ocekf_1 ]);ylabel(Avg. Landmark NEES,FontWeight,bold)print(-dpng,lm_nees)% RMSEfigurebar([rmsL_avg_id,...rmsL_avg_std,...rmsL_avg_fej,...rmsL_avg_ocekf_1 ]);ylabel(Avg. Landmark RMSE,FontWeight,bold)print(-dpng,lm_rms)NEES_L [neesL_avg_id,neesL_avg_std,neesL_avg_fej, neesL_avg_ocekf_1]RMS_L [rmsL_avg_id,rmsL_avg_std,rmsL_avg_fej, rmsL_avg_ocekf_1 ] 参考文献往期回顾扫扫下方二维码
【滤波跟踪】基于可观测性约束的扩展卡尔曼滤波（OC-EKF） 2D SLAM 仿真代码，对比四种滤波算法（Ideal EKF、Standard EKF、FEJ-EKF、OC-EKF）matlab代码
✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍2D SLAM 技术致力于同时确定机器人在二维空间中的位置和构建周围环境的地图。在机器人导航领域它是实现自主移动的核心技术使机器人能够在未知环境中探索并找到路径。在自动驾驶领域2D SLAM 为车辆提供高精度的定位和地图信息辅助车辆做出决策。在 2D SLAM 过程中传感器如激光雷达、摄像头等会产生大量含有噪声的数据。滤波算法的作用是对这些数据进行处理通过融合传感器信息来估计机器人的位姿以及地图特征从而实现准确的定位和地图构建。不同的滤波算法在处理非线性系统和噪声方面各有特点对 2D SLAM 系统的性能影响显著。因此深入了解和对比不同滤波算法在 2D SLAM 中的应用具有重要意义。扩展卡尔曼滤波EKF基本原理扩展卡尔曼滤波EKF是一种广泛应用于非线性系统状态估计的算法。由于实际中的 2D SLAM 系统通常是非线性的EKF 通过对非线性方程进行线性化近似来处理此类问题。四种滤波算法原理Ideal EKF理想扩展卡尔曼滤波Ideal EKF 假设系统模型和测量完全准确不存在任何噪声和误差。在这种理想情况下EKF 能够完美地跟踪系统状态变化准确估计机器人位姿和地图信息。虽然在现实世界中这种理想条件几乎不可能实现但 Ideal EKF 作为一种理论参考模型有助于我们清晰地理解 EKF 的基本原理和潜在的最佳性能表现为评估其他实际应用的滤波算法提供了一个理论上限。Standard EKF标准扩展卡尔曼滤波Standard EKF 是实际应用中常用的算法它充分考虑了系统噪声和测量噪声的影响。在 2D SLAM 中机器人的运动模型如里程计模型和传感器观测模型如激光雷达测量模型通常是非线性的。Standard EKF 通过计算 Jacobian 矩阵对这些非线性方程进行线性化处理以此来近似估计系统状态。然而这种线性化近似不可避免地引入了误差尤其是在系统状态变化较大或非线性程度较高的情况下线性化误差可能会逐渐累积导致估计精度下降。尽管如此在许多实际场景中Standard EKF 仍能提供较为可靠的状态估计。FEJ - EKFFirst - Estimate - Jacobian EKF首估计雅可比扩展卡尔曼滤波FEJ - EKF 为了简化计算过程在整个估计过程中使用初始线性化点来计算 Jacobian 矩阵即首估计雅可比矩阵。这样做的好处是减少了每次迭代中计算 Jacobian 矩阵的次数从而降低了计算量。然而由于使用固定的首估计雅可比矩阵当系统状态发生较大变化时该矩阵可能无法准确反映系统的非线性特性导致估计误差逐渐积累影响最终的估计精度。因此FEJ - EKF 在系统状态变化较为平缓的场景下可能表现良好但在复杂多变的 2D SLAM 环境中其性能可能受到限制。OC - EKF基于可观测性约束的扩展卡尔曼滤波OC - EKF 是在 Standard EKF 基础上发展而来的一种改进算法它引入了可观测性分析。在 2D SLAM 中系统的某些状态可能在特定情况下难以准确观测这会影响滤波算法的估计精度和稳定性。OC - EKF 通过构建可观测性矩阵来评估系统状态的可观测性该矩阵反映了系统状态与观测值之间的关系。依据可观测性条件OC - EKF 对滤波过程进行约束例如当某些状态的可观测性较差时适当调整卡尔曼增益减少这些状态对估计结果的影响或者采取其他措施来增强对这些状态的观测。通过这种方式OC - EKF 能够提高估计精度增强滤波稳定性尤其在处理 2D SLAM 中复杂的非线性问题和不确定的观测环境时展现出更好的性能。算法对比意义在 2D SLAM 应用中对比这四种滤波算法Ideal EKF、Standard EKF、FEJ - EKF、OC - EKF具有重要意义。不同的算法在估计精度、计算效率和稳定性等方面各有优劣。例如Ideal EKF 提供了理论上的最佳性能参考Standard EKF 是常用的实际算法但存在线性化误差FEJ - EKF 牺牲一定精度换取计算效率OC - EKF 则在提高精度和稳定性方面有独特优势。通过对比这些算法在不同场景下的性能表现我们可以根据具体的应用需求和硬件条件选择最适合的滤波算法从而优化 2D SLAM 系统的性能实现更准确的定位和地图构建。总结四种滤波算法Ideal EKF、Standard EKF、FEJ - EKF、OC - EKF在 2D SLAM 中各有其背景原理和特点。Ideal EKF 作为理论参考展现了 EKF 的理想性能Standard EKF 是实际应用基础FEJ - EKF 在计算效率上有所侧重OC - EKF 通过可观测性约束提升了精度和稳定性。理解这些算法的原理以及对比它们的性能对于优化 2D SLAM 系统推动机器人导航和自动驾驶等相关领域的发展具有关键作用。⛳️ 运行结果 部分代码close all% % setupfor nr 1%1:nRunsfigureplot(xR_true(1,:,nr), xR_true(2,:,nr), b-.,Linewidth,1), hold onplot(xL_true(1,:,nr), xL_true(2,:,nr), b*,Linewidth,1)title(Robot trajectory and landmarks)legend(True Trajectory,True Landmarks)xlabel(x (m),FontWeight,bold), ylabel(y (m),FontWeight,bold)axis equalprint(-dpng,setup_sim)end% % robot % %start 1; incr 5;% Robot NEESfigure, hold onplot([start:incr:nSteps],neesR_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],neesR_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],neesR_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],neesR_avg_ocekf_1(start:incr:end),k--,Linewidth,2);xlabel(Time (sec),FontWeight,bold), ylabel(Robot pose NEES,FontWeight,bold)legend( Ideal-EKF,Std-EKF,FEJ-EKF,OC-EKF)%,OC-EKF3,Robocentric)print(-dpng,slam_robot_nees_oc3_sim)% Robot RMSEfiguresubplot(2,1,1), hold onplot([start:incr:nSteps],rmsRp_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],rmsRp_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],rmsRp_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],rmsRp_avg_ocekf_1(start:incr:end),k--,Linewidth,2);ylabel(Position RMSE (m),FontWeight,bold)legend( Ideal-EKF,Std-EKF,FEJ-EKF,OC-EKF)%,OC-EKF3,Robocentric) %#ok*LEGINTPARsubplot(2,1,2), hold onplot([start:incr:nSteps],rmsRth_avg_id(start:incr:end),g:,Linewidth,3);plot([start:incr:nSteps],rmsRth_avg_std(start:incr:end),b-o,Linewidth,1);plot([start:incr:nSteps],rmsRth_avg_fej(start:incr:end),m-.,Linewidth,2);plot([start:incr:nSteps],rmsRth_avg_ocekf_1(start:incr:end),k--,Linewidth,2);xlabel(Time (sec),FontWeight,bold),ylabel(Heading RMSE (rad),FontWeight,bold)print(-dpng,slam_robot_rms_oc3_sim)NEES_Robot [mean(neesR_avg_id(1:end)),mean(neesR_avg_std(1:end)), mean(neesR_avg_fej(1:end)), mean(neesR_avg_ocekf_1(1:end)) ]RMS_Position [mean(rmsRp_avg_id(1:end)),mean(rmsRp_avg_std(1:end)), mean(rmsRp_avg_fej(1:end)), mean(rmsRp_avg_ocekf_1(1:end)) ]RMS_Heading [mean(rmsRth_avg_id(1:end)),mean(rmsRth_avg_std(1:end)), mean(rmsRth_avg_fej(1:end)), mean(rmsRth_avg_ocekf_1(1:end))]% % landmarks % %% NEESfigurebar([neesL_avg_id,...neesL_avg_std,...neesL_avg_fej,...neesL_avg_ocekf_1 ]);ylabel(Avg. Landmark NEES,FontWeight,bold)print(-dpng,lm_nees)% RMSEfigurebar([rmsL_avg_id,...rmsL_avg_std,...rmsL_avg_fej,...rmsL_avg_ocekf_1 ]);ylabel(Avg. Landmark RMSE,FontWeight,bold)print(-dpng,lm_rms)NEES_L [neesL_avg_id,neesL_avg_std,neesL_avg_fej, neesL_avg_ocekf_1]RMS_L [rmsL_avg_id,rmsL_avg_std,rmsL_avg_fej, rmsL_avg_ocekf_1 ] 参考文献往期回顾扫扫下方二维码
