【现代信号处理】 06 - 卡尔曼滤波

卡尔曼滤波

1. 问题引入

  我们研究的问题一般都是从观测数据入手。我们观测的对象是一组随机变量，我们需要用这组随机变量与估计另外一个随机变量

$$Observation:(Z_1,...,Z_n)->Y$$

  我们以前学习到的估计方法，都是一些直接的方法。我们试图构造一个估计，直接通过我们的观测数据，去逼近我们的随机变量。我们通常会构造一个滤波器，输入就是我们的观测数据，输出就是我们得到的近似值。而维纳滤波器就是最优的线性估计

$$g(Z_1,..,Z_n)=>Y\to \boxed{H}\to \hat{Y}\text{ Direct Method}$$

  但是使用这种估计的思路是有一些问题的。因为这是一种黑箱的思想。一般来说，我们估计的对象对观测数据是有影响的，但是我们的观测数据是怎么来的，我们并不关心，我们把观测数据和估计变量之间的关系当做黑箱，我们只关心输入和输出。

  我们是采用这样的思路在进行信号处理

$$\begin{array}{ccccc}Y& \to & \boxed{H_1}& \to & Z\\ & & & & \\ & \nwarrow & \boxed{H_2}& \swarrow & \end{array}$$

  本来有一个随机变量Y，会通过某种规律产生观测数据Z。这是一个系统，但是我们并不需要关心这个系统是怎么样的。我们的处理方法是这样的，我们根据观测数据，重新构造了一个系统H₂，然后把输入和输出联系在了一起。

  这种方法的缺陷在于，通常我们研究某个问题的时候是有先验知识的，比如物理规律，化学反应等等。我们的估计方法直接把输入和输出关联在了一起，但是并不需要使用先验知识，只是通过统计学规律实现的。

  但是现在，我们希望从观测数据出发，了解引起观测数据变化的系统到底是怎么样子的，我们希望能够使用我们的先验知识做点什么。

2. Kalman Filter

2.1 模型建立–状态空间

  我们现在希望从黑箱入手，找到具体的系统结构，不仅仅满足于把输入和输出关系当做着眼点，把变量Y对观测数据Z的影响当做黑箱。我们想要了解这个具体的影响是什么，我们要对黑箱进行建模。

  我们要建立的模型就是状态空间模型。我们在学习信号与系统的时候，其实就有状态空间的概念了。状态空间和传递函数都是对系统描述的方法。

  状态空间和传递函数是有本质区别的。传递函数关注的是输入和输出的关系，对系统内部结构视而不见，只是把系统看成了黑箱。状态空间关注的是系统内部的结构。状态空间的建立能够用上我们的先验知识，不然统计信号处理就与科学和工程脱节了。

  建立状态空间模型涉及到两个方程。第一个方程叫做状态方程，第二个方程叫做观测方程。

