Kalman Filter

贝叶斯滤波框架下的卡尔曼滤波

卡尔曼滤波可以在贝叶斯滤波的框架下进行解释, 在讲解卡尔曼滤波之前, 我们先说说贝叶斯滤波.

主要参考：https://blog.csdn.net/varyshare/article/details/97642209

https://blog.csdn.net/varyshare/article/details/97642209

https://www.cnblogs.com/ycwang16/p/5999034.html

https://blog.csdn.net/weixin_42647783/article/details/88080646

贝叶斯滤波

贝叶斯滤波的作用

贝叶斯滤波的作用，可以用一句话来解释：用已有的信息来计算概率。我们举个例子：

假如有这样一个场景，根据你输入的信息，对一个人的性别进行判别，那么贝叶斯的做法如下：

1. 当你什么信息都没有的时候，贝叶斯滤波会告诉你：$P(女)=0.5$。注意，$P(女)=0.5$表示：在已有的信息下，我可以推断出那个人的性别是女性的概率是0.5。这其实是一个初始值，因为事实上，没有任何已有信息。

2. 当你告诉贝叶斯滤波算法：这个人有长头发，那么贝叶斯滤波会告诉你：在知道一个人有长头发的情况下，这个人性别是女性的概率是$P(女|长头发)=0.87$。至于如何推导，后面会有解释。

3. 当你再告诉算法：这个人使用口红，那么贝叶斯滤波会告诉你：在知道一个人有长头发，而且有口红的情况下，这个人性别是女的概率是$P(女|长头发,口红)=0.91$。

可以看到，贝叶斯滤波的思想其实是根据不断接收到的新信息和一些已经知道的统计值，来不断的更新概率。

那么贝叶斯滤波起到什么作用？其实就一句话，估算对象的状态。

那么贝叶斯滤波到底是怎么推导这些概率的呢？请接着看。

贝叶斯观测模型的推导

我们知道贝叶斯公式如下：
$$P(x|y)=\frac{P(y|x)P(x)}{P(y)}$$
在估算系统状态时，一般$x$是状态，$y$则是观测。

如果具有多个观测，那么可以得到多观测下的贝叶斯公式：
先来看两个观测的情况下：
$$P(x|y,z)=\frac{P(x,y,z)}{P(y,z)}=\frac{P(y|x,z)P(x,z)}{P(y,z)}=\frac{P(y|x,z)P(x|z)P(z)}{P(y|z)P(z)}=\frac{P(y|x,z)P(x|z)}{P(y|z)}$$

推广到多个观测，设 z 1 : n = { z 1 , . . . , z n } z_{1:n} = \{z_1, ..., z_n\} z1:n​={z1​,...,zn​}：
$$P(x|z_{1:n})=\frac{P(z_n|x,z_{1:n-1})P(x|z_{1:n-1})}{P(z_n|z_{1:n-1})}$$

假设$z_n$和$z_{1:n-1}$无关，也就是所有观测是独立的。所以有：
$$P(x|z_{1:n})=\frac{P(z_n|x)P(x|z_{1:n-1})}{P(z_n|z_{1:n-1})}$$
由于分母$P(z_n|z_{1:n-1})$与x无关，可以将其看成是一个常量${\eta }_n$。

根据上式，我们可以看到有一项$P(x|z_{1:n-1})$，这一项的推导过程和$P(x|z_{1:n})$是一样的。那么就有贝叶斯递推公式如下：
$$P(x|z_{1:n})={\Pi }_{i=1}^n{\eta }_i{\Pi }_{i=1}^{n}P(z_i|x)P(x)$$

其中$P(z_i|x)$被称为观测模型，我理解的$P(x|z_{1:n})$也可以称为观测模型，这两者可以转换。

上述推导过程都是在理想状态下的，也就是只有观测对物体的状态产生影响。然而在实际问题中，并不仅仅是观测对状态产生影响，也会有一些其他的影响，比如简单的环境变化干扰（风速，摩擦力，零件损耗）。我们将这些环境变化称之为动作，显然，动作带来的影响总是不确定的。相比于观测，动作一般会使得对象的状态更加的不确定。

