💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

🎁完整资源、论文复现、期刊合作、论文辅导及科研仿真定制事宜点击：

👉👉👉本文完整资源下载

⛳️座右铭：行百里者，半于九十。

 ⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

     或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥第一部分——内容介绍

基于可观测性的EKF-SLAM不一致性问题研究

摘要

基于扩展卡尔曼滤波器的同时定位与地图构建（EKF-SLAM）是移动机器人自主导航的核心经典算法，凭借结构简洁、实时性强的优势被广泛应用于各类无人设备场景。但算法固有的状态估计不一致性问题，会导致机器人位姿与环境地图的估计误差随迭代累积、协方差矩阵失真，严重制约SLAM系统的长期运行精度与稳定性。现有研究多聚焦于算法迭代优化与误差修正，较少从系统本质的可观测性缺陷层面剖析不一致性的根源。本文从系统可观测性理论出发，深入探究EKF线性化过程、状态空间维度匹配偏差、雅可比矩阵更新机制引发的可观测性畸变问题，揭示EKF-SLAM不一致性的内在机理，梳理可观测性失配引发的各类不一致性表现形式，总结现有一致性优化方法的核心原理与局限性，并结合SLAM动态运行特性，提出基于可观测性约束的EKF-SLAM一致性优化思路，为解决传统EKF-SLAM的估计偏差累积问题、提升机器人定位建图的鲁棒性提供理论支撑。

关键词：EKF-SLAM；可观测性；状态估计；不一致性；线性化误差；协方差失真

1 引言

同时定位与地图构建技术是移动机器人实现自主感知、自主导航的核心支撑技术，能够让机器人在未知环境中同步完成自身位姿解算与环境地图重建，广泛应用于智能无人车、室内服务机器人、无人机勘测、水下探测等领域。在众多SLAM算法体系中，EKF-SLAM凭借递归迭代的滤波架构、较低的硬件算力需求，成为传统SLAM框架的代表性算法，在轻量化、实时性场景中具备不可替代的应用价值。

在实际工程应用与理论研究中，EKF-SLAM始终存在难以根治的状态估计不一致性问题，具体表现为滤波迭代过程中，状态估计误差无法通过观测信息有效收敛、理论协方差矩阵与实际误差分布严重偏离、无观测约束维度出现虚假收敛现象，最终导致位姿漂移、地图畸变、定位失效等问题。相较于突发噪声干扰、传感器故障等外部误差，不一致性属于算法固有结构性缺陷，无法通过简单的滤波参数调优、噪声抑制策略彻底解决，是制约EKF-SLAM高精度、长时程运行的核心瓶颈。

现有针对EKF-SLAM不一致性的研究，大多从误差补偿、迭代策略优化、观测模型改进等表层角度开展，能够在一定程度上缓解估计偏差，但未能从根源上消除不一致性的产生条件。随着机器人应用场景愈发复杂，动态环境、长距离航行、弱纹理场景等工况对SLAM系统稳定性的要求持续提升，表层优化方法的局限性逐渐凸显。相关理论研究证实，EKF-SLAM的不一致性本质源于滤波线性化模型与真实非线性系统的可观测性失配，传统EKF迭代过程破坏了SLAM系统固有的可观测子空间结构，引发无信息维度的虚假观测约束，最终造成估计失真与协方差失效。

可观测性是判定动态系统状态能否通过外部观测信息完全解算的核心准则，直接决定滤波估计的有效性与合理性。完整的SLAM非线性系统存在固定维度的不可观测子空间，对应全局坐标系的平移与旋转自由度，这部分维度无法通过局部观测信息确定，属于系统固有特性。而传统EKF在每一步迭代中基于最新状态估计更新雅可比矩阵，导致线性化后的误差状态系统可观测维度高于真实非线性系统，人为创造了虚假观测信息，进而引发一系列不一致性问题。基于此，本文从可观测性理论视角切入，系统性剖析EKF-SLAM不一致性的产生机理、表现特征与演化规律，梳理现有一致性优化算法的可观测性改进逻辑，分析各类方法的优势与短板，最终构建基于可观测性保真的EKF-SLAM优化思路，为从根源上解决算法不一致性问题提供理论依据。

