运动、观测概念

什么是运动：考虑从k-1时刻到k时刻，位置x如何变化
什么是观测：在k时刻 $x_k$ 处探测一个路标 $y_j$

运动方程：
$x_k=f(x_{k-1},u_k,w_k)$
$u_k$ 表示传感器的读数， $w_k$ 表示噪声

观测方程：在 $x_k$ 处看到某个路标点 $y_j$ ，产生一个观测数据 $z_{k,j}$
$z_{k,j}=h(y_j,x_k,v_{k,j})$
$v_{k,j}$ 是观测的噪声

多元高斯分布

高斯分布有两种表达方式：

协方差矩阵+均值
信息矩阵+信息矢量

协方差矩阵+均值的方式比较常见，如下

在这里插入图片描述

左边常数项记为 $η$ ， $p (x)$ 可以记为：

在这里插入图片描述

其中对称正定矩阵 $Σ$ 为随机变量x的协方差矩阵，μ为x的均值，简记为
在这里插入图片描述

信息矩阵+信息矢量的形式可以由上式推导而来

在这里插入图片描述

现在定义信息矩阵 $Ω=Σ^{?1}$ ，信息矢量 $ξ=Σ^{?1}μ=Ωμ$ ，则

在这里插入图片描述

卡尔曼滤波推导过程

卡尔曼滤波中的条件独立：

当给定某个时刻的状态变量 $x_n$ ，该时刻的观测变量 $z_n$ 与之前的任意观测变量条件独立
${\color{Red} p(z_k|x_k,z_{k-1},...,z_1)=p(z_k|x_k) } \tag{1}$

根据卡尔曼滤波模型的假设，当我们知道了状态变量的取值时，那么对应的观测变量的分布就确定了，它与更早之前的观测无关（事实上与更早之前的任意状态变量也无关），状态变量的取值就是所需的全部信息。
当给定某一时刻的状态变量 $x_{k-1}$ ，下一时刻的状态变量 $x_k$ 与之前的任意观测变量条件独立
${\color{Red} p(x_k|x_{k-1},z_{k-1},...,z_1) = p(x_k|x_{k-1})} \tag{2}$

根据卡尔曼滤波模型的假设，当我们知道了上一时刻状态变量t的取值时，那么下一时刻状态变量的分布就确定了，它与更早之前的观测无关（事实上与更早之前的任意状态变量也无关），前一时刻状态变量的取值就是所需的全部信息。

在推到卡尔曼滤波过程中，都是为了“凑出”运动变量、观测变量的条件概率 $p(x_k|x_{k-1})$ 和 $p(z_k|x_k)$ ，基于以上两点，应用贝叶斯定理：
${\color{Red} \begin{aligned} p(x_k|z_k,z_{k-1},...,z_1)&= \frac{p(z_k|x_k,z_{k-1},...,z_1)p(x_k|z_{k-1},...,z_1)}{p(z_k|z_{k-1},...,z_1)} \\ &= \frac{p(z_k|x_k)p(x_k|z_{k-1},...,z_1)}{p(z_k|z_{k-1},...,z_1)} \tag{3} \end{aligned}}$
上式用到式（1）的条件独立性，已经凑出观测变量的条件概率： $p(z_k|x_k)$ ，为了推到出目标表达式，分子的第二项着手，引入 $x_{k-1}$ ，对其积分得：
${\color{Red} \begin{aligned} p(x_k|z_{k-1},...,z_1) &= \int_{x_{k-1}}^{}p(x_k|x_{k-1},z_{k-1},...,x_1)p(x_{k-1}|z_{k-1},...,z_1) dx_{k-1}\\ & = \int_{x_{k-1}}^{}p(x_k|x_{k-1})\hat {\alpha }(x_{k-1})dx_{k-1} \tag{4} \end{aligned}}$
上式用到式（2）的条件独立性，又凑出 $p(x_k|x_{k-1})$ ，将式（4）带入式（3）得：

${\color{Red} \begin{aligned} \hat {\alpha }(x_{k})=\frac{p(z_k|x_k)\int_{x_{k-1}}^{}p(x_k|x_{k-1})\hat {\alpha }(x_{k-1})dx_{k-1}}{p(z_k|z_{k-1},...,z_1)} \tag{5} \end{aligned}}$

