Kalman滤波(Part-1：信号模型基础)

Kalman Filters

Dynamical Signal Models

一阶高斯-马尔可夫过程(first-order Gauss-Markov process):描述采样点之间（相邻）的相关性：
$$s[n]=as[n-1]+u[n]\text{\hspace{1em}(1)}$$

其中$u[n]$是高斯白噪声(White Gaussian Noise, WGN)，方差为${\sigma }_u^2$，$s[-1]\sim \mathcal{N}({\mu }_s,{\sigma }_s^2)$，$s[-1]$与$u[n]$相互独立($\forall n\ge 0$)。该模型也常被称为dynamical model / state model

将$s[n]$表示为初始条件$s[-1]$的函数形式：
$$\begin{array}{rl}s[0]& =as[-1]+u[0]\\ s[1]& =as[0]+u[1]\\ & =a^{2}s[-1]+au[0]+u[1]\\ & \text{etc.}\end{array}$$

一般地，我们有
$$s[n]=a^{n+1}s[-1]+\sum _{k=0}^n{a}^{k}u[n-k]\text{\hspace{1em}(2)}$$

高斯随机过程(Gaussian random process):给定$k$，对任意的采样点 { s [ n 1 ] , ⋯   , s [ n k ] } \{s[n_1],\cdots,s[n_k]\} {s[n1​],⋯,s[nk​]}，$k$维随机向量$\mathbf{s}=[s[n_1],\cdots ,s[n_k]{]}^T$的分布为高维的高斯PDF，就认为$s[n]$是一个高斯随机过程。

因为随机变量$s[-1]$和$u[\cdot ]$都是高斯随机变量并且相互独立，不难看出，$s[n]$是一个高斯随机过程。另外，
$$\mathbb{E}[s[n]]=a^{n+1}\mathbb{E}[s[-1]]=a^{n+1}{\mu }_s$$

采样点$s[m]$与$s[n]$之间的协方差为：
$$\begin{array}{rl}{c}_s[m,n]& =\mathbb{E}\left[(s[m]-\mathbb{E}[s[m]])(s[n]-\mathbb{E}[s[n]])\right]\\ & =\mathbb{E}\left[\left(a^{m+1}(s[-1]-{\mu }_s)+\sum _{k=0}^m{a}^{k}u[m-k]\right)\cdot \left(a^{n+1}(s[-1]-{\mu }_s)+\sum _{l=0}^n{a}^{l}u[n-l]\right)\right]\\ & =a^{m+n+2}{\sigma }_s^2+\sum _{k=0}^m\sum _{l=0}^n{a}^{k+l}\mathbb{E}[u[m-k]u[n-l]]\end{array}$$

但是
$$\mathbb{E}[u[m-k]u[n-l]]={\sigma }_u^2\delta [k-(l+m-n)]$$

因此，当$m\ge n$时
$$\begin{array}{rl}{c}_s[m,n]& =a^{m+n+2}{\sigma }_s^2+{\sigma }_u^2{a}^{m-n}\sum _{l=0}^n{a}^{2l}\end{array}$$

当$m<n$时，$c_s[m,n]=c_s[n,m]$。基于上述协方差，可以得到方差为
$$\begin{array}{rl}\text{var}[s[n]]& =c_s[n,n]\\ & =a^{2n+2}{\sigma }_s^2+{\sigma }_u^2\sum _{l=0}^n{a}^{2l}\end{array}$$

显然，因为$\mathbb{E}[s[n]]=a^{n+1}{\mu }_s$与$n$相关，且协方差与$m,n$相关，因此$s[n]$不是一个广义平稳过程(Wide-sense stationary, WSS)。然而，当$n\to \infty$时
$$\begin{array}{rl}\mathbb{E}[s[n]]& \to 0\\ c_s[m,n]& \to \frac{{\sigma }_u^2{a}^{m-n}}{1-a^2}\end{array}$$

因为$|a|<1$（该条件对于整个过程的稳定是必要的，否则，均值和方差将会随着$n$呈指数形式增长）

因为高斯-马尔可夫过程的特殊形式，其均值和方差也可以被迭代地表征(式(3,4)被称为均值与方差传播公式)
$$\mathbb{E}[s[n]]=a\mathbb{E}[s[n-1]]\text{\hspace{1em}(3)}$$