$$\begin{gathered} \text{State Space Model} \\ \{\begin{array}{ll}{Z}_{n+1}=f(Z_n,V_n)& \text{State Equation}\\ Y_n=g(Z_n,W_n)& \text{Observation Equation}\end{array} \end{gathered}$$
  其中V_n是状态噪声，W_n是观测噪声

$$\{\begin{array}{ll}{Z}_{n+1}=f(Z_n,V_n,I_n)& \text{State Equation}\\ Y_n=g(Z_n,W_n)& \text{Observation Equation}\end{array}$$
  一般来说，状态方程会有一个输入变量。但是很多时候会把输入变量给忽略。我们认为决定系统输出的关键，也许并不在输入上，而是状态Z。这是系统内部的属性。

  状态Z具有两个特性

• 真正的决定了系统的性质，状态有自己的运行发展规律，随着时间变化，状态也发生变化，影响系统的发生机制。状态有自身的运行发展规律。状态的演化决定了系统内部的运行机制。
• 状态往往不能直接被观测到

  状态从外部看不到，但是又很重要，我们怎么得到状态呢？我们有一个观测，观测代表一个过程，可以通过状态得到我们的观测。这个过程就是窥探系统内部结构的窗口，就能够获得关于系统内部的某种认识。

  状态是不可观测的，但是又说状态能够被看到，我们看到的是什么?

  我们实际上是在对系统进行建模，我们对系统进行建模的时候，就能够用上我们的先验知识（自然界基本运行规律，专业知识等)。但是，我们在建模的同时，尽管状态是按照模型在演化，但是我们不能直接观测到这个状态。我们观测到的仅仅是一个侧面

  现在我们对信号处理的任务就改变了，我们希望能够通过得到的观测数据，找到系统的状态变化。一旦系统状态变化找到了，我们就把系统内部变化的规律给找到了。

{ Y 1 , . . . , Y n } → { Z 1 , . . . , Z n } \{Y_1,...,Y_n\} \rightarrow \{Z_1,...,Z_n \} {Y1​,...,Yn​}→{Z1​,...,Zn​}

  一般来说，状态空间模型中是有噪声的，我们在做信号处理的时候，一方面要注重，通过观测数据进行逆运算，得到状态；另一方面也要克服噪声的影响

  现在，我们想对状态空间模型进行简化。开始的时候，我们建立的状态和观测方程是个通常的非线性模型。现在我们建立一个线性的模型，简化问题。

$$\{\begin{array}{l}{Z}_{n+1}=F_n{Z}_n+V_n\\ Y_n=H_n{Z}_n+W_n\end{array}$$
  需要注意一点，在这个线性模型中，系统参数F和H是时变的，我们允许系统参数随着时间变化。这就是意味着，我们允许我们的系统是非平稳的。

  我们在维纳滤波，莱文森迭代，前向后向预测的时候，都需要平稳性的条件。这里的非平稳性，能够让模型的应用更加广泛。

  现在我们对噪声进行假设。我们假设噪声的均值为0，并且噪声是白噪声。而且状态噪声和观测噪声之间没有相关性。
$$\{\begin{array}{l}{Z}_{n+1}=F_n{Z}_n+V_n\\ Y_n=H_n{Z}_n+W_n\end{array}$$

$$\begin{gathered} E(V_n)=E(W_n)=0E(V_n\ast W_n^T)=0 \\ E(V_n{V}_m^T)=Q_n{\delta }_{mn}E(W_n{W}_m^T)=R_n{\delta }_{mn} \end{gathered}$$
  这里我们建立的模型非常具有一般性，因为我们的Z和Y都是矢量，F和H都是矩阵

2.2 处理流程–预测校正

  我们完成了系统建模之后，后面进行的处理流程就叫做卡尔曼滤波。

  其实卡尔曼滤波一共包含了两种思想，第一种就是前面状态空间，第二种就是这里要介绍的预测校正了。Prediction-Correection。

  预测校正是一种递推。这种递推是沿着时间进行递推，而不是系统的阶数。

  我们首先要得到一个估计，就是基于观测数据Y₁,…,Y_n来对Z进行估计的结果。我们假设这种估计是线性的。也就是估计值就是Z_n投影到Y张成的子空间上

Z ^ n ∣ n = E ( Z n ∣ Y 1 , . . . , Y n ) = P r o j { Y 1 , . . . , Y n } Z n \hat Z_{n|n} = E(Z_n |Y_1,...,Y_n) = Proj_{\{Y_1,...,Y_n \} }Z_n Z^n∣n​=E(Zn​∣Y1​,...,Yn​)=Proj{Y1​,...,Yn​}​Zn​

  根据对时间的递推，我们还可以得到两个新的估计。

Z ^ n + 1 ∣ n = P r o j { Y 1 , . . . , Y n } Z n + 1 Z ^ n + 1 ∣ n + 1 = P r o j { Y 1 , . . . , Y n + 1 } Z n + 1 \hat Z_{n+1|n} =Proj_{\{Y_1,...,Y_n \} }Z_{n+1} \\ \hat Z_{n+1|n+1} =Proj_{\{Y_1,...,Y_n+1 \} }Z_{n+1} Z^n+1∣n​=Proj{Y1​,...,Yn​}​Zn+1​Z^n+1∣n+1​=Proj{Y1​,...,Yn​+1}​Zn+1​

  前者是通过1~n的观测数据对Z的新状态进行预测。但是预测就一定不是很准确，需要进行校正，因此我们还需要第二个方程，用新材料进行校正。也就是用1~n+1 的数据对Z的n+1状态进行估计。第二步的校正就是基于预测的结果进行线性变换

Prediction-Correction Z ^ n ∣ n = E ( Z n ∣ Y 1 , . . . , Y n ) = P r o j { Y 1 , . . . , Y n } Z n ⇓ P r e d i c t i o n Z ^ n + 1 ∣ n = P r o j { Y 1 , . . . , Y n } Z n + 1 ⇓ C o r r e c t i o n Z ^ n + 1 ∣ n + 1 = P r o j { Y 1 , . . . , Y n + 1 } Z n + 1 \text{Prediction-Correction} \\ \hat Z_{n|n} = E(Z_n |Y_1,...,Y_n) = Proj_{\{Y_1,...,Y_n \} }Z_n \\ \Downarrow Prediction \\ \hat Z_{n+1|n} =Proj_{\{Y_1,...,Y_n \} }Z_{n+1} \\ \Downarrow Correction \\ \hat Z_{n+1|n+1} =Proj_{\{Y_1,...,Y_n+1 \} }Z_{n+1} Prediction-CorrectionZ^n∣n​=E(Zn​∣Y1​,...,Yn​)=Proj{Y1​,...,Yn​}​Zn​⇓PredictionZ^n+1∣n​=Proj{Y1​,...,Yn​}​Zn+1​⇓CorrectionZ^n+1∣n+1​=Proj{Y1​,...,Yn​+1}​Zn+1​

  我们可以大概来猜测一个

$${\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+g(Y_{n+1}-{\hat{Y}}_{n+1})$$

  我们做的校正可能与新数据的偏差有关系，是新数据偏差的一个函数，但是我们希望尽可能简单一点，我们把它做成线性的。同时因为系统是时变的，因此这个系数我们也让他是时变的

$${\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+K_n(Y_{n+1}-{\hat{Y}}_{n+1})$$

2.3 求解

2.3.1 预测

  现在我们可以做Kalman Filter了

  首先我们要对新的状态Z进行预测

( 1 ) Z ^ n ∣ n ⇒ Z ^ n + 1 ∣ n Z ^ n + 1 ∣ n = P r o j { Y 1 , . . . , Y n } Z n + 1 = P r o j { Y 1 , . . . , Y n } ( F n Z n + V n ) = P r o j { Y 1 , . . . , Y n } ( F n Z n ) + P r o j { Y 1 , . . . , Y n } ( V n ) = P r o j { Y 1 , . . . , Y n } ( F n Z n ) (1) \quad \hat Z{n|n} \Rightarrow \hat Z_{n+1|n} \\ \hat Z_{n+1|n} = Proj_{\{Y_1,...,Y_n\}}Z_{n+1} = Proj_{\{Y_1,...,Y_n\}}(F_n Z_n + V_n) \\ = Proj_{\{Y_1,...,Y_n\}}(F_n Z_n)+ Proj_{\{Y_1,...,Y_n\}}(V_n) \\ = Proj_{\{Y_1,...,Y_n\}}(F_n Z_n) (1)Z^n∣n⇒Z^n+1∣n​Z^n+1∣n​=Proj{Y1​,...,Yn​}​Zn+1​=Proj{Y1​,...,Yn​}​(Fn​Zn​+Vn​)=Proj{Y1​,...,Yn​}​(Fn​Zn​)+Proj{Y1​,...,Yn​}​(Vn​)=Proj{Y1​,...,Yn​}​(Fn​Zn​)

  我们可以看到Y_n这组观测数据是与Z_n有关系的，而Z_n与前一个状态的状态噪声V_n-1有关，因此可以判断Y_n这组数据和V_n是无关的，因此投影为0

  根据投影公式

P r o j { Y } Z = E ( Z Y T ) [ E ( Y Y T ) ] − 1 Y Proj_{\{Y\}} Z = E(ZY^T)[E(YY^T)]^{-1}Y Proj{Y}​Z=E(ZYT)[E(YYT)]−1Y

  如果前面有系数矩阵

P r o j { Y } A ∗ Z = A ∗ E ( Z Y T ) [ E ( Y Y T ) ] − 1 Y Proj_{\{Y\}} A*Z = A*E(ZY^T)[E(YY^T)]^{-1}Y Proj{Y}​A∗Z=A∗E(ZYT)[E(YYT)]−1Y

  因此

Z ^ n + 1 ∣ n = P r o j { Y 1 , . . . , Y n } ( F n Z n ) = F n ∗ P r o j { Y 1 , . . . , Y n } ( Z n ) = F n ∗ Z ^ n ∣ n \hat Z_{n+1|n} = Proj_{\{Y_1,...,Y_n\}}(F_n Z_n) \\ = F_n * Proj_{\{Y_1,...,Y_n\}}(Z_n) = F_n * \hat Z_{n|n} Z^n+1∣n​=Proj{Y1​,...,Yn​}​(Fn​Zn​)=Fn​∗Proj{Y1​,...,Yn​}​(Zn​)=Fn​∗Z^n∣n​

2.3.2 校正

  得到了预测值之后，我们还需要计算校正值

$${\hat{Z}}_{n+1|n}\Rightarrow {\hat{Z}}_{n+1|n+1}$$

Z ^ n + 1 ∣ n + 1 = P r o j { Y 1 , . . . , Y n , Y n + 1 } Z n + 1 \hat Z_{n+1|n+1} = Proj_{\{Y_1,...,Y_n,Y_{n+1}\}}Z_{n+1} Z^n+1∣n+1​=Proj{Y1​,...,Yn​,Yn+1​}​Zn+1​

  我们已经得到了Y₁,…Y_n对Z_n+1的估计，如果我们要得到了Y₁,…Y_n+1对Z_n+1的估计，我们只需要让Y_n+1投影在前n个观测数据张成的向量空间即可

Z ^ n + 1 ∣ n + 1 = P r o j { Y 1 , . . . , Y n , Y n + 1 } Z n + 1 = P r o j { Y 1 , . . . , Y n } Z n + 1 + P r o j { Y ˉ n + 1 } Z n + 1 \hat Z_{n+1|n+1} = Proj_{\{Y_1,...,Y_n,Y_{n+1}\}}Z_{n+1} \\ = Proj_{\{Y_1,...,Y_n\}}Z_{n+1} + Proj_{\{\bar Y_{n+1}\}}Z_{n+1} Z^n+1∣n+1​=Proj{Y1​,...,Yn​,Yn+1​}​Zn+1​=Proj{Y1​,...,Yn​}​Zn+1​+Proj{Yˉn+1​}​Zn+1​
  其中

Y ˉ n + 1 = Y n + 1 − P r o j { Y 1 , . . . , Y n } Y n + 1 \bar Y_{n+1} = Y_{n+1}-Proj_{\{Y_1,...,Y_n\}}Y_{n+1} \\ Yˉn+1​=Yn+1​−Proj{Y1​,...,Yn​}​Yn+1​

  则

Z ^ n + 1 ∣ n + 1 = P r o j { Y 1 , . . . , Y n } Z n + 1 + P r o j { Y ˉ n + 1 } Z n + 1 = Z ^ n + 1 ∣ n + E ( Z n + 1 Y ˉ n + 1 T ) [ E ( Y ˉ n + 1 Y ˉ n + 1 T ) ] − 1 Y ˉ n + 1 ( i ) \hat Z_{n+1|n+1}=Proj_{\{Y_1,...,Y_n\}}Z_{n+1} + Proj_{\{\bar Y_{n+1}\}}Z_{n+1} \\ = \hat Z_{n+1|n} + E(Z_{n+1}\bar Y_{n+1}^T)[E(\bar Y_{n+1}\bar Y_{n+1}^T)]^{-1}\bar Y_{n+1} \quad\quad(i) Z^n+1∣n+1​=Proj{Y1​,...,Yn​}​Zn+1​+Proj{Yˉn+1​}​Zn+1​=Z^n+1∣n​+E(Zn+1​Yˉn+1T​)[E(Yˉn+1​Yˉn+1T​)]−1Yˉn+1​(i)

  我们继续来看

Y ˉ n + 1 = Y n + 1 − P r o j { Y 1 , . . . , Y n , Y n + 1 } Y n + 1 = Y n + 1 − P r o j { Y 1 , . . . , Y n } ( H n + 1 Z n + 1 + W n + 1 ) = Y n + 1 − P r o j { Y 1 , . . . , Y n } ( H n + 1 Z n + 1 ) = Y n + 1 − H n + 1 Z ^ n + 1 ∣ n ( i i ) \bar Y_{n+1} = Y_{n+1}-Proj_{\{Y_1,...,Y_n,Y_{n+1}\}}Y_{n+1} \\ = Y_{n+1}-Proj_{\{Y_1,...,Y_n\}}(H_{n+1}Z_{n+1}+W_{n+1}) \\ = Y_{n+1}-Proj_{\{Y_1,...,Y_n\}}(H_{n+1}Z_{n+1}) \\ = Y_{n+1}-H_{n+1} \hat Z_{n+1|n} \quad\quad(ii) Yˉn+1​=Yn+1​−Proj{Y1​,...,Yn​,Yn+1​}​Yn+1​=Yn+1​−Proj{Y1​,...,Yn​}​(Hn+1​Zn+1​+Wn+1​)=Yn+1​−Proj{Y1​,...,Yn​}​(Hn+1​Zn+1​)=Yn+1​−Hn+1​Z^n+1∣n​(ii)

  由于Y_n数据的观测噪声是W_n,因此W_n+1和Y_n时间上对不上，因此这个噪声也与Y_n不相关是0

  把(ii)代入(i)中得

$$\begin{gathered} {\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+K_{n+1}\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}) \\ {K}_{n+1}=E(Z_{n+1}{\bar{Y}}_{n+1}^T)[E({\bar{Y}}_{n+1}{\bar{Y}}_{n+1}^T){]}^{-1} \end{gathered}$$

  所以我们推测的与实际的是一致的

2.3.3 计算卡尔曼增益

  我们要把k计算出来，我们把k叫做卡尔曼增益 kalman gain。k在实际应用中是要调节的

  卡尔曼增益是把估计的残差做转化，然后来校正预测值。卡尔曼增益决定了，你在多大程度上信任你原来的预测值，和在多大程度上信息新的观测。

  我们以雷达跟踪为例。

  比如我们对数据进行预测，得到一个预测值，但是跟观测值相差很大，计算机就需要猜测，观测值到底是真的还是野数据。如果我们认为他是野数据，就会让卡尔曼增益减小，不考虑新值

  但是我们后面发现，往后的观测值全部在偏差很大，那我们就需要继续考虑卡尔曼增益。雷达跟踪最困难的就是如何把控这个gain

  下面来计算增益矩阵

$$K_{n+1}=E(Z_{n+1}{\bar{Y}}_{n+1}^T)[E({\bar{Y}}_{n+1}{\bar{Y}}_{n+1}^T){]}^{-1}$$

---

  先计算前半部分

$$\begin{gathered} E(Z_{n+1}{\bar{Y}}_{n+1}^T)=E(Z_{n+1}(Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}{)}^T) \\ =E(Z_{n+1}(H_{n+1}{Z}_{n+1}+W_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}{)}^T) \\ =E(Z_{n+1}\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T\ast H_{n+1}^T) \end{gathered}$$

  因为估计的残差肯定与原材料正交，而 \hat Z_n+1|n是 Z₁,…,Z_n的线性组合，就是估计的原材料，所以我们可以补一项。同时，这里面H是个确定值，没有随机性，可以放到期望外面

$$\begin{gathered} E(Z_{n+1}{\bar{Y}}_{n+1}^T)=E(Z_{n+1}\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T\ast H_{n+1}^T) \\ =E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T\ast H_{n+1}^T) \\ =E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T)\ast H_{n+1}^T \\ =P_{n+1|n}\ast H_{n+1}^T \end{gathered}$$

  其中

