Kalman Filter (Bayesian Derivation)

Table of Contents

1 Kalman滤波器描述

Kalman滤波器是Bayes滤波的一个实现。先回顾一下Bayes滤波,如图 1 所示。

bf_bayes_filter.png

Figure 1: The general algorithm for Bayes filtering

Kalman滤波器基于下面三个假设:
(1) 运动方程是线性的,且存在高斯噪声。 即运动方程可以这样表示:
\[x_t = A_t x_{t-1} + B_t u_t + \varepsilon_t\]
其中, \(x_t\) 是状态向量(设它有 \(n\) 个分量), \(u_t\) 是控制量向量(设它有 \(m\) 个分量), \(A_t\) 是 \(n \times n\) 的矩阵, \(B_t\) 是 \(n \times m\) 的矩阵,随机变量 \(\varepsilon_t\) 表示运动噪声,它是高斯随机向量,它和状态向量维度相同,它服从均值为零,协方差矩阵为 \(Q_t\) 的 \(n\) 维高斯分布,即 \(\varepsilon_t \sim N(0, Q_t)\) 。所以:
\[p(x_t \mid u_t, x_{t-1}) \sim N(A_t x_{t-1} + B_t u_t, Q_t)\]
用公式表达上面这个 \(n\) 维高斯分布,就是:
\[p(x_t \mid u_t, x_{t-1}) = \det(2 \pi Q_t)^{-\frac{1}{2}} \exp \left\{ - \frac{1}{2} (x_t - A_t x_{t-1} - B_t u_t)^{\mathsf{T}} Q_t^{-1} (x_t - A_t x_{t-1} - B_t u_t) \right\}\]

(2) 测量方程是线性的,且存在高斯噪声。 即测量方程可以这样表示:
\[z_t = C_t x_t + \delta_t\]
其中, \(z_t\) 是测量向量(设它有 \(k\) 个分量), \(C_t\) 是 \(k \times n\) 的矩阵,随机变量 \(\delta_t\) 表示测量噪声,它和测量向量维度相同,它服从均值为零,协方差矩阵为 \(R_t\) 的 \(k\) 维高斯分布,即 \(\delta_t \sim N(0, R_t)\) 。所以:
\[p(z_t \mid x_t) \sim N(C_t x_t, R_t)\]
用公式表达上面这个 \(k\) 维高斯分布,就是:
\[p(z_t \mid x_t) = \det(2 \pi R_t)^{-\frac{1}{2}} \exp \left\{ - \frac{1}{2} (z_t - C_t x_t)^{\mathsf{T}} R_t^{-1} (z_t - C_t x_t) \right\}\]

(3) 初始分布 \(bel(x_0)\) 是高斯分布。

基于上面3个假设,Kalman滤波器实现了Bayes滤波器。
Kalman滤波器用均值向量 \(\mu_t\) 和协方差矩阵 \(\Sigma_t\) 来表示后验概率 \(bel(x_t)\) 。 Kalman滤波器算法描述如图 2 所示。

kalman_filter_algorithm.png

Figure 2: Kalman Filter Algorithm

说明:这里运动方程的噪声的协方差矩阵用 \(Q_t\) 表示,而测量方程的噪声的协方差矩阵用 \(R_t\) 表示。而Probabilistic Robotics, by Sebastian Thrun一书中运动方程的噪声的协方差矩阵用 \(R_t\) 表示,而测量方程的噪声的协方差矩阵用 \(Q_t\) 表示。

2 Kalman滤波器推导

完整地推导Kalman滤波器算法多少有些复杂。下面我们将仅考虑“状态向量和测量向量都是一维时的Kalman滤波器”,因为它的推导比较简单。
设一维线性系统如下所示:
\[\begin{aligned} x_t &= a_t x_{t-1} + b_t u_t + \varepsilon_t \\ z_t &= c_t x_t + \delta_t \\ \end{aligned}\]
其中 \(a_t,b_t,c_t\) 都是标量,且:
\[\varepsilon_t \sim N(0, \sigma_{Q,t}^2), \quad \delta_t \sim N(0, \sigma_{R,t}^2)\]

本节主要参考:Notes on Univariate Gaussian Distributions and One-Dimensional Kalman Filters (这里是它的一个 pdf backup)。

2.1 一维高斯分布概率知识

为了顺利推导一维Kalman滤波器,我们先介绍一维高斯分布的相关概率知识。

