1. 从两个经典问题说起第一次接触卡尔曼滤波和非线性优化时很多人都会产生这样的疑问为什么EKF扩展卡尔曼滤波的推导过程中会出现雅可比矩阵为什么SLAM后端优化中的高斯牛顿法和EKF看起来如此相似我在研究生阶段就被这两个问题困扰许久直到某天在推导IEKF迭代扩展卡尔曼滤波时突然意识到——它们本质上都是贝叶斯框架下的概率估计问题。2. 理论基础贝叶斯滤波的两种视角2.1 状态估计的概率解释任何状态估计问题都可以表述为在已知观测数据Z的条件下估计系统状态X的后验概率P(X|Z)。根据贝叶斯定理P(X|Z) ∝ P(Z|X)P(X)这个看似简单的公式却衍生出了两种不同的实现路径滤波流派通过维护状态量的均值和协方差一阶、二阶矩来近似表达概率分布典型代表就是卡尔曼滤波系列优化流派直接求解最大后验概率(MAP)通过迭代优化寻找概率峰值2.2 卡尔曼滤波的优化视角传统教材中卡尔曼滤波常被表述为预测-更新的两步过程。但换个角度看其更新方程K P_prior H^T (H P_prior H^T R)^-1 X_post X_prior K(Z - HX_prior)实际上等价于求解以下优化问题min ||X - X_prior||_{P_prior^-1} ||Z - HX||_R^-1其中||e||_Σ表示马氏距离e^TΣ^-1e。这与最小二乘法的形式完全一致3. EKF与高斯牛顿法的血缘关系3.1 EKF的线性化本质当系统存在非线性时EKF的做法是在当前估计点X_op处对运动模型和观测模型进行一阶泰勒展开 f(X) ≈ f(X_op) F(X - X_op) h(X) ≈ h(X_op) H(X - X_op)然后套用标准KF公式这个过程实际上构建了一个局部线性近似。有趣的是这与牛顿法中的一阶近似如出一辙。3.2 高斯牛顿法的概率解释考虑非线性最小二乘问题min Σ||z_i - h_i(X)||^2高斯牛顿法的迭代步骤为ΔX (J^T J)^-1 J^T (Z - h(X)) X_new X ΔX其中J是雅可比矩阵。将其改写为ΔX (J^T R^-1 J)^-1 J^T R^-1 (Z - h(X))这与EKF的更新方程几乎完全一致区别仅在于EKF显式维护了P矩阵高斯牛顿法隐含在正规方程中4. IEKF与优化方法的深度联系4.1 IEKF的迭代过程IEKF相比EKF的改进在于在每次更新时多次重新线性化迭代直至收敛其核心步骤for k1:N H_k ∂h/∂X|X_k K_k P H_k^T (H_k P H_k^T R)^-1 X_{k1} X_k K_k (Z - h(X_k) - H_k(X_k - X_prior)) end4.2 与LM算法的对比莱文贝格-马夸特(LM)算法可以视为高斯牛顿法的改进版其迭代步长ΔX (J^T J λI)^-1 J^T (Z - h(X))与IEKF相比IEKF中的P矩阵起到了类似λI的调节作用两者都是通过引入保守性来改善收敛性迭代思路完全一致5. 统一框架下的实现差异虽然数学本质相通但工程实现上仍有区别特性滤波方法优化方法状态维护方式显式维护均值与协方差仅维护当前估计值内存消耗O(n^2)O(n)实时性适合流式处理需要完整数据线性化点通常只在预测点线性化每次迭代重新线性化历史信息利用通过协方差矩阵隐含需要显式处理实际经验在SLAM中滤波方法更适用于前端里程计而优化方法更适合后端全局调整。我曾在一个无人机项目中混合使用IEKF前端和因子图优化后端取得了很好的效果。6. 工程实践中的选择建议6.1 何时选择滤波方法系统状态维度较低100维需要严格的实时处理运动模型相对简单对内存占用敏感的场景6.2 何时选择优化方法存在闭环检测等全局约束系统状态维度较高可以接受一定延迟需要处理非常复杂的观测模型6.3 混合架构设计在一些复杂系统中可以采用分层架构前端使用IEKF进行快速状态预测后端定期执行基于滑动窗口的优化关键帧在特殊事件触发时执行全局优化这种架构既保证了实时性又能维持全局一致性。我在某机器人定位项目中采用该方案将位置漂移控制在0.3%以内。7. 实现细节与常见陷阱7.1 线性化一致性问题无论是EKF还是高斯牛顿法都必须保证预测线性化点与状态估计点一致协方差传播与状态传播使用相同的线性化点常见错误是不同步骤使用了不同的线性化点这会导致估计偏差。解决方法在IEKF中严格同步线性化点在优化方法中使用first-estimate Jacobian7.2 数值稳定性处理两种方法都会面临矩阵求逆不稳定协方差矩阵失去正定性实用技巧使用QR分解代替直接求逆对协方差矩阵执行Joseph形式更新添加小的正则化项类似LM方法7.3 初始值敏感性非线性系统都存在局部最优问题。改善方法使用多假设跟踪(MHT)采用粗到精的初始化策略引入旋转平均等全局初始化方法8. 现代发展从滤波到因子图近年来因子图表示法逐渐成为主流它完美统一了滤波与优化视角滤波方法对应着逐步构建的因子图优化方法对应着批量求解因子图增量平滑算法(iSAM)则实现了二者的融合以GTSAM库为例其内部既支持传统的EKF更新也能执行全非线性优化而接口保持统一。这验证了我们讨论的统一性不仅理论成立工程上也可实现。