第三章 卡尔曼滤波

3.1 引言

卡尔曼滤（Kalman）滤波和维纳（Wiener）滤波都是以最小均方误差为准则的最佳线性估计或滤波。但是，维纳滤波自由适用于平稳随机信号，而卡尔曼滤波则没有这个限制，这是它们最大的区别。另外，在处理方法上，它们也有很大不同。维纳滤波是根据全部过去和当前观测数据 $x(n),x(n-1),\cdots$ 来估计信号的当前值，它的解是以均方误差最小条件下所得到的系统函数 $H(z)$ 或冲激响应 $h(n)$ 的形式给出的；而卡尔曼滤波则不需要全部的过去观测数据，它只是根据前一个估计值 $({\hat{x}}_{k-1})$ 和最近一个观测数据 $(y_k)$ 来估计信号的当前值。它是用状态方程和递推方程进行估计的，而且所得的解是以估计值的形式给出的。

两种方法的主要区别

序号	维纳滤波	卡尔曼滤波
1	给出的是平稳太的 z 域解 $H_{opt}(z)$	给出的是迭代得到的时域解
2	只用在标量上	可以用在矢量上，也适用于多输入输出
3	只能用在平稳随机过程中	可以用在变化不快的的非平稳信号

3.2 卡尔曼滤波器的信号模型

基本概念：根据前一个估计值和当前的观测值对信号（状态、变量）做递推估计。

这里解释一下（状态、变量），如下 $A,B,C,D$ 均为系数，可以是矩阵（变换矩阵）；$X(n)$ 为输入， $Y(n)$ 为观测值。
$$\begin{array}{rl}W(n+1)& =AW(n)+BX(n)状态方程/动态方程\\ Y(n)& =CW(n)+DX(n)观测方程/代数方程\end{array}$$
卡尔曼滤波的核心思想：系统内部有很多节点，如果已知输入信号和 $n_0$ 时刻的一组节点的变量值，就可以计算出 $n\ge n_0$ 时刻输出信号以及系统内部任意节点变量值，这样的一组最少的节点变量值就是状态变量（不唯一）。

模型分析

离散时间系统的状态方程定义为
$$x(k+1)=Ax(k)+Be(k)$$
$x(k)$ 表示一组状态变量组成的多维状态向量，$e(k)$ 是激励信号， $A,B$ 都是由系统的结构确定的矩阵。

当已知初始状态 $x(0)$ ，可以用递推方法得到它的解 $x(k)$ ：
$$\begin{array}{rl}x(1)& =Ax(0)+Be(0)\\ x(2)& =Ax(1)+Be(1)=A^{2}x(0)+ABe(0)+Be(1)\\ & \\ & ⋮\\ x(k)& =A^{k}x(0)+\sum _{j=0}^{k-1}{A}^{k-1-j}Be(j)\end{array}$$

上式中展开式中等号右边第一项只与初始状态和系统结构有关，与激励 $e(\cdot )$ 无关，故称为零输入响应；第二项与初始状态无关，只与激励和系统结构有关，故称为零状态响应。

