卡尔曼滤波
作者：互联网
在信号与系统里面，我们研究一个系统的特性，通常是通过系统的输入输出来求解系统函数，这样的研究思路相当于将系统当成了一个黑箱，并未对系统本身特性进行建模处理，因此也未将关于系统模型的一些先验知识应用上去。现在我们尝试用另外一种思路来对一个系统进行研究，在此需要引入状态空间的概念。在这种研究思路下，系统的特性能通过系统的状态变化完全体现出来。通常，系统的状态具有如下两个特点：一是系统的状态能够体现系统本身的特性，它的变化遵循一定的物理规律，可对其进行建模，进而将与之相关的一些先验知识应用起来；二是系统的状态通常难以直接测量得到，因此需要依赖特定的观测向量来了解系统的状态。根据上面的描述我们建立下面的状态方程和观测方程
\[\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=f(\boldsymbol{x}_n,\boldsymbol{v}_n)\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=g(\boldsymbol{x}_n,\boldsymbol{w}_n) \end{aligned} \end{equation}\tag{1} \]在上面的表达式中状态方程表示不同时刻系统状态之间的变化规律，它反映了系统最本质的特性，由系统模型决定。观测方程表示了观测向量和系统状态之间的转化关系。其中\(\boldsymbol{v}_n\)，\(\boldsymbol{w}_n\)分别表示状态噪声和观测噪声，通常我们假设两者为零均值白噪声。(1)中两个方程是一般化的情况，在实际应用中通常可用线性模型进行转化，以方便计算，为此，将(1)改写为
\[\begin{equation} \begin{aligned} {\rm State \quad Equation:}\quad &\boldsymbol{x}_{n+1}=\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n\\ {\rm Observation \quad Equation:}\quad &\boldsymbol{y}_n=\boldsymbol{H}_n \boldsymbol{x}_n+\boldsymbol{w}_n \end{aligned} \end{equation}\tag{2} \]在上面的表达式中\(\boldsymbol{F}_n\)，\(\boldsymbol{H}_n\)分别表示状态转移矩阵和观测矩阵，需要注意的是这两者都带下标\(n\)，说明这两者是时变的，这也就意味着，此时允许系统是非平稳的。
基于上面的状态方程和观测方程，我们的目的是更好地估计系统的状态。为此，我们先定义如下一些符号，方便后续说明：
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n\\ \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}\\ \end{aligned} \end{equation}\tag{3} \]其中\(\boldsymbol{x}_n,\boldsymbol{x}_{n+1}\)分别表示\(n\)时刻和\(n+1\)时刻的状态真值，\(\hat{\boldsymbol{x}_{n|n}}\)表示用\(1-n\)时刻的观测向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)估计得到的\(n\)时刻的状态向量，\(\hat{\boldsymbol{x}_{n+1|n}}\)表示用\(1-n\)时刻的观测向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)估计得到的\(n+1\)时刻的状态向量，\(\hat{\boldsymbol{x}_{n+1|n+1}}\)表示用\(1-n+1\)时刻的观测向量\((\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})\)估计得到的\(n+1\)时刻的状态向量。\({\rm Proj}\)表示投影操作，且有\({\rm Proj}_\boldsymbol{Y}\boldsymbol{X} ={\rm E}(\boldsymbol{X}\boldsymbol{Y}^{\rm T})[{\rm E}(\boldsymbol{Y}\boldsymbol{Y}^{\rm T})]^{-1}\boldsymbol{Y}\)。
我们在\(n\)时刻拥有的信息包括：观测向量\(\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)，\(n\)时刻的状态估计向量\(\hat{\boldsymbol{x}}_{n|n}\)。同时在\(n+1\)时刻，我们将得到一个新的观测向量\(\boldsymbol{y}_{n+1}\)。我们想要利用上述信息，更好地估计系统在\(n+1\)时刻的状态向量，即\(\hat{\boldsymbol{x}}_{n+1|n+1}\)。一个很自然的思路就是，首先利用\(\hat{\boldsymbol{x}}_{n|n}\)结合状态转移矩阵\(\boldsymbol{F}_n\)(即系统模型)，可以预测预测得到\(\hat{\boldsymbol{x}}_{n+1|n}\)。此时，由于系统建模不一定完全准确(\(\boldsymbol{F}_n\)不一定完全符合实际模型)或者状态噪声的影响，使得\(\hat{\boldsymbol{x}}_{n+1|n}\)并不一定准确。因此，我们需要利用新增加的信息\(\boldsymbol{y}_{n+1}\)对上述估计结果进行修正，得到\(\hat{\boldsymbol{x}}_{n+1|n+1}\)。修正的过程我们越简单越好，因此我们可以大致猜测修正的方法为:\(\hat{\boldsymbol{x}}_{n+1|n+1}=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\hat{\boldsymbol{y}}_{n+1})\)。该方法用观测向量的估计误差来进行修正，\(\boldsymbol{K}_{n+1}\)为修正系数，一方面它用于表示我们相信模型预测结果和相信观测值的程度，另一方面可以将其理解为对观测向量和状态向量之间相互转换的矩阵，因为观测向量和状态向量不仅可能量纲不同，甚至物理含义也不一样，因此不能直接用观测向量的估计误差来修正状态向量的预测误差，因此需要在两者之间做一个转化。显然上面的研究思路遵循了预测->修正的思路，即由\(\hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1}\)的路线。以下通过严谨的推导来证明上述思路的可行性。
（1）\(\hat{\boldsymbol{x}}_{n|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n}\)的过程推导
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{F}_n \boldsymbol{x}_n+\boldsymbol{v}_n)\\ &=\boldsymbol{F}_n{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_n+{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n} \end{aligned} \end{equation}\tag{4} \]在(4)中需要理解的是\({\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{v}_n=0\)，即\(\boldsymbol{v}_n\)与\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)是正交的，因为由观测方程可知，\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)依赖\((\boldsymbol{x}_1,...,\boldsymbol{x}_n)\)，而根据状态方程\((\boldsymbol{x}_1,...,\boldsymbol{x}_n)\)受\(n\)时刻以前的状态噪声影响，并不受\(n\)时刻的状态噪声影响，因此\(\boldsymbol{v}_n\)与\((\boldsymbol{y}_1,...,\boldsymbol{y}_n)\)是正交的。
(2) \(\hat{\boldsymbol{x}}_{n+1|n} \rightarrow \hat{\boldsymbol{x}}_{n+1|n+1}\)的过程推导
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1} \end{aligned} \end{equation}\tag{5} \]由于在(4)中我们得到了\(\hat{\boldsymbol{x}}_{n+1|n}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}\)，因此我们自然希望若(5)式满足\({\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\boldsymbol{y}_{n+1}}\boldsymbol{x}_{n+1}\)，则可以利用到已有的结论。但是，遗憾的是，这个等式并不一定满足，因为\(\boldsymbol{y}_{n+1}\)并不一定与\((\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})\)张成的空间正交。因此一个很自然的思路就是对\(\boldsymbol{y}_{n+1}\)进行正交化，令\(\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}\)，据此，可将(5)改写为
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n,\boldsymbol{y}_{n+1})}\boldsymbol{x}_{n+1}={\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{x}_{n+1}+{\rm Proj}_{\overline{\boldsymbol{y}}_{n+1}}\boldsymbol{x}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1} \end{aligned} \end{equation}\tag{6} \]由于\(\overline{\boldsymbol{y}}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}\boldsymbol{y}_{n+1}=\boldsymbol{y}_{n+1}-{\rm Proj}_{(\boldsymbol{y}_1,...,\boldsymbol{y}_n)}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}+\boldsymbol{w}_{n+1})=\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}\)，将其代入(6)有
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+{\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\overline{\boldsymbol{y}}_{n+1}\\ &=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}) \end{aligned} \end{equation}\tag{7} \]从(7)可以看出，该修正形式完全符合上面的猜想，即用观测向量的估计误差来修正状态向量的预测值。其中修正权值\(\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\)被称为卡尔曼增益。
(3) \(\boldsymbol{K}_{n+1}={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\)的计算和化简
\[\begin{equation} \begin{aligned} {\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &={\rm E}(\boldsymbol{x}_{n+1}(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}\\ &={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T} \end{aligned} \end{equation}\tag{8} \]在(8)中关键的化简步骤是第3个等号到第4个等号的变换，这边利用了\(\hat{\boldsymbol{x}}_{n+1|n}\)和\((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})\)正交的性质。
\[\begin{equation} \begin{aligned} {\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})&={\rm E}((\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})^{\rm T})\\ &=\boldsymbol{H}_{n+1}{\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\boldsymbol{H}_{n+1}^{\rm T}+{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^T)\\ &=\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1} \end{aligned} \end{equation}\tag{9} \]在(9)中\(\boldsymbol{R}_{n+1}\)表示观测噪声协方差矩阵。所以
\[\begin{equation} \begin{aligned} \boldsymbol{K}_{n+1}&={\rm E}(\boldsymbol{x}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})[{\rm E}(\overline{\boldsymbol{y}}_{n+1}\overline{\boldsymbol{y}}_{n+1}^{\rm T})]^{-1}\\ &=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})^{-1} \end{aligned} \end{equation}\tag{10} \]上面似乎完成了系统状态向量的预测和修正过程，但是需要注意的是，\(\boldsymbol{P}_{n+1|n}\)表示状态向量预测误差的协方差矩阵，这个东西无法从已知的可用的信息中获取，因此上面状态向量的更新和迭代过程，其实并没有真正完成。因此我们需要思考如何能够获得\(\boldsymbol{P}_{n+1|n}\)？由于系统真实的状态向量没有办法得知，因此直接根据定义是没有办法计算\(\boldsymbol{P}_{n+1|n}\)的，这时我们自然想到能不能仿照状态向量的更新迭代过程，用迭代的形式来获取\(\boldsymbol{P}_{n+1|n}\)，我们希望完成下面的过程: \(\boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1}\)。
(4) \(\boldsymbol{P}_{n|n} \rightarrow \boldsymbol{P}_{n+1|n}\)的推导过程
\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{v}_n)^{\rm T})\\ &={\rm E}((\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)(\boldsymbol{F}_n \boldsymbol{x}_n-\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}+\boldsymbol{v}_n)^{\rm T})\\ &=\boldsymbol{F}_n{\rm E}((\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})(\boldsymbol{x}_n-\hat{\boldsymbol{x}}_{n|n})^{\rm T})\boldsymbol{F}_n^{\rm T}+{\rm E}(\boldsymbol{v}_n\boldsymbol{v}_n)^{\rm T}\\ &=\boldsymbol{F}_n \boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n \end{aligned} \end{equation}\tag{11} \]在(11)的推导过程中用到了(1)的推导结果。\(\boldsymbol{Q}_n\)表示状态噪声协方差矩阵。
(5) \(\boldsymbol{P}_{n+1|n} \rightarrow \boldsymbol{P}_{n+1|n+1}\)的推导过程
\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1})^{\rm T}) \end{aligned} \end{equation}\tag{12} \]其中
\[\begin{equation} \begin{aligned} \boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n+1}&=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ &=\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n}-\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{x}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{w}_{n+1})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1} \end{aligned} \end{equation}\tag{13} \]所以
\[\begin{equation} \begin{aligned} \boldsymbol{P}_{n+1|n+1}&={\rm E}(((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})((\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})-\boldsymbol{K}_{n+1}\boldsymbol{w}_{n+1})^{\rm T})\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}){\rm E}((\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})(\boldsymbol{x}_{n+1}-\hat{\boldsymbol{x}}_{n+1|n})^{\rm T})(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}{\rm E}(\boldsymbol{w}_{n+1}\boldsymbol{w}_{n+1}^{\rm T})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n}(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}+\boldsymbol{K}_{n+1}\boldsymbol{R}_{n+1}\boldsymbol{K}_{n+1}^{\rm T}\\ &=\boldsymbol{P}_{n+1|n}-\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}\boldsymbol{K}_{n+1}^{\rm T}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}+\boldsymbol{R}_{n+1})\boldsymbol{K}_{n+1}^{\rm T}\\ &=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{14} \]需要注意的是在(14)中最后一个等式的化简过程中，用到了(3)的推导结果，并进行合并同类项。
至此，我们完成了卡尔曼滤波的整个推导过程，对上面整个推导过程进行整合，可以得到卡尔曼滤波包含以下5个关系式:
\[\begin{equation} \begin{aligned} \hat{\boldsymbol{x}}_{n+1|n}&=\boldsymbol{F}_n \hat{\boldsymbol{x}}_{n|n}\\ \hat{\boldsymbol{x}}_{n+1|n+1}&=\hat{\boldsymbol{x}}_{n+1|n}+\boldsymbol{K}_{n+1}(\boldsymbol{y}_{n+1}-\boldsymbol{H}_{n+1}\hat{\boldsymbol{x}}_{n+1|n})\\ \boldsymbol{K}_{n+1}&=\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}^{\rm T}(\boldsymbol{H}_{n+1}\boldsymbol{P}_{n+1|n}\boldsymbol{H}_{n+1}+\boldsymbol{R}_{n+1})^{-1}\\ \boldsymbol{P}_{n+1|n}&=\boldsymbol{F}_n\boldsymbol{P}_{n|n}\boldsymbol{F}_n^{\rm T}+\boldsymbol{Q}_n\\ \boldsymbol{P}_{n+1|n+1}&=(\boldsymbol{I}-\boldsymbol{K}_{n+1}\boldsymbol{H}_{n+1})\boldsymbol{P}_{n+1|n} \end{aligned} \end{equation}\tag{15} \]可以看出，上述5个方程描述了两个方面的内容，一个是状态向量的预测和修正(前三个方程)，另一个是预测协方差矩阵的迭代调整(相当于预测误差的实时调整)。在实际应用过程中需要给定状态向量初始值、状态向量预测误差协方差矩阵初始值、状态噪声协方差矩阵、观测噪声协方差矩阵，然后系统模型(状态转移矩阵)和观测模型(观测矩阵)即可按上式进行迭代处理。
标签：end,卡尔曼滤波,boldsymbol,rm,aligned,hat,equation 来源： https://www.cnblogs.com/PHZWJ/p/15163500.html