我们用u来描述动作，在$x^{\prime }$状态下，引入了动作$u$，对象状态变为$x$的概率表述如下：
$$P(x|x^{\prime },u)$$

动作对状态的影响一般由状态转移模型来描述，也就是上式。可以看到这个公式和观测模型的不一样之处，这个公式是序列的，即前一时刻的状态融入一个动作之后，发生当前状态的概率。举个例子：关门这个动作有0.1的失败概率，所以当门是open状态时，执行“关门”动作，门有0.9的概率转为closed状态，有0.1的概率保持在open状态。门是closed的状态下，执行“关门”动作，门仍然是关着的。这就是动作带来的不确定性影响。

如何结合状态转移模型和观测模型来对物体的状态进行估计呢？首先我们明确一下这个估计算法的输入：

Input:

	1. 1到t时刻的状态观测序列和动作序列，$d_t = \{u_{1:t}, z_{1:t}\}$；
	2. 观测模型: $P(z|x)$；
	3. 动作的状态转移模型：$P(x|u,x')$；
	4. 系统状态的先验概率分布$P(x)$

Output

	1. 计算状态的后验概率，称为状态的置信概率：$P(x_t|u_{1:t}, z_{1:t})$

说白了，就是给定1到t时刻的观测和动作，估算t时刻物体的状态。

贝叶斯滤波的基本假设

贝叶斯滤波具有以下几个基本假设：

1. Markov性假设：t时刻的状态只受到t-1时刻的状态和t时刻的动作所影响，t时刻的观测只受到t时刻的状态所影响。
2. 静态环境，即对象周边的环境假设是不变的；
3. 观测噪声，模型噪声是相互独立的。

强调一下，在上面第1条，所谓t时刻的状态只受到t-1时刻的状态和t时刻的动作所影响。这里的状态是一种状态转移描述，如果我们需要估算出物体当前的真实状态，则需要融合状态转移模型和观测模型这两个来进行。

贝叶斯滤波算法推导

有了上面的信息，我们可以给出t时刻估算状态的后验概率，这里我直接截图了。

其实上式可以分为两步，分别是状态预测和状态更新。

状态转移模型负责状态预测。也就是：
$$\overline{bel}(x_t)=\int P(x_t|u_t,x_{t-1})bel(x_{t-1})d{x}_{t-1}$$
基于t时刻的观测对当前状态进行更新，也就是：
$$bel(x_t)=\eta P(z_t|x_t)\overline{bel}(x_t)$$
如果用一句话来总结贝叶斯滤波方法，我个人的理解是这样的：通过状态的观测模型和状态转移模型两部分结合来估算物体的真实状态。

卡尔曼滤波

上一节讲了贝叶斯滤波的基本原理，我们知道了，贝叶斯滤波主要是有两部分的输入，分别是观测模型和状态转移模型。一个比较麻烦的情况是：如果$bel(x_t)$是任意分布，我们需要在$x_t$的所有可能取值点上，计算该取值的概率，这在计算上是难于实现的。这个计算问题可以有多种方法来近似。

卡尔曼滤波是贝叶斯滤波的一种情况，它假设的是$bel(x_t)$服从高斯分布。该假设有什么好处呢？高斯分布无需在$x_t$的每个取值点上进行概率计算，因为均值和方差可以完全刻画分布。这样，就可以通过均值和方差对$bel(x_t)$完全描述，然后可以推导出这两个数值的递推公式。从而在每个时刻由这两个数值的递推公式完全获得状态估计。

正态分布具有以下特点：

1. 高斯随机变量线性变换后，仍服从高斯分布；
2. 两个高斯变量线性组合，仍服从高斯分布；
3. 两个相互独立的高斯变量的乘积，仍服从高斯分布。

正因为高斯分布有这些特点，所以，在Bayes滤波公式中的随机变量的加法、乘法，可以用解析的公式计算均值和方差，这使得Bayes滤波的整个计算过程非常简便，即Kalman滤波器的迭代过程。
那么卡尔曼滤波能干什么？我们举一个室内实际温度估算的例子：

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值），那么卡尔曼滤波就可以根据这两个值估算出实际温度。

模型假设

Kalman滤波所解决的问题，是对一个动态变化的系统的状态跟踪的问题，基本的模型假设包括：1）系统的状态方程是线性的；2）观测方程是线性的；3）过程噪声符合零均值高斯分布；4）观测噪声符合零均值高斯分布；从而，一直在线性变化的空间中操作高斯分布，状态的概率密度符合高斯分布。

模型描述

1. 状态方程
   $$x_t=A_t{x}_{t-1}+B_t{u}_t+{ϵ}_t$$

2. 观测方程
   $$z_t=H_t{x}_t+{\delta }_t$$
   其中过程噪声${ϵ}_t$假设服从零均值高斯分布，观测噪声${\delta }_t$假设服从零均值高斯分布。可以用如下参数描述整个问题：

3. $x_t$，n维向量，表示t时刻观测状态的均值。

4. $P_t$，$n\times n$方差矩阵，表示t时刻被观测的n个状态的方差。

5. $u_t$，$l$维向量，表示t时刻的输入（动作）。

6. $z_t$，m维向量，表示t时刻的观测。

7. $A_t$，$n\times n$矩阵，表示状态从t-1到t在没有输入动作影响时的转移方式。状态转移矩阵。

8. $B_t$，$n\times n$矩阵，表示$u_t$如何影响$x_t$。

9. $H_t$，$n\times n$矩阵，表示状态$x_t$如何被转换为观测$z_t$。

10. $R_t$，$n\times n$矩阵，表示过程噪声的方差矩阵。

11. $Q_t$，$n\times n$矩阵，表示过程噪声的方差矩阵。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qyg9wFPb-1640264009473)(./系统状态转移.png)]