$$\begin{array}{rl}\text{var}[s[n]]& =\mathbb{E}\left[(s[n]-\mathbb{E}[s[n]{]}^2{)}^2\right]\\ & =\mathbb{E}\left[{\left(as[n-1]+u[n]-a\mathbb{E}[s[n-1]]\right)}^2\right]\\ & =a^2\text{var}[s[n-1]]+{\sigma }_u^2\end{array}\text{\hspace{1em}(4)}$$

其中我们使用了$\mathbb{E}[u[n]s[n-1]]=0$，这是因为$s[n-1]$只取决于 { s [ − 1 ] , u [ 0 ] , ⋯   , u [ n − 1 ] } \{s[-1], u[0], \cdots, u[n-1]\} {s[−1],u[0],⋯,u[n−1]}，且这些随机变量独立于$u[n]$。注意到，在式（4）中，第一项$as[n-1]$会造成方差减小，第二项的积累${\sigma }_u^2$会造成方差增大，在达到稳态(steady state)后，或者$n\to \infty$，两项的作用相互平衡，收敛为${\sigma }_u^2/(1-a^2)$.

考虑一个$p$阶的高斯-马尔可夫过程：
$$s[n]=-\sum _{k=1}^{p}a[k]s[n-k]+u[n]\text{\hspace{1em}(5)}$$

因为$s[n]$取决于前$p$个采样点，所以均值和方差传播式变得更加复杂。为了拓展之前的结论，我们指定 { s [ n − 1 ] , s [ n − 2 ] , ⋯   , s [ n − p ] } \{s[n-1],s[n-2],\cdots,s[n-p]\} {s[n−1],s[n−2],⋯,s[n−p]}为采样时刻$n$的系统状态(system state)，我们定义状态向量：
$$\mathbf{s}\left[n-1\right]=\left[\begin{array}{c}\begin{array}{c}s\left[n-p\right]\\ s\left[n-p+1\right]\end{array}\\ ⋮\\ s\left[n-1\right]\end{array}\right]\text{\hspace{1em}(6)}$$

我们可以把式(10)写为
$$\left[\begin{array}{c}\begin{array}{c}s\left[n-p+1\right]\\ s\left[n-p+2\right]\end{array}\\ ⋮\\ \begin{array}{c}s\left[n-1\right]\\ s\left[n\right]\end{array}\end{array}\right]=\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{ccccc}0& 1& 0& \cdots & 0\\ 0& 0& 1& \cdots & 0\\ 0& 0& 0& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ -a\left[p\right]& -a\left[p-1\right]& -a\left[p-2\right]& \cdots & -a\left[1\right]\end{array}\right]}}\left[\begin{array}{c}\begin{array}{c}s\left[n-p\right]\\ s\left[n-p+1\right]\end{array}\\ ⋮\\ \begin{array}{c}s\left[n-2\right]\\ s\left[n-1\right]\end{array}\end{array}\right]+\underset{\mathbf{B}}{\underbrace{\left[\begin{array}{c}0\\ 0\\ ⋮\\ 1\end{array}\right]}}u\left[n\right]$$

其中的前$(p-1)$个方程为方阵，根据定义，将上式写为状态向量的形式：
$$\mathbf{s}[n]=\mathbf{A}\mathbf{s}[n-1]+\mathbf{B}\mathbf{u}[n]\text{\hspace{1em}(7)}$$

其中$\mathbf{A}$是一个$p\times p$的非奇异矩阵（称为状态转移矩阵：state transition matrix），$\mathbf{B}$是一个$p\times 1$的向量。式(7)的形式被称为向量高斯-马尔可夫模型(Vector Gauss-Markov Model)。更一般的模型可表示为，
$$\mathbf{s}[n]=\mathbf{A}\mathbf{s}[n-1]+\mathbf{B}\mathbf{u}[n]\text{\hspace{1em}(8)}$$

其中$\mathbf{A},\mathbf{B}$都是固定的矩阵，$\mathbf{A}$的维度为$p\times p$，$\mathbf{B}$的维度为$p\times r$。$\mathbf{s}[n]$是一个$p\times 1$的信号向量，$\mathbf{u}[n]$是一个驱动噪声矢量(driving noise vector)。我们称式(8)为状态模型(state model)，该模型的统计假设有：

