Kalman滤波(Part-2：标量形式)

在开始之前，先回顾一下正交、不相关和独立之间的联系与差别

• 正交
  随机变量：$\mathcal{R}(x,y)=\mathbb{E}[xy]$为相关函数，若$\mathcal{R}(xy)=0$，则认为$x,y$正交。(类比内积，注意，相关函数为0，是正交，不是不相关)
  随机过程：$\mathcal{R}(X(t),Y(t))=\mathbb{E}[X(t)Y(t)]$，若$\mathcal{R}(X(t),Y(t))=0$，则认为$X(t),Y(t)$正交。

• 不相关
  随机变量：$\mathbb{E}[xy]=\mathbb{E}[x]\mathbb{E}[y]$，则认为$x,y$不相关。
  随机过程：$\mathbb{E}[X(t)Y(t)]=\mathbb{E}[X(t)]\mathbb{E}[Y(t)]$，则认为$X(t),Y(t)$不相关。
  注意：当随机变量为高斯随机变量，或随机过程为高斯随机过程时，不相关与独立等价。

• 独立
  若联合分布$p(x,y)=p(x)\cdot p(y)$，则认为$x,y$独立。

• 协方差的相关和独立
  协方差函数$\text{Cov}(x,y)=\mathbb{E}\left[(x-\mathbb{E}[x])(y-\mathbb{E}[y])\right]$，若$\text{Cov}(x,y)=0$，则称$x,y$不相关（不相关只是说明两者没有线性关系，但是不代表有任何关系）

正交、不相关与独立之间的关系：

• 独立$\Rightarrow$不相关
• 高斯随机变量时，独立$\iff$不相关
• 当其中一个变量的均值为0时，不相关$\iff$正交，否则没关系

Kalman滤波：标量形式

考虑标量的状态方程(scalar state equation)和标量观测方程(scalar observation equation):
$$s[n]=as[n-1]+u[n]\text{\hspace{1em}(1)}$$

$$x[n]=s[n]+w[n]\text{\hspace{1em}(2)}$$

其中，我们假设$s[-1]\sim \mathcal{N}({\mu }_s,{\sigma }_s)$。$u[n]$是零均值的高斯噪声，$\mathbb{E}[u^2[n]]={\sigma }_u^2$，且 { u [ n ] } \{u[n]\} {u[n]}之间相互独立。$w[n]$是零均值的高斯噪声，$\mathbb{E}[w^2[n]]={\sigma }_n^2$，且 { w [ n ] } \{w[n]\} {w[n]}之间相互独立。为了简化过程，我们假设${\mu }_s=0$。我们要从观测值 { x [ 0 ] , x [ 1 ] , ⋯   , x [ n ] } \{x[0],x[1],\cdots,x[n]\} {x[0],x[1],⋯,x[n]}中估计出$s[n]$。我们指定基于 { x [ 0 ] , x [ 1 ] , ⋯   , x [ n ] } \{x[0],x[1],\cdots,x[n]\} {x[0],x[1],⋯,x[n]}来估计$s[n]$的估计器为$\hat{s}[n|m]$。我们的最优准则(criterion of optimality)基于最小化贝叶斯MSE(minimum Bayes MSE)，用公式表示为
$$\mathbb{E}\left[(s[n]-\hat{s}[n|n]{)}^2\right]$$

求该期望所对应的概率为联合概率密度函数$p(x[0],x[1],\cdots ,x[n],s[n])$ (在这一点上要区别于经典的MSE，经典的MSE与Bayes-MSE区别在于如何看待$s[n]$：经典的MSE是把$s[n]$看作是一个未知的参数，所以MSE求期望的是基于$p(x[0],x[1],\cdots ,x[n];s[n])$；而Bayes-MSE把$s[n]$看作是一个随机变量。)

MMSE估计器是后验均值：
$$\hat{s}[n|n]=\mathbb{E}\left[s[n]|x[0],x[1],\cdots ,x[n]\right]\text{\hspace{1em}(3)}$$

令$\theta =s[n]$和$\mathbf{x}=[x[0],x[1],\cdots ,x[n]{]}^T$是联合高斯的，所以有
$$\hat{s}[n|n]={\mathbf{C}}_{\theta x}{\mathbf{C}}_{xx}^{-1}\mathbf{x}\text{\hspace{1em}(4)}$$

因为我们假设的统计特征都是基于高斯的，所以MMSE估计器是线性的，也就与LMMSE估计器一致。

关于MMSE估计器：估计$\theta$，我们给出两个性质：

