卡尔曼滤波器的证明

准备知识

矩阵的期望：
正交矩阵：如果$A{A}^T$=E（E为单位矩阵，$A^T$表示“矩阵A的转置矩阵”）或$A^{T}A=E$，则n阶实矩阵A称为正交矩阵。
协方差矩阵：越线性相关，协方差越大， 两个变量完全线性无关，协方差为零

1、状态空间模型

$$X_{n+1}=An+1,n{X}_n+W_n\text{\hspace{1em}(式1)}$$
$$Y_n=B_n{X}_n+V_n\text{\hspace{1em}(式2)}$$
该模型涉及的参数如下：
（1）状态转移矩阵$A_{n+1,n}$，它是可逆的。（在线性代数中，给定一个n阶方阵A，若存在一n阶方阵B使得AB=BA=E（或AB=E、BA=E任满足一个），其中E为n阶单位矩阵，则称A是可逆的，且B是A的逆阵，记作$A^{-1}$）；
（2）测量矩阵$B_n$，通常情况下它是长方形矩阵；
（3）高斯动态噪声$W_n$，假设它有零均值且有协方差矩阵$Q_{w,n}$;
（4）高斯测量噪声$V_n$，假设它有零均值且有协方差矩阵$Q_{v,n}$；

2、新息过程

处理此类优化估计问题，是利用观察量$y_n$的所谓新息过程。其定义如下：
$$a_n=y_n-{\hat{y}}_{n|n-1}\text{\hspace{1em}(式3)}$$

其中${\hat{y}}_{n|n-1}$是在给定至$n-1$时刻所有观测值的情况下，对$y_n$的最小均方差的估计。
可以说：
新息过程$a_n$是包含在测量值$y_n$但不在${\hat{y}}_{n|n-1}$的预测部分的新信息的测量，因为$y_n$可以预测的部分（记为${\hat{y}}_{n|n-1}$）是完全由序列 { y k } k = 1 n − 1 \lbrace y_k\rbrace_{k=1} ^{n-1} {yk​}k=1n−1​决定的。
新息过程有如下的重要性质：
性质 1：
与观测值$y_n$有关的新息过程$a_n$与之前的所有观测值$y_1,y_2,...,y_{n-1}$正交，表示为：
$$E[a_n{y}_k^T]=0,0\le k\le n-1\text{\hspace{1em}(式4)}$$
性质 2：
新息过程由一系列相互正交的随机向量构成，表示为：
$$E[a_n{a}_k^T]=0，0\le k\le n-1\text{\hspace{1em}(式5)}$$
性质3：
代表观测数据的随机向量序列 { y 1 , y 2 , . . . . , y n − 1 } \lbrace y_1,y_2,....,y_{n-1} \rbrace {y1​,y2​,....,yn−1​}，与表示更新过程的序列$a_1,a_2,...,a_n$一 一对应。因此，通过能够保证线性稳定并且不丢失任何信息的操作，可以从一个序列得到另一个序列，因此可以写作：
{ y 1 , y 2 , . . . , y n }    ⟺    { a 1 , a 2 , . . . , a n } (式6) \lbrace y_1,y_2,...,y_n\rbrace \iff { \lbrace a_1,a_2,...,a_n \rbrace} \tag {式6} {y1​,y2​,...,yn​}⟺{a1​,a2​,...,an​}(式6)

总结：

该步骤就是给定给$n-1$时刻所有观测值，然后$n$时刻的观察值做出估计，得到${\hat{y}}_{n|n-1}$，得到的估计值与观察值，理论上是存在误差的，定义为新息过程$a_n$。

3、新息过程的协方差矩阵