• 输入的$\mathbf{u}[n]$是高斯白噪声，q向量，即$\mathbf{u}[n]$是一个不相关的联合高斯分布的序列，且$\mathbb{E}[\mathbf{u}[n]]=0$，
  $$\mathbb{E}[\mathbf{u}[m]{\mathbf{u}}^T[n]]=0,m\ne n$$ $\mathbf{u}[n]$的协方差为：
  $$\mathbb{E}[\mathbf{u}[n]{\mathbf{u}}^T[n]]=\mathbf{Q}$$ 其中$\mathbf{Q}$ 是一个$r\times r$的正定矩阵。

• 初始状态$\mathbf{s}[-1]$是随机向量: $$\mathbf{s}[-1]\sim \mathcal{N}({\mathbf{\mu }}_s,{\mathbf{C}}_s)$$独立于$\mathbf{u}[n],\forall n\ge 0$

我们进一步推导向量高斯-马尔可夫模型的统计特征（标量模型的扩展），依据式（8），
$$\begin{array}{rl}\mathbf{s}[0]& =\mathbf{A}\mathbf{s}[-1]+\mathbf{B}\mathbf{u}[0]\\ \mathbf{s}[1]& =\mathbf{A}\mathbf{s}[0]+\mathbf{B}\mathbf{u}[1]\\ & ={\mathbf{A}}^2\mathbf{s}[-1]+\mathbf{A}\mathbf{B}\mathbf{u}[0]+\mathbf{B}\mathbf{u}[1]\\ & \text{etc.}\end{array}$$

一般地，我们可以推广得到
$$\mathbf{s}[n]={\mathbf{A}}^{n+1}\mathbf{s}[-1]+\sum _{k=0}^n{\mathbf{A}}^k\mathbf{B}\mathbf{u}[n-k]$$

其中${\mathbf{A}}^0=\mathbf{I}$，可以看出，$\mathbf{s}[n]$初始条件$\mathbf{s}[-1]$和$\mathbf{u}[\cdot ]$的线性组合，因此，$\mathbf{s}[n]$是一个高斯随机过程，那么就只需要决定其均值和方差。
$$\mathbb{E}[\mathbf{s}[n]]={\mathbf{A}}^{n+1}\mathbb{E}[\mathbf{s}[-1]]={\mathbf{A}}^{n+1}{\mathbf{\mu }}_s\text{\hspace{1em}(9)}$$

其协方差：
$$\begin{array}{rl}{\mathbf{C}}_s[m,n]& =\mathbb{E}\left[\left(\mathbf{s}[m]-\mathbb{E}[\mathbf{s}[m]]\right){\left(\mathbf{s}[n]-\mathbb{E}[\mathbf{s}[n]]\right)}^T\right]\\ & =\mathbb{E}\left[\left({\mathbf{A}}^{m+1}(\mathbf{s}[-1]-{\mathbf{\mu }}_s)+\sum _{k=0}^m{\mathbf{A}}^k\mathbf{B}\mathbf{u}[m-k]\right)\cdot {\left({\mathbf{A}}^{n+1}(\mathbf{s}[-1]-{\mathbf{\mu }}_s)+\sum _{l=0}^n{\mathbf{A}}^l\mathbf{B}\mathbf{u}[n-l]\right)}^T\right]\\ & ={\mathbf{A}}^{m+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}+\sum _{k=0}^m\sum _{l=0}^n{\mathbf{A}}^k\mathbf{B}\mathbb{E}\left[\mathbf{u}[m-k]{\mathbf{u}}^T[n-l]\right]{\mathbf{B}}^T{\mathbf{A}}^{l^T}\end{array}$$

注意到，
$$\mathbb{E}\left[\mathbf{u}[m-k]{\mathbf{u}}^T[n-l]\right]=\mathbf{Q}\delta [l-(n-m+k)]$$

因此，当$m\ge n$时，
$${\mathbf{C}}_s[m,n]={\mathbf{A}}^{m+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}+\sum _{l=0}^n{\mathbf{A}}^{l+m-n}{\mathbf{B}\mathbf{Q}\mathbf{B}}^T{\mathbf{A}}^{l^T}\text{\hspace{1em}(10)}$$

当$m<n$时，
$${\mathbf{C}}_s[m,n]={\mathbf{C}}_s^T[n,m]$$

那么协方差矩阵可以表示为：
$$\begin{array}{rl}\mathbf{C}[n]& ={\mathbf{C}}_s[n,n]\\ & ={\mathbf{A}}^{n+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}+\sum _{k=0}^n{\mathbf{A}}^k{\mathbf{B}\mathbf{Q}\mathbf{B}}^T{\mathbf{A}}^{k^T}\end{array}\text{\hspace{1em}(11)}$$

