KF融合角度分析

由$k-1$到$k$时刻，系统状态方程
$$x_k=A_k{x}_{k-1}+B_k{u}_k+C{w}_k\text{\hspace{1em}(1)}$$
系统观测方程
$$y_k=H_k{x}_k+v_k\text{\hspace{1em}(2)}$$
其中，$x_k$为真实值；$A_k$为状态转移矩阵；$u_k$为系统输入；$B_k$为输入增益矩阵；$y_k$为测量值；$H_k$为观测矩阵；$w_k\sim N\left(0,Q\right)$为过程高斯噪声，$Q$为其协方差阵；$C$为过程噪声增益矩阵；$v_k\sim N\left(0,R\right)$为测量高斯噪声，$R$为其协方差阵；$w_k$和$v_k$不相关。

注意：式(1)是实际系统方程，式(2)是得用实际传感器来测量。因为这两个式子都有噪声，所以不可能准确知道每一时刻的$x_k$。只能知道个大概范围，下面要做的就是得到一个最好${\hat{x}}_k$来估计$x_k$。

先说状态方程这部分。

假设$x_0\sim N\left({\hat{x}}_0,P_0\right)$，由于我们前一时刻对当前时刻的状态预测时不知道$w_k$是多少（但是知道$u_k$,因为这是我们输入的）,所以我们就先产生一个状态先验预测，即
$${\hat{x}}_k^{\prime }=A_k{\hat{x}}_{k-1}+B_k{u}_k\text{\hspace{1em}(3)}$$
同样地，这个预测是有不确定性的，也就是会有一个协方差矩阵来描述不确定性。因为是对先验状态的描述，所以叫先验协方差矩阵，即
$$P_k^{\prime }=A_k{P}_{k-1}{A}_k^T+CQ{C}^T\text{\hspace{1em}(4)}$$
需要解释一下这个式(4)的由来，先说式(4)的第一项，当前时刻的先验状态是从前一时刻的后验来的，式(3)可知；先说式(4)的第二项，我们是知道$w_k$的统计特性的。所以二者相加就是后一时刻的先验协方差矩阵。

注意：这里解释一下为什么是上面这样。假设有一个随机向量$x\sim N\left(\hat{x},P\right)$,那么随机向量的变换$Ax+Bu\sim N(A\hat{x}+Bu,AP{A}^T)$。同样，假设有另一个随机向量$w\sim N(0,Q)$，$Cw\sim N(0,CQ{C}^T)$。两者的和$Ax+Bu+Cw\sim N(A\hat{x}+Bu,AP{A}^T+CQ{C}^T)$。能这样算的前提条件是这两个随机向量都是高斯不相关的，刚好这个条件满足。具体详细的推导可以参考数理统计、随机过程、矩阵论方面的书。

总结：前一时刻的真实后验$x_{k-1}\sim N\left({\hat{x}}_{k-1},P_{k-1}\right)$，当前时刻的真实先验$x_k^{\prime }\sim N({\hat{x}}_k^{\prime },P_k^{\prime })$

再说观测方程这部分。

由于我们对当前时刻的观测时不知道$v_k$是多少，而且不知道真实的$x_k$，所以我们就对式(3)观测，先产生一个观测预测，这个预测也是有不确定性的,即
$$y_k^{\prime }\sim N(H_k{\hat{x}}_k^{\prime },H_k{P}_k^{\prime }{H}_k^T)\text{\hspace{1em}(5)}$$
由观测方程(2)，假设传感器读数分布的均值等于我们观察到传感器的读数$y_k$，其协方差为$R$。

这样一来，我们有了两个高斯分布：一个围绕通过状态转移预测的平均值，另一个围绕实际传感器读数。我们的目标是融合这两个分布，也就是相交的部分，很自然就是二者的乘积。

融合。

首先考虑一维高斯情况：一个均值为$\mu$，方差${\sigma }^2$为的高斯分布的形式为：
$$N(x;\mu ,\sigma )=\frac{1}{\sqrt{2\pi {\sigma }^2}}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\text{\hspace{1em}(6)}$$
我们想知道将两个高斯曲线相乘会发生什么，就是融合后的${\mu }^{\prime }$和${\sigma }^{\prime }$是多少（这里有个前提，因为知道两个高斯相乘还是高斯）。
$$N(x;{\mu }_0,{\sigma }_0)N(x;{\mu }_1,{\sigma }_1)=N(x;{\mu }^{\prime },{\sigma }^{\prime })\text{\hspace{1em}(7)}$$
将式(6)带入式(7)，我们可以得到新的高斯分布的均值和方差如下所示：
$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}={\mu }_0+k({\mu }_1-{\mu }_0)\\ {\sigma }^{\prime 2}& ={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}={\sigma }_0^2-k{\sigma }_0^2\\ k& \triangleq \frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}(8)}$$
详细推导见这里和这里。
这样一来，公式的形式就简单多了！我们顺势将上式的矩阵形式写在下面：
$$\begin{array}{rl}{\mathbf{\mu }}^{\prime }& ={\mathbf{\mu }}_0+\mathcal{K}({\mathbf{\mu }}_1-{\mathbf{\mu }}_0)\\ {\Sigma }^{\prime }& ={\Sigma }_0-\mathcal{K}{\Sigma }_0\\ \mathcal{K}& \triangleq {\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\end{array}\text{\hspace{1em}(9)}$$
其中${\Sigma }^{\prime }$表示新高斯分布的协方差矩阵，${\mathbf{\mu }}^{\prime }$是每个维度的均值。

现在开始融合：

我们有两个高斯分布，一个是我们的观测预测$({\mathbf{\mu }}_0,{\Sigma }_0)=(H_k{\hat{x}}_k^{\prime },H_k{P}_k^{\prime }{H}_k^T)$，另外一个是实际的观测(传感器读数)$({\mu }_1,{\Sigma }_1)=(y_k,R)$，我们将这两个高斯分布带入公式(9)中就可以得到二者相乘的重叠区域：
$$\begin{array}{rl}{H}_k{\hat{x}}_k& =H_k{\hat{x}}_k^{\prime }+{\mathcal{K}}_k(y_k-H_k{\hat{x}}_k^{\prime })\\ H_k{P}_k{H}_k^T& =H_k{P}_k^{\prime }{H}_k^T-{\mathcal{K}}_k{H}_k{P}_k^{\prime }{H}_k^T\\ {\mathcal{K}}_k& \triangleq H_k{P}_k^{\prime }{H}_k^T(H_k{P}_k^{\prime }{H}_k^T+R{)}^{-1}\end{array}\text{\hspace{1em}(10)}$$
由式(10)可得：
$$\begin{array}{rl}{\hat{x}}_k& ={\hat{x}}_k^{\prime }+K_k(y_k-H_k{\hat{x}}_k^{\prime })\\ P_k& =P_k^{\prime }-K_k{H}_k{P}_k^{\prime }\\ K_k& \triangleq P_k^{\prime }{H}_k^T(H_k{P}_k^{\prime }{H}_k^T+R{)}^{-1}\end{array}\text{\hspace{1em}(11)}$$
式(3)(4)(11)就是完整的KF。

最后说一下为什么协方差矩阵可以表示不确定性。以三维空间，也就是三维协方差矩阵为例：随机向量的均值表示空间一个确定点；将协方差矩阵特征值分解可以得到三个正交的单位向量，对应的三个特征值对该方向的单位向量进行放大或缩小，这样可以生成一个三维椭球。随机向量的所有可能在空间的点有99.74%在以均值点为中心的椭球内。