上图给出了在没有观测，只有输入$u_t$时，状态变量的均值和方差从t-1到t的转移方式，可见均值和方差的计算，完全是基于高斯分布的线性变化的方法来的。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-VIuVDCiF-1640264009473)(/media/wyx/DATA1/公司/贝叶斯框架下的卡尔曼滤波/kalmanfilter.png)]

上图则给出了卡尔曼滤波所解决的问题，即在获得t时刻的输入和观测的情况下，如何更新$x_t$的均值和方差的问题。$u_t$和$z_t$不是每一个时刻都需要同时获得，就像贝叶斯滤波一样，可以在获得$u_t$的时候做状态预测，在获得$z_t$时做状态更新。

下面先给出卡尔曼滤波的算法流程，随后给出推导过程。

算法流程

$$\begin{array}{rl}kalman& \_filter(x_{t-1},P_{t-1},u_t,z_t):\\ & Prediction:\\ & {\bar{x}}_t=A_t{x}_{t-1}+B_t{u}_t\\ & {\bar{P}}_t=A_t{P}_{t-1}{A}_t^T+R_t\\ & Correction:\\ & K_t={\bar{P}}_t{H}_t^T(H_t{\bar{P}}_t{H}_t^T+Q_t{)}^{-1}\\ & x_t={\bar{x}}_t+K_t(z_t-H_t{\bar{x}}_t)\\ & P_t=(I-K_t{H}_t){\bar{P}}_t\end{array}$$

• 第一行基于转移矩阵和控制输入，预测𝑡t时刻的状态
• 第二行是预测方差矩阵
• 第三行计算Kalman增益，$K_t$
• 第四行基于观测的新息进行状态更新
• 第五行计算更新状态的方差矩阵。

卡尔曼滤波算法公式可以在贝叶斯滤波的框架下进行推导。$\overline{bel}(x_t)$是状态转移方程和观测方程所服从分布的卷积结果。如下图

更新过程则是两个分布的乘积

上式仍然是高斯分布，最后可得更新公式如下：

这个例子给出了卡尔曼滤波的一个应用。