前言

卡尔曼滤波，动态模型里面比较具有代表性的一个模型。其和隐马尔可夫模型具有差不多相同的结构，但隐马尔可夫模型要求隐状态必须时离散的。而卡尔曼滤波则没有这个限制。本文将从贝叶斯派的角度去推导Filter问题。如果你没听过马尔可夫链，可以先去了解一下，否则估计很难看懂。或者尝试先去看看案例实现卡尔曼滤波运动轨迹优化
数学基础：【概率论与数理统计知识复习-哔哩哔哩】

引入

具体流程图就这样了，和隐马尔可夫模型都差不多。比如从时刻1开始，有隐状态z1，观测到状态x1。并且下一个隐状态可以通过$A\ast z1+B$得到，而观测状态x通过$C\ast z2+D$得到(图中画错了)。

比如你可以将其用在小车运动轨迹的案例中的话，假如在时刻1，z1表示小车的的位置为0，而观测到的位置可以通过x1=Cz1+D计算。假设C=1，D=1，那么为$x1=1$。再到时刻2，可以通过时刻1的z1，计算z2.关系是z2=Az1+B。假如A=1，B=2。那么就可以计算出z2=2。再计算出x2…一直到最后

我们的目标就是求出$P(z_t|x_1,\cdot ,x_t)$。就是给定观测位置的情况下，综合它们的关系式，如何去求这个概率的参数。

推导

先对变量作一下定义
$$Z=\left(\begin{array}{c}{z}_1\\ z_2\\ ⋮\\ z_T\end{array}\right);X=\left(\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_T\end{array}\right)$$
其中，$Z$代表隐序列，$X$代表观测序列。

为了简化运算和可行性，卡尔曼滤波仍然需要与隐马尔可夫模型的两个独立性假设。

①齐次马尔可夫假设：当前隐状态只和前一时刻的隐状态有关，其余都无关。数学表达：
$$P(z_t|z_1,z_2,\cdots ,z_{t-1},x_1,\cdots ,x_{t-1})=P(z_t|z_{t-1})$$
②观测独立性假设：当前观测状态仅和当前的隐状态有关，其余都无关。数学表达：
$$P(x_t|x_1,x_2,\cdots ,x_{t-1},z_1,\cdots ,z_t)=P(x_t|z_t)$$
除此以外，在卡尔曼滤波当中，其还有一个很重要的点就是，与隐马尔可夫模型中的状态转移概率和发射概率不同，其要求这两个概率必须是线性的，即（$z_t,x_t,z_{t-1},u,v$都是随机变量）
$$\begin{gathered} z_t=A{z}_{t-1}+B+u;\to P(z_t|z_{t-1}) \\ {x}_t=C{z}_t+D+v;\to P(x_t|z_t) \\ P(z_1)=N({\mu }_0,{\Sigma }_0); \end{gathered}$$
其中，$u\sim N(0,Q)$，$v\sim N(0,R)$，意为$u$和$v$都服从高斯分布，且协方差为$Q$和$R$。

并且，$P(z_t|z_{t-1})\sim N(A{z}_{t-1}+B,Q)$，$P(x_t|z_t)\sim N(C{z}_t+D,R)$，为什么会这样？感兴趣的可以参考线性动态系统中的概率求解-CSDN博客

所以参数 θ = { A , B , C , D , Q , R , μ 0 , Σ 0 } \theta=\left\{A,B,C,D,Q,R,\mu_0,Σ_0\right\} θ={A,B,C,D,Q,R,μ0​,Σ0​}。

在卡尔曼滤波中，我们暂且不讲参数如何估计的问题。主要针对其Filter问题进行推导

Filter

所谓求解Filter问题，实际上就是求解
$$\begin{array}{c}\begin{array}{rl}P(z_t|x_1,x_2,\cdots ,x_t)=& \frac{P(z_t,x_1,\cdots ,x_t)}{P(x_1,\cdots ,x_t)}\\ =& \frac{P(x_t|z_t,x_1,\cdots ,x_{t-1})P(z_t,x_1,\cdots ,x_{t-1})}{P(x_1,\cdots ,x_t)}\\ =& \frac{P(x_t|z_t)P(z_t,x_1,\cdots ,x_{t-1})}{P(x_1,\cdots ,x_t)}\\ =& \frac{P(x_t|z_t)P(z_t|x_1,\cdots ,x_{t-1})P(x_1,\cdots ,x_{t-1})}{P(x_1,\cdots ,x_t)}\\ =& P(x_t|z_t)P(z_t|x_1,\cdots ,x_{t-1})\end{array}\end{array}$$