令 $\Phi (k)=A^k$ ，并代入上式，得
$$x(k)=\Phi (k)x(0)+\sum _{j=0}^{k-1}\Phi (k-1-j)Be(j)$$
当 $e(k)=0$ 时，$x(k)=\Phi (k)x(0)=A^{k}x(k)$ 。通过 $A^k$ ，可以将 $k=0$ 时的状态过渡到任何 $k>0$ 的状态，故称 $\Phi (k)$ 为转移矩阵或过渡矩阵。当已知 $x(0)$ ，激励 $e(j)$ 以及 $A,B$ 矩阵，就可以根据上式求得 $x(k)$ 的解。若用 $k_0$ 表示 $k$ 的起始值，则从 $x(k_0)$ 开始递推，会有
$$x(k)={\Phi }_{k,k_0}x(k_0)+\sum _{j=k_0}^{k-1}{\Phi }_{k,j+1}Be(j)$$
式中 ${\Phi }_{k,k_0}$ 表示从 $k_0$ 状态到 $k$ 状态的转移矩阵。若 $k_0=k-1$ ，就可以得到一步递推公式
$$\begin{array}{rl}x(k)& ={\Phi }_{k,k-1}x(k_0)+\Phi (0)Be(k-1)\\ & ={\Phi }_{k,k-1}x(k_0)+Be(k-1)\end{array}$$
假设激励源为白噪声，即 $Be(k-1)=w(k-1)$ 称为系统动态噪声，而系统是时变的，即 ${\Phi }_{k,k-1}=A(k)$ 则上式又可改写成
$$x(k)=A(k)x(k-1)+w(k-1)或x(k)=A_k{x}_{k-1}+w_{k-1}$$
上式表明 $k$ 时刻的状态 $x(k)$ 可由它前一时刻状态 $x(k-1)$ 来求得，故该式又称递推状态方程。

卡尔曼滤波需要依据观测数据对系统状态进行估计，因此，除了需要建立系统的状态方程，还需要建立一个观测方程。一般假设观测系统是线性的，离散时间系统的观测方程
$$y_k=c_k{x}_k+v_k$$
式中 $y_k$ 为观测到的信号矢量序列，$v_k$ 为观测噪声序列，$c_k$ 为观测矩阵 $(m\times n)$ ，$m$ 为 $y_k$ 的维数，$n$ 为 $v_k$ 的维数，$c_k{x}_k$ 是信号真值，它是状态变量 $x_k$ 各分量的线性组合，即
$$s_k=c_k{x}_k$$
将上式代入观测方程
$$y_k=s_k+v_k$$

3.3 卡尔曼滤波算法

我们要研究离散系统的 $n$ 维状态方程与 $m$ 维观测方程，如下
$$\begin{array}{rl}{x}_k& =A_k{x}_{k-1}+w_{k-1}\\ y_k& =c_k{x}_k+v_k\end{array}$$
假设动态噪声 $w_k$ 与观测噪声 $v_k$ 都是均值为零的正态噪声，且两者互不相关，即
$$\begin{array}{rl}& E[w_k]=0,cov[w_k,w_j]=E[w_k{w}_j^T]=Q_k{\delta }_{kj}\\ & E[v_k]=0,cov[v_k,vj]=E[v_k{v}_j^T]=R_k{\delta }_{kj}\\ & cov[w_k,v_j]=E[w_k{v}_j^T]=0,k,j=0,1,2,\cdots \end{array}$$
滤波条件下的卡尔曼算法
暂时不考虑噪声 $w_k$ 与 $v_k$ 从状态方程和观测方程可以得到的 $x_k$ 和 $y_k$ 分别用 ${\hat{x}}_k^{{}^{\prime }}$ 和 ${\hat{y}}_k^{{}^{\prime }}$ 表示，则有
$$\begin{array}{rl}{\hat{x}}_k^{{}^{\prime }}& =A_k{\hat{x}}_{k-1}\\ {\hat{y}}_k^{{}^{\prime }}& =C_k{\hat{x}}_k^{{}^{\prime }}=C_k{A}_k{\hat{x}}_{k-1}\end{array}$$
其中 ${\hat{x}}_{k-1}$ 是 $x_{k-1}$ 的估计值。实际观测值 $y_k$ 和估计值 ${\hat{y}}_k^{{}^{\prime }}$ 的差用 ${\tilde{y}}_k=y_k-{\hat{y}}_k^{{}^{\prime }}$ 由于 $w_{k-1}$ 和 $v_k$ 的影响而产生了偏移 ${\tilde{y}}_k$ ，${\tilde{y}}_k$ 称为新息，它有以下重要性质：

