卡尔曼最优预测以及最优滤波方程的相关推导--最优预测篇

最近期末考试完了，终于有时间静下心来好好推一下卡尔曼最优预测跟滤波的相关方程了，虽然之前也用过也知道怎么用，但是也仅仅停留在知道怎么用能用来干什么的境界。老早就想好好理一理卡尔曼的几个基本方程，也算是来填个坑。

最早接触卡尔曼滤波还是在三年前调试惯性MEMS的时候，就那么十几几十行代码甚至神奇，当时理解起来也只能是人云亦云，什么陀螺仪是短时测量精准但是存在积分误差，加速度计短时测量没有陀螺仪准但是不存在积分误差，可以用加速度计来修正陀螺仪的积分误差等等等等......反正那会儿也没去深究，能用就行了，但是心里总感觉有一个梗......

现在有时间也有相关的知识储备来好好探究一下那几个方程了，打算用两篇文章来详细地推导一下卡尔曼最优预测方程跟卡尔曼最优滤波方程，如果过年回老家有时间精力的话我会写一个demo来实际评估一下卡尔曼的效果。前排提醒，这两篇文章不是卡尔曼最优预测跟最优滤波的科普性文章，不会举什么养猪、开车的例子，因此想看通俗解释的请提前绕道，个人尽量做到严谨的推导。若发现错误，望读者不吝指正。

一、基础知识复习

在正式开篇之前，希望读者能有最基本的概率论与数理统计、一点点的线性代数、一点点的微积分、亿点点的随机过程以及一些线性系统的状态空间描述的知识储备。

先给出几个简单的定义跟定理：

1.正定对称矩阵的相关性质：

(1)若 $A\in \mathbb{R}^{n \times n}}$ 为正定对称矩阵，有,且对 $\forall x\in \mathbb{R}^n$ ，有 $x^TAx\geqslant 0$ 当且仅当时为0

(2)若 $A\in \mathbb{R}^{n \times n}}$ 为正定对称矩阵，则的各特征值均大于0

2.接下来帮大家复(yu)习概率论里面的相关知识：

(1)随机变量(全称应写为 $\left \{X(\omega),\omega \in \Omega \right \}$ , $\Omega$ 为样本空间，一般直接简写为)的分布函数为：

$F_X(x)=P\left \{ X\leqslant x,x\in \mathbb{R} \right \}$

若为连续型随机变量，则有：

$F_X(x)=\int_{-\infty}^{x}f_X(t)dt$

其中为随机变量的概率密度，且有 $f_X(x)=F_X'{(x)}$

(2)随机变量的数学期望(也称均值)为：

$E[X]=m_X=\int_{-\infty }^{+\infty }xdF(x)$

若为离散型随机变量，则有：

$E[X]=m_X=\sum_{i=0}^{+\infty }x_iP(x_i)$

若为连续型随机变量，则有：

$E[X]=m_X=\int_{-\infty}^{+\infty}xf(x)dx$ ,数学期望的本质为一阶矩

(3)随机变量的方差定义为：

若为离散型随机变量，则：

$D[X]=\sum_{i=0}^{+\infty}(x_i-m_X)^2P(x_i)$

若为连续型随机变量，则：

$D[X]=\int_{-\infty}^{+\infty}(x-m_X)^2f(x)dx$

称 $\sigma _X=\sqrt {D[X]}$ 为标准差，方差其实是二阶中心矩

(4)对于维连续型随机向量 $\boldsymbol {X}=[X_1,X_2...X_n]^T$ ，其联合分布函数为：

$F_{X_1,X_2,...,X_n}(x_1,x_2,...,x_n)=P\left \{ X_1\leqslant x_1,X_2\leqslant x_2, ...Xn\leqslant x_n, \right \}=\int_{-\infty}^{x_1}\int_{-\infty}^{x_2}...\int_{-\infty}^{x_n}f(x_1,x_2,...x_n)dx_1dx_2...dx_n$

其中为维联合概率密度。维随机向量的期望向量(也称均值向量)为:

$E[\boldsymbol {X}]=m_X=E[X_1,X_2,...,X_n]^T=[E[X_1],E[X_2],...,E[X_n]]^T=[m_{X_1},m_{X_2},...,m_{X_n}]^T$

方差阵(有的地方也叫协方差矩阵，本质上一样，但是这里为了与下面的不同随机向量之间做区分记为方差阵)为: $D[\textbf{X}]=E[(X-m_X)(X-m_X)^T]=E\begin{bmatrix} (X_1-m_{X_1})^2 & (X_1-m_{X_1})(X_2-m_{X_2}) & ... & (X_1-m_{X_1})(X_n-m_{X_n})\\ (X_2-m_{X_2})(X_1-m_{X_1}) & (X_2-m_{X_2})^2 & ... & (X_2-m_{X_2})(X_n-m_{X_n})\\ ... & ... & ... & ... \\(X_n-m_{X_n})(X_1-m_{X_1}) & (X_n-m_X_n})(X_2-m_{X_2}) & ... & (X_n-m_{X_n})^2\end{bmatrix}$