从初始状态$x_0$开始，可以用式1所描述的系统模型表示$k$时刻的系统状态:
$$X_k=A_{k,0}{X}_0+\sum _{i=1}^{k-1}Ak，i{W}_i\text{\hspace{1em}(式7)}$$
式7表明状态$X_k$是$X_0$以及$W_1,W_2,W_3，...，W_n$的线性组合。
根据假设，测量噪声$V_n$与初始状态$X_0$以及动态噪声$W_i$无关。因此，在式7两边同时乘以$V_n^T$后得到：
$$E[X_k{V}_n^T]=0,k,n\ge 0\text{\hspace{1em}(式8)}$$
同理可以从测量公式2得到：
$$E[Y_k{V}_n^T]=0,0\le k\le n-1\text{\hspace{1em}(式9)}$$
和
$$E[Y_k{W}_n^T]=0,0\le k\le n\text{\hspace{1em}(式10)}$$
给定先前的观测值$y_1,...,y_k$，我们可以从测量公式式2中得出当前的观测值$y_n$的最小均方估计为：
$${\hat{y}}_{n|n-1}=B{\hat{x}}_{n|n-1}+{\hat{v}}_{n|n-1}\text{\hspace{1em}(式11)}$$
其中${\hat{v}}_{n|n-1}$是给先前的观测值$y_1,...,y_{n-1}$后所对应的测量噪声估计。因为根据式9，$v_n$与先前的观测值是正交的，因此估计值${\hat{v}}_{n|n-1}$为零。于是化式11得到：
$${\hat{y}}_{n|n-1}=B_n{\hat{x}}_{n|n-1}\text{\hspace{1em}(式12)}$$
由于 ($y_n=b_n{x}_n+v_n$)，再根据式12代入式3（$a_n=y_n-{\hat{y}}_{n|n-1}$），将项合并得：
$$a_n=B_n{\epsilon }_{n|n-1}+v_n\text{\hspace{1em}(式13)}$$
其中，新引入的项 ${\epsilon }_{n|n-1}$是状态预测误差向量。其定义为：
$${\epsilon }_{n|n-1}=x_n-{\hat{x}}_{n|n-1}\text{\hspace{1em}(式14)}$$

${\epsilon }_{n|n-1}$与动态噪声$w_n$以及测量噪声$v_n$均是正交的。由此定义零均值新息过程$a_n$的协方差矩阵为：
$$R_n=E[a_n{a}_n^T]\text{\hspace{1em}(式15)}$$
利用式13，能得到：
$$R_n=B_n{P}_{n|n-1}{B}_n^T+Q_n\text{\hspace{1em}(式16)}$$
其中$Q_{v,n}$是测量噪声$v_n$的协方差矩阵，新引入的项：
$$P_{n|n-1}=E[{\epsilon }_{n|n-1}{\epsilon }_{n|n-1}^T]\text{\hspace{1em}(式17)}$$
为预测误差协方差矩阵，式16为卡尔曼滤波算法的第一步。

总结：

4 、利用新息过程进行滤波状态估计：预测-修正公式(${\hat{X}}_{n|n}={\hat{X}}_{n|n-1}+G_n{a}_n$ )

