卡尔曼滤波算法

简介

本部分大部分内容参考Wiki百科，有兴趣可以直接查看。

对信号处理比较了解的人应该清楚，信号的传播伴随着大量的内部和外部的噪声干扰，使得输出信号具有随机性，为了降低这种干扰，需要从混杂的大量信号中提取出自己想要的，该过程就叫做滤波。

对于确定性信号，由于其具有确定的频谱，因此只要设计满足频率特性的过滤器（filter）如LPF来抑制干扰信号的通过就行，这类滤波方法称为常规滤波。

而对于随机信号，其具有确定的功率谱，因此可以根据有用信号和干扰信号的功率谱来设计对于的过滤器，比较著名的是Winer滤波，它基于功率谱的分解而来，基本原理类似常规滤波，但是其基于频域且计算量较大，使用较少。

后来，Kalman提出一种思想：通过那些与有用信号相关的观测量估计出有用信号的当前状态。这就是卡尔曼滤波算法。Kalman滤波引入了状态空间这一概念，使用状态方程来描述输入输出，估计过程中使用状态方程、观测方程以及系统噪声和观测噪声的激励来构成完整的滤波算法。而且，Kalman滤波是基于时域的，相比之前的频域滤波有了较大突破，不仅仅可以估计平稳信号，而且可以估计非平稳信号以及多维信号。

阿波罗计划的轨道估计就曾使用过Kalman滤波算法。

估计问题

熟悉Kalman滤波算法的应该知道，该算法对于物理上很多运动具有较好的估计效果，该算法本质上是一种实时递推滤波算法（自回归滤波器），以随机信号为处理目标，输入为系统观测量，输出为系统状态的估计值。输入与输出之间通过时间更新和观测更新联系在一起（估计值由状态方程和估计方程估计得来），因此，Kalman滤波和常规滤波区别很大，本质上是一种最优估计算法。

将通过观测数据进行处理得到所需参数的估计值这一过程所针对的问题定义为估计问题，通常分为参数估计与状态估计两类，前者指系统的结构参数部分或全部位置，后者指已知系统状态时由于部分闲置导致系统部分或全部状态未知。典型的估计问题包含三个部分：先验知识、约束条件和估计准则函数，其中准则函数用于评估估计的效果。

实际问题中，当然希望估计值越接近实际值越好，即最优估计。但实际情况往往很难做到，这主要是由于估计准则函数的选取不唯一，因而对应的最优估计不唯一。只有选择合适的准则函数，才能得到需要的最优估计，而准则函数的选取往往是依据先验知识设定，使其最小化或者最大化。常用的估计准则函数有直接误差准则、直接概率准则等，估计方法与估计准则联系密切，有最小二乘估计、最小平方误差估计等。

卡尔曼滤波

Kalman滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。Kalman滤波器与大多数滤波器不同之处，在于它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

Kalman滤波的递归过程中，主要有两个更新过程：时间更新和观测更新，时间更新包括状态预测和协方差预测，主要是对系统的预测；观测更新主要包括计算卡尔曼增益、状态更新和协方差更新。上述就是很多文献中提到的5个计算（5大公式）。

状态预测

$X(k | k-1)=\Phi X(k-1 | k-1)+B U(k)$

其中， $X(k | k-1)$ 表示k时刻的系统状态， $U(k)$ 是k时刻对系统的控制量（无控制量，认为是0即可）， $\Phi$ 是状态转移矩阵， $B$ 是系统参数。该步骤表示利用上一状态 $X(k-1 | k-1)$ 更新当前状态 $X(k | k-1)$ 。

协方差预测

$P(k | k-1)=\Phi P(k-1 | k-1) \Phi^{T}+Q$

其中， $P(k-1|k-1)$ 是上一状态 $X(k-1|k-1)$ 对应的协方差矩阵， $P(k|k-1)$ 是当前状态 $X(k|k-1)$ 对应的协方差矩阵， $Q$ 是系统噪声（假设为高斯噪声）的协方差矩阵。该步骤表示利用上一状态更新当前状态的协方差矩阵。

计算卡尔曼增益

$K=\frac{P(k | k-1) H^{T}}{H P(k | k-1) H^{T}+R}$

其中， $K$ 为卡尔曼增益， $H$ 为观测矩阵， $R$ 为测量噪声（假设为高斯噪声）对应的协方差矩阵。该步骤用于后续的两个更新步骤，因为前面两步的预测得到了当前状态的预测结果，在得到当前状态的观测结果时，通过预测值和观测值以及增益就可以计算当前状态的最优估计，即状态更新和协方差更新。

状态更新

$X(k | k)=X(k | k-1)+K(Z(k)-H X(k | k-1))$

其中， $Z(k)$ 是当前状态的观测值。该步骤表示利用预测值和增益以及观测值计算当前时刻下的最优状态估计值 $X(k | k)$ 。

协方差更新

$P(k | k)=(I-K H) P(k | k-1)$

其中， $I$ 为单位矩阵。该步骤表示，在更新了状态的基础上为了保证递推的进行也相应更新协方差矩阵。

上述的5个步骤就是卡尔曼滤波算法的全部步骤了，当然，对于这个递归过程需要给定初始化参数，能否收敛和初始参数的选择关系紧密，不同的任务有很多建议的初始化参数，可以自行查找。

代码实现

Kalman滤波在计算机视觉中常用来进行目标的运动预测，即位置状态预测。下面以Wiki上小车运动为例，代码实现如何对匀速运动的小车进行位置和速度的预测，具体推导见Wiki描述。

"""
Author: Zhou Chen
Date: 2020/3/27
Desc: 使用卡尔曼滤波进行状态预测
"""

import numpy as np
import matplotlib.pyplot as plt
plt.style.use('fivethirtyeight')

# 创建位置观测矩阵，表示0-99的位移
z_observed = np.arange(100)
# 创建高斯噪声矩阵
z_noise = np.random.normal(0, 1, 100)
z = z_observed + z_noise
# 创建初始状态矩阵X
x = np.array([[0, ], [0, ]])
# 创建初始状态协方差矩阵P，这里的1也可以是一个较为合适的较大数字
p = np.array([[1, 0], [0, 1]])
# 创建状态转移矩阵，采样频率为1秒，所以△t为1
f = np.array([[1, 1], [0, 1]])
# 创建状态转移协方差矩阵，协方差较小
q = np.array([[1e-6, 0], [0, 1e-6]])
# 创建观测矩阵
h = np.mat([1, 0])
# 创建观测噪声的协方差矩阵
r = np.mat([1])

for i in range(100):
    # 迭代，计算5大步骤
    x_predict = f * x
    p_predict = f * p * f.T + q
    kalman = (p_predict * h.T) / (h * p_predict * h.T + r)
    x = x_predict + kalman * (z[i] - h * x_predict)
    p = (np.eye(2) - kalman * h) * p_predict
    plt.plot(x[0, 0], x[1, 0], 'bo')

plt.xlabel("location")
plt.ylabel("speed")
plt.savefig("rst.png")
plt.show()