Kalman滤波器推导与实现（Python版本）

• 卡尔曼滤波(Kalman Filter)

本文在参考文献的基础上添加了自己的理解，若有不当之处，敬请指正
卡尔曼滤波以前接触过，但是没有仔细推导，这次参考文献仔细推导实现，也是第一次完全的通过vscode+markdown来完成写作。
卡尔曼滤波，也被称为线性二次估计(Liner Quadratic Estimation, LQE)，可以作为平滑数据、预测数据、滤波器；本人理解：是一个观察→预测的过程，我接触到的卡尔曼滤波主要有KF(卡尔曼滤波，线性),EKF(扩展卡尔曼滤波，非线性)，UKF(无迹卡尔曼滤波，强非线性)。此处，只研究基本的KF。

　　　　 图1 来自老爷子的亲切凝视

---

• 参考：

• . python源码参考
• . kalman滤波器介绍和实例参考
• . kalman方法推导1
• . 极大似然估计
• . 矩阵迹求导
• . kalman方法推导2

---

1. 导论

1960年, R.E. Kalman发表了他使用迭代方法解决离散线性滤波问题的文章（《A New Approach to Linear Filtering and Prediction Problems》），揭开了卡尔曼滤波的历史篇章，得益于数字计算能力的发展，卡尔曼滤波方法在得到了广泛应用，尤其是在自主/辅助导航方面。 现在该方法在计算机图像处理，多数据融合方面也得到广泛应用。
1）第一个问题：什么是卡尔曼滤波
　　卡尔曼滤波是一种最优化自回归数据处理算法。其中最优化体现在：1）动态使用系统和测量设备的信息；2）该理论包含了系统噪声、测量误差、动态模型的不确定性；3）和感兴趣变量初始状态相关的各种可用信息。其中的自回归表现在：卡尔曼滤波器不需要使用/储存过去所有时刻的数据，使得卡尔曼滤波器计算性能好。
2）第二个问题:为什么用卡尔曼滤波
　　卡尔曼滤波最初是为了解决控制问题。通常在做系统分析或者控制系统设计的时候，研究人员都会期望从变量的内在联系来建立一个理论模型，通过对理论模型的研究来研究问题。但是，理论模型并不完美，而且模型的结果也只是近似罢了，而观测到的数据也不是完整和完美的。基于这些存在的问题，所以研究人员探索能够解决这些问题的方法，也就有了卡尔曼滤波。

---

2. 卡尔曼滤波方法推导

本方法注重思路，详细过程请参考文章开始列出的文献。

2.1 lossfunction

前边提到，卡尔曼是一种线性二次估计，相当于一种优化算法，优化算法的目标就是针对损失函数，并使得损失函数最大或者最小。首先考虑一个最简单的线性模型：

• $y_k=a_k{x}_k+n_k$
  其中$x_k$是系统的状态量，但是这个状态量一般我们建模不准确，会存在一个误差，量化这个误差就使用如下形式：
• $e_k=x_k-\widehat{x_k}$ 　　－－这个误差越小，建模状态量越准确，得到的输出值也就越准确
  但是$e_k$有正有负不好优化，所以引入均方差（mean squared error， MSE）, 可以只向一个方向优化：
• $lossfunciton=E(e_k^2)$

2.2. 极大似然估计

原理：从已知结果出发，反推参数值，这个参数值将使得结果出现的概率最大。
抽象一下问题：已知观测结果y，需要反推状态参数$\widehat{x_k}$。所以最大似然估计的目标就是使得y的条件概率最大：

• $max(P[y|\hat{x}])$
  假设的随机噪声是符合标准偏差为$\sigma _k $的高斯分布（正态分布），那么条件概率可以写为：

• $P(y_k|{\hat{x}}_k)=K_{k}exp-(\frac{(y_k-a_k{\hat{x}}_k{)}^2}{2{\sigma }_k^2})$
  那么极大似然概率就是取得乘积：

• $P(y_k|{\hat{x}}_k)=\underset{k}{\Pi }{K}_{k}exp-(\frac{(y_k-a_k{\hat{x}}_k{)}^2}{2{\sigma }_k^2})$
  有exp, 可以通过取对数，将乘积转换为加和运算：

• $logP(y_k|{\hat{x}}_k)=-\frac{1}{2}\sum _k(\frac{(y_k-a_k{\hat{x}}_k{)}^2}{2{\sigma }_k^2})+constant$

  所以目标就是，找到${\hat{x}}_k$使得$logP(y_k|{\hat{x}}_k)$最大，那么${\hat{x}}_k$就是最有可能的状态值。可以看到$logP(y_k|{\hat{x}}_k)$的表达式其实就是2.1中描述的均方差的形式。我们可以通过最小化均方误差来求${\hat{x}}_k$。

2.3 状态方程推导

总体思路还是比较简单：由状态构建一个误差表达式，并将该表达式构建为MSE形式，根据已知条件推导，将MES改写称为一个包含已知量的表达式，求导，令导数等于零求得极小点，将该点回代至表达式，结束！整个推导过程的步骤也比较简单，只是涉及的量比较多，以及矩阵表达的形式，看起来比较繁杂而已。

---

已知条件，6个
一般情况下，我们将一个系统描述为状态方程和观测方程：

• 状态方程： $x_{k+1}=\Phi x_k+w_k$
• 观测方程： $z_k=H{x}_k+v_k$

$w_k$为已知协方差的过程白噪声，$v_k$为已知协方差的测量白噪声，且二者不相关。
所以，以下两个值已知：

• $Q=E(w_k{w}_k^T)$
• $R=E(v_k{v}_k^T)$
• $e_k=x_k-{\hat{x}}_k$
• ${\hat{x}}_k={\hat{x}}_k^{{}^{\prime }}+K_k(z_k-H{\hat{x}}_k^{{}^{\prime }})$ $\dots {\hat{x}}_k^{{}^{\prime }}$表示直接估计的值，该式将在下一节的极大似然估计中推导。

---

求解问题
优化的目标根据2.1就是：

$lossfunciton=E(e_k^2)=E(e_k{e}_k^T)=P_k$ 一切都是从这个方程出发

---

推导过程

先处理已知条件：

• $z_k$ 代入表达式：${\hat{x}}_k={\hat{x}}_k^{{}^{\prime }}+K_k(z_k-H{\hat{x}}_k)={\hat{x}}_k^{{}^{\prime }}+K_k(H{x}_k+v_k-H{\hat{x}}_k^{{}^{\prime }})=K_{k}H{x}_k+(I-K_{k}H)x_k^{{}^{\prime }}+K_k{v}_k$
• ${\hat{x}}_k$ 代入表达式:$e_k$