方差阵主对角线为随机向量各分量的方差，其余元素为分量之间的相关矩。特别的，对于两个随机变量，若有，则称正交，后面我们会证明的正交定理就是基于此

(5)对于二维随机向量，其协方差为：

，当 $\boldsymbol {X}$ , $\boldsymbol {Y}$ 均为维随机向量时，称 $COV[\boldsymbol {X},\boldsymbol {Y}]$ 为 $\boldsymbol {X},\boldsymbol {Y}$ 的协方差矩阵，且:

$COV[\boldsymbol {X},\boldsymbol {Y}]=E[(\boldsymbol {X}-m_X)(\boldsymbol {Y}-m_Y)^T]=E\begin{bmatrix} (X_1-m_{X_1})(Y_1-m_{Y_1})) & (X_1-m_{X_1})(Y_2-m_{Y_2}) & ... & (X_1-m_{X_1})(Y_n-m_{Y_n}) \\ (X_2-m_{X_2})(Y_1-m_{Y_1}) & (X_2-m_{X_2})(Y_2-m_{Y_2}) & ... & (X_2-m_{X_2})(Y_n-m_{Y_n}) \\ ... & ... & ... & ...\\ (X_n-m_{X_n})(Y_1-m_{Y_1}) & (X_n-m_{X_n})(Y_2-m_{Y_2}) & ... & (X_n-m_{X_n})(Y_n-m_{Y_n})\end{bmatrix}$

(6)正态随机变量在实际工程中广泛存在，我们所熟知的高斯白噪声就是一类幅值服从正态分布，功率谱密度分布服从均匀分布的一类噪声。正态分布的随机变量在自然界中也是广泛存在的，中心极限定理告诉我们,无穷多个独立同分布甚至不同分布的随机变量的叠加的结果近似服从正态分布，因此单个因素可能影响很小，但是无穷多因素叠加会影响整体分布。

正态分布又称高斯分布，其概率密度为：

$f(x)=\frac{1}{\sqrt {2 \pi}\sigma}exp\left \{ -\frac{(x-m_X)^2}{2\sigma^2} \right \}$

记为 $X\sim N(m_X,\sigma ^2)$ ，其中为的均值(数学期望)， $\sigma ^2$ 为的方差。

正态分布有一个非常重要的性质，也就是正态分布的线性函数也服从正态分布。即若,则 $Y \sim N(am_X+b,(a\sigma )^2)$ 。这一点非常重要，在实际采样过程中，由于采样信号中混杂着零均值的高斯白噪声，那么对采样信号叠加的时候实际有用信号幅值会叠加，但是零均值的高斯白噪声叠加均值仍然为零，相当于变相地提高了信噪比。

(8)对于维正态随机向量 $\boldsymbol {X}=[X_1,X_2,...X_n]^T$ ，其均值向量为 $\boldsymbol {\mu}=[m_{X_1},m_{X_2},...m_{X_n}]^T$ ,协方差矩阵为

$C=(c_{ij}), \ c_{ij}= E[(X_i-m_{X_i})(X_j-m_{X_j})]\ ,\ (i,j= 1,2,...,n)$

若协方差矩阵非奇异即 $det(C)\neq 0$ ,则其维联合概率密度为:

$f(x_1,x_2,...x_n)=\frac{1}{(2\pi)^{\frac{n}{2}}(det(C))^{\frac{1}{2}}}exp\left \{ -\frac{1}{2}(\boldsymbol {X}-\boldsymbol {\mu})^TC^{-1}(\boldsymbol {X}-\boldsymbol {\mu}) \right \}$ ，记为 $\boldsymbol {X}\sim N(\boldsymbol {\mu},C)$

此时协方差矩阵为一个正定对称矩阵。若协方差矩阵为奇异矩阵， $C^{-1}$ 不存在，则上面的联合概率密度公式无意义，此时称该维随机向量服从退化正态分布(也称奇异正态分布)。上面提到正态分布的线性函数也服从正态分布，对于多维正态随机向量，有同样的性质。即对于维正态随机向量 $\boldsymbol {X}=[X_1,X_2,...X_n]^T$ 服从 $N(\boldsymbol {\mu},C)$ 的正态分布，且 $K\in \mathbb{R}^{m*n}$ 为任意矩阵，则线性变换 $\boldsymbol {Y}=K\boldsymbol {X}$ 服从维正态分布 $N(K\boldsymbol {\mu},KCK^T)$ ，此性质称为正态随机变量的线性不变性。