• 性质1：基于两个不相关数据向量${\mathbf{x}}_1,{\mathbf{x}}_2$，假设他们服从联合高斯分布，那么
  $$\begin{array}{rl}\hat{\theta }& =\mathbb{E}\left[\theta |{\mathbf{x}}_1,{\mathbf{x}}_2\right]\\ & =\mathbb{E}\left[\theta |{\mathbf{x}}_1\right]+\mathbb{E}\left[\theta |{\mathbf{x}}_2\right]\end{array}$$关于该性质，我们做出两种证明或解释，如下所述：
  解释1：因为$\mathbf{x}=[{\mathbf{x}}_1^T,{\mathbf{x}}_2^T{]}^T$服从高斯分布，所以
  $$\begin{array}{rl}\hat{\theta }=\mathbb{E}[\theta |\mathbf{x}]& =\mathbb{E}[\theta ]+{\mathbf{C}}_{\theta x}{\mathbf{C}}_{xx}^{-1}(\mathbf{x}-\mathbb{E}[\mathbf{x}])\\ & ={\mathbf{C}}_{\theta x}{\mathbf{C}}_{xx}^{-1}\mathbf{x}\end{array}$$因为我们假设$\mathbb{E}[\theta ]=0$，$\mathbb{E}[\mathbf{x}]=0$，这样的假设是合理的，因为我们可以在开始处理之前先减掉均值。
  考虑到${\mathbf{x}}_1,{\mathbf{x}}_2$不相关，且$\mathbb{E}[{\mathbf{x}}_1]=\mathbb{E}[{\mathbf{x}}_2]=0$，所以$\mathbb{E}[{\mathbf{x}}_1{\mathbf{x}}_2^T]=\mathbb{E}[{\mathbf{x}}_1]\mathbb{E}[{\mathbf{x}}_2^T]=0$，因此可以得到，
  $$\begin{array}{rl}{\mathbf{C}}_{xx}^{-1}& ={\left[\begin{array}{cc}{{\mathbf{C}}_{{\mathbf{x}}_1}}_{{\mathbf{x}}_1}& {{\mathbf{C}}_{{\mathbf{x}}_1}}_{{\mathbf{x}}_2}\\ {{\mathbf{C}}_{{\mathbf{x}}_2}}_{{\mathbf{x}}_1}& {{\mathbf{C}}_{{\mathbf{x}}_2}}_{{\mathbf{x}}_2}\end{array}\right]}^{-1}\\ & ={\left[\begin{array}{cc}{{\mathbf{C}}_{{\mathbf{x}}_1}}_{{\mathbf{x}}_1}& 0\\ 0& {{\mathbf{C}}_{{\mathbf{x}}_2}}_{{\mathbf{x}}_2}\end{array}\right]}^{-1}\\ & =\left[\begin{array}{cc}{\mathbf{C}}_{{\mathbf{x}}_1{\mathbf{x}}_1}^{-1}& 0\\ 0& {\mathbf{C}}_{{\mathbf{x}}_2{\mathbf{x}}_2}^{-1}\end{array}\right]\end{array}$$并且，
  $${\mathbf{C}}_{\theta x}=\mathbb{E}\left[\mathbf{\theta }{\left[\begin{array}{c}{\mathbf{x}}_1\\ {\mathbf{x}}_2\end{array}\right]}^T\right]=\left[\begin{array}{cc}{\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_1}& {\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_2}\end{array}\right]$$因此，
  $$\begin{array}{rl}\mathbf{\theta }& =\left[\begin{array}{cc}{\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_1}& {\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_2}\end{array}\right]\left[\begin{array}{cc}{\mathbf{C}}_{{\mathbf{x}}_1{\mathbf{x}}_1}^{-1}& 0\\ 0& {\mathbf{C}}_{{\mathbf{x}}_2{\mathbf{x}}_2}^{-1}\end{array}\right]\left[\begin{array}{c}{\mathbf{x}}_1\\ {\mathbf{x}}_2\end{array}\right]\\ & ={\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_1}{\mathbf{C}}_{{\mathbf{x}}_1{\mathbf{x}}_1}^{-1}{\mathbf{x}}_1+{\mathbf{C}}_{{\mathbf{\theta }\mathbf{x}}_2}{\mathbf{C}}_{{\mathbf{x}}_2{\mathbf{x}}_2}^{-1}{\mathbf{x}}_2\\ & =\mathbb{E}\left[\theta |{\mathbf{x}}_1\right]+\mathbb{E}\left[\theta |{\mathbf{x}}_2\right]\end{array}$$解释2：从线性空间的角度来看，应该会比较形象，因为$\mathbb{E}[{\mathbf{x}}_1{\mathbf{x}}_2^T]=\mathbb{E}[{\mathbf{x}}_1]\mathbb{E}[{\mathbf{x}}_2^T]=0$，我们知道${\mathbf{x}}_1$与${\mathbf{x}}_2$是相互正交的，所以可以表征为各自估计的结果的和。