2 相关工作综述

2.1 EKF-SLAM不一致性研究现状

EKF-SLAM的不一致性问题自算法落地以来便受到学界广泛关注，早期研究主要通过实验现象总结不一致性的外在表现，发现机器人静止状态下，无有效观测更新时，位姿姿态方差会出现无依据衰减，地图特征点估计误差持续累积，首次证实了滤波协方差矩阵的不合理收缩现象。后续研究逐步明确，这种异常收敛并非由传感器噪声、迭代误差累积导致，而是EKF线性化迭代机制的固有缺陷。

在机理研究层面，主流研究证实传统EKF-SLAM的核心缺陷在于线性化过程的不稳定性。EKF通过一阶泰勒展开近似拟合非线性SLAM系统，每一次迭代均以当前最新的状态估计值更新雅可比矩阵，导致系统线性化点持续偏移。频繁变化的线性化点破坏了系统状态空间的连续性，使得误差状态模型与真实物理系统的特性产生偏差，最终引发估计不一致。部分研究进一步区分了不一致性的类型，将其分为全局不一致与局部不一致，全局不一致表现为长期迭代后的整体位姿漂移与地图畸变，局部不一致表现为单次迭代中观测约束的虚假生效。

在优化方法层面，现有主流解决方案可分为三类：一是基于固定雅可比矩阵的优化方法，典型代表为首次估计雅可比（FEJ）算法，通过固定关键状态的雅可比计算值，避免线性化点频繁偏移，维持系统模型稳定性；二是基于仿射变换的EKF优化框架，通过约束不可观测子空间的独立性，实现可观测性的精准保真；三是基于状态约束的滤波改进方法，通过添加先验约束抑制无观测维度的虚假收敛。上述方法均能有效缓解不一致性问题，但各类算法的可观测性适配效果、实时性表现存在明显差异，且部分方法在动态复杂场景中仍存在可观测性畸变风险。

2.2 可观测性在SLAM一致性研究中的应用

可观测性理论是解析动态估计系统合理性的核心工具，其核心内涵是判断系统全部状态能否通过有限时长的观测信息唯一确定。对于SLAM系统而言，真实的非线性定位建图系统具备固定的可观测与不可观测子空间划分，全局坐标系的绝对位置、绝对姿态属于不可观测维度，无法通过局部环境观测数据解算，仅能确定机器人与环境特征的相对位姿关系。

早期SLAM可观测性研究仅聚焦于系统整体可观测性判定，未关联滤波一致性问题。直至近年研究证实，EKF-SLAM的不一致性与可观测性失配存在严格的因果关系：真实非线性SLAM系统的不可观测子空间维度与EKF线性化误差模型的不可观测子空间维度不匹配，线性化后的滤波系统过度观测了原本不可观测的全局自由度，导致滤波器在无真实观测信息的维度上错误收缩协方差，产生虚假的估计精度，最终引发不一致性。这一结论确立了可观测性分析在EKF-SLAM一致性研究中的核心地位，也为从根源解决不一致性问题提供了全新研究视角。

3 EKF-SLAM可观测性与不一致性机理分析

3.1 SLAM系统固有可观测性特征

完整的非线性SLAM系统描述了机器人运动状态与环境特征状态的动态演化过程，其可观测性具备固定的物理特性，不受滤波算法、迭代策略影响。从物理本质来看，机器人的局部观测传感器仅能获取自身与周边环境特征的相对距离、相对角度等相对信息，无法感知自身在全局坐标系下的绝对位置与绝对航向。因此，真实SLAM非线性系统存在固定的不可观测自由度，集中对应全局坐标系的平移与旋转维度，这些维度不存在有效观测约束，无法通过观测数据实现状态解算。