3.坐稳了，下面开始复(yu)习随机过程的相关知识

(1)随机过程的严格定义是：对于给定概率空间 $(\Omega,\mathcal F,P)$ 和指标集，若对于每一个 $t\in T$ ,有定义在 $(\Omega,F,P)$ 上的随机变量 $X_t(\omega),\omega \in \Omega$ 与之对应，称依赖于的随机变量族为随机过程，记为 $\left\{X_t(\omega),t \in T \right \}$ ，或记为 $\left\{X_t,t \in T \right \}$ 以及 $\left\{X(t),t \in T \right \}$ ，定义比较抽象，这里举个简单的例子，拿我们熟知的电阻的热噪声来讲，指标集就是时间，对于 $\forall t \in T$ 时刻我们对热噪声采样，则实际采样得到的数据其实是一个随机变量，其服从正态分布，因此电阻热噪声就是个正态过程，当然指标集可以是连续的也可以是离散的,可以是时间也可以是其他。常见的随机过程分类就是正态过程、更新计数过程、平稳过程、马尔可夫过程等。随机过程可以看成一个无穷维的随机向量，我们可以用有限维分布函数族来近似的刻画随机过程，但是较为复杂，实际工程不可能采用此方法来分析随机过程，因此我们经常研究随机过程的一阶矩、二阶矩统计特性。

一阶矩其实就是我们在概率论里所熟知的均值，但是这里是的一个函数，即：

$m_X(t)=E[X(t)]=\int_{-\infty}^{+\infty}xdF_t(x),t \in T$

称为随机过程的均值函数。有了均值函数，我们可以定义随机过程的方差函数 $D_X(t)=E[(X-m_X(t))^2],t \in T$

下面给出随机过程的一些重要的数字特征：

对于随机过程 $\left \{ X(t),t \in T \right \}$ ，称

$R(s,t)=E[X_sX_t],\ \forall s,t \in T$

为随机过程的自相关函数，自相关函数是随机过程中最常用的数字特征。称

$C(s,t)=COV(X_s,X_t)=E[(X_s-m_X(s))(X_t-m_X(t))],\ \forall s,t \in T$

为随机过程的协方差函数，不难得出以及

(2)就像多维随机变量一样，也存在多维随机过程，给定两个随机过程,则称

$C_{XY}(s,t)=COV(X_s,Y_t)=E[(X_s-m_X(s))(Y_t-m_Y(t))]$

为两随机过程的互协方差函数，其中。称

$R_{XY}(s,t)=E[X_sY_t]$

为两个随机过程的互相关函数，同样，若对 $\forall s,t \in T$ 有 $R_{XY}(s,t)=0$ ，则称随机过程相互正交

(3)二阶矩过程：对于随机过程,若对 $\forall t \in T$ 有 $E[|X_t|^2] < +\infty$ ,则称该随机过程为二阶矩过程

(4)平稳过程：若随机过程的任意维联合分布函数不随时间推移而改变，则称此随机过程为严平稳过程，但是由于严平稳过程要求过于严格，且证明较困难，因而在实际工程技术中应用更多的一类是宽平稳过程。对于二阶矩过程，若满足

(a)为一个常数

(b)对 $\forall s,t \in T, R_X(s,t)=R_X(t-s)=R_X(\tau),\tau=t-s$ 只与时间区间长度有关，与起始时间无关

则称此二阶矩过程为宽平稳过程，一般来讲严平稳过程不一定是宽平稳过程，宽平稳过程也不一定是严平稳过程，但是对于正态过程，严平稳与宽平稳等价。此外，正态随机过程还有一个非常重要的性质就是正态随机过程经过线性系统之后还是正态随机过程。本文所讲的平稳过程均指宽平稳过程

*(5)关于平稳过程的均方遍历性(也称为各态历经性)，通俗的来讲就是可以仅利用一条样本轨迹来估计整体的统计特性，虽然实际工程中许多随机过程都满足均方遍历性，但是其定义涉及到均方微积分的相关概念，数学验证也往往很困难，不过很多时候可以根据实际工程背景来确定。在此就不展开叙述，读者若有兴趣可以翻阅随机过程相关书籍的相关章节

*(6)傅里叶变换是一种有效的信号分析方法，同样我们可以用傅里叶变换来分析平稳过程获得平稳过程的功率谱。对于平稳过程 $\left \{ X(t),t \in \mathbb{R} \right \}$ 是均方连续的，若其自相关函数 $R(\tau)$ 满足