$$P_{n+1|n}=E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T)$$

  得到的就是预测误差的协方差矩阵

---

  再计算后半部分

$$\begin{gathered} E({\bar{Y}}_{n+1}{\bar{Y}}_{n+1}^T)=E[(Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n})\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}{)}^T] \\ =E[(H_{n+1}{Z}_{n+1}+W_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n})\ast (H_{n+1}{Z}_{n+1}+W_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}{)}^T] \\ =E[(W_{n+1}+H_{n+1}(Z_{n+1}-{\hat{Z}}_{n+1|n}))\ast (W_{n+1}+H_{n+1}(Z_{n+1}-{\hat{Z}}_{n+1|n}){)}^T] \\ =H_{n+1}\ast E[(Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T]\ast H_{n+1}^T+E(W_{n+1}\ast W_{n+1}^T) \\ =H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1} \end{gathered}$$

  其中

$$\begin{gathered} P_{n+1|n}=E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T) \\ {R}_{n+1}=E(W_{n+1}\ast W_{n+1}^T) \end{gathered}$$

  计算得到卡尔曼增益]

$$K_{n+1}=P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}$$

  整理一下目前的结果

• 预测：在原有估计的基础上，通过状态方程进行预测
• 校正：在预测的基础上，通过卡尔曼增益，实现校正
• 卡尔曼增益：也算出来结果了

