Kalman

卡尔曼滤波所要解决的问题？

离散时间线性系统的状态进行最优估算

滤波器模型

考虑一个有多个输入量(又称控制量)和多个输出量(又称观测量)的离散时间线性动态系统(MIMO)，其控制过程一般可用以下的线性差分方程式来表示:

$$\begin{aligned} &\boldsymbol{x}(t_{k})=A(t_{k},t_{k-1})\boldsymbol{x}(t_{k-1})+\boldsymbol{B}(t_{k-1})\boldsymbol{u}(t_{k-1})+\boldsymbol{w}(t_{k-1})&& \left(1\right) \\ \end{aligned}$$

其中：

$t_k$ 代表第 $k$ 个测量时刻或第 $k$ 测量历元所对应的时间；

$\boldsymbol{u}$ 代表系统的输入向量；

$\boldsymbol{x}$ 代表系统的状态向量；

$\boldsymbol{w}$ 代表过程噪声向量；

$A(t_{k},t_{k-1})$ 代表从 $t_{k-1}$ 到 $t_{k}$ 时刻的状态转移矩阵；

$ \boldsymbol{B}(t_{k-1})$代表在 $t_{k-1}$时刻系统输入量与系统状态之间的关系矩阵（输入矩阵）。系统的输入量是个可选项，即有些系统可以没有输入量，比如用户 GPS接收机这个定位系统通常视为没有任何输入；

状态转移矩阵$A(t_{k},t_{k-1})$与输入关系矩阵$ \boldsymbol{B}(t_{k-1})$在不同时刻可以是不同的(时变系统)，但为了简化公式表达，假定其值固定不变（时不变系统）。这样式(1)可简写成：

$$x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} \left(2\right)$$

式(2)称为卡尔曼滤波的状态方程，它描述了系统状态如何随时间变化，是系统的动态模型系统的控制过程。

由多个状态变量组成的状态向量 $ x_{k} $ 全面描述了系统在当前时刻的运行状况，它们的值通常是未知的，但一般却是我们所想要了解、掌握的。各个状态变量必须具有可观性，即它们的值能直接或间接地反映在对系统的观测量之中，而我们应用卡尔曼滤波器的目的正是为了达到从系统观测量 $ y_{k} $ 来估算系统状态 。卡尔曼波假定系统的状态向量 与观测向量存在以下线性关系:

$$y_{k} =Cx_{k}+\nu_{k}\quad(3)$$

其中，$C$ 代表观测量与系统状态之间的关系矩阵，以代表测量噪声向量。式(3)称为卡尔曼滤波的测量方程，它描述了当前的系统测量值与系统状态之间的关系。尽管卡尔曼滤波允许测量关系矩阵的值随时间而变化，但是为了简化起见，我们在上式中将它视为一个常系数阵$C$ 。

卡尔曼滤波用上述的一个状态方程式(1)和一个测量(观测)方程式(2)来完整描述一个线性动态系统，它们是该线性动态系统的数学模型。对于一个有 $L$ 个输入变量、$M$ 个观测变量以及 $N$个状态变量的系统，卡尔曼滤波器涉及以下一些系统参变量:

$x_{k}$ 由N个状态变量$(x_{1,k},x_{2,k},\cdots,x_{N,k})$所组成的${N\times1}$的状态向量；

kalman滤波两大假设

• 假定向量 $w_{k}$ 中的每个过程噪声变量为一个均值为零的正态白噪声，向量 $w_{k}$ 中的各个过程噪声变量代表系统输入量 $u$ 所包含的以及由系统内部所产生的随机噪声误差。即：

  $$\begin{aligned} E({\boldsymbol{w}}_{k})=0& \quad{(4) } \\ \mathrm{Cov}(w_{k})=E(w_{k}w_{k}^{\mathrm{T}})=Q& \quad{(5) } \end{aligned}$$

  也就是说 $w_k$ 的概率分布呈$N(0,Q)$。过程噪声向量的协方差矩阵为一个$N\times N$的对称矩阵,但它并不一定是个对角阵。

• 假定向量$v_{k}$中的每个观测噪声变量为一个均值为零的正态白噪声，向量$v_{k}$中的各个测量噪声变量代表系统观测量 $y_{k} $ 所包含的随机测量误差与噪声。即：

  $$\begin{aligned}E(\nu_k)&=\boldsymbol{0}&(6)\\ \mathrm{Cov}(\nu_k)&=E(\nu_k\nu_k^\mathrm{T})=\boldsymbol{R}&(7)\end{aligned}$$