$\int_{-\infty}^{+\infty}|R(\tau)|d\tau < +\infty$

则此平稳过程功率谱密度 $S(\omega)$ 为：

$S(\omega)=\int_{-\infty}^{+\infty}R(\tau)e^{-j\omega \tau}d\tau$

同时

$R(\tau)=\frac{1}{2\pi}\int_{-\infty}^{+\infty}S(\omega)e^{j\omega \tau}d\omega$

即平稳过程 $\left \{ X(t),t \in \mathbb{R} \right \}$ 的相关函数与功率谱密度构成一对傅里叶变换。称

$F_X(\omega)=\int_{-\infty}^{+\infty}S_X(u)du,\omega \in \mathbb{R}$ 为平稳过程 $\left \{ X(t),t \in \mathbb{R} \right \}$ 的谱函数

4.线性系统的状态空间描述

(1)线性系统的定义在这里就不给出了，线性系统最重要的性质是满足齐次跟叠加原理。线性系统又分为线性时变系统跟线性定常系统。线性时变系统是指系统各结构参数会随时间变化的线性系统，而线性定常系统则不会随时间变化，拿我们人体为例，我们身体各器官各功能从长远看一定在变化，比如我们的听力、视力等一定会衰减，这时就是一个时变系统，但是在很短的时间内我们的各项器官功能可以看作不变的，这时可以看成是一个定常系统。

描述一个动态系统，最常用的两种方式是传递函数描述外部的输入输出关系以及状态空间描述内部各状态变化规律。传递函数的相关介绍在此就不展开，可以参考信号与系统相关章节，本文的重点是系统的状态空间描述。卡尔曼等人在20世纪60年代将状态以及状态空间的概念引入到系统控制理论中，极大地推动了系统的时间域理论的发展，状态空间描述不仅可以用于分析单输入单输出的线性定常、时变系统、还可以分析多输入多输出的线性系统、非线性系统，本文所讲的卡尔曼滤波就是基于线性系统的状态空间描述。

(2)系统的状态是指一组能完整描述系统的时间域行为或运动过程的线性无关(数目最少)的变量，当状态表示为以各状态变量为分量的向量时，称为状态向量，记为:

状态空间定义为状态向量的集合，是以状态向量的个线性无关的分量为基底生成的维线性空间。这里可以举个简单的例子，还是拿我们人体来说，我们人体本身就是一个复杂的系统，描述我们人体的时候我们经常选取的一些变量可以是身高、体重、性别、年龄....这些变量就可以称为我们人体这个系统的状态。系统的状态空间描述通常需要两个过程，即系统的输入引起系统内部状态变化的过程以及状态与输入导致的输出变化的过程。输入引起状态变化是一个动态过程，通常用微分方程来描述，称其为状态方程。而状态与输入导致输出的变化是一个转化的过程，通常用代数方程来描述，称其为输出方程或者量测方程。记系统的个输入为:

系统的个输出为:

则对于连续时间线性时变系统可以描述为:

$\left\{ \begin{array}{ll} \dot{X(t)} = A(t)X(t)+B(t)U(t) \\ Y(t)=C(t)X(t)+D(t)U(t) \end{array} \right.$

其中为与时间相关的 $n\times n$ 维矩阵，称为系统矩阵；为 $n \times p$ 维输入矩阵；为 $q \times n$ 维输出矩阵；为 $q \times p$ 维前馈矩阵。它们均由系统的参数或结构决定。特别的，当系统的各项参数结构不随时间变化时，则退化为与时间无关的常数矩阵，此时系统变为线性定常系统，其状态空间描述可写为:

$\left\{ \begin{array}{ll} \dot{X(t)} = AX(t)+BU(t) \\ Y(t)=CX(t)+DU(t) \end{array} \right.$

(3)我们实际经常使用的数字系统本质上是离散的线性系统，此时可以用差分方程的形式给出离散时间线性时变系统的状态空间描述，即:

$\left\{ \begin{array}{ll} X(k+1) = G(k)X(k)+H(k)U(k) \\ Y(k)=C(k)X(k)+D(k)U(k) \end{array} \right.$

同样，对于离散时间线性定常系统的状态空间描述为:

$\left\{ \begin{array}{ll} X(k+1) = GX(k)+HU(k) \\ Y(k)=CX(k)+DU(k) \end{array} \right.$

5.相关定理的证明：

在推导卡尔曼最优预测以及最优滤波方程时，经常用到正交定理，下面我们来证明一下正交定理。

在讨论线性最小方差估计的时候，通常使用观测值的线性函数来表示的估计值，即：

$\hat{X}=aZ+b$ ，式中为待定的常数，

则估计误差方差要最小，即：

$J=E[(X-\hat{X})^2]=E[(X-(aZ+b))^2]=min$

当达到最小时，应满足：

$\frac{\partial J}{\partial a}=0$ ，即 $\frac{\partial J}{\partial a}=-2E\left \{ [X-(aZ+b)]Z \right \}=0$

式中

$X-(aZ+b)=\overline X$

为估计误差，立即可得到：

$E[\overline XZ]=0$

即估计误差与观测值的乘积的期望为0，也就是估计误差与观测值正交。

充分性的证明在此略去，仅需证明在估计误差与观测值正交的条件下确实是方差最小的。

同样我们可以证明估计误差 $\overline X$ 与 $\widehat{X}$ 也是正交的。由于线性最小方差估计是无偏估计，即有：

$E[\overline X]=0$

则我们有：

$E[\overline X \widehat X]=E[\overline X(aZ+b)]=aE[\overline XZ]+bE[\overline X]=0$ ，即 $\overline X$ 与 $\hat{X}$ 正交。

以上对所需的数学基础知识以及线性系统的相关知识做了一些复习，下一节我们将正式进入线性系统的卡尔曼滤波部分的学习。

二、问题的提法

在实际系统中，信号总是受到各种随机噪声的干扰，因此引起我们系统各状态变化的因素里面还应加上一项干扰项，此时系统的状态方程应改写为： $\dot{X(t)} = A(t)X(t)+B(t)U(t) +F(t)W(t)$ 。同时我们的各传感器、ADC等量测得出的信号其实也都是受到随机噪声干扰的信号，在没有前馈通道的情况下，量测方程实际上应写为(这里用符号区别于之前讲的量测方程，其实本质上都是对状态的反映)。上两式中，为干扰向量，为观测向量，为观测噪声向量。为了实现最优控制，我们需要实际的状态向量，但是状态向量受到随机噪声的干扰，因此需要估计状态向量，并且需要状态向量的估计值 $\hat{X(t)}$ 尽可能的接近实际状态向量。但是在大家实际应用的基本上都是离散的数字系统，因此我们主要来研究线性离散系统的卡尔曼滤波。

我们给出卡尔曼滤波问题的提法：

对于线性离散的随机系统的卡尔曼滤波问题可以描述为：

设有线性离散随机系统状态空间描述如下：

$\left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k)+G(k+1,k)U(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right. \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (2.1)$

式中：

为维状态向量；

为维输入控制向量；

为维随机干扰向量；

为维量观测向量；

为维随机观测噪声向量；

$\Phi(k+1,k),G(k+1,k),\Gamma(k+1,k),H(k)$ 分别为 $n\times n,n \times r,n \times p,m \times n$ 维矩阵。

并且假定：

(1)均为零均值的白噪声或高斯白噪声序列，且相互独立，即有：

$\ \ \ \ \ \ \ \ E[W(k)]=E[V(k)]=0,COV(W(k),V(k))=0,\\ \ \ \ \\ COV(W(k),W(j))=\left\{ \begin{array}{ll} Q_k \ \ \ & k=j \\ 0 \ \ \ \ & k\neq j \end{array} \right. \ \ \ ,COV(V(k),V(j))=\left\{ \begin{array}{ll} R_k \ \ \ & k=j \\ 0 \ \ \ \ & k\neq j \end{array} \right.$

其中，为的方差阵，是一个非负定对称矩阵，为的方差阵，是一个正定对称矩阵。

(2)状态向量的初始时刻均值为 $E[X(0)]=\boldsymbol{ \mu_0 }$ ,方差阵 $P_0=E[(X(0)-\boldsymbol{\mu_0})(X(0)-\boldsymbol{\mu_0})^T]$

(3)与不相关，即

这些假定是合理的，因为通常我们所接触到的噪声都是高斯白噪声，都是正态随机过程，经过采样之后成为高斯白噪声序列，且系统噪声与量测噪声分属不同部分一般都是不相关的，且实际有用信号也跟噪声是不相关的。

对于式所描述的系统，已经得到观测序列，要求找出的线性最优估计 $\hat{X(j|k)}$ ，使得估计值 $\hat{X(j|k)}$ 与之间的误差 $X(j|k)=X(j)-\hat{X(j|k)}$ 的方差 $E \left \{[X(j)-\hat{X(j|k)}]^T[X(j)-\hat{X(j|k)}] \right \}$ 最小，同时要求估计值 $\hat{X(j|k)}$ 是观测序列的线性函数，且估计是无偏的，即 $E[\hat{X(j|k)}]=E[X(j)]$

根据和的大小，估计问题可以分为三类：

(a) 称为预测问题

(b) 称为滤波问题

这里仅研究预测以及滤波问题。

三、线性离散系统的卡尔曼最优预测方程

通常在推导卡尔曼最优预测方程时，我们均忽略控制输入信号的作用，即输入为0，这样我们可以得到系统的状态空间描述为：

$\left\{ \begin{array}{ll} X(k+1) = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) \\ Z(k)=H(k)X(k)+V(k) \end{array} \right.\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.1)$

给定观测序列，我们需要找出的最优估计 $\hat{X(k+1|k)}$ ，称此估计为一步最优估计，使得估计误差：

$X(k+1|k)=X(k+1)-\hat{X(k+1|k)}$

的方差最小，也就是使得：

$E\left \{ [X(k+1)-\hat{X(k+1|k)}]^T [X(k+1)-\hat{X(k+1|k)}]\right \}=min$

当我们已经获得观测序列后，假定我们已经找到了状态向量的一个最优线性预测估计 $\hat{X(k|k-1)}$ 。在我们还没有获得观测值的情况下，先对时刻的状态向量做出估计，观察状态方程，由于为零均值的白噪声序列，我们无法做具体预测，其最优估计为，因此只能把

$\hat{X(k+1|k-1)} = \Phi(k+1,k)\hat{X(k|k-1)}\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.2)$

当作状态向量的一个预测估计。

则当 $\hat{X(k|k-1)}$ 是的最优线性估计时，我们所得到的 $\hat{X(k+1|k-1)}$ 也是的最优线性估计，下面给出证明：

证明：由 $X(k+1|k-1)=X(k+1)-\hat{X(k+1|k-1)}$

$=\Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k) -\Phi(k+1,k)\hat{X(k|k-1)}$

$=\Phi(k+1,k)(X(k)-\hat{X(k|k-1)})+ \Gamma(k+1,k)W(k)$

$=\Phi(k+1,k)X(k|k-1)+ \Gamma(k+1,k)W(k)$

由于 $\hat{X(k|k-1)}$ 是的最优线性估计，根据正交定理，估计误差应正交于，同时，由于均值为零且与相互独立，即有 $E[(W(k)Z(i))]=E[W(k)]E[Z(i)]=0,\ \ i=1,2,...,k-1$ ，因此与也正交，根据线性代数的相关知识，我们可以得出的线性变换 $\Phi(k+1,k)X(k|k-1)$ 与的线性变换 $\Gamma(k+1,k)W(k)$ 的叠加也应与正交，即也与正交。所以，在获取到观测向量之前， $\hat{X(k+1|k-1)}$ 是的最优线性估计。

在获取到观测向量之后，我们就可以利用观测值来修正 $\hat{X(k+1|k-1)}$ 。

由于 $\hat{X(k|k-1)}$ 是的最优线性估计，则通过量测方程，且由于也是零均值的白噪声序列且无法预测，我们可以得到时刻观测向量的最优线性估计为：

$\hat{Z(k|k-1)}=H(k)\hat{X(k|k-1)} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.3)$

若获取到的观测值恰好等于预测估计 $\hat{Z(k|k-1)}$ ，则新的观测值并没有提供任何有用的信息，无法用于修正 $\hat{X(k+1|k-1)}$ 。关于这一点，只需要证明估计误差与 $\hat{Z(k|k-1)}$ 正交即可，下面给出证明：

证明：根据正交定理，我们有 $E[(X(k|k-1))(\hat{X(k|k-1})^T]=0$

接下来我们计算 $E[(X(k+1|k-1))(\hat{Z(k|k-1})^T]$

$=E[(\Phi(k+1,k)X(k|k-1)+ \Gamma(k+1,k)W(k))(H(k)\hat{X(k|k-1)})^T]$

$=E[\Phi(k+1,k) X(k|k-1) {\hat{X(k|k-1)}}^TH(k)^T+ \Gamma(k+1,k)W(k)){\hat{X(k|k-1)}}^TH(k)^T]$

$=\Phi(k+1,k)E[X(k|k-1) {\hat{X(k|k-1)}}^T]H(k)^T+ \Gamma(k+1,k)E[W(k)){\hat{X(k|k-1)}}^T]H(k)^T$

不难看出上式中右侧第一项与第二项均为0，因此可以得出误差与 $\hat{Z(k|k-1)}$ 正交。

因此，若获取到的观测值恰好等于预测估计 $\hat{Z(k|k-1)}$ ，则 $\hat{X(k+1|k-1)}$ 就是的最优线性估计。但是在实际中，一般观测值不可能恰好等于预测估计 $\hat{Z(k|k-1)}$ ，但是我们可以分析一下误差的来源：

由

$=H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)$

而 $Z(k|k-1)=Z(k)-\hat{Z(k|k-1)}$

$=H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)-H(k)\hat{X(k|k-1)}$