一维高斯分布的概率密度函数为:
\[p(x) = \frac{1}{\sqrt{2 \pi \sigma^2}} \exp \left\{ - \frac{1}{2} \frac{(x-\mu)^2}{\sigma^2} \right\}\]
如果随机变量 \(X\) 服从高斯分布,则记为 \(X \sim N(\mu, \sigma^2)\) 。
其两个参数计算如下:
\[\begin{aligned} \mu &= E_X(X) = \int_{- \infty}^{\infty} x p(x) \, \mathrm{d} x \\ \sigma^2 &= E_X[(X -\mu)^2] = \int_{- \infty}^{\infty} (x-\mu)^2 p(x) \, \mathrm{d} x \\ \end{aligned}\]

2.1.1 两个一维高斯分布的乘积

假设有两个随机变量 \(X_1,X_2\) 分别服从高斯分布。即:
\[\begin{aligned} X_1 \sim N(\mu_1, \sigma_1^2) & \implies p_1(x) = \frac{1}{\sqrt{2 \pi \sigma_1^2}} \exp \left\{ - \frac{1}{2} \frac{(x-\mu_1)^2}{\sigma_1^2} \right\} \\ X_2 \sim N(\mu_2, \sigma_2^2) & \implies p_2(x) = \frac{1}{\sqrt{2 \pi \sigma_2^2}} \exp \left\{ - \frac{1}{2} \frac{(x-\mu_2)^2}{\sigma_2^2} \right\} \end{aligned}\]

那么他们的乘积 \(\hat{p}(x) = p_1(x) \cdot p_2(x)\) 的分布是什么呢?
我们计算:
\[p_1(x) \cdot p_2(x) = \eta \exp \left\{ - \frac{1}{2} \left( \frac{(x-\mu_1)^2}{\sigma_1^2} + \frac{(x-\mu_2)^2}{\sigma_2^2} \right) \right\}\]
上式中, \(\eta\) 是归一化因子(确保 \(p_1(x) \cdot p_2(x)\) 对 \(x\) 积分为1即可)。
我们接着把上式中指数部分写为 \(x\) 的降幂形式:
\[\begin{aligned} \frac{(x-\mu_1)^2}{\sigma_1^2} + \frac{(x-\mu_2)^2}{\sigma_2^2} &= \frac{\sigma_2^2 \cdot (x- \mu_1)^2 + \sigma_1^2 \cdot (x-\mu_2)^2}{\sigma_1^2 \cdot \sigma_2^2} \\ &= \frac{\sigma_1^2 + \sigma_2^2}{\sigma_1^2 \cdot \sigma_2^2} x^2 - 2 \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 \cdot \sigma_2^2} x + \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \end{aligned}\]
我们考虑一个均值为 \(\hat{\mu}\) ,方差为 \(\hat{\sigma}^2\) 的高斯分布,其密度函数中对应指数部分也写为 \(x\) 的降幂形式:
\[\frac{(x-\hat{\mu})^2}{\hat{\sigma}^2} = \frac{1}{\hat{\sigma}^2} x^2 - 2 \frac{\hat{\mu}}{\hat{\sigma}^2} x + \frac{\hat{\mu}^2}{\hat{\sigma}^2}\]
如果把上面两个式子中,对应部分划等号后求解 \(\hat{\mu},\hat{\sigma}\) :
\[\begin{cases} \frac{1}{\hat{\sigma}^2} = \frac{\sigma_1^2 + \sigma_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \frac{\hat{\mu}}{\hat{\sigma}^2} = \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \frac{\hat{\mu}^2}{\hat{\sigma}^2} = \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \end{cases}\]
上面方程组是无解的,其实我们可以不考虑和 \(x\) 无关的常数项部分(即方程组中第3个方程)。因为这个部分可以计入到归一化因子 \(\eta\) 中。即只需求解下面的方程组:
\[\begin{cases} \frac{1}{\hat{\sigma}^2} = \frac{\sigma_1^2 + \sigma_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \frac{\hat{\mu}}{\hat{\sigma}^2} = \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 \cdot \sigma_2^2} \\ \end{cases}\]
求解得:
\[\begin{cases} \hat{\mu} = \frac{\sigma_2^2 \mu_1^2 + \sigma_1^2 \mu_2^2}{\sigma_1^2 + \sigma_2^2} \\ \hat{\sigma}^2 = \frac{\sigma_1^2 \cdot \sigma_2^2}{\sigma_1^2 + \sigma_2^2} \\ \end{cases}\]
所以:两个一维高斯分布的乘积还是高斯分布,它的参数如上式所述。

2.1.2 两个一维高斯分布的卷积

假设有两个随机变量 \(X,Y\) 分别满足下面约束:
\[\begin{aligned} X \sim N(\mu_X, \sigma_X^2) & \implies p_X(x) = \frac{1}{\sqrt{2 \pi \sigma_X^2}} \exp \left\{ - \frac{1}{2} \frac{(x-\mu_X)^2}{\sigma_X^2} \right\} \\ Y \mid x \sim N(ax + b , \sigma_r^2) & \implies p_{Y \mid X}(x) = \frac{1}{\sqrt{2 \pi \sigma_r^2}} \exp \left\{ - \frac{1}{2} \frac{(y - ax -b)^2}{\sigma_r^2} \right\} \end{aligned}\]
那么随机变量 \(Y\) 满足什么分布呢?
由全概率公式有:
\[p_Y(y) = \int_{- \infty}^{\infty} p_{Y \mid X}(y \mid x) p_X(x) \, \mathrm{d}x\]
所以 \(Y\) 可以看作是两个高斯分布的卷积(说明:卷积有不同形式的定义)。
结论是: \(Y\) 也是高斯分布,其均值和方差计算如下:
\[\begin{cases} \mu_Y = a \mu_X + b \\ \sigma_Y^2 = \sigma_r^2 + a^2 \sigma_X^2 \\ \end{cases}\]
其证明略。证明过程可参考:http://ais.informatik.uni-freiburg.de/teaching/ss15/robotics/etc/gaussian_notes.pdf

2.2 一维Kalman滤波器推导过程

Kalman滤波器算法共有五个公式。
一维Kalman滤波器预测阶段两个公式为:
\[\begin{aligned} \overline{\mu}_t &= a_t \overline{\mu}_{t-1} + b_t u_t \\ \overline{\sigma}_t^2 &= a_t^2 \overline{\sigma}_{t-1}^2 + \overline{\sigma}_{Q,t}^2 \\ \end{aligned}\]
一维Kalman滤波器更新阶段三个公式为:
\[\begin{aligned} K_t &= \frac{\overline{\sigma}_t^2 c_t}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2} \\ \mu_t & = \overline{\mu}_t + K_t(z_t - c_t \overline{\mu}_t) \\ \sigma_t^2 & = (1 - K_t c_t) \overline{\sigma}_t^2 \\ \end{aligned}\]
下面分别推导它们。