基于可观测性空间划分理论，SLAM系统的状态空间可严格划分为可观测子空间与不可观测子空间。可观测子空间包含机器人相对位姿变化、环境特征相对位置、特征间相对关联等可通过观测数据约束的状态维度，是SLAM系统有效估计的核心范围；不可观测子空间为全局绝对位姿维度，属于系统固有冗余自由度，正常情况下不会对相对定位建图精度产生影响，仅需保持该子空间无虚假观测约束即可。稳定的可观测子空间划分是SLAM系统实现一致、精准状态估计的前提，也是滤波算法设计的核心约束条件。

3.2 传统EKF线性化引发的可观测性畸变

传统EKF算法解决SLAM非线性估计问题的核心方式是一阶线性化近似，通过泰勒展开将非线性的运动模型与观测模型转化为线性误差状态模型，进而通过卡尔曼滤波迭代完成状态更新与协方差修正。该线性化近似是引发可观测性畸变、最终导致不一致性的核心根源。

传统EKF的迭代机制中，每一次滤波更新均会利用当前最新的状态估计结果重新计算运动模型与观测模型的雅可比矩阵，即每一步迭代的线性化参考点均处于动态变化中。这种动态线性化方式会直接改变误差状态系统的空间特性，使得线性化后的EKF误差模型可观测子空间维度大于真实非线性SLAM系统的可观测子空间维度。简单而言，真实系统无法观测的全局绝对位姿维度，在EKF线性化模型中被错误判定为可观测维度，人为引入了不存在的观测约束信息。

这种可观测性失配会引发连锁的滤波异常：滤波器误将无观测信息的全局自由度纳入有效估计范围，持续对该维度的协方差矩阵进行收缩更新，产生虚假的估计收敛效果。随着滤波迭代次数增加，这种错误更新不断累积，使得EKF估计的协方差矩阵远小于真实误差的波动范围，出现“估计精度虚高”的现象。此时滤波系统的理论误差分布与实际物理系统的误差分布完全脱节，状态估计结果持续偏离真实值，最终形成系统性、持续性的不一致性问题。

3.3 可观测性畸变诱发的不一致性具体表现

基于可观测性失配引发的EKF-SLAM不一致性，在机器人运行过程中呈现出三类典型特征，且各类特征均与可观测子空间畸变直接对应。

第一，协方差矩阵不合理收缩。在机器人静止、无新增观测数据的稳态工况下，真实SLAM系统无有效信息更新，状态误差应保持稳定，协方差矩阵无明显变化。但传统EKF因可观测性畸变，会持续对全局不可观测维度进行错误更新，导致位姿、地图特征的协方差方差持续衰减，呈现虚假收敛状态，无法真实反映系统误差不确定性。

第二，长时程迭代的位姿漂移与地图畸变。短期迭代中，可观测性畸变带来的误差累积较小，估计偏差不明显；但随着机器人持续运动、滤波不断迭代，无观测维度的虚假约束误差持续叠加，导致机器人全局位姿出现缓慢漂移，环境地图特征的位置估计逐步偏离真实位置，出现地图拉伸、扭曲、错位等问题，最终导致SLAM系统失效。

第三，状态更新过拟合与观测失效。在线性化可观测性畸变的影响下，EKF滤波器过度依赖动态更新的雅可比矩阵，对局部观测噪声产生过拟合现象。当传感器存在轻微噪声或环境出现小幅动态扰动时，滤波器无法区分有效观测信息与噪声干扰，将噪声信息误判为有效状态更新依据，进一步加剧估计不一致性，降低系统鲁棒性。

4 现有基于可观测性约束的一致性优化方法分析

4.1 首次估计雅可比优化方法

