SLAM学习——卡尔曼滤波（KF/EKF）

符号意义

$\hat{x}$ ：状态x的估计状态，后验
$\bar{x}$ ：估计状态 $\hat{x}_k$ 的预估值，先验
$P$ ：协方差

一、卡尔曼滤波（KF）

1.1 状态观测器（State Observers）

在这里插入图片描述
通过反馈，计算实际观测值 $y$ 与估计观测值 $\hat{y}$ 的误差，调整增益K使误差减少，让数学模型更加接近实际系统。

当数学模型与实际系统接近时，通过数学模型计算获得估计状态值 $\hat{x}$ 将会与实际状态值接近 $x$ 。

实际系统

$\dot{x} = Ax+Bu\\ y = Cx$

数学模型

$\dot{\hat{x}} = A\hat{x}+Bu + K(y- \hat{y})\\ \hat{y} = C \hat{x}$

误差

$e_{obs}= x -\hat{x}$

使数学模型和实际系统相减
在这里插入图片描述
根据 $\dot{e}_{obs} = (A-KC)e_{obs}$ 求 $e_{obs}$ 积分为
$e_{obs}(t) = e^{(A-KC)t}e_{obs}(0)$

在这里插入图片描述

在上式中，若 $A-KC<0$ ， $e_{obs}$ 会随着 $t$ 的增加而减少，最终趋向于0，即 $\hat{x}$ 接近 $x$ 。

在这个过程中， $KC$ 非必需， $A$ 也可以使 $e_{obs}$ 随时间衰减，但因模型中不确定性的存在， $A$ 无法确定，因此只能通过 $K$ 控制衰减速度，使 $\hat{x}$ 更快地收敛到 $x$ ，确定 $K$ 的最佳方法就是使用卡尔曼滤波器。

1.2 公式推导

$\left\{\begin{matrix} x_k = A_k x_{k-1}+u_k+w_ k\ (运动方程)\\ z_k = C_kx_k+v_k \ (状态方程) \end{matrix}\right.$

$w_k$ 和 $v_k$ 为噪声，此处假设符合零均值的高斯分布
$w_k \sim N(0,R),v_k \sim N(0,Q)$

$P(x)$ 为事件x发生的概率， $P(y$ )为事件y发生的概率， $P(x)$ 和 $P(y)$ 称为先验概率(prior probability)， $P(x|y)$ 为事件y发生的条件下，事件x发生的概率，称为x的后验概率(posterior probability)

贝叶斯定理是关于随机事件A和B的条件概率的一则定理。
$P(A|B) = P(A) \frac{P(B|A)}{P(B)}$
贝叶斯定理简单描述为：
$新信息出现后的A概率 = A概率 \times 新信息带来的调整$

通过运动方程确定 $x_k$ 的先验概率
$P(x_k|x_0,u_{1:k},z{1:k-1}) = N(A_k \hat{x}_{k-1}+u_k,A_k \hat{P}_{k-1}A^T_k+R)$
这一步称为预测，即根据输入，从上一个时刻状态推断当前状态
$\bar{x}_k = A_k \hat{x}_{k-1}+u_k, \bar{P}_k=A_k \hat{P}_{k-1}A^T_k+R$
$\bar{x}_k$ 和 $\bar{P}_k$ 为状态和协方差的先验分布，是根据公式直接通过上一个状态计算获得，用于卡尔曼增益和后验 $\hat{x}_k$ ， $\hat{P}_k$ 的计算。
$P(z_k|x_k) = N(C_kx_k,Q)$
$P(z_k|x_k)$ 为观测数据 $z_k$ 的后验概率，描述某个状态下 $x_k$ 会产生怎样的观测数据。

设状态分布为 $x_k \sim N(\hat{x}_k,\hat{P}_k)$ ，通过两组概率的乘法获得
$N(\hat{x}_k,\hat{P}_k) = N(C_k x_k,Q) N(\bar{x}_k,\bar{P}_k)$
求解上式可以获得
$\hat{x}_k = \bar{x}_k + K(z_k-C_k\bar{x}_k)\\ \hat{P}_k = (I-KC_k)\bar{P}_k$
其中
$K = \hat{P}_k C_k^TQ^{-1}\\ I = KC_k+ \hat{P}_k \bar{P}_k^{-1}$
此处 $K$ 实际上可以不通过 $\hat{P}_k$ 计算获得。