从上式我们不难看出误差主要来源于的估计误差以及量测噪声。因此我们可以考虑用量测误差来修正我们之前的预测值 $\hat{X(k+1|k-1)}$ 使其变为 $\hat{X(k+1|k)}$ 。由于采用的是线性估计方法，通常我们使用加权的方法来修正，也就是在后加上一项与成正比的修正项，如此，我们可以得到：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)Z(k|k-1)$

即：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)(Z(k)-H(k)\hat{X(k|k-1)}) \ \ \ \ \ \ \ \ \ \ (3.4)$

式中为一个待定的矩阵，称为最优增益矩阵或加权矩阵。式可进一步写成:

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)H(k)X(k|k-1)+K(k)V(k) \ \ \ \ \ \ \ \ \ (3.5)$

要让 $\hat{X(k+1|k)}$ 是的最优线性估计，那么估计误差：

$X(k+1|k)=X(k+1)-\hat{X(k+1|k)}$

必定与观测序列均正交。

由

$X(k+1) = \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k)$

则估计误差：

$X(k+1|k)=X(k+1)-\hat{X(k+1|k)}$

$= \Phi(k+1,k)X(k) + \Gamma(k+1,k)W(k)-\Phi(k+1,k)\hat{X(k|k-1)}- \\ {\ \ \ \ \ \ \ \ K(k)H(k)X(k|k-1)-K(k)V(k)}$