$$\begin{gathered} {\hat{Z}}_{n+1|n}=F_n\ast {\hat{Z}}_{n|n}(1) \\ {\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+K_{n+1}\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n})(2) \\ {K}_{n+1}=P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}(3) \end{gathered}$$

  这里有个问题，我们并没有计算清楚协方差矩阵P，因此我们还需要继续计算

2.3.4 计算协方差矩阵

  我们依旧采用递推的方法进，我们类比状态的递推

$$\begin{gathered} {\hat{Z}}_{n|n}\to {\hat{Z}}_{n+1|n}\to {\hat{Z}}_{n+1|n+1} \\ {P}_{n|n}\to P_{n+1|n}\to P_{n+1|n+1} \end{gathered}$$

  先完成第一步

$$P_{n|n}\to P_{n+1|n}$$

$$\begin{gathered} P_{n+1|n}=E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T) \\ =E((F_n{Z}_n+V_n-F_n{\hat{Z}}_{n|n})(F_n{Z}_n+V_n-F_n{\hat{Z}}_{n|n}{)}^T) \\ =E((F_n(Z_n-{\hat{Z}}_{n|n})+V_n)\ast (F_n(Z_n-{\hat{Z}}_{n|n})+V_n{)}^T) \\ =F_n\ast E((Z_n-{\hat{Z}}_{n|n})\ast (Z_n-{\hat{Z}}_{n|n}{)}^T)\ast F_n^T+E(V_n\ast V_n^T) \\ =F_n{P}_{n|n}{F}_n^T+Q_n(9) \end{gathered}$$

  其中

