SLAM优化问题

---

前言

一、状态估计问题

• 位姿表示: 把位姿 $x$ 看作是服从某种概率分布的随机变量。
• 运动方程: 受噪声影响，误差逐渐累积。
• 观测数据: 通过观测数据减小不确定性。
• 状态估计方式:
  • 批量式（非线性优化）
  • 增量式（滤波）

运动方程与观测方程，公式表示为：

$$x_k=f(x_{k-1},u_k,w_k)$$
$$z_k=h(y_k,x_k,v_k)$$
解释:

• $x_k$: 当前时刻的位姿。
• $f$: 运动方程，表示如何从前一状态 $x_{k-1}$ 和控制输入 $u_k$ 及过程噪声 $w_k$ 计算当前状态。
• $z_k$: 当前观测值。
• $y_k$: 路标y。
• $h$: 观测方程，表示如何从当前状态 $x_k$ 和观测噪声 $v_k$ 计算观测值。

二、最大后验估计

问题描述:直接求后验概率分布是困难的，转而求一个状态最优估计，使得该状态下后验概率最大化。

公式表示为：

$$x^{\ast }=\arg \underset{x,y}{\max }P(x,y|z,u)$$

该公式表示在给定观测数据 $z$ 和控制输入 $u$ 的情况下，求解最可能的状态 $x$ 和标志 $y$。
似然与先验公式表示为：

$$P(z|u,x,y)\propto P(x,y)P(z|u,x,y)$$
观测的似然性 $P(z|u,x,y)$ 与状态的先验分布 $P(x,y)$ 的乘积成比例。

三、最小二乘问题

• 假设高斯分布，某一次观测：

  • 对联合分布进行因式分解：

  $$P(z|u,x,y)=P(u)\cdot P(x|x_{k-1})\cdot P(z|x,y)$$

解释: 这里的公式表示在某次观测中，观测值 $z$ 的概率分布可以通过输入 $u$、状态 $x$ 和标志 $y$ 的联合分布进行分解。观测模型，公式表示为：

$$P(z|x,y)=N(h(y,x),Q)$$

• $P(z|x,y)$ 表示在给定状态 $x$ 和标志 $y$ 的情况下，观测 $z$ 的概率分布是高斯分布，其中均值是 $h(y,x)$，协方差矩阵是 $Q$。高斯分布，任意高斯分布可以表示为：
  $$x\sim N(\mu ,\Sigma )$$
  这里 $x$ 是一个高斯分布的随机变量，其均值为 $\mu$，协方差矩阵为 $\Sigma$。 目标函数，定义输入、观测数据与模型之间的误差：

$$e_k=x_k-f(x_{k-1},u_k)$$
$$e_z=z_k-h(y_k,x_k)$$
目标函数表示为：

$$J=\sum _i{e}_k^T{R}^{-1}{e}_k+\sum _j{e}_j^T{Q}^{-1}{e}_j$$

• $e_k$ 表示当前状态的误差，$e_z$ 表示观测的误差。目标函数 $J$ 是加权误差的和，旨在最小化这些误差。

四、卡尔曼滤波（KF）

• 假设系统为线性高斯系统。

• 令：

  $$x_k=A_k{x}_{k-1}+B_k{u}_k+w_k$$
  $$z_k=C_k{x}_k+v_k$$

• $A_k$, $B_k$, $C_k$ 分别是状态转移矩阵、控制输入矩阵和观测矩阵。

• $w_k$ 和 $v_k$ 分别是过程噪声和观测噪声，通常假设它们是高斯分布。
  预测（先验概率分布），公式表示为：
  $${\hat{x}}_k=A_k{\hat{x}}_{k-1}+B_k{u}_k$$
  $$P_k=A_k{P}_{k-1}{A}_k^T+R$$

• ${\hat{x}}_k$ 是对当前状态的预测，$P_k$ 是预测的协方差矩阵。

更新（后验概率分布），公式表示为：

$$K_k=P_k{C}_k^T(C_k{P}_k{C}_k^T+Q{)}^{-1}$$
$${\hat{x}}_k={\hat{x}}_k+K_k(z_k-C_k{\hat{x}}_k)$$
$K_k$ 是卡尔曼增益，用于计算更新后的状态估计。

五、扩展卡尔曼滤波 (EKF)

• 记:

  • 对于非线性系统，通过一阶泰勒展开：

  $$\begin{gathered} x_k=f(x_{k-1},u_k,w_k) \\ {z}_k=h(y_k,x_k,v_k) \end{gathered}$$

• $x_k$ 表示当前时刻的状态，$u_k$ 是控制输入，$w_k$ 是过程噪声。$z_k$ 是观测值，$v_k$ 是观测噪声。

• 更新（后验概率分布）:

  $${\hat{x}}_k\approx {\hat{x}}_{k-1}+\frac{\partial f}{\partial x}{|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+w_k$$

  $$z_k\approx h({\hat{x}}_k)+v_k$$

• EKF 通过对非线性系统进行线性化处理，使得在离工作点较远时，可能导致线性化程度不够的问题。

六、迭代扩展卡尔曼滤波 (EKF)

• 在 $k$ 时刻推导到 $k+1$ 的过程中进行多次迭代，减小非线性的影响（误差）。公式表示为：

$$\begin{gathered} x_k=f(x_{k-1},u_k,w_k) \\ {z}_k=h(y_k,x_k,v_k) \end{gathered}$$

• iEKF 通过多次迭代更新来减小非线性带来的误差。

• 与 EKF 相同的线性化:

  • 步骤 1: 进行初步估计。
  • 步骤 2: 计算处的雅可比矩阵和增益矩阵，重复至满足收敛条件。
  • 步骤 3: 更新状态 $x$ 和协方差 $P$。

更新公式表示为：

$${\hat{x}}_{k|1}={\hat{x}}_k+K(z_k-h({\hat{x}}_k))$$

$$P_{k|1}=(I-KH)P_k$$

• $K$ 是卡尔曼增益，$H$ 是雅可比矩阵，$I$ 是单位矩阵。

• 减小非线性影响:

  • 与高斯-牛顿法等价，通过迭代优化来提高估计精度。

总结

扩展卡尔曼滤波 (EKF) 和迭代扩展卡尔曼滤波 (iEKF) 是处理非线性系统的重要工具。EKF 通过一阶泰勒展开线性化系统，而 iEKF 通过多次迭代来进一步减小非线性影响。这些方法在许多应用中，如SLAM（同步定位与地图构建）和机器人导航等领域，具有重要的理论和实践价值。

七、简明ESKF推导

此处参考链接：知乎
更新中…