接下来需要做的是利用新息过程实现任意时刻$i$系统状态$x_i$的最小均方误差估计，为此，给定新息序列$a_1,a_2,...,a_n$,首先线性展开的形式表示对状态$x_i$的估计：
$${\hat{x}}_{i|n}=\sum _{k=1}^n{C}_{i,k}{a}_k\text{\hspace{1em}(式18)}$$
其中 { C j , k } k = 1 n \lbrace C_{j,k} \rbrace_{k=1} ^n {Cj,k​}k=1n​是i时刻的展开式系数矩阵的集合，状态预测误差与新息过程满足下述正交条件：
$$E[{\epsilon }_{i|n}{a}_k^T]=0当k=1,2,...,n且i\le n\text{\hspace{1em}(式19)}$$
因此将式18代入式19，使用式5所描述的信息过程的正交性，可得：
$$E[x_i{a}_k^T]=C_{i,k}{R}_k\text{\hspace{1em}(式20)}$$
$R_n$是新息过程的协方差，解此方程的系数矩阵$C_{i,k}$，得到：
$$C_{i,k}=E[x_i{a}_k^T]R_k^{-1}\text{\hspace{1em}(式21)}$$
再利用式18得到：
$${\hat{x}}_{i|n}=\sum _{k=1}^{n}E[x_i{a}_k^T]R_k^{-1}{a}_k\text{\hspace{1em}(式22)}$$
当i=n时，为滤波过程，因此式26描述该状态的滤波估计为：
$${\hat{x}}_{n|n}=\sum _{k=1}^{n}E[x_n{a}_k^T]R_k^{-1}{a}_k=\sum _{k=1}^{n-1}E[x_n{a}_k^T]R_k^{-1}{a}_k+E[x_n{a}_k^T]R_k^{-1}{a}_k\text{\hspace{1em}(式23)}$$
把等式的第二项，$k=n$的项从求和中分离出来，首先将：
$${\hat{x}}_{n|n-1}=\sum _{k=1}^{n-1}E[x_n{a}_k^T]R_k^{-1}{a}_{k}v\text{\hspace{1em}(式24)}$$
其次引入下述定义：
$$G_n=E[x_n{a}_k^T]R_k^{-1}\text{\hspace{1em}(式25)}$$
由此我们可以将状态滤波估计表示为下述递归的形式：
$${\hat{x}}_{n|n}={\hat{x}}_{n|n-1}+G_n{a}_n\text{\hspace{1em}(式26)}$$
（1）${\hat{x}}_{n|n-1}$ 表示单步预测，其表示在给定$n-1$时刻前所有观测值的基础上对状态$x_n$的预测估计。
（2）$G_n{a}_n$表示修正项，新息过程$a_n$表示由观测值$y_n$引入滤波过程的新新息，乘以"增益因子"。因此，$G_n$通常被称为卡尔曼增益矩阵。

5、卡尔曼增益的计算 $G_n=P_{n|n-1}{B}_n^T{R}_n^{-1}$

式26中引入了卡尔曼增益矩阵，需要得到相应的计算公式。
应用式13得：
$$E[X_n{a}_n^T]=E[X_n(B_n{\epsilon }_{n|n-1}+v_n{)}^T]=E[X_n{\epsilon }_{n|n-1}^T]B_n^T$$
在上式的第二行$E[X_n(B_n{\epsilon }_{n|n-1}+v_n{)}^T]$，利用状态$X_n$与测量噪声$v_n$无关性，根据正交原理，状态预测误差向量${\epsilon }_{n|n-1}$与状态估计${\hat{X}}_{n|n-1}$是正交的，因此${\epsilon }_{n|n-1}$与${\hat{X}}_{n|n-1}$外积的期望为零，进而我们用${\epsilon }_{n|n-1}$代替$X_n$不影响期望$E[X_n{a}_n^T]$,由此可得：
$$E[X_n{a}_n^T]=E[{\epsilon }_{n|n-1}{\epsilon }_{n|n-1}^T]B_n^T=P_{n|n-1}{B}_n^T$$
因此式25卡尔曼增益矩阵$G_n$可以表示为：
$$G_n=P_{n|n-1}{B}_n^T{R}_n^{-1}\text{\hspace{1em}(式27)}$$

6、用于更新预测误差协方差矩阵的黎卡堤查分方程