$$\begin{gathered} P_{n|n}=E((Z_n-{\hat{Z}}_{n|n})\ast (Z_n-{\hat{Z}}_{n|n}{)}^T) \\ {Q}_n=E(V_n\ast V_n^T) \end{gathered}$$

  再完成第二步

$$P_{n+1|n}\to P_{n+1|n+1}$$

$$P_{n+1|n+1}=E((Z_{n+1}-{\hat{Z}}_{n+1|n+1})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n+1}{)}^T)$$

  我们前面已经计算过了校正结果

$${\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+K_{n+1}\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n})$$

  所以，代入(2)式

$$\begin{gathered} Z_{n+1}-{\hat{Z}}_{n+1|n+1}=Z_{n+1}-{\hat{Z}}_{n+1|n}-K_{n+1}\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}) \\ =Z_{n+1}-{\hat{Z}}_{n+1|n}-K_{n+1}\ast (H_{n+1}\ast Z_{n+1}+W_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n}) \\ =(Z_{n+1}-{\hat{Z}}_{n+1|n})-H_{n+1}\ast K_{n+1}\ast (Z_{n+1}-{\hat{Z}}_{n+1|n})-K_{n+1}\ast W_{n+1} \\ =(I-H_{n+1})(Z_{n+1}-{\hat{Z}}_{n+1|n})-K_{n+1}\ast W_{n+1} \end{gathered}$$
  代入