首次估计雅可比（FEJ）算法是当前解决EKF-SLAM不一致性最经典、应用最广泛的方法，其核心优化逻辑是从线性化源头修正可观测性畸变问题。该算法摒弃了传统EKF逐迭代更新雅可比矩阵的机制，对每个状态变量仅采用首次有效估计值完成雅可比矩阵计算与线性化操作，全程固定线性化参考点。

该优化方式能够有效稳定误差状态系统的空间结构，使线性化后EKF模型的可观测子空间、不可观测子空间维度与真实非线性SLAM系统完全匹配，彻底消除虚假可观测维度，从根源上抑制无信息维度的协方差不合理收缩，显著提升滤波一致性。FEJ算法结构简单、计算成本低，能够兼容各类EKF-SLAM场景，实时性优势明显。但该方法存在固有局限性，固定的线性化点无法适配机器人大范围运动、环境剧烈变化的工况，当状态真实值与首次估计值偏差过大时，线性化近似误差会显著增大，反而降低估计精度，在高速运动、大转角姿态变化场景中优化效果大幅下降。

4.2 仿射变换EKF优化框架

仿射EKF（Aff-EKF）是近年提出的新型一致性优化框架，基于可观测性维持的充要条件构建优化体系。该方法通过严格的理论推导，证明了EKF滤波保持一致性的核心条件：线性化后的不可观测子空间需独立于系统状态变量，不受迭代更新影响。基于该条件，仿射EKF通过引入仿射变换约束，重构误差状态更新机制，实现可观测子空间与不可观测子空间的精准划分与稳定维持。

相较于FEJ算法，仿射EKF无需固定线性化点，能够在动态迭代过程中持续保障可观测性匹配，有效适配复杂动态场景，一致性优化效果更稳定，能够同时抑制线性化误差与可观测性畸变带来的双重不一致性问题。但该算法的变换约束流程复杂，计算复杂度高于传统EKF与FEJ-EKF，对硬件算力要求更高，难以适配轻量化、高实时性的机器人应用场景，工程落地性受限。

4.3 观测模型约束优化方法

部分研究从观测模型层面切入，通过优化观测约束逻辑修正可观测性失配问题。该类方法核心思路是约束观测信息的有效作用范围，杜绝观测数据对全局不可观测维度产生错误约束，仅将观测更新作用于可观测子空间对应的相对状态维度。通过构建差异化的状态更新策略，分离可观测维度与不可观测维度的迭代更新逻辑，避免全局自由度被虚假观测约束修正。

该类方法能够针对性解决观测模型引发的可观测性畸变问题，对静态、低速场景的一致性优化效果良好，但通用性较差，需针对不同的传感器观测模型、不同的SLAM场景定制优化，无法形成通用的EKF-SLAM一致性解决方案，且无法规避运动模型线性化带来的可观测性误差。

5 基于可观测性保真的EKF-SLAM一致性优化思路

结合现有优化方法的优势与局限性，本文立足可观测性保真核心目标，兼顾算法一致性、实时性与场景通用性，提出一套适配动态复杂场景的EKF-SLAM优化思路，核心目标是全程维持线性化滤波模型与真实SLAM非线性系统的可观测性空间一致性，从根源消除不一致性产生条件。

第一，构建自适应线性化点更新机制。针对FEJ算法固定线性化点的局限性，摒弃绝对固定或逐帧更新的极端模式，设计基于状态偏差阈值的自适应线性化策略。当当前状态估计值与历史首次估计值偏差在合理阈值范围内时，沿用首次估计雅可比矩阵，保障可观测子空间稳定；当偏差超出阈值、线性化误差过大时，适度更新线性化参考点，同时通过可观测性校验约束雅可比矩阵更新范围，确保更新前后系统可观测维度不发生畸变，兼顾线性化精度与可观测性保真需求。