• 性质2：MMSE估计器是可加的，如果$\theta ={\theta }_1+{\theta }_2$，那么
  $$\begin{array}{rl}\hat{\theta }& =\mathbb{E}[\theta |\mathbf{x}]\\ & =\mathbb{E}[{\theta }_1+{\theta }_2|\mathbf{x}]\\ & =\mathbb{E}[{\theta }_1|\mathbf{x}]+\mathbb{E}[{\theta }_2|\mathbf{x}]\end{array}$$

在描述完两个性质后，我们令$\mathbf{X}[n]=[x[0],x[1],\cdots ,x[n]{]}^T$，令$\tilde{x}[n]$为innovation(The innovation is the part of $x[n]$ that is uncorrelated with the previous samples { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}):
$$\tilde{x}[n]=x[n]-\hat{x}[n|n-1]\text{\hspace{1em}(5)}$$

这里我想强调一下为什么$\tilde{x}[n]$与 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}不相关，因为$\hat{x}[n|n-1]$是基于观测数据 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}所做的关于$x[n]$的MMSES估计，根据正交原理：估计误差$\tilde{x}[n]$与观测数据的线性组合(这里为数据本身)正交，所以得到$\tilde{x}[n]$与 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}不相关。事实上，我们可以把$\mathbf{X}[n]$和$\tilde{x}[n]$等效为集合 { x [ 0 ] , ⋯   , x [ n − 1 ] , x [ n ] } \{x[0],\cdots,x[n-1],x[n]\} {x[0],⋯,x[n−1],x[n]}，因为$x[n]$可以被恢复为：
$$\begin{array}{rl}x[n]& =\tilde{x}[n]+\hat{x}[n|n-1]\\ & =\tilde{x}[n]+\sum _{k=0}^{n-1}{a}_{k}x[k]\end{array}$$

其中$a_k$是MMSE估计器对应的相关系数，我们可以把式(3)写为：
$$\hat{s}[n|n]=\mathbb{E}\left[s[n]|\mathbf{X}[n-1],\tilde{x}[n]\right]$$

又因为$\mathbf{X}[n-1]$与$\tilde{x}[n]$不相关，根据性质1可以得到：
$$\hat{s}[n|n]=\mathbb{E}\left[s[n]|\mathbf{X}[n-1]\right]+\mathbb{E}\left[s[n]|\tilde{x}[n]\right]$$

其中，$\mathbb{E}[s[n]|\mathbf{X}[n-1]]$是基于先前观测数据对$s[n]$的预测，令其为$\hat{s}[n|n-1]$，根据式(1)和性质2，我们可以进一步得到：
$$\begin{array}{rl}\hat{s}[n|n-1]& =\mathbb{E}\left[s[n]|\mathbf{X}[n-1]\right]\\ & =\mathbb{E}\left[as[n-1]+u[n]|\mathbf{X}[n-1]\right]\\ & =a\mathbb{E}\left[s[n-1]|\mathbf{X}[n-1]\right]\\ & =a\hat{s}[n-1|n-1]\end{array}$$

因为$\mathbb{E}\left[u[n]|\mathbf{X}[n-1]\right]=0$，这是因为
$$\mathbb{E}\left[u[n]|\mathbf{X}[n-1]\right]=\mathbb{E}[u[n]]=0$$

这是因为$u[n]$独立于 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}(该独立性来源于两个方面：首先，$u[n]$独立于所有的$w[n]$；其次，$s[0],s[1],\cdots ,s[n-1]$是随机变量 { u [ 0 ] , u [ 1 ] , ⋯   , u [ n − 1 ] , s [ − 1 ] } \{u[0],u[1],\cdots,u[n-1],s[-1]\} {u[0],u[1],⋯,u[n−1],s[−1]}的线性组合，这些随机变量独立于$u[n]$)。现在，我们有
$$\hat{s}[n|n]=\hat{s}[n|n-1]+\mathbb{E}\left[s[n]|\tilde{x}[n]\right]\text{\hspace{1em}(6)}$$

