扩展卡尔曼滤波（EKF）的泰勒展开（雅可比矩阵）详解

1. 雅可比矩阵介绍

概念 雅可比矩阵（Jacobian Matrix）是向量值函数的一阶偏导数构成的矩阵。对于一个从 R^n 映射到 R^m 的向量函数 F(x) ，其雅可比矩阵定义为：

在这里插入图片描述

核心原理 雅可比矩阵的本质是提供多变量函数在某一点的最佳线性近似，对于函数 F(x) ，在点 a 附近的线性近似为： F(x)≈F(a)+J(a)(x−a)

作用与效果 雅可比矩阵描述了输入空间的小变化如何映射到输出空间，表示了在局部区域内，函数如何"拉伸"、"旋转"和"扭曲"空间，雅可比行列式（矩阵的行列式）表示了局部体积的变化率和方向，即雅可比矩阵的每个元素∂Fi∂xj∂xj∂Fi 表示当第 j 个输入变量发生微小变化时，第 i 个输出变量的变化率，在以下场景中至关重要：

• 评估系统对参数变化的敏感度
• 分析系统的稳定性
• 优化问题中确定搜索方向

2. EKF-SLAM回顾

在EKF-SLAM中，需要对非线性的运动模型和观测模型进行一阶泰勒展开，以实现线性化处理。泰勒展开的核心是计算雅可比矩阵，它提供了函数在某一点的局部线性近似。

状态向量：

算法流程

1. 预测步骤：
   • 预测状态
   • 计算雅可比矩阵F_t（对x_{t-1} 求导）
   • 预测协方差
2. 更新步骤（对于每个观测到的路标 i ）：
   • 计算观测预测
   • 计算雅可比矩阵 H_t （对 x_t 求导）
   • 计算卡尔曼增益
   • 更新状态
   • 更新协方差

3.运动模型及其雅可比矩阵

3.1 运动模型

运动模型描述了机器人如何根据控制输入 u_t，从 x_t−1 移动到 x_t：

3.2 雅可比矩阵 运动模型的雅可比矩阵 F_t 是对前一时刻的状态 x_t−1 求偏导，在估计值 x^{t−1∣t−1}处计算：

3.3 说明 运动模型雅可比矩阵的特点：

• 只有机器人位姿部分有非零元素
• 地图特征部分为单位矩阵，表示运动不影响地图特征

在预测步骤中，需要计算状态预测的协方差，这里 F_t 表示前一时刻状态的微小变化如何影响当前时刻的状态预测，因此需要对 x_t−1 求导，用于状态预测和协方差预测。

4. 观测模型及其雅可比矩阵

4.1 观测模型 观测模型描述了机器人如何观测环境中的路标：

4.2 雅可比矩阵 观测模型的雅可比矩阵 H_t 是对当前时刻的状态 x_t求偏导，在预测值 x^{t∣t−1}处计算：

4.3 解释说明

观测模型雅可比矩阵的特点：

• 只对机器人位姿和被观测路标有非零导数
• 对其他路标的导数为零（条件独立性）
• 这体现了SLAM问题中的稀疏结构
• 当观测到新路标时，需要扩展状态向量并初始化新路标

在更新步骤中，需要计算卡尔曼增益，这里 H_t 表示当前状态的微小变化如何影响观测预测，因此需要对 x_t 求导，用于计算卡尔曼增益和状态更新。