$$\begin{gathered} P_{n+1|n+1}=E((Z_{n+1}-{\hat{Z}}_{n+1|n+1})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n+1}{)}^T) \\ =(I-K_{n+1}{H}_{n+1})\ast E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T)\ast (I-K_{n+1}{H}_{n+1}{)}^T-K_{n+1}\ast E(W_{n+1}{W}_{n+1}^T)\ast K_{n+1}^T \\ =(I-K_{n+1}{H}_{n+1})\ast P_{n+1|n}\ast (I-K_{n+1}{H}_{n+1}{)}^T+K_{n+1}\ast R_{n+1}\ast K_{n+1}^T \end{gathered}$$

  其中

$$\begin{gathered} P_{n+1|n}=E((Z_{n+1}-{\hat{Z}}_{n+1|n})\ast (Z_{n+1}-{\hat{Z}}_{n+1|n}{)}^T) \\ {R}_{n+1}=E(W_{n+1}{W}_{n+1}^T) \end{gathered}$$

  继续化简

$$\begin{gathered} (I-K_{n+1}{H}_{n+1})\ast P_{n+1|n}\ast (I-K_{n+1}{H}_{n+1}{)}^T+K_{n+1}\ast R_{n+1}\ast K_{n+1}^T \\ =P_{n+1|n}-K_{n+1}{H}_{n+1}\ast P_{n+1|n}-P_{n+1|n}\ast H_{n+1}^T{K}_{n+1}^T \\ +K_{n+1}{H}_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T{K}_{n+1}^T+K_{n+1}\ast R_{n+1}\ast K_{n+1}^T \\ =P_{n+1|n}-K_{n+1}{H}_{n+1}\ast P_{n+1|n}-P_{n+1|n}\ast H_{n+1}^T{K}_{n+1}^T \\ +K_{n+1}\ast (H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1})\ast K_{n+1}^T(4) \end{gathered}$$

  计算一下最后一步