其中，
$$\hat{s}[n|n-1]=a\hat{s}[n-1|n-1]$$

注意到，$\mathbb{E}\left[s[n]|\tilde{x}[n]\right]$是基于$\tilde{x}[n]$对$s[n]$的MMSE估计，因此该估计器是线性的，$\mathbb{E}\left[s[n]|\tilde{x}[n]\right]$可以被表征为：
$$\begin{array}{rl}\mathbb{E}\left[s[n]|\tilde{x}[n]\right]& =K[n]\tilde{x}[n]\\ & =K[n](x[n]-\hat{x}[n|n-1])\end{array}$$

（因为$s[n]$的均值为0，所以这里没有所谓的“截距”项），其中
$$K[n]=\frac{\mathbb{E}\left[s[n]\tilde{x}[n]\right]}{\mathbb{E}[{\tilde{x}}^2[n]]}\text{\hspace{1em}(7)}$$

上式是对$\theta ,x$联合高斯分布的MMSE估计器，即
$$\hat{\theta }=C_{\theta x}{C}_{xx}^{-1}x=\frac{\mathbb{E}[\theta x]}{\mathbb{E}[{\tilde{x}}^2[n]]}$$

又因为标量观测方程：$x[n]=s[n]+w[n]$，根据性质2，我们可以得到
$$\begin{array}{rl}\hat{x}[n|n-1]& =\hat{s}[n|n-1]+\hat{w}[n|n-1]\\ & =\hat{s}[n|n-1]\end{array}$$

根据式(6)，我们知道
$$\hat{s}[n|n]=\hat{s}[n|n-1]+K[n](x[n]-\hat{s}[n|n-1])\text{\hspace{1em}(8)}$$

其中
$$\hat{s}[n|n-1]=a\hat{s}[n-1|n-1]\text{\hspace{1em}(9)}$$

现在只剩增益因子$K[n]$需要决定，根据式(7)，我们知道
$$K[n]=\frac{\mathbb{E}\left[s[n](x[n]-\hat{s}[n|n-1])\right]}{\mathbb{E}\left[(x[n]-\hat{s}[n|n-1]{)}^2\right]}\text{\hspace{1em}(10)}$$

为了进一步完善$K[n]$，我们先给出两个结论：

• 1. $\mathbb{E}\left[s[n](x[n]-\hat{s}[n|n-1])\right]=\mathbb{E}\left[(s[n]-\hat{s}[n|n-1])(x[n]-\hat{s}[n|n-1])\right]$
• 2. $\mathbb{E}\left[w[n]\left(s[n]-\hat{s}[n|n-1]\right)\right]=0$

第一个结论是因为
$$\begin{array}{rl}\tilde{x}[n]& =x[n]-\hat{x}[n|n-1]\\ & =x[n]-\hat{s}[n|n-1]\end{array}\text{\hspace{1em}(11)}$$

与之前的观测数据 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}不相关，必然也就与$\hat{s}[n|n-1]$(为 { x [ 0 ] , ⋯   , x [ n − 1 ] } \{x[0],\cdots,x[n-1]\} {x[0],⋯,x[n−1]}的线性组合)不相关，因此$\mathbb{E}[\hat{s}[n|n-1](x[n]-\hat{s}[n|n-1])]=0$，也就得到了结论1。第二个结论比较直接，这里不做解释。把这两个结论代入到式$(10)$中，增益因子变为：
$$\begin{array}{rl}K[n]& =\frac{\mathbb{E}\left[(s[n]-\hat{s}[n|n-1])(x[n]-\hat{s}[n|n-1])\right]}{\mathbb{E}\left[{\left(s[n]-\hat{s}[n|n-1]+w[n]\right)}^2\right]}\\ & =\frac{\mathbb{E}\left[(s[n]-\hat{s}[n|n-1]{)}^2\right]}{{\sigma }_n^2+\mathbb{E}\left[(s[n]-\hat{s}[n|n-1]{)}^2\right]}\end{array}\text{\hspace{1em}(12)}$$

上式的分子变为平方项是因为$x[n]=s[n]+w[n]$，而$w[n]$独立于$s[n]$和$\hat{s}[n|n-1]$。另外，注意到，分子项$\mathbb{E}\left[(s[n]-\hat{s}[n|n-1]{)}^2\right]$就是基于先前观测数据MMSE估计所对应的最小MSE，记为$M[n|n-1]$，那么
$$K[n]=\frac{M[n|n-1]}{{\sigma }_n^2+M[n|n-1]}\text{\hspace{1em}(13)}$$