第二，增设可观测性校验约束模块。在EKF滤波迭代流程中嵌入可观测性判定环节，每一次雅可比矩阵更新、状态更新后，实时校验误差状态系统的可观测子空间维度与结构，判定是否存在虚假可观测维度。一旦检测到可观测性失配，立即修正迭代更新权重，屏蔽不可观测维度的错误更新，杜绝协方差虚假收缩，保障滤波模型始终贴合真实SLAM系统的物理特性。

第三，优化差异化状态迭代策略。基于可观测性空间划分结果，区分可观测子空间与不可观测子空间的迭代更新逻辑。对于机器人相对位姿、环境特征相对位置等可观测维度，保留正常的观测更新与协方差修正机制；对于全局绝对位姿等固有不可观测维度，屏蔽所有无效观测约束，禁止无依据的协方差更新，从迭代机制上杜绝人为引入的观测误差与估计偏差。

该优化思路无需复杂的仿射变换运算，算力消耗接近传统EKF算法，能够有效规避现有方法的短板，在高速运动、动态复杂、长时程运行场景中持续维持滤波一致性，同时保障算法实时性，具备更强的工程通用性与落地价值。

6 结论与展望

本文从可观测性理论视角，系统性探究了EKF-SLAM算法的不一致性问题。研究明确证实，EKF-SLAM的固有不一致性本质是线性化滤波模型与真实非线性系统的可观测性失配所致：传统EKF逐迭代动态更新雅可比矩阵的机制，改变了SLAM系统固有的可观测子空间结构，人为激活了全局不可观测自由度的虚假观测约束，引发协方差失真、位姿漂移、地图畸变等一系列不一致性现象。

本文系统梳理了FEJ、仿射EKF、观测模型约束等主流一致性优化方法的可观测性改进原理，分析了各类方法的优势与场景局限性。在此基础上，结合可观测性保真核心准则，提出了自适应线性化更新、可观测性实时校验、差异化状态迭代的一体化优化思路，有效兼顾了EKF-SLAM的估计一致性、运行实时性与场景通用性，为解决算法固有不一致性问题提供了新的理论思路。

未来研究可围绕两个方向深化：一是进一步量化可观测性畸变程度与不一致性误差的对应关系，构建精准的可观测性评价体系，实现不一致性问题的定量预判与修正；二是将可观测性约束优化思路拓展至视觉SLAM、激光惯性SLAM等多传感器融合EKF框架，提升多场景下滤波系统的一致性与鲁棒性，推动EKF-SLAM算法在高精度、长时程自主导航场景的规模化应用。

📚第二部分——运行结果

部分代码：

sigma_v = sigma/sqrt(2); %.1*v_true; %
sigma_w = 2*sqrt(2)*sigma; %.1*omega_true; 1*pi/180; %
Q = diag([sigma_v^2 sigma_w^2]);

sigma_p = .1; %noise is the percentagae of distance measurement, BUT double check rws.m since sometimes we use this as a constant absolute sigma
sigma_r = 1; %range measmnt noise
sigma_th = 10*pi/180;%bearing measuremnt noise

nL = 20; %number of landmarks
nSteps = 2500; %nubmer of time steps
nRuns = 5; %number of monte carlo runs
if nL==1
    max_range = 200;%always observe this landmark
else
    max_range = 5;%env_size/10;%.5*v_true/omega_true;%
end
min_range = .5;
init_steps = 0;%3
max_delay = 10;%for delayed initial
NIncr = 0; %increment number of incremental MAP: 0 - not runing

%% preallocate memory for saving resutls

% Ideal EKF
xRest_id = zeros(3,nSteps,nRuns); %estimated traj
xRerr_id = zeros(3,nSteps,nRuns); %all err state
Prr_id = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_id = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_id =  zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_id = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_id = zeros(2,nL,nSteps,nRuns);
xLerr_id = zeros(2,nL,nSteps,nRuns);
Pll_id = zeros(2,nL,nSteps,nRuns);
neesL_id = zeros(1,nL,nSteps,nRuns);
rmsL_id = zeros(1,nL,nSteps,nRuns);
nees_id = zeros(1,nSteps,nRuns); %nees for the whole state