性质1：新息序列是相互正交的序列，即
$$E[{\tilde{y}}_n{\tilde{y}}_k]=0$$
性质2：$n$ 时刻的新息 ${\tilde{y}}_n$ 与过去所有的观测数据正交，即
$$E[{\tilde{y}}_n{y}_k]=0,1\le k\le n-1$$
性质3：表示观测数据的随机序列 { y 1 , y 2 , ⋯   , y n } \{y_1,y_2,\cdots,y_n\} {y1​,y2​,⋯,yn​} 与表示新息过程的随机序列 { y ~ 1 , y ~ 2 , ⋯   , y ~ n } \{\widetilde{y}_1,\widetilde{y}_2,\cdots,\widetilde{y}_n\} {y ​1​,y ​2​,⋯,y ​n​} 一一对应，即
{ y 1 , y 2 , ⋯   , y n } ∼ { y ~ 1 , y ~ 2 , ⋯   , y ~ n } \{y_1,y_2,\cdots,y_n\} \sim \{\widetilde{y}_1,\widetilde{y}_2,\cdots,\widetilde{y}_n\} {y1​,y2​,⋯,yn​}∼{y ​1​,y ​2​,⋯,y ​n​}

为了获得最优估计值，引入一系数矩阵 $H_k$ 来矫正估计值 ${\hat{x}}_k^{{}^{\prime }}$ ，使得均方误差 $P_k=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$ 最小，这样能得到更好的估计值。
$$\begin{gathered} {\hat{x}}_k=A_k{\hat{x}}_{k-1}+H_k(y_k-{\hat{y}}_k^{{}^{\prime }})=A_k{\hat{x}}_{k-1}+H_k(y_k-C_k{A}_k{\hat{x}}_{k-1}) \\ \begin{array}{rlr}& A_k{\hat{x}}_{k-1}& 预测值\\ & H_k& 加权矩阵\\ & y_k-C_k{A}_k{\hat{x}}_{k-1}& 新息\end{array} \end{gathered}$$
填空：求 $H_k$ ，使 $P_k$ 最小，最优增益 $H_k$ 的选取与 $w_k,v_k$ 的统计特性相关，$H_k$ 与 $Q_{k-1}$ 成正比，与 $R_k$ 成反比。

说明：

如果 $Q_{k-1}$ 增大，说明状态方程中的噪声影响会变大，预测值偏差可能会变大，需要加强新息的修正能力 $H_k$ 对应增大，成正比。

如果 $R_k$ 增大，说明观测方程中的噪声影响会变大，观测值的修正能力下降，故需要下调 $H_k$ ，成反比。

算法总结

1.状态方程：
$$x_k=A_k{x}_{k-1}+w_{k-1}$$
2.测量方程：
$$y_k=c_k{x}_k+v_k$$

3.统计特性：
$$\begin{array}{rl}& E[w_k]=0,cov[w_k,w_j]=E[w_k{w}_j^T]=Q_k{\delta }_{kj}\\ & E[v_k]=0,cov[v_k,vj]=E[v_k{v}_j^T]=R_k{\delta }_{kj}\\ & cov[w_k,v_j]=E[w_k{v}_j^T]=0,k,j=0,1,2,\cdots \end{array}$$
4.初始条件：
$$\begin{array}{rl}& E[x_0]={\mu }_0,var[x_0]=E[(x_0-{\mu }_0)(x_0-{\mu }_0{)}^T]=p_0\\ & cov[x_0,w_j]=E[x_0{w}_k^T]=0\\ & cov[x_0,v_j]=E[x_0{v}_k^T]=0\end{array}$$

5.递推公式：
$${\hat{x}}_k=A_k{\hat{x}}_{k-1}+H_k(y_k-C_k{A}_k{\hat{x}}_{k-1})$$

6.增益方程：
$$H_k=P_k^{{}^{\prime }}{C}_k^T(C_k{P}_k^{{}^{\prime }}{C}_k^T+R_k{)}^{-1}$$