$$K_{n+1}\ast (H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1})\ast K_{n+1}^T(5)$$

$$K_{n+1}=P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}(3)$$

  （5）代入(3)式

  这里需要注意一下，协方差矩阵R是对称的，然后H_n+1P_n+1|nH_n+1^T是个合同矩阵，也是对称的，因此这部分的转置是他本身。P也是协方差矩阵，转置是他本身

$$\begin{gathered} K_{n+1}\ast (H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1})\ast K_{n+1}^T \\ =P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}\ast \\ (H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1})\ast \\ [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}\ast H_{n+1}\ast P_{n+1|n} \\ =P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}{H}_{n+1}^T+R_{n+1}{]}^{-1} \\ \ast H_{n+1}\ast P_{n+1|n} \\ =K_{n+1}\ast H_{n+1}\ast P_{n+1|n}(6) \end{gathered}$$

  然后我们发现(5)式子是对称的，因此化简得到的矩阵(6)也是对称的,所以根据转置相等，可以得到(7)

$$K_{n+1}\ast H_{n+1}\ast P_{n+1|n}=P_{n+1|n}\ast H_{n+1}^T\ast K_{n+1}^T(7)$$

  (7)代入(4)中

$$\begin{gathered} P_{n+1|n+1} \\ =(I-K_{n+1}{H}_{n+1})\ast P_{n+1|n}\ast (I-K_{n+1}{H}_{n+1}{)}^T+K_{n+1}\ast R_{n+1}\ast K_{n+1}^T \\ =P_{n+1|n}-K_{n+1}{H}_{n+1}\ast P_{n+1|n}-P_{n+1|n}\ast H_{n+1}^T{K}_{n+1}^T \\ +K_{n+1}\ast (H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1})\ast K_{n+1}^T \\ =P_{n+1|n}-K_{n+1}{H}_{n+1}\ast P_{n+1|n}-P_{n+1|n}\ast H_{n+1}^T{K}_{n+1}^T \\ +P_{n+1|n}\ast H_{n+1}^T\ast K_{n+1}^T \\ =P_{n+1|n}-K_{n+1}{H}_{n+1}\ast P_{n+1|n} \\ =(I-K_{n+1}{H}_{n+1})\ast P_{n+1|n}(8) \end{gathered}$$

2.3.5 卡尔曼滤波完整方程式

  模型