​ 也就是说 $v_k$ 的概率分布呈$N(0,R)$。过程噪声向量的协方差矩阵为一个$M\times M$的对称矩阵,但它并不一定是个对角阵。

​

尽管噪声向量 $w_{k}$ 和 $v_{k}$的值是未知的，但它们的协方差矩阵Q和R对卡尔曼波器来说是已知的。尽管协方差矩阵Q和R的值一般会随着时间的不同而变化，但是为了简化公式表达，我们这里认为它们均是一个常系数矩阵。此外，卡尔曼滤波假定过程噪声向量 $w_{k}$ 中的各分量与测量噪声向量 $v_{k}$ 中的各分量互不相关，即

$$E(w_{i}v_{j}^{\mathrm{T}})=0 \quad{(8) }$$

滤波算法

卡尔曼滤波用一套数学递推公式对系统状态进行最优估计，使系统状态的估计值有最小均方误差(MMSE)。假设 $\hat{x}_{k-1}$ 代表第$k-1$个时刻卡尔曼滤波器对系统状态 ${x}_{k-1}$ 的最优估计值，那么在同时给出输人量 $u_{k-1}$ 和观测量 $y_{k}$ 的条件下，来推导第 $k$ 个时刻卡尔曼滤波器对系统状态${x}_{k}$ 的最优估计值。上标 “^” 代表估计值，从 $\hat{x}_{k-1}$和 $u_{k-1}$ 出发，卡尔曼波利用状态方程式(1)来预测第 $k$ 时刻的状态 ${x}_{k}$ ，即：

$$\hat{x}_{k}^{-}=A\hat{x}_{k-1}+Bu_{k-1} \quad{(9) }$$

因为估计值$ \hat{x}_{k}^{-} $ 还没有得到测量值 $ y_{k}$ 的验证，所以通常被称为先验估计值，并用右上标 $ ^- $代表先验。先验估计误差 $e_{k}^{-}$ 定义为系统状态的真实值${x}_{k}$ 与其先验估计值 $ \hat{x}_{k}^{-} $ 之间的差异，即

$$e_{k}^{-}=x_{k}-\hat{x}_{k}^{-} \quad{(10)}$$

先验估计误差 $e_{k}^{-}$ 的协方差阵 $P_{k}^{-}$ 称为状态均方误差阵(或称误差协方差阵)，它可直接根据协方差的定义而表达成：

$$P_{k}^{-}=E(\vec{e_{k}}\vec{e_{k}}^{-\mathrm{T}})\quad(11)$$

有了对当前状态 ${x}_{k}$ 的先验估计值$ \hat{x}_{k}^{-} $ ，就可根据测量方程式(2)而认为第k 历元的观测量值应该等于 $C\hat{x}_{k}^{-}$ 再加上未知的测量噪声。观测量的实际值 $y_{k}$ 与其预测值 $C\hat{x}_{k}^{-}$之间的差异，即称为观测量的残余。即：

$$r_{k}=y_{k}-C\hat{x}_{k}^{-} \quad(12)$$

接着，卡尔曼滤波将先验估计值 $ \hat{x}_{k}^{-} $ 与观测量残余 $r_{k}$ 的线性组合作为对状态 ${x}_{k}$ 的最优估计值$\hat{x}_{k}$，即

$$\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}r_{k}=\hat{x}_{k}^{-}+K_{k}\left(y_{k}-C\hat{x}_{k}^{-}\right)\quad(13)$$

其中，系数矩阵 $K_{k}$ 称为卡尔曼滤波增益。因为实际观测量$y_{k}$ 已经核对、校正了估计值$\hat{x}_{k}$，所以，通常$\hat{x}_{k}$称为后验估计值。

类似于式(10)和式(11)，可定义后验估计值的误差ek为

$$e_{k}=x_{k}-\hat{x}_{k} \quad(14)$$

定义后验估计误差$e_{k}$的均方误差阵$P_{k}$为

$$P_{k}=E(e_{k}e_{k}^{\mathrm{T}}) \quad(15)$$

上述后验估计均方误差阵 $P_{k}$ 中的各个对角线元素分别对应于各个状态变量估计值的均方误差(附录C随机变量和随机过程)。下面要做的是推导出增益矩阵$K_{k}$的最优值，从而使得均方误差阵 $P_{k}$ 的对角线元素之和最小，那么由此得到的卡尔曼滤波状态估计值 $\hat{x}_{k}$ 就有最小均方误差，相应地卡尔曼滤波器在此意义上也就是一种最优的滤波器。