期望和方差的传播方程可以写为：
$$\mathbf{E}[\mathbf{s}[n]]=\mathbf{A}\mathbf{E}[\mathbf{s}[n-1]]\text{\hspace{1em}(12)}$$

$$\mathbf{C}[n]=\mathbf{A}\mathbf{C}[n-1]{\mathbf{A}}^T+{\mathbf{B}\mathbf{Q}\mathbf{B}}^T$$

注意，只有当$\mathbf{A}$的特征值幅度都小于1，才是一个稳定的过程(steady process)。

当$n\to \infty$时，
$$\mathbb{E}[\mathbf{s}[n]]={\mathbf{A}}^{n+1}{\mathbf{\mu }}_s\to 0$$

$${\mathbf{A}}^{n+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}\to 0$$

因此，
$$\mathbf{C}[n]\to \mathbf{C}=\sum _{k=0}^{\infty }{\mathbf{A}}^k{\mathbf{B}\mathbf{Q}\mathbf{B}}^T{\mathbf{A}}^{k^T}\text{\hspace{1em}(13)}$$

另外，当$n\to \infty$，$\mathbf{C}[n-1]=\mathbf{C}[n]$，那么稳态的协方差矩阵为方程(14)的解：
$$\mathbf{C}={\mathbf{A}\mathbf{C}\mathbf{A}}^T+{\mathbf{B}\mathbf{Q}\mathbf{B}}^T\text{\hspace{1em}(14)}$$

该方程被称为Lyapunov equation.

将上述模型和定理总结如下：

定理-1：向量高斯马尔可夫模型(Vector Gauss-Markov Model)：对一个$p\times 1$的信号向量$\mathbf{s}[n]$，其高斯-马尔可夫模型为：
$$\mathbf{s}[n]=\mathbf{A}\mathbf{s}[n-1]+\mathbf{B}\mathbf{u}[n],n\ge 0\text{\hspace{1em}(15)}$$

$\mathbf{A}(p\times p)$和$\mathbf{B}(p\times r)$已知，假设$\mathbf{A}$的特征值幅度小于1，$\mathbf{u}[n](r\times 1)$为高斯白噪声向量，$\mathbf{u}[n]\sim \mathcal{N}(0,\mathbf{Q})$且 { u [ n ] } \{\boldsymbol{u}[n]\} {u[n]}之间相互独立。初始条件$\mathbf{s}[-1]\sim \mathcal{N}({\mathbf{\mu }}_s,{\mathbf{C}}_s)$，独立于 { u [ n ] } \{\boldsymbol{u}[n]\} {u[n]}，那么该信号过程是高斯的，且其均值为
$$\mathbb{E}[\mathbf{s}[n]]={\mathbf{A}}^{n+1}{\mathbf{\mu }}_s\text{\hspace{1em}(16)}$$

当$m\ge n$时，协方差为
$${\mathbf{C}}_s[m,n]={\mathbf{A}}^{m+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}+\sum _{l=0}^n{\mathbf{A}}^{l+m-n}{\mathbf{B}\mathbf{Q}\mathbf{B}}^T{\mathbf{A}}^{l^T}\text{\hspace{1em}(17)}$$

当$m<n$时，
$${\mathbf{C}}_s[m,n]={\mathbf{C}}_s^T[n,m]$$

那么协方差矩阵可以表示为：
$$\begin{array}{rl}\mathbf{C}[n]& ={\mathbf{C}}_s[n,n]\\ & ={\mathbf{A}}^{n+1}{\mathbf{C}}_s{\mathbf{A}}^{{n+1}^T}+\sum _{k=0}^n{\mathbf{A}}^k{\mathbf{B}\mathbf{Q}\mathbf{B}}^T{\mathbf{A}}^{k^T}\end{array}\text{\hspace{1em}(18)}$$

期望和方差的传播方程可以写为：
$$\mathbf{E}[\mathbf{s}[n]]=\mathbf{A}\mathbf{E}[\mathbf{s}[n-1]]\text{\hspace{1em}(19)}$$

$$\mathbf{C}[n]=\mathbf{A}\mathbf{C}[n-1]{\mathbf{A}}^T+{\mathbf{B}\mathbf{Q}\mathbf{B}}^T\text{\hspace{1em}(20)}$$