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

符号意义

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

一、卡尔曼滤波（KF）

1.1 状态观测器（State Observers）

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

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

• 实际系统

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

• 数学模型

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

• 误差

$$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 公式推导

$$\{\begin{array}{c}{x}_k=A_k{x}_{k-1}+u_k+w_k(运动方程)\\ z_k=C_k{x}_k+v_k(状态方程)\end{array}$$

$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},z1:k-1)=N(A_k{\hat{x}}_{k-1}+u_k,A_k{\hat{P}}_{k-1}{A}_k^T+R)$$
这一步称为预测，即根据输入，从上一个时刻状态推断当前状态
$${\bar{x}}_k=A_k{\hat{x}}_{k-1}+u_k,{\bar{P}}_k=A_k{\hat{P}}_{k-1}{A}_k^T+R$$
${\bar{x}}_k$和${\bar{P}}_k$为状态和协方差的先验分布，是根据公式直接通过上一个状态计算获得，用于卡尔曼增益和后验${\hat{x}}_k$，${\hat{P}}_k$的计算。
$$P(z_k|x_k)=N(C_k{x}_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)$$
求解上式可以获得
$$\begin{gathered} {\hat{x}}_k={\bar{x}}_k+K(z_k-C_k{\bar{x}}_k) \\ {\hat{P}}_k=(I-K{C}_k){\bar{P}}_k \end{gathered}$$
其中
$$\begin{gathered} K={\hat{P}}_k{C}_k^T{Q}^{-1} \\ I=K{C}_k+{\hat{P}}_k{\bar{P}}_k^{-1} \end{gathered}$$
此处$K$实际上可以不通过${\hat{P}}_k$计算获得。

总结

二、扩展卡尔曼滤波（EKF）

卡尔曼滤波针对的是线性系统，当需要拓展到非线性系统时，通常使用扩展卡尔曼滤波器（Extended Kalman Filter，EKF），通过对运动方程以及观测方程在某个点附近进行一阶泰勒展开并保留一阶项，进行线性化操作，然后按照线性系统进行推导。

$$\{\begin{array}{c}{x}_k=f(x_{k-1},u_k)+w_k(运动方程)\\ z_k=h(x_k)+v_k(状态方程)\end{array}$$

雅可比矩阵为

线性化系统
$$\begin{gathered} △x_k\approx F△x_{k-1}+w_k \\ △z_k\approx H△x_k+v_k \end{gathered}$$
总结

1. 预测

$${\bar{x}}_k=f({\hat{x}}_{k-1},u_k),{\bar{P}}_k=F{\hat{P}}_k{F}^T+R_k$$

2. 更新

   卡尔曼增益$K_k$
   $$K_k={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+Q_k{)}^{-1}$$
   后验概率
   $${\hat{x}}_k={\bar{x}}_k+K_k(z_k-h({\bar{x}}_k)),{\hat{P}}_k=(I-K_{k}H){\bar{P}}_k$$

三、卡尔曼滤波讨论

不同卡尔曼滤波算法对比

EKF的局限性

1. 由于复杂的导数，可能导致难以计算雅可比矩阵
2. 通过数值法计算，会导致计算成本的大量增加
3. EKF不适用于具有不连续模型的系统，因为系统不可微分时，雅可比矩阵不存在
4. 高度非线性系统的线性化效果不好

参考

[Matlab官方教程]卡尔曼滤波器（Kalman Filters）（很好的视频教程）

怎样用非数学语言讲解贝叶斯定理（Bayes theorem）？

初学者的卡尔曼滤波——扩展卡尔曼滤波（一）

图说卡尔曼滤波，一份通俗易懂的教程

卡尔曼滤波器（Kalman Filters）

Understanding Kalman Filters

《视觉SLAM十四讲》