滤波算法与SLAM：Kalman Filter（卡尔曼滤波）算法与示例

滤波算法与SLAM

第二章：Kalman Filter算法

---

---

前言

本章简单介绍Kalman Filter的原理以及运用，附带Python示例
具体公式推导不会介绍，具体可参考《视觉SLAM十四讲》以及《概率机器人》相关篇幅
本章节内容多参考《概率机器人》中的内容

---

一、Bayes公式与Kalman Filter

要学习Kalman Filter，最好是先理解Bayes公式，即：（$\propto$代表“正比于”）
$$后验\propto 先验\ast 似然$$考虑到本章节内容始终与SLAM相关，那么公式定义也从SLAM问题中出发
定义状态变量为$X$，观测为$Z$，本科的大学物理知识告诉我们，$X$的真值永远不可能得到，要通过观测来推断$X$最有可能的情况
通过条件概率，这个概率可以定义为$P(X|Z)$，即已知$Z$的情况下$X$发生的概率
这叫做后验概率，而Bayes公式的目的就是为了找到最大后验概率
即：

$Z$已经发生的情况下，最可能的$X$是什么

为了求最大后验概率，需要通过先验概率$P(X)$和似然概率$P(Z|X)$进行计算
先验概率不必多解释，依赖于事件的本身，解释一下似然概率
通过条件概率的定义能够知道，$P(Z|X)$是指的在$X$已经发生的情况下，$Z$发生的概率
结合本文中的定义，也就成为了：当现实状态为$X$的情况下，观测为$Z$的概率
那么很明显，观测数据由传感器给定，似然概率也就代表着传感器观测的置信度
比如测量一个物体的长度，用尺子量，同一个物体量多少次，数值都不会差太多
但若是用手机APP去测量，很可能上一次是3cm，下一次就是6cm，明显手机APP这一传感器就不如尺子的可信度高
注意：似然指的是可信度，不是误差！尺子就算量不准，也是“稳定”的不准，方差是很小的
所以似然的理解可以认为是：

真值为$X$的情况下，输出的观测$Z$究竟有多可信

Bayes公式理解后，基本上所有的滤波算法都是以此为核心，区别就是在于如何求概率

二、Kalman Filter五大公式以及简单推导

经典的Kalman Filter算法分为两个步骤，预测（predict）和更新（update），基本等同于求先验和求似然，并且针对线性系统
那么构建状态方程和观测方程如下：$$X_k=A\cdot X_{k-1}+B{u}_k+{\omega }_k$$$$Z_k=C\cdot X_k+{\nu }_k$$其中$X_k$代表状态变量，$Z_k$代表观测变量，${\omega }_k\sim N(0,Q)$和${\nu }_k\sim N(0,R)$分别代表状态转移的噪声和观测噪声
推导之前先明确几个符号：

$\bar{x}$代表最优估计状态
$\hat{x}$代表预测状态
$\bar{P}$代表最优估计状态对应的协方差矩阵
$\hat{P}$代表预测状态对应的协方差矩阵

假设现在是第k次的滤波，已获得第$k-1$次的最优估计为${\bar{x}}_{k-1}$，对应的协方差矩阵为${\bar{P}}_{k-1}$，此时系统输入为$u_k$
那通过状态方程能够预测第$k$次的状态是$${\hat{x}}_k=A{\bar{x}}_{k-1}+B{u}_k$$同样的，预测的不确定性通过协方差矩阵的改变而传递下去（附录中的方差计算公式可以解释这一步）$${\hat{P}}_k=A{\bar{P}}_{k-1}{A}^T+Q$$到此预测步骤结束，传感器观测到了$z_k$，那么使用该观测对${\hat{x}}_k$进行修正，定义卡尔曼增益为$$K=\frac{{\hat{P}}_k{C}^T}{C{\hat{P}}_k{C}^T+R}$$通过该系数，对状态的更新为$${\bar{x}}_k={\hat{x}}_k+K(z_k-C{\hat{x}}_k)$$对应地，也对协方差矩阵进行更新$${\bar{P}}_k=(I-KC){\hat{P}}_k$$这五个公式就是Kalman Filter的核心公式了
作为工科生，对这个公式的具体推导并不需要太在意，只需要有个大致的感受，并且记下来就行

协方差矩阵在状态变量是一维数组时，其实就是常识中的方差，代表数据的分布可能性，方差越小则数据分布越集中
Kalman Filter滤除的是什么呢，就是状态方程和观测方程中的噪声
实际上Filter起作用的方式就是保持状态变量的协方差尽可能的小

三、具体示例

不去理解公式的推导，但是要理解公式怎么应用，所以本文介绍一个具体的例子
现在我们让一个机器人在二维平面上运动了一段距离，并且能够获得机器人对这一轨迹的观测
这个轨迹肯定是有误差在的，所以需要用Kalman Filter去剥离出更准确的轨迹

1）状态变量、观测变量、状态方程、观测方程的定义