对于第四和第五个等号，因为x是给定的，可以算出来的，所以可以暂时忽略（为什么可以忽略？因为我们只是想找到它的递推式，而不是要去计算它，并且后面我们采用的计算方法跟这个东西无关）

对里面的$P(z_t|x_1,\cdots ,x_{t-1})$，一般叫做prediction
$$\begin{array}{c}\begin{array}{rl}P(z_t|x_1,\cdots ,x_{t-1})=& {\int }_{z_{t-1}}P(z_{t-1},z_t|x_1,\cdots ,x_{t-1})d{z}_{t-1}\\ =& {\int }_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots ,x_{t-1})P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\\ =& {\int }_{z_{t-1}}P(z_t|z_{t-1})P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\end{array}\end{array}$$

到这里你会发现$P(z_{t-1}|x_1,\cdots ,x_{t-1})$就是上一个时刻的Filter，实际上其已经实现了递归式的求解方式。

在频率派中，他们更趋向于算出每一次的$P(z_i|x_1,\cdots ,x_i)$，实际上，在贝叶斯派中，我们每一次我们都会算出前一个概率值，如果我们保留了所有的值，实际上跟频率派也是差不多的。

在应用的时候，实际上在一般的情况下，我们也是保留所有的值。比如在运动的位置预测当中。

所以，在卡尔曼滤波中，实际上分为两个部分

①预测：求出$P(z_t|x_1,\cdots ,x_{t-1})$

②更新：求出$P(z_t|x_1,\cdots ,x_t)$

所以，现在的问题就变成了，如何求出这两个概率值。也就是求出他们的均值和协方差

在求解的时候，我们利用的是正太分布的一个特性：正态分布的相乘，相加，联合概率，条件概率等等操作后都仍然是正态分布。

在此之前，我们先对它们的参数做一下定义

对于$P(z_t|x_1,\cdots ,x_{t-1})\sim N({\bar{\mu }}_t,{\bar{\Sigma }}_t)$

对于$P(z_t|x_1,\cdots ,x_t)\sim N({\hat{\mu }}_t,{\hat{\Sigma }}_t)$

求解

$P(z_t|x_1,\cdots ,x_{t-1})$

先来看一些公式