2.2.1 预测阶段两公式的推导

Bayes滤波器的预测阶段为:
\[\overline{bel}(x_t) = \int_{- \infty}^{\infty} p(x_t \mid x_{t-1}, u_t) bel(x_{t-1}) \, \mathrm{d} x_{t-1}\]
由于Kalman滤波器中假设“运动方程是线性的,且存在高斯噪声”,所以有:
\[p(x_t \mid u_t, x_{t-1}) \sim N(a_t x_{t-1} + b_t u_t, \sigma_{Q,t}^2)\]
又由于Kalman滤波器中假设“初始分布是高斯分布”,所以有:
\[bel(x_{t-1}) \sim N(\mu_{t-1}, \sigma_{t-1}^2)\]
从而 \(\overline{bel}(x_t)\) 是两个一维高斯分布的卷积,利用之前介绍过的“两个一维高斯分布的卷积”一节给出的结论可知 \(\overline{bel}(x_t)\) 也服从高斯分布,且:
\[\begin{aligned} \overline{bel}(x_t) & \sim N(\overline{\mu}_t, \overline{\sigma}_t^2) \\ \overline{\mu}_t &= a_t \overline{\mu}_{t-1} + b_t u_t \\ \overline{\sigma}_t^2 &= a_t^2 \overline{\sigma}_{t-1}^2 + \overline{\sigma}_{Q,t}^2 \\ \end{aligned}\]

至此,一维Kalman滤波器算法中预测阶段两公式推导完毕。

2.2.2 更新阶段三公式的推导

