Kalman Filter (Bayesian Derivation)
Table of Contents
1. Kalman 滤波器描述
Kalman 滤波器是 Bayes 滤波的一个实现。先回顾一下 Bayes 滤波,如图 1 所示。
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 所示。
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