因为在卡尔曼滤波模型中，观测模型和状态转移概率模型都是线型高斯分布，因此基于此推导出的所有概率分布也是高斯分布。高斯分布由均值向量和协方差矩阵表示，接下来就是要把（5）式关于概率分布的递推式转换为关于均值向量和协方差矩阵的递推式，这样才能用计算机实现。为此，需要引入另一个预备知识：

高斯分布的贝叶斯定理
已知一个多维高斯分布：
$\sim N(x| \mu, \Lambda ^{-1})$
另一个多维随机变量[公式]在已知[公式]的条件下也服从高斯分布，且其均值为[公式]的线性变换，即
$\sim N(y| Ax+b,L ^{-1})$
其中， $A$ 和 $b$ 是线性变换参数， $L$ 和 $\Lambda$ 是信息矩阵（协方差的逆矩阵）
$p (y ∣ x)$ 定义了一个线性高斯模型，则 $p (y)$ 和 $p (x ∣ y)$ 也服从高斯分布，均值和方差分别为：
$E(y)=A\mu +b \\ cov(y)=L^{-1}+A \Lambda ^{-1}A^T \tag{6}$
$(\Lambda +A^T LA)^{-1}(A^TL(y-b)+\Lambda \mu) \\ cov(x|y)= (\Lambda +A^TLA)^{-1} \tag{7}$
结论：已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布及其条件高斯分布。

记 $\hat {\alpha }$ 的均值和方差为 $\mu_k$ 和 $V_k$ ，有：
$\color{Red} \hat {\alpha } (x_k)= N(x_k|\mu_k,V_k) \tag{8}$

转化为问题：已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布
$\color{Red} \int_{x_{k-1}}^{}p(x_k|x_{k-1})\hat \alpha (x_{k-1})dx_{k-1}=N(x_k|A\mu_{k-1},P_{k-1}) \tag{9}$
其中：
$\color{Red} P_{k-1}= L^{-1} +AV_{k-1}A^T \tag{10}$

贝叶斯公式：
$p(x_k|z_k)=\frac{p(z_k|x_k)p(x_k)}{p(z_k)} \tag{11}$

${\color{Red} \begin{aligned} p(x_k|z_k,z_{k-1},...,z_1) &=\hat {\alpha }(x_{k})=\frac{p(z_k|x_k).N(x_k|A\mu_{k-1},P_{k-1})}{p(z_k|z_{k-1},...,z_1)} \tag{12} \end{aligned}}$
对比贝叶斯公式（11）和式（12），（9）~（10）式的结果相当于我们已经求出了其中的 $p(x_k)$ 。通过观察，这又是“已知一个随机变量的高斯分布及其条件高斯分布，求另一个随机变量的高斯分布”的问题，只不过此时变量的对应关系变为：

$x……x_k\\ y……z_k\\ \mu……A\mu_{k-1}\\ \Lambda^{-1}……P_{k-1}\\ A……C\\ b……0\\ L^{-1}……\sum$
根据（7）式可以直接写出：
$\color{Red} \mu_k =(P^{-1}_{k-1} + C^T\sum{}{}^{-1} C）^{-1}z_k+P_{k-1}^{-1}A\mu_{k-1} \\ V_{k-1}=(P_{k-1}^{-1}+C^T\sum{}{}^{-1}C)^{-1} \tag{13}$

上式为卡尔曼滤波公式的原始形式，对该式继续变形：

Woodbury等式
$(A+BD^{-1}C)^{-1} = A^{-1}-A^{-1}B(D+CA^{-1}B)^{-1}CA^{-1} \tag{14}$

依式（14）对式（13）变形：
$V_k =（P^{-1}_{k-1}+C^T\sum{}{}^{-1}C)^{-1} \\ = P_{k-1}-P_{k-1}C^T(\sum{}{}+CP_{k-1}C^T)^{-1}CP_{k-1} \tag{15}$

记卡尔曼增益 $K_k=P_{k-1}C^T(\sum{}{}+CP_{k-1}C^T)^{-1}$
从而：
$V_k =（P^{-1}_{k-1}+C^T\sum{}{}^{-1}C)^{-1} \\ = P_{k-1}-K_kCP_{k-1} \tag{16} = (I-K_kC)P_{k-1}$