% Standard EKF
xRest_std = zeros(3,nSteps,nRuns); %estimated traj
xRerr_std = zeros(3,nSteps,nRuns); %all err state
Prr_std = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_std = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_std =  zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_std = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_std = zeros(2,nL,nSteps,nRuns);
xLerr_std = zeros(2,nL,nSteps,nRuns);
Pll_std = zeros(2,nL,nSteps,nRuns);
neesL_std = zeros(1,nL,nSteps,nRuns);
rmsL_std = zeros(1,nL,nSteps,nRuns);
nees_std = zeros(1,nSteps,nRuns); %nees for the whole state
kld_std = zeros(1,nSteps,nRuns); % KLD

% FEJ-EKF
xRest_fej = zeros(3,nSteps,nRuns); %estimated traj
xRerr_fej = zeros(3,nSteps,nRuns); %all err state
Prr_fej = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_fej = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_fej =  zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_fej = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_fej = zeros(2,nL,nSteps,nRuns);
xLerr_fej = zeros(2,nL,nSteps,nRuns);
Pll_fej = zeros(2,nL,nSteps,nRuns);
neesL_fej = zeros(1,nL,nSteps,nRuns);
rmsL_fej = zeros(1,nL,nSteps,nRuns);
nees_fej = zeros(1,nSteps,nRuns); %nees for the whole state
kld_fej = zeros(1,nSteps,nRuns); % KLD

% OC-EKF
xRest_ocekf_1 = zeros(3,nSteps,nRuns); %estimated traj
xRerr_ocekf_1 = zeros(3,nSteps,nRuns); %all err state
Prr_ocekf_1 = zeros(3,nSteps,nRuns); %actually diag of Prr
neesR_ocekf_1 = zeros(1,nSteps,nRuns); %nees (or mahalanobis distance)
rmsRp_ocekf_1 =  zeros(1,nSteps,nRuns); %rms of robot position
rmsRth_ocekf_1 = zeros(1,nSteps,nRuns); %rms of robot orientation
xLest_ocekf_1 = zeros(2,nL,nSteps,nRuns);
xLerr_ocekf_1 = zeros(2,nL,nSteps,nRuns);
Pll_ocekf_1 = zeros(2,nL,nSteps,nRuns);
neesL_ocekf_1 = zeros(1,nL,nSteps,nRuns);
rmsL_ocekf_1 = zeros(1,nL,nSteps,nRuns);
nees_ocekf_1 = zeros(1,nSteps,nRuns); %nees for the whole state
kld_ocekf_1 = zeros(1,nSteps,nRuns); % KLD

%% LANDMARK GENERATION: same landmarks in each run
if nL==1
    xL_true_fixed = [0;v_true/omega_true];
elseif nL==2
    xL_true_fixed = [5 -5; 5 15];
else
    xL_true_fixed = gen_map(nL,v_true,omega_true,min_range, max_range, nSteps,dt);%max_range=5
end

%% Monte Carlo Simulations

tic
for kk = 1:nRuns
    
    kk
    
    % % real world simulation data % %
    xL_true(:,:,kk) = xL_true_fixed;
    [v_m,omega_m, v_true_all,omega_true_all, xR_true(:,:,kk), z,R] = rws(nSteps, dt,v_true,omega_true,sigma_v,sigma_w,sigma_r,sigma_th,sigma_p,xL_true(:,:,kk),max_range,min_range);
    
    
    % % INITIALIZATION
    x0 = zeros(3,1);
    if init_steps
        P0 = zeros(3);
    else
        P0 = diag([(.0001)^2,(.0001)^2,(.0001)^2]);

🎉第三部分——参考文献 

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)

​​​​​​🌈第四部分——本文完整资源下载

资料获取，更多粉丝福利，MATLAB|Simulink|Python|数据|文档等完整资源获取

本文完整资源下载