卡尔曼滤波公式推导（1）

一、背景知识

卡尔曼滤波器和其衍生出来的一系列滤波器被验证是行之有效的多源数据融合方法，在工程上有广泛的应用。应用的场景主要分两种：1. 预测无法直接测量的状态量；2.融合多源的信息得出更加准确的状态量。大家都知道卡尔曼滤波器分为预测和更新两个过程，一共有5个公式，但是很多人不知道公式是怎么推导出来的。这两篇文章会用两种方式详细推导卡尔曼滤波器的公式。
卡尔曼滤波器的5个经典公式如下：
预测（时间更新过程）
${\hat{x}}_k^{-}=A_k{\hat{x}}_{k-1}+B_k{u}_k+w_k$
${\hat{P}}_k^{-}=A_k{P}_{k-1}{A}_k^T+Q_k$
更新（状态更新过程）
$K_k=P_k^{-}{H}_K^T(H_k{P}_k^{-}{H}_k^T+R_k{)}^{-1}$
${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$
$P_k=P_k^{-}-K_k{H}_k{P}_k^{-}$
此外，在测量状态时有：
$x_k=z_k+v_k$
以下是各个变量的含义：
$x_k$: 状态向量的真实值
${\hat{x}}_k^{-}$: 状态向量的先验估计值
${\hat{x}}_{k-1}$: 上个时刻状态向量的后验估计值
${\hat{x}}_k$: 当前时刻状态向量的后验估计值
$A_k$: 状态转移矩阵
$B_k$: 状态控制矩阵
$u_k$: 状态控制向量
$w_k$: 过程噪声，符合正态分布$w_k\sim N(0,Q_k)$
${\hat{P}}_k^{-}$: 预测协方差矩阵的先验估计值
$P_{k-1}$: 上个时刻预测协方差矩阵的后验估计值
$P_k$: 当前时刻预测协方差矩阵的后验估计值
$Q_k$: 过程噪声协方差矩阵
$z_k$: 测量向量
$v_k$: 测量噪声，符合正态分布$v_k\sim N(0,R_k)$
$R_k$: 测量噪声的协方差矩阵
$H_k$: 状态向量到测量向量的转换矩阵

二、卡尔曼滤波公式的推导

在预测和更新阶段分别能得到两个近似状态向量真实值的值，记为${\hat{x}}^{predict}=H_k{\hat{x}}_k^{-}$和${\hat{x}}^{measure}=z_k$。基于假设，预测的值和测量的值都是符合正态分布的，即${\hat{x}}^{predict}\sim N(u_0,{\sigma }_0^2)(u_0=H_k{\hat{x}}_k^{-},{\sigma }_0^2=H_k{P}_k^{-}{H}_k^T)$，${\hat{x}}^{measure}\sim N(u_1,{\sigma }_1^2)(u_1=z_k,{\sigma }_1^2=R_k)$，而最终得到的状态向量估计值$\hat{x}$的概率分布为这两个正态分布相乘的结果（注*）。下面计算一下两个正态分布函数相乘能得到什么？
$$\begin{array}{rl}N(x,u_0,{\sigma }_0^2)N(x,u_1,{\sigma }_1^2)=& \frac{1}{\sqrt{2\pi }{\sigma }_0}{e}^{-\frac{(x-u_0{)}^2}{2{\sigma }_0^2}}\frac{1}{\sqrt{2\pi }{\sigma }_1}{e}^{-\frac{(x-u_1{)}^2}{2{\sigma }_1^2}}\\ =& \frac{1}{2\pi {\sigma }_0{\sigma }_1}{e}^{-\frac{(x-u_0{)}^2}{2{\sigma }_0^2}-\frac{(x-u_1{)}^2}{2{\sigma }_1^2}}\end{array}$$
指数部分用$-\beta$表示，则有
$$\begin{array}{rl}\beta =& \frac{(x-u_0{)}^2}{2{\sigma }_0^2}+\frac{(x-u_1{)}^2}{2{\sigma }_1^2}\\ =& \frac{(x-u_0{)}^2{\sigma }_1^2+(x-u_1{)}^2{\sigma }_0^2}{2{\sigma }_0^2{\sigma }_1^2}\\ =& \frac{{\sigma }_1^2{x}^2-2{\sigma }_1^2{u}_{0}x+{\sigma }_1^2{u}_0^2+{\sigma }_0^2{x}^2-2{\sigma }_0^2{u}_{1}x+{\sigma }_0^2{u}_1^2}{2{\sigma }_0^2{\sigma }_1^2}\\ =& \frac{({\sigma }_0^2+{\sigma }_1^2)x^2-2({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)x+{\sigma }_1^2{u}_0^2+{\sigma }_0^2{u}_1^2}{2{\sigma }_0^2{\sigma }_1^2}\\ =& \frac{x^2-\frac{2({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}x+\frac{{\sigma }_1^2{u}_0^2+{\sigma }_0^2{u}_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}\\ =& \frac{(x-\frac{({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}{)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}+\frac{\frac{{\sigma }_1^2{u}_0^2+{\sigma }_0^2{u}_1^2}{({\sigma }_0^2+{\sigma }_1^2)}-(\frac{({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}{)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}\end{array}$$
令$\gamma =\frac{(x-\frac{({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}{)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}$，$\lambda =\frac{\frac{{\sigma }_1^2{u}_0^2+{\sigma }_0^2{u}_1^2}{({\sigma }_0^2+{\sigma }_1^2)}-(\frac{({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}{)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}$，可以看出来$\lambda$为一个常数项，而$\gamma$符合正态分布，可以将$\gamma$继续转化为：
$$\begin{array}{rlr}\gamma =& \frac{(x-\frac{({\sigma }_1^2{u}_0+{\sigma }_0^2{u}_1)}{({\sigma }_0^2+{\sigma }_1^2)}{)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}& \\ =& \frac{(x-(u_0+\frac{({\sigma }_0^2{u}_1-{\sigma }_0^2{u}_0)}{({\sigma }_0^2+{\sigma }_1^2)}){)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}& \\ =& \frac{(x-(u_0+\frac{{\sigma }_0^2(u_1-u_0)}{({\sigma }_0^2+{\sigma }_1^2)}){)}^2}{\frac{2{\sigma }_0^2{\sigma }_1^2}{({\sigma }_0^2+{\sigma }_1^2)}}& \\ =& \frac{(x-(u_0+G(u_1-u_0)){)}^2}{2({\sigma }_0^2-G{\sigma }_0^2)}& \text{其中G=σ02+σ12σ02}\end{array}$$
令$u^{{}^{\prime }}=u_0+G(u_1-u_0)$，${\sigma }^{{}^{\prime }2}={\sigma }_0^2-G{\sigma }_0^2$，则
$$\begin{array}{rlr}N(x,u_0,{\sigma }_0^2)N(x,u_1,{\sigma }_1^2)=& S_g\frac{1}{\sqrt{2\pi }{\sigma }^{{}^{\prime }}}{e}^{-\frac{(x-u^{{}^{\prime }}{)}^2}{2{\sigma }^{{}^{\prime }2}}}& \text{(Sg为一个常数)}\end{array}$$
到这里，我们验证了两个正态分布函数相乘之后得到带缩放因子$S_g=\frac{1}{\sqrt{2\pi ({\sigma }_0^2+{\sigma }_1^2})}{e}^{\frac{-(u_0-u_1{)}^2}{2({\sigma }_0^2+{\sigma }_1^2)}}$的正态分布函数，即有$\hat{x}\sim N(u^{{}^{\prime }},{\sigma }^{{}^{\prime }2})$。接下来将各个变量代入公式有：
$G=H_k{P}_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k)$
$H_k{\hat{x}}_k=H_k{\hat{x}}_k^{-}+G(z_k-H_k{\hat{x}}_k^{-})$
$H_k{P}_k{H}_k^T=H_k{P}_k^{-}{H}_k^T-G{H}_k{P}_k^{-}{H}_k^T$
令$K_k=H_k^{-1}G$，即为卡尔曼增益，则
$K_k=P_k^{-}{H}_k^T(H_k{P}_k^{-}{H}_k^T+R_k)$
${\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H_k{\hat{x}}_k^{-})$
$P_k=P_k^{-}-K_k{H}_k{P}_k^{-}$
以上完成了卡尔曼滤波器公式推导。

注*：这里的推导直接用了一个前提，即状态向量后验估计值$\hat{x}$的概率分布为两个正态分布相乘的结果，该前提在后续文章再做详细探讨。下面我们可以先从曲线上直观理解。

绿色的曲线表示上一次的后验估计值的正态分布曲线，在经过状态转移矩阵$A_k$之后，正态分布曲线变成了蓝色的曲线（前提是$A_k$是线性的），红色的曲线则为测量值的正态分布曲线，我们的任务就是利用蓝色和红色两个正态分布曲线得到一个方差更小的正态分布曲线。

直接将蓝色和红色两个正态分布曲线相乘能够得到底下天蓝色填充的曲线，这条曲线也为正态分布曲线，这也验证了我们上面的计算结果没有问题。此外，可以发现相乘后的正态分布曲线的方差更小了，但是幅度经过了缩放，即对这个曲线进行积分的结果不为1，需要对其进行归一化。青色的曲线就是归一化后的正态分布曲线，也就是当前时刻后验估计值${\hat{x}}_k$的正态分布曲线。

以下为画图的代码：

import numpy as np
from matplotlib import pyplot as plt

x = np.arange(-10, 20, 0.01)
u1 = 3
sigma1 = 2
y1 = 1/(np.sqrt(2*np.pi)*sigma1)*np.exp(-(x-u1)*(x-u1)/(2*sigma1*sigma1))

u2 = 5
sigma2 = 2.5
y2 = 1/(np.sqrt(2*np.pi)*sigma2)*np.exp(-(x-u2)*(x-u2)/(2*sigma2*sigma2))

u3 = 8
sigma3 = 1.5
y3 = 1/(np.sqrt(2*np.pi)*sigma3)*np.exp(-(x-u3)*(x-u3)/(2*sigma3*sigma3))

y5 = y2 * y3

K = sigma2*sigma2/(sigma2*sigma2+sigma3*sigma3)
Sg = 1/np.sqrt(2*np.pi*(sigma2*sigma2+sigma3*sigma3))*np.exp(-(u2-u3)*(u2-u3)/(2*(sigma2*sigma2+sigma3*sigma3)))
u4 = u2 + K*(u3 - u2)
sigma4 = np.sqrt(sigma2*sigma2 - K*sigma2*sigma2)
y4 = 1/(np.sqrt(2*np.pi)*sigma4)*np.exp(-(x-u4)*(x-u4)/(2*sigma4*sigma4))

fig, axs = plt.subplots()
axs.plot(x, y1, 'g')
axs.text(u1, 1/(np.sqrt(2*np.pi)*sigma1), r"$\leftarrow N(\hat x_{k-1}, u_1, \sigma_1)$")
axs.plot(x, y2, 'b')
axs.text(u2, 1/(np.sqrt(2*np.pi)*sigma2), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)$")
axs.plot(x, y3, 'r')
axs.text(u3, 1/(np.sqrt(2*np.pi)*sigma3), r"$\leftarrow N(z_{k}, u_3, \sigma_3)$")
axs.grid(linewidth = 0.5, linestyle = ':')

fig, axs1 = plt.subplots()
axs1.plot(x, y2, 'b')
axs1.text(u2, 1/(np.sqrt(2*np.pi)*sigma2), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)$")
axs1.plot(x, y3, 'r')
axs1.text(u3, 1/(np.sqrt(2*np.pi)*sigma3), r"$\leftarrow N(z_{k}, u_3, \sigma_3)$")
axs1.plot(x, y5, 'y')
axs1.text(u4, Sg*1/(np.sqrt(2*np.pi)*sigma4), r"$\leftarrow N(\hat x_{k}^{-}, u_2, \sigma_2)N(z_{k}, u_3, \sigma_3)$")
axs1.fill_between(x, y5)
axs1.plot(x, y4, 'c')
axs1.text(u4, 1/(np.sqrt(2*np.pi)*sigma4), r"$\leftarrow N(\hat x_{k}, u_4, \sigma_4)$")
axs1.grid(linewidth = 0.5, linestyle = ':')
plt.show()

总结一下，在预测阶段和更新阶段我们能得到两个真实值的估计，它们都服从正态分布，直接对两个正态分布曲线相乘能够得到方差更小的正态分布曲线，使得最终估计的值更加接近真实值。