因为$s[n]=as[n-1]+u[n],\hat{s}[n|n-1]=a\hat{s}[n-1|n-1]$，我们有
$$\begin{array}{rl}M[n|n-1]& =\mathbb{E}\left[(s[n]-\hat{s}[n|n-1]{)}^2\right]\\ & =\mathbb{E}\left[(as[n-1]+u[n]-\hat{s}[n|n-1]{)}^2\right]\\ & =\mathbb{E}\left[{\left(a(s[n-1]-\hat{s}[n-1|n-1])+u[n]\right)}^2\right]\end{array}$$

不难发现，
$$\mathbb{E}\left[\left(s[n-1]-\hat{s}[n-1|n-1]\right)u[n]\right]=0$$

因此，我们可以得到
$$M[n|n-1]=a^{2}M[n-1|n-1]+{\sigma }_u^2$$

最终，我们需要对$M[n|n]$进行迭代，利用式(8)：$\hat{s}[n|n]=\hat{s}[n|n-1]+K[n](x[n]-\hat{s}[n|n-1])$，我们有
$$\begin{array}{rl}M[n|n]& =\mathbb{E}\left[(s[n]-\hat{s}[n|n]{)}^2\right]\\ & =\mathbb{E}\left[{\left(s[n]-\hat{s}[n|n-1]-K[n](x[n]-\hat{s}[n|n-1])\right)}^2\right]\\ & =\mathbb{E}\left[(s[n]-\hat{s}[n|n-1]{)}^2\right]-2K[n]\cdot \mathbb{E}\left[(s[n]-\hat{s}[n|n-1])(x[n]-\hat{s}[n|n-1])\right]\\ & +K^2[n]\cdot \mathbb{E}\left[(x[n]-\hat{s}[n|n-1]{)}^2\right]\end{array}$$

注意到，第二项的期望就是式(12)中$K[n]$的分子，最后一项的期望是$K[n]$的分母项，得到
$$\mathbb{E}\left[(s[n]-\hat{s}[n|n-1])(x[n]-\hat{s}[n|n-1])\right]=K[n](M[n|n-1]+{\sigma }_n^2)$$

$$\mathbb{E}\left[(x[n]-\hat{s}[n|n-1]{)}^2\right]=\frac{M[n|n-1]}{K[n]}$$

因此，
$$\begin{array}{rl}M[n|n]& =M[n|n-1]-2K^2[n](M[n|n-1]+{\sigma }_n^2)+K[n]M[n|n-1]\\ & =M[n|n-1]-2K[n]M[n|n-1]+K[n]M[n|n-1]\\ & =(1-K[n])M[n|n-1]\end{array}$$

至此，我们完成了标量形式Kalman滤波的推导，总结为：$\forall n\ge 0$，
Prediction:
$$\hat{s}[n|n-1]=a\hat{s}[n-1|n-1]\text{\hspace{1em}(14)}$$

Minimum Prediction MSE:
$$M[n|n-1]=a^{2}M[n-1|n-1]+{\sigma }_u^2\text{\hspace{1em}(15)}$$

Kalman Gain:
$$K[n]=\frac{M[n|n-1]}{{\sigma }_n^2+M[n|n-1]}\text{\hspace{1em}(16)}$$

Correction:
$$\hat{s}[n|n]=\hat{s}[n|n-1]+K[n](x[n]-\hat{s}[n|n-1])\text{\hspace{1em}(17)}$$

Minimum MSE:
$$M[n|n]=(1-K[n])M[n|n-1]\text{\hspace{1em}(18)}$$

回顾之前的推导，我们知道均值为0的假设（包括${\mu }_s=0,\mathbb{E}[s[n]]=0$）是为了利用正交性原理，但事实上，即使${\mu }_s\ne 0$，最终得到的公式与(14-18)式是一致的。在初始化过程中，我们使用$\hat{s}[-1|-1]=\mathbb{E}[s[-1]]={\mu }_s$和$M[-1|-1]={\sigma }_s^2$，因为这是没有观测数据之前所能掌握的数据。另外，我们可以把增益部分的估计视为对$u[n]$的估计$\hat{u}[n]$，公式表征为：
$$\hat{s}[n|n]=a\hat{s}[n-1|n-1]+\hat{u}[n]$$

其中$\hat{u}[n]=K[n](x[n]-\hat{s}[n|n-1])$，某种程度上来说，该估计可以认为是对$u[n]$的估计，所以合理地认为$\hat{s}[n|n]\approx s[n]$。