要滤除的是在二维平面的位置信息噪声，假设机器人沿着直线匀速运动那么设状态变量和观测变量为$$X=[x,y,v_x,v_y{]}^T$$ $$Z=[x,y]$$
那么显而易见的，状态方程为$$X_{k+1}=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]X_k+B\cdot U_k$$观测方程为$$Z_{k+1}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]X_{k+1}$$

2）对应公式

根据方程获得$$A=\left[\begin{array}{cccc}1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],B=I,C=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]$$取$\Delta t=0.2$，噪声的协方差矩阵也需要自己给定，本文指定为$$Q=0.01I_{4\times 4},R=0.1I_{2\times 2}$$对应地套公式即可

3）相关代码

使用python构建卡尔曼滤波，需要使用到numpy和matplotlib函数库
假设机器人的期望轨迹为（0，3）到（20，3）的一条直线，速度为1m/s，时间间隔为0.2s
那么生成期望轨迹以及带有噪声的观测轨迹

import numpy as np
import matplotlib.pyplot as plt

def generate_trajectory(n_points=20/0.2):
	# 生成期望轨迹
    x = np.linspace(0, 20, int(n_points))
    y = np.linspace(3, 3, int(n_points))
    return x, y

def add_noise(x, y, noise_variance=0.01):
    # 为期望轨迹增加噪声，作为观测值
    noise_x = np.random.normal(0, np.sqrt(noise_variance), x.shape)
    noise_y = np.random.normal(0, np.sqrt(noise_variance), y.shape)
    return x + noise_x, y + noise_y

接着就可以构建Kalman Filter算法

def main():
	# 初始化各个矩阵
    delta_t = 0.2
    A = np.float64([
        [1.0, 0.0, delta_t, 0.0],
        [0.0, 1.0, 0.0, delta_t],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0]
    ])
    B = np.eye(4)
    C = np.float64([
        [1.0, 0.0, 0.0, 0.0],
        [0.0, 1.0, 0.0, 0.0]
    ])

    Q = 0.001*np.eye(4)
    R = 0.01*np.eye(2)
    # 初始化第一帧信息
    X = np.float64([
        [0],[3],[1],[0]
    ])
    
    I = np.eye(4)
    P = I
	
	# 生成轨迹等
    x_true, y_true = generate_trajectory()
    x_obs, y_obs = add_noise(x_true, y_true)
    z = np.column_stack((x_obs, y_obs))
    loop_count = len(z)
    x_opt = []
    y_opt = []
    x_opt.append(0)
    y_opt.append(3)

	# 核心部分
    for i in range(loop_count-1):
        Z = np.float64([[z[i+1][0]],[z[i+1][1]]])
        # 预测部分两个公式，不含B矩阵是因为匀速运动加速度为0，则U为0，可以忽略
        X = A @ X 
        P = A @ P @ np.linalg.inv(A) + Q
	
		# 更新部分三个公式，先计算Kalman增益，再更新状态和协方差矩阵
        K_down = C @ P @ C.T + R
        K = P @ C.T @ np.linalg.inv(K_down)
        X = X + K @ (Z - C @ X)
        P = (I - K @ C) @ P

		# 记录结果
        x_opt.append(X[0])
        y_opt.append(X[1])	

	# 画图观察
    plt.figure(figsize=(10, 6))
    plt.plot(x_true, y_true, label="Expected Trajectory", color="blue")
    plt.plot(x_obs, y_obs, label="Noisy Observations", color="red")
    plt.plot(x_opt, y_opt, label="Filter Result", color="black")
    plt.legend()
    plt.show()

if __name__ == "__main__":
	# 运行程序
    main()

从代码不难看出，Kalman Filter实际上就是确定的几个矩阵进行翻来覆去的计算，所以相比最小二乘优化中多次求导迭代而言，Kalman Filter始终能够在计算速度上面占据优势
而调整$Q,R$的大小，也可以使得输出结果不同，误差越大则置信度越小
比如：$Q=0.001I,R=0.01I$的结果如下：

但是如果设$R=0.5I$，则结果又如下：

当$R$变大时，代表观测值的误差增大，置信度降低，所以滤波结果更加靠近预测结果，也就是期望路径
当$R$变小时，观测值的置信度提高，那么滤波就更愿意相信观测值
所谓的调参，也就是调整这个噪声协方差，从而影响Kalman增益
但是注意，这个噪声并不是随便调整的，是跟传感器本身的置信度有关，只是在大部分赶鸭子上架的情况下不得已才根据实用情况去调整

所以Kalman Filter的使用观感上来看，就是一个基于概率的加权平均，对于预测和更新，哪个置信度更高就更偏向哪个。
另外，请注意这里蓝色的线是期望轨迹，并不是真实轨迹，到底机器人是怎么走的，我们也不能知道确切的轨迹
所以并不是说黑线越靠近蓝色就效果越好，具体希望黑线怎么走要根据具体问题去分析

---

附录 – Kalman公式推导

以下推导均来自于《概率机器人》，并不需要掌握，这里仅作学习记录和探讨
（后面再补吧，要事在身）