✅作者简介：热爱科研的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 - EKF（First - 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
% % setup
for nr= 1%1:nRuns
figure
plot(xR_true(1,:,nr), xR_true(2,:,nr), 'b-.','Linewidth',1), hold on
plot(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 equal
print('-dpng','setup_sim')
end
% % robot % %
start= 1; incr= 5;
% Robot NEES
figure, hold on
plot([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 RMSE
figure
subplot(2,1,1), hold on
plot([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<*LEGINTPAR>
subplot(2,1,2), hold on
plot([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 % %
% NEES
figure
bar([neesL_avg_id,...
neesL_avg_std,...
neesL_avg_fej,...
neesL_avg_ocekf_1 ]);
ylabel('Avg. Landmark NEES','FontWeight','bold')
print('-dpng','lm_nees')
% RMSE
figure
bar([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 ]