Bayes滤波器的更新阶段为:
\[bel(x_t) = \eta \, p(z_t \mid x_t) \overline{bel}(x_t)\]
由于Kalman滤波器中假设“测量方程是线性的,且存在高斯噪声”,所以有:
\[p(z_t \mid x_t) \sim N(c_t x_t, \sigma_{R,t}^2)\]
在前一节中,已经推导出:
\[\overline{bel}(x_t) \sim N(\overline{\mu}_t, \overline{\sigma}_t^2)\]
从而 \(bel(x_t)\) 是两个一维高斯分布的乘积,但我们却无法直接利用之前介绍过的“两个一维高斯分布的乘积”一节给出的结论。因为 \(p(z_t \mid x_t) \sim N(c_t x_t, \sigma_{R,t}^2)\) 中 \(x_t\) 出现在高斯分布的参数中。不过,我们可以利用介绍“两个一维高斯分布的乘积”时类似的思路来推导 \(bel(x_t)\) 的分布。

\[bel(x_t) = \eta \exp \left\{ -\frac{1}{2} \left( \frac{(z_t - c_t x_t)^2}{\sigma_{R,t}^2} + \frac{(x_t - \overline{\mu}_t)^2}{\overline{\sigma}_t^2} \right) \right\}\]
把上式中指数部分写为 \(x_t\) 的降幂形式:
\[\frac{(z_t - c_t x_t)^2}{\sigma_{R,t}^2} + \frac{(x_t - \overline{\mu}_t)^2}{\overline{\sigma}_t^2} = \frac{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2}{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2} x_t^2 - 2 \frac{\sigma_{R,t}^2 \overline{\mu}_t + \overline{\sigma}_t^2 c_t z_t}{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2} x_t + \frac{\sigma_{R,t}^2 \overline{\mu}_t^2 + \overline{\sigma}_t^2 z_t^2}{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2}\]

我们考虑一个均值为 \(\mu\) ,方差为 \(\sigma^2\) 的高斯分布,其密度函数中对应指数部分也写为 \(x\) 的降幂形式:
\[\frac{(x-\mu)^2}{\sigma^2} = \frac{1}{\sigma^2} x^2 - 2 \frac{\mu}{\sigma^2} x + \frac{\mu^2}{\sigma^2}\]
和“两个一维高斯分布的乘积”一节类似,联立下面方程组:
\[\begin{cases} \frac{1}{\sigma^2} & = \frac{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2}{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2} \\ \frac{\mu}{\sigma^2} &= \frac{\sigma_{R,t}^2 \overline{\mu}_t + \overline{\sigma}_t^2 c_t z_t}{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2} \\ \end{cases}\]
求解后,得:
\[\begin{cases} \mu_t &= \frac{\sigma_{R,t}^2 \overline{\mu}_t + \overline{\sigma}_t^2 c_t z_t}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2} \\ \sigma_t^2 &= \frac{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2} \end{cases}\]
即 \(bel(x_t)\) 服从均值和方差如上式所示的高斯分布, \(bel(x_t) \sim N(\mu_t, \sigma_t^2)\) 。

至此,我们已经计算出了 \(bel(x_t)\) 的均值和方差。但是,它们和一维Kalman滤波器算法中更新阶段的三个公式是等价的吗?
答案是肯定的。因为,令:
\[K_t = \frac{\overline{\sigma}_t^2 c_t}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2}\]
容易验证 \(\overline{\mu}_t + K_t(z_t - c_t \overline{\mu}_t)\) 可以化为 \(\frac{\sigma_{R,t}^2 \overline{\mu}_t + \overline{\sigma}_t^2 c_t z_t}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2}\) (即 \(\mu_t\) )。类似地,容易验证 \((1 - K_t c_t) \overline{\sigma}_t^2\) 可以化为 \(\frac{\overline{\sigma}_t^2 \cdot \sigma_{R,t}^2}{\overline{\sigma}_t^2 c_t^2 + \sigma_{R,t}^2}\) (即 \(\sigma_t^2\) )。

至此,一维Kalman滤波器算法中更新阶段三公式推导完毕。

3 Kalman滤波器的其它推导方法

前面介绍的Kalman滤波器的推导过程,是基于Bayes滤波器的。Kalman滤波器还有其它形式的推导,如基于最小二乘估计法,这里不详细介绍。

参考:
http://stats.stackexchange.com/questions/86075/kalman-filter-equation-derivation
http://blog.csdn.net/xcq2000/article/details/11129189


Author: cig01

Created: <2016-06-12 Sun 00:00>

Last updated: <2016-08-11 Thu 23:19>

Creator: Emacs 25.1.1 (Org mode 9.0.7)