假设给定
$$\{\begin{array}{c}P(x)\sim N(x|{\mu }_x,{\Sigma }_x)\\ P(y|x)\sim N(y|Ax+B,Q)\\ y=Ax+B+u\end{array}\text{\hspace{1em}(1)}$$
我们有
$$\{\begin{array}{c}P(y)\sim N(y|A{\mu }_x+B,A{\Sigma }_x{A}^T+Q)\\ P(x,y)\sim N\left(\begin{array}{c}\begin{array}{c}x,y\end{array}|\left(\begin{array}{c}{\mu }_x\\ A{\mu }_x+B\end{array}\right),\left(\begin{array}{cc}{\Sigma }_x& {\Sigma }_x{A}^T\\ A{\Sigma }_x& A{\Sigma }_x{A}^T+Q\end{array}\right)\end{array}\right)\end{array}\text{\hspace{1em}(2)}$$
至于这个东西怎么来的？感兴趣的话，可以去看看线性动态系统中的概率求解-CSDN博客

好，那么这个东西是什么东西呢？又有什么用呢？

我们先来看公式1，里面的意思是，对随机变量x，它服从期望为${\mu }_x$，协方差矩阵为${\Sigma }_x$的正态分布。而对于随机变量$y|x$，它服从期望为$Ax+B$，协方差矩阵为$Q$的正态分布。(N代表正态分布，|之后表示期望和协方差)

我们说，如果满足公式1的条件，则我们可以推出公式2。即

①对随机变量$y$，满足$y=Ax+B+u$，其中$u\sim N(0,Q)$

②对随机变量$y$，有$y\sim N(A{\mu }_x+B,A\Sigma A^T+Q)$

③对于随机变量$x,y$，有
$$x,y\sim \left(\begin{array}{c}\left(\begin{array}{c}{\mu }_x\\ A{\mu }_x+B\end{array}\right),\left(\begin{array}{cc}{\Sigma }_x& {\Sigma }_x{A}^T\\ A{\Sigma }_x& A{\Sigma }_x{A}^T+Q\end{array}\right)\end{array}\right)$$

好，现在我们知道了有这些公式了，有什么用呢？

我们前面计算出来过
$$\begin{array}{c}\begin{array}{rl}P(z_t|x_1,\cdots ,x_{t-1})=& {\int }_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots ,x_{t-1})P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\\ =& {\int }_{z_{t-1}}P(z_t|z_{t-1})P(z_{t-1}|x_1,\cdots ,x_{t-1})d{z}_{t-1}\end{array}\end{array}$$
来看$P(z_{t-1}|x_1,\cdots ,x_{t-1})$，它的期望和协方差分别是${\hat{\mu }}_{t-1},{\hat{\Sigma }}_{t-1}$，并且，我们前面是给定
$$z_t=A{z}_{t-1}+B+u$$
那么，如果我们给这个随机变量$z_t$变成条件随机变量，则会变成
$$z_t|x_1,\cdots ,x_{t-1}=A(z_{t-1}|x_1,\cdots ,x_{t-1})+B+u\text{\hspace{1em}(3)}$$
好，现在，如果我们将$P(z_{t-1}|x_1,\cdots ,x_{t-1})$比作$P(x)$。将${\int }_{z_{t-1}}P(z_t|z_{t-1},x_1,\cdots ,x_{t-1})$比作是$P(y|x)$，依托公式3，则最终我们会发现，其刚好也符合我们上述所提到的公式1的条件。

那么，我们所要求的东西是什么？是$P(z_t|x_1,\cdots ,x_{t-1})$，这个东西是什么呢？没错，他可以比作$P(y)$，我只能讲到这样了(如果你对随机变量不太理解，你可以将条件随机变量$z_{t-1}|x_1,\cdots ,x_{t-1}$当作一个普通的随机变量，这样我相信你自然可以理解了)

既然如何，那么我们只需要求解出随机变量$y$的参数就相当于求解出$P(z_t|x_1,\cdots ,x_{t-1})$所对应的参数。依据公式2，我们直接往里面代入，直接得到
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_t=& A{\hat{\mu }}_{t-1}+B\\ {\bar{\Sigma }}_t=& A{\hat{\Sigma }}_{t-1}{A}^T+Q\end{array}\end{array}$$

$P(z_t|x_1,\cdots ,x_t)$

求解这个问题，我们一样的采用前面的方法

先来看公式

假设给定
$$x,y\sim N\left(\begin{array}{c}\left(\begin{array}{c}{\mu }_x\\ {\mu }_y\end{array}\right),\left(\begin{array}{cc}{\Sigma }_{xx}& {\Sigma }_{xy}\\ {\Sigma }_{yx}& {\Sigma }_{yy}\end{array}\right)\end{array}\right)\text{\hspace{1em}(4)}$$
则
$$P(x|y)\sim N(x|{\mu }_k+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}y,{\Sigma }_k)\text{\hspace{1em}(5)}$$
其中${\mu }_k={\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\mu }_y$，${\Sigma }_k={\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}$

啥意思呢？意思就是假设联合随机变量$x$和$y$服从公式4，则我们能够得出公式5。其实也就是如果能够得到联合概率分布的参数，那么我们就可以求出其条件概率的参数。推导过程参见线性动态系统中的概率求解-CSDN博客

那么，这个东西有什么用捏？我们仔细来看，我们可以不可以将$P(z_t,x_t|x_1,\cdots ,x_{t-1})$当作是$P(x,y)$，那么$P(x|y)$不就是我们要求的$P(z_t|x_1,\cdots ,x_t)$

没错啊，所以我们只需要求出$P(z_t,x_t|x_1,\cdots ,x_{t-1})$即可。也就是要求出它里面对应的参数。如果

对于这个联合分布，我们当燃是叠罗汉式地将$P(z_t|x_1,\cdots ,x_{t-1})$，$P(x_t|x_1,\cdots ,x_{t-1})$，而第一个我们已经在前面解出来过了。

接下来我们来看$P(x_t|x_1,\cdots ,x_{t-1})$
$$\begin{array}{c}\begin{array}{rl}P(x_t|x_1,\cdots ,x_{t-1})=& {\int }_{z_t}P(x_t,z_t|x_1,\cdots ,x_{t-1})d{z}_t\\ =& {\int }_{z_t}P(x_t|x_1,\cdots ,x_{t-1},z_t)P(z_t|x_1,\cdots ,x_{t-1})d{z}_t\\ =& {\int }_{z_t}P(x_t|z_t)P(z_t|x_1,\cdots ,x_{t-1})d{z}_t\end{array}\end{array}$$
发现了没有，对于$P(z_t|x_1,\cdots ,x_{t-1})$，就是我们前面求出来的，而对于$P(x_t|z_t)$，我们前面是一样的有$x_t=C{z}_t+D+v$。