$= \Phi(k+1,k)(X(k)- \hat{X(k|k-1)})+ \Gamma(k+1,k)W(k)-\\ {\ \ \ \ \ \ \ \ K(k)H(k)X(k|k-1)- K(k)V(k)}$

$= (\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k)$

观察上式右边已正交于，因而也正交于，因此，只要还正交于，那么 $\hat{X(k+1|k)}$ 就是的最优线性估计。因此我们可以根据最优线性估计的条件来反算出最优增益矩阵。(学过现代控制论的同学可能注意到，上式右侧第一项的系数是不是非常眼熟(矩阵)？其实就是观测器的系统矩阵。)

由与正交，即：

也就是：

$E\left \{ [(\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)- K(k)V(k)][H(k)[\hat{X(k|k-1)}+X(k|k-1)]+V(k)]^T \right \}$

$=E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1){\hat{X(k|k-1)}^T}H(k)^T]+ \\ {\ \ \ \ \ \ \ \ E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1){X(k|k-1)^T}H(k)^T}]}+ \\ {\ \ \ \ \ \ \ \ E[(\Phi(k+1,k)-K(k)H(k))X(k|k-1)V(k)^T H(k)^T] }+ \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){\hat{X(k|k-1)}^T}H(k)^T]} + \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){X(k|k-1)}^T}H(k)^T]} + \\ {\ \ \ \ \ \ \ \ E[\Gamma(k+1,k)W(k){V(k)^T}H(k)^T]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){\hat{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ E[K(k)V(k){V(k)^T}]}$

$=(\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){\hat{X(k|k-1)}^T}]H(k)^T+ \\ {\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){X(k|k-1)^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)V(k)^T]H(k)^T }+ \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){\hat{X(k|k-1)}^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){X(k|k-1)}^T}]H(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k){V(k)^T}]H(k)^T} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){\hat{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){{X(k|k-1)}^T}]} -\\ {\ \ \ \ \ \ \ \ K(k)E[V(k){V(k)^T}]}=0$

观察上式右侧，不难看出第1、3、4、5、6、7、8项均为0，因此可以进一步化简为：

${(\Phi(k+1,k)-K(k)H(k))E[X(k|k-1){X(k|k-1)^T}]H(k)^T - \\ K(k)E[V(k){V(k)^T}]=0}$

上式中 $E[X(k|k-1){X(k|k-1)^T}]$ 为误差方差阵，简记为，且又 $E[V(k){V(k)^T}]=R_k$ ,则：

$(\Phi(k+1,k)-K(k)H(k))P(k|k-1))H(k)^T - K(k)R_k=0$

即：

$\Phi(k+1,k)P(k|k-1))H(k)^T-K(k)(H(k))P(k|k-1)H(k)^T+R_k)=0$

因此可以计算出最优增益矩阵：

$K(k)=\Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}\ \ \ \ \ \ \ \ \ \ \ \ (3.6)$

上式中引入了误差方差阵，下面开始计算他的迭代式：

由其定义式可知：

$P(k+1|k)=E[X(k+1|k){X(k+1|k)^T}]$

$=E[((\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k)) \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))X(k|k-1)+ \Gamma(k+1,k)W(k)-K(k)V(k))^T}]$