7.均方误差：
$$\begin{array}{rlr}& P_k^{{}^{\prime }}=A_k{P}_{k-1}{A}_k^T+Q_{k-1}& 未考虑噪声\\ & P_k=(I-H_k{C}_k)P_k^{{}^{\prime }}& 滤波的\end{array}$$

---

例题 3.1 设 $x_k$ 与 $y_k$ 为实离散时间随机过程，具有功率谱密度
$$\begin{gathered} {\Phi }_{xx}(z)=\frac{0.36}{(1-0.8z^{-1})(1-0.8z)} \\ {\Phi }_{vv}(z)=1,{\Phi }_{vx}(z)=0 \end{gathered}$$
并已知 ${\hat{x}}_{-1}=0,P_0=var[x_0]=1$ ，在 $k=0$ 时开始观测信号 $y_k(y_k=x_k+v_k)$ ，试用卡尔曼滤波公式求 ${\hat{x}}_k$ 。

解：由 ${\Phi }_{xx}(z)$ 可因式分解成如下形式
$${\Phi }_{xx}(z)=0.36\frac{z^{-1}}{1-0.8z^{-1}}\cdot \frac{z}{1-0.8z}$$

注 ：这里的分解不能分解成下面这种形式
$${\Phi }_{xx}(z)=0.36\frac{1}{1-0.8z^{-1}}\cdot \frac{1}{1-0.8z}$$
因为如果分解成这种形式那么下面推导出来的观测方程中 $w$ 前会多一个系数 $z$ 导致 $w_{k-1}$ 时移变成 $w_k$ ！！！

所以有
$${\sigma }_w^2=0.36,B(z)=\frac{z^{-1}}{1-0.8z^{-1}},B(z^{-1})=\frac{z}{1-0.8z}$$
有 $B(z)$ 对应的表达式可以推出时域方程
$$\begin{gathered} x_k=0.8x_{k-1}+w_{k-1} \\ {A}_k=0.8 \end{gathered}$$

注 ：这里分析方法式这样的
$$X(z)=B(z)W(z)$$
得到
$$(1-0.8z^{-1})X(z)=z^{-1}W(z)$$
从而得到时域表达式。

但是！！！！ 注意没有 $X(z)$ 这个东西，$x(k)$ 是一个无始无终的序列不存在 z 变换，这步不能出现在试卷上！！！ 这步不能出现在试卷上！！！ 这步不能出现在试卷上！！！

根据 $y_k=x_k+v_k$ 以及 ${\Phi }_{xx}(z)$ d得
$$C_k=1,Q_k=0.36,R_k=var[v_k]={\Phi }_{vv}(0)=1$$
将上式代入卡尔曼滤波算法得
$$\begin{array}{rl}& {\hat{x}}_k=0.8{\hat{x}}_{k-1}+H_k(y_k-0.8{\hat{x}}_{k-1})\\ & H_k=P_k^{{}^{\prime }}(P_k+1{)}^{-1}\\ & P_k^{{}^{\prime }}=0.64P_{k-1}+0.36\\ & P_k=(1-H_k)P_k^{{}^{\prime }}\end{array}$$
解上述方程组得
$$P_k=\frac{0.64P_{k-1}+0.36}{0.64P_{k-1}+1.36},P_k=H_k$$
求稳态解，用 $p_{\infty }$ 代替 $P_k,P_{k-1}$ 得
$$0.64P_{\infty }^2+0.72P_{\infty }-0.36=0$$
解二次方程并取正解
$$P_{\infty }=\frac{3}{8}$$
所以
$$H_k=P_k=P_{\infty }=\frac{3}{8}$$

$$\begin{array}{rl}{\hat{x}}_k& =0.8{\hat{x}}_{k-1}+\frac{3}{8}(y_k-0.8{\hat{x}}_{k-1})\\ & =0.5{\hat{x}}_{k-1}+\frac{3}{8}{y}_k\end{array}$$

---