为了解决状态估计，将式14中的n替换为n+1，得到：
$${\epsilon }_{n+1|n}=x_{n+1}-{\hat{x}}_{n+1|n}$$
通过上式可以看出含有滤波估计的项${\hat{x}}_{n+1|n}$表示状态的预测估计是有益的。故而将式24中的n替换为n+1，并运用式1，可得：
$${\hat{x}}_{n+1|n}=\sum _{k=1}^{n}E[x_{n+1}{a}_k^T]R_k^{-1}{a}_k=\sum _{k=1}^{n}E[A_{n+1,n}{X}_n+W_n{a}_k^T]R_k^{-1}{a}_k=A_{n+1,n}\sum _{k=1}^{n}E[X_n{a}_n^T]R_k^{-1}{a}_k=A_{n+1,n}{\hat{X}}_{n|n}$$
$$\text{\hspace{1em}(式28)}$$
因为动态噪声$W_n$与观测值是相互独立的，故期望$E[w_k{a}_k^T]$为零。对滤波估计${\hat{x}}_{n|n}$,应用式23，以及式28对状态$x_n$的预测滤波估计的关系，利用${\epsilon }_{n+1|n}$的公式得到：
$${\epsilon }_{n+1|n}=(A_{n+1,n}{X}_n+W_n)-A_{n+1,n}{\hat{X}}_{n|n}=A_{n+1,n}(X_n-{\hat{X}}_{n|n})+W_n=A_{n+1,n}{\epsilon }_{n|n}+W_n\text{\hspace{1em}(式29)}$$
滤波误差向量定义为：
$${\epsilon }_{n|n}=X_n-{\hat{X}}_{n|n}\text{\hspace{1em}(式30)}$$
因为滤波误差向量${\epsilon }_{n|n}$与动态噪声$w_n$是无关，可以将预测误差协方差矩阵表示为：
$$P_{n+1,n}=E[{\epsilon }_{n+1|n}{\epsilon }_{n+1|n}^T]=A_{n+1,n}{P}_{n|n}{A}_{n+1}^T+Q_{m,n}\text{\hspace{1em}(式31)}$$
其中$Q_{m,n}$为动态噪声$w_n$的误差协方差矩阵。在式30中引入了最后一个参数，称为滤波误差协方差矩阵，其定义为：
$$P_{n|n}=E[{\epsilon }_{n|n}{\epsilon }_{n|n}^T]\text{\hspace{1em}(式32)}$$
为了计算误差协方差矩阵$P_{n|n}$的式子，因此将式26代入式30得：
$${\epsilon }_{n|n}=X_n-{\hat{X}}_{n|n-1}-G_n{a}_n={\epsilon }_{n|n-1}-G_n{a}_n$$
然后应用式32,得到：
$$P_{n|n}=E[({\epsilon }_{n|n-1}-G_n{a}_n)(({\epsilon }_{n|n-1}-G_n{a}_n{)}^T]$$
$$=E[{\epsilon }_{n|n-1}{\epsilon }_{n|n-1}^T]-G_{n}E[a_n{\epsilon }_{n|n-1}^T]-E[{\epsilon }_{n|n-1}{a}_n^T]G_n^T+G_{n}E[a_n{a}_n^T]G_n^T$$
$$=P_{n|n-1}-G_{n}E[a_n{\epsilon }_{n|n-1}^T]-E[{\epsilon }_{n|n-1}{a}_n^T]+G_n{R}_n{G}_n^T\text{\hspace{1em}(式33)}$$
因为$${\hat{X}}_{n|n-1}$$与新息过程$a_n$正交，于是得到:
$$E[{\epsilon }_{n|n-1}{a}_n^T]=E[(X_n-{\hat{X}}_{n|n-1})a_n^T]=E[X_n{a}_n^T]$$
同理：
$$E[a_n{\epsilon }_{n|n-1}^T]=E[a_n{X}_n^T]$$
利用式27中对卡尔曼增益的定义，易得：
$$G_{n}E[a_n{\epsilon }_{n|n-1}^T]=E[{\epsilon }_{n|n-1}{a}_n^T]G_n^T=G_n{R}_n{G}_n^T$$
根据式33化简得
$$P_{n|n}=P_{n|n-1}-G_n{R}_n{G}_n^T$$
利用式27以及协方差矩阵$R_n$和$P_{n|n-1}$的对称性得到：
$$P_{n|n}=P_{n|n-1}-G_n{B}_n{P}_{n|n-1}\text{\hspace{1em}(式33)}$$

总结