所以，同前面求$P(z_t|x_1,\cdots ,x_{t-1})$一样，我们将$P(z_r|x_1,\cdots ,x_{t-1})$当作是$P(x)$，将$P(x_t|x_1,\cdots ,x_{t-1},z_t)$当作是$P(y|x)$，而我们所要求解的$P(x_t|x_1,\cdots ,x_{t-1})$理所当然就是$P(y)$.

所以，接下来我们按部就班，我们设它的期望和协方差矩阵为${\mu }_x,{\Sigma }_x$
$$\begin{array}{c}\begin{array}{rl}{\mu }_x=& C{\bar{\mu }}_t+D\\ {\Sigma }_x=& C{\bar{\Sigma }}_t{C}^T+R\end{array}\end{array}$$
好。现在我们就可以叠罗汉了
$$(z_t,x_t|x_1,\cdots ,x_{t-1})\sim N\left(\begin{array}{c}\left(\begin{array}{c}{\bar{\mu }}_t\\ C{\bar{\mu }}_t+D\end{array}\right),\left(\begin{array}{cc}{\bar{\Sigma }}_t& {\Sigma }_{xy}\\ {\Sigma }_{yx}& C{\bar{\Sigma }}_t{C}^T+R\end{array}\right)\end{array}\right)$$
那么接下来就只剩下${\Sigma }_{xy}$，至于这个东西怎么求解，我们在公式2中也给出了呃，在线性动态系统中的概率求解-CSDN博客也有推导了，最终${\Sigma }_{xy}={\bar{\Sigma }}_t{C}^T$，而${\Sigma }_{yx}$也就是$C{\bar{\Sigma }}_t$

所以，最终得到
$$(z_t,x_t|x_1,\cdots ,x_{t-1})\sim N\left(\begin{array}{c}\left(\begin{array}{c}{\bar{\mu }}_t\\ C{\bar{\mu }}_t+D\end{array}\right),\left(\begin{array}{cc}{\bar{\Sigma }}_t& {\bar{\Sigma }}_t{C}^T\\ C{\bar{\Sigma }}_t& C{\bar{\Sigma }}_t{C}^T+R\end{array}\right)\end{array}\right)$$
所以对于条件概率$P(z_t|x_1,\cdots ,x_t)$，我们只需要代公式即可求出，将公式5里面的值都代换，即可得到期望和协方差矩阵

我们先回顾一下公式5
$$P(x|y)\sim N(x|{\mu }_k+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}y,{\Sigma }_k)$$
其中${\mu }_k={\mu }_x-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\mu }_y$，${\Sigma }_k={\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{yx}$

我们定义
$$\begin{array}{c}\begin{array}{rl}K=& {\Sigma }_{xy}{\Sigma }_{yy}^{-1}\\ =& {\bar{\Sigma }}_t{C}^T{\left(C{\bar{\Sigma }}_t{C}^T+R\right)}^{-1}\end{array}\end{array}$$
所以期望为
$$\begin{array}{c}\begin{array}{rl}{\hat{\mu }}_t=& {\mu }_k+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}y\\ =& {\bar{\mu }}_t-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\mu }_y+{\Sigma }_{xy}{\Sigma }_{yy}^{-1}y\\ =& {\bar{\mu }}_t-K(C{\bar{\mu }}_t+D)+K{x}_t\\ =& {\bar{\mu }}_t+K(x_t-C{\bar{\mu }}_t-D)\end{array}\end{array}$$
协方差矩阵为
$$\begin{array}{c}\begin{array}{rl}{\hat{\Sigma }}_t=& {\Sigma }_{xx}-{\Sigma }_{xy}{\Sigma }_{yy}^{-1}{\Sigma }_{xy}\\ =& {\bar{\Sigma }}_t-KC{\bar{\Sigma }}_t\\ =& (I-KC){\bar{\Sigma }}_t\end{array}\end{array}$$
其中$I$为单位矩阵

结论

至此，我们就推导出来了广为人知的卡尔曼黄金五式，即

①预测
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_t=& A{\hat{\mu }}_{t-1}+B\\ {\bar{\Sigma }}_t=& A{\hat{\Sigma }}_{t-1}{A}^T+Q\end{array}\end{array}$$
②更新
$$\begin{gathered} K={\bar{\Sigma }}_t{C}^T{\left(C{\bar{\Sigma }}_t{C}^T+R\right)}^{-1} \\ {\hat{\mu }}_t={\bar{\mu }}_t+K(x_t-C{\bar{\mu }}_t-D) \\ {\hat{\Sigma }}_t=(I-KC){\bar{\Sigma }}_t \end{gathered}$$
其中$K$被称为卡尔曼增益

结束

到此，卡尔曼滤波的Filter问题已经全部推导完成。如有错误，还望指出。阿里嘎多。