$$\{\begin{array}{l}{Z}_{n+1}=F_n{Z}_n+V_n\\ Y_n=H_n{Z}_n+W_n\end{array}$$

$$\begin{gathered} E(V_n)=E(W_n)=0E(V_n\ast W_n^T)=0 \\ E(V_n{V}_m^T)=Q_n{\delta }_{mn}E(W_n{W}_m^T)=R_n{\delta }_{mn} \end{gathered}$$

  我们得到了卡尔曼滤波的完整方程

• 预测：在原有估计的基础上，通过状态方程进行预测
• 校正：在预测的基础上，通过卡尔曼增益，实现校正
• 卡尔曼增益：也算出来结果了
• 协方差矩阵的递推

$$\begin{gathered} {\hat{Z}}_{n+1|n}=F_n\ast {\hat{Z}}_{n|n}(1) \\ {\hat{Z}}_{n+1|n+1}={\hat{Z}}_{n+1|n}+K_{n+1}\ast (Y_{n+1}-H_{n+1}{\hat{Z}}_{n+1|n})(2) \\ {K}_{n+1}=P_{n+1|n}\ast H_{n+1}^T\ast [H_{n+1}\ast P_{n+1|n}\ast H_{n+1}^T+R_{n+1}{]}^{-1}(3) \\ {P}_{n+1|n}=F_n{P}_{n|n}{F}_n^T+Q_n(9) \\ {P}_{n+1|n+1}=(I-K_{n+1}{H}_{n+1})\ast P_{n+1|n}(8) \end{gathered}$$

  卡尔曼滤波非常有意思的地方在于，除了递推状态方程以外，还需要递推误差，也就是协方差矩阵。这个递推误差非常重要，这是一个性能评估。

  卡尔曼在信号处理领域，首先实现了在线误差评估。

  我们一开始需要有一个初值，这个初值可以随便设置，卡尔曼滤波会随着数据的增多慢慢学习。初值给的很离谱都无所谓，随着时间的变化会慢慢的接近真实值。

3. 举例

  假设我们要对空中飞行目标进行建模,我们假设状态包括，位置，速度，加速度三个量

$$(X_n,X_n^{\prime },X_n^{\prime \prime })$$

  我们要计算我们的状态方程

$$X_{n+1}=X_n+X_n^{\prime }\ast \Delta t+\frac{1}{2}{X}_n^{\prime \prime }(\Delta t{)}^2$$

$$X_{n+1}^{\prime }=X_n^{\prime }+X_n^{\prime \prime }\Delta t$$

$$X_n^{\prime \prime }=X_n^{\prime \prime }$$

  建立矩阵方程。我们建模是通过先验知识建立的
$$\left(\begin{array}{c}{X}_{n+1}\\ X_{n+1}^{\prime }\\ X_{n+1}^{\prime \prime }\end{array}\right)=\left(\begin{array}{ccc}1& \Delta t& \frac{1}{2}(\Delta t{)}^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right)\ast \left(\begin{array}{c}{X}_n\\ X_n^{\prime }\\ X_n^{\prime \prime }\end{array}\right)+V_n$$

  
$$F=\left(\begin{array}{ccc}1& \Delta t& \frac{1}{2}(\Delta t{)}^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right)$$

  我们的观测方程就是位置

$$y_n=X_n+W_n=\left(\begin{array}{ccc}1& 0& 0\end{array}\right)\ast \left(\begin{array}{c}{X}_n\\ X_n^{\prime }\\ X_n^{\prime \prime }\end{array}\right)+W_n$$

$$H=\left(\begin{array}{ccc}1& 0& 0\end{array}\right)$$

  我们要明确一个事情，没有一个模型是完美无缺的。也没有一个模型是一无是处的。我们建立的这个模型，在时间足够小的时候，可以近似认为加速度是不变的。

  不过模型确实可能建立错误。卡尔曼滤波最怕的就是模型失配–Model Miswatch。

  比如。矢量喷管技术能够让飞机横向平移，虽然这是违背牛顿力学的。因此矢量喷管技术能够让飞机加速度突变，让牛顿力学失效，也就能让飞机躲避导弹追踪。

  因此，模型建立的准确性对卡尔曼滤波有着决定性作用。比如看看弹道学去估计导弹飞行轨迹的建模。卡尔曼滤波写于1960年，卡尔曼滤波最成功的应用，就是在航空航天上。