为了推导 $K_{k}$，我们得先对$e_{k}$ 与 $P_{k}$ 进行一些整理。将式(3)、式(10)和式(13)代入式(14)，经整理后得:

$$e_{k}=(I-K_{k}C)e_{k}^{-}-K_{k}\nu_{k} \quad(16)$$

然后将式(7)、式(11)和式(16)代人式(15)，经整理后得

$$P_{k}=P_{k}^{-}-K_{k}CP_{k}^{-}-\left(K_{k}CF_{k}^{-}\right)^{\mathrm{T}}+K_{k}\left(CP_{k}^{-}C^{\mathrm{T}}+R\right)K_{k}^{\mathrm{T}} \quad(17)$$

TODO:详细推导过程

$P_{k}$ 是一个关于 $K_{k}$的二次型函数，关于$P_{k}$ 的二次型函数存在最小值。求导，并使其导数值等于零，我们就可得到使$P_{k}$ 的秩值最小的$K_{k}$，的解，即

$$K_k=P_k^-C^\mathrm{T}\left(CP_k^-C^\mathrm{T}+R\right)^{-1} \quad(18)$$

再将式(18)中的$K_{k}$代回到式(17)，得:

$$P_k=(I-K_kC)P_k^- \quad(19)$$

在此，我们将卡尔曼滤波的递推算法总结一下。如图 所示，卡尔曼滤波算法可分为预测和校正两个过程。

(1)预测:预测过程又叫时间更新过程，它在上一个历元( 即第$k-1$)状态估计值的基础上利用系统的状态方程来预测当前这一(即第$ k $)个历元的状态值。这一过程涉及以下两个公式

$$\begin{aligned}\hat{\boldsymbol{x}}_k^-&=A\hat{\boldsymbol{x}}_{k-1}+B\boldsymbol{u}_{k-1}&(6.31)\\P_k^-&=A\boldsymbol{P}_{k-1}A^\mathrm{T}+\boldsymbol{Q}&(6.32)\end{aligned}$$

其中，式(6.31)只是重复了式(6.17)，它利用状态方程来预测当前的系统状态。在卡尔曼滤波中每一个状态估计值必须跟随一个用来衡量此状态估计值可靠性的均方误差阵。在预测状态时，由于过程噪声 label 的值未知因而状态先验估计值的均方误差阵计算公式(6.32)添加了过程噪声的方差lable，以降低状态先验估计值的可靠性和保持均方误差阵的正确性。在完成这一预测过程后系统的状态估计均方误差通常会变大。

(2)校正:校正过程又叫测量更新过程，它是利用实际测量值来校正经上一步预测得到的状态先验估计值。这一过程涉及以下三个公式

$$\begin{gathered} K_{k}=P_{k}^{-}C^{\mathrm{T}}\left(CP_{k}^{-}C^{\mathrm{T}}+R\right)^{-1}\quad (6.33) \\ \\ \hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(\boldsymbol{y}_{k}-\boldsymbol{C}\hat{\boldsymbol{x}}_{k}^{-}\right) \quad (6.34) \\ \\ P_{k}=(I-K_{k}C)P_{k}^{-} \text{(6.35)} \end{gathered}$$

其中，式(6.33)就是我们前面推导出的卡尔曼增益计算公式(6.29)，而式(6.34)和式(6.35)对系统的状态估计值及其均方误差进行更新。在这一过程中，由于状态估计值得到了实际测量值的验证因而它的均方误差值变小，即可靠性增加。卡尔曼滤波器综合、平衡了它的先验估计和实际测量这两方面的信息，使测量更新以后的状态估计值具有最小的均方误差。

通过该方程可知，卡尔曼增益 $K_{k}$ 实际上表征了状态最优估计过程中模型预测误差与量测误差的比重，即 $K_{k}∈[0,1]$ 。当 $K_{k}=0$ 时，即预测误差为0，系统的状态值完全取决与预测值（$\hat{x}_{k}=\hat{x}_{k}^{-}$）；而当 $K_{k} =1$ 时，即量测误差为0，系统的状态值完全取决于量侧值。

EKF(Extented Kalman Filter)

Todo list:

:black_square_button: 系统建模方法

:black_square_button: EKF和KF

Reference

GPS原理和接收机设计 谢钢

卡尔曼滤波（Kalman Filter）原理与公式推导 - 知乎 (zhihu.com)

（二）. 细说Kalman滤波：The Kalman Filter - 王永才 - 博客园 (cnblogs.com)

扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）_ekf算法-CSDN博客