$=E[{((\Phi(k+1,k)-K(k)H(k))X(k|k-1)X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} +{\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))X(k|k-1)W(k)^T\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))X(k|k-1)V(k)^TK(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)W(k)^T\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)W(k)V(k)^TK(k)^T} - \\ {\ \ \ \ \ \ \ \ K(k)V(k))X(k|k-1)^T(\Phi(k+1,k)-K(k)H(k))^T} - \\ {\ \ \ \ \ \ \ \ K(k)V(k))W(k)^T\Gamma(k+1,k)^T}+ \\ {\ \ \ \ \ \ \ \ K(k)V(k))V(k)^TK(k)^T}]$

$=((\Phi(k+1,k)-K(k)H(k))P(k|k-1)(\Phi(k+1,k)-K(k)H(k))^T +{\ \ \ \ \ \ \ \ (\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)W(k)^T]\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ ((\Phi(k+1,k)-K(k)H(k))E[X(k|k-1)V(k)^T]K(k)^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)X(k|k-1)^T](\Phi(k+1,k)-K(k)H(k))^T} + \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)W(k)^T]\Gamma(k+1,k)^T} - \\ {\ \ \ \ \ \ \ \ \Gamma(k+1,k)E[W(k)V(k)^TK(k)^T]} - \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))X(k|k-1)^T](\Phi(k+1,k)-K(k)H(k))^T} - \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))W(k)^T]\Gamma(k+1,k)^T}+ \\ {\ \ \ \ \ \ \ \ K(k)E[V(k))V(k)^T]K(k)^T}$

不难看出上式右侧第2、3、4、6、7、8项均为0，因此可以进一步化简为：

$P(k+1|k)=(\Phi(k+1,k)-K(k)H(k))P(k|k-1)(\Phi(k+1,k)-K(k)H(k))^T + {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)R_kK(k)^T}$

进一步展开得：

$P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T - \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(k+1,k)P(k|k-1)H(k)^TK(k)^T}- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k))P(k|k-1)\Phi(k+1,k)^T} +\\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k))P(k|k-1)H(k))^TK(k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)R_kK(k)^T} \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (3.7)$

观察上式中右侧第4项以及第6项，我们有：

将式代入上式右侧中前面的有：

$={\Phi(k+1,k)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}[H(k)P(k|k-1)H(k)^T+R_k]K(k)^T}$

$=\Phi(k+1,k)P(k|k-1)H(k)^TK(k)^T$

将上式代回式可得：

$P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ K(k)H(k)P(k|k-1)\Phi(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T}$

将式代入上式中的有：

$\begin{array}{ll} P(k+1|k)=\Phi(k+1,k)P(k|k-1)\Phi(k+1,k)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k]}^{-1}{H(k)P(k|k-1)\Phi(k+1,k)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(k+1,k)Q_k\Gamma(k+1,k)^T} \end{array} \ \ \ \ \ \ \ (3.9)$

由此得到误差方差阵的递推公式。

最后将式、、综合起来可以得到卡尔曼最优预测方程如下：

最优预测估计方程：

$\hat{X(k+1|k)} = \Phi(k+1,k)\hat{X(k|k-1)}+K(k)(Z(k)-H(k)\hat{X(k|k-1)})$

最优增益矩阵方程：

$K(k)=\Phi(k+1,k)P(k|k-1)H(k)^T [H(k)P(k|k-1)H(k)^T + R_k}]^{-1}$

误差方差阵递推方程：

至此推导完成。

四、线性离散系统的卡尔曼最优预测的算法实现

(1)给定时刻的初始值：

$\hat{X(0|0_-)}=E[X(0)]=\mu_0$

估计误差的方差阵为：

$P(0|0_-)=E[(X(0)-\mu_0)(X(0)-\mu_0)^T]$

(2)计算时刻的最优增益矩阵：

$K(0)=\Phi(1,0)P(0|0_-)H(0)^T[H(0)P(0|0_-)H(0)^T+R_0]^{-1}$

(3)此时应获取到新的观测值，结合新观测值，计算的最优预测估计：

$\hat{X(1|0)}=\Phi(1,0)\hat{X(0|0_-)}+K(0)[Z(0)-H(0)\hat{X(0|0_-)}]$

(4)更新估计误差方差阵：

$\begin{array}{ll} P(1|0)=\Phi(1,0)P(0|0_-)\Phi(1,0)^T- \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \Phi(1,0)P(0|0_-)H(0)^T [H(0)P(0|0_-)H(0)^T + R_0]}^{-1}{H(0)P(0|0_-)\Phi(1,0)^T} + \\ {\ \ \ \ \ \ \ \ \ \ \ \ \ \Gamma(1,0)Q_0\Gamma(1,0)^T} \end{array}$