卡尔曼滤波器来源
介绍卡尔曼滤波器，我们首先简单地介绍一下这个大名鼎鼎的滤波器的发明者--卡尔曼。全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）[1]。
最开始，卡尔曼提出的滤波器主要针对的是线性系统。后来，很多学者在这基础上对其进行了扩展应用，包括针对非线性系统的扩展卡尔曼滤波器(EKF/CKF)[2]、伪线性卡尔曼滤波器（PLKF）[3]，和无迹卡尔曼滤波器（UKF）[4][5]，针对多传感器系统（multi-sensor system）的集中式卡尔曼滤波器[6]，针对分布式系统的（DKF）[7]等。
为了能让大家更快地熟悉和了解什么是卡尔曼滤波器，卡尔曼滤波器是如何工作的。这里，我们通过建立一维小车的例子并进行卡尔曼滤波器的推导。
图1 一维小车运动位置
▌线性卡尔曼滤波器推导
我们假设现在有一个小车系统，我们需要估计小车的状态，其状态包含位置和速度：
而我们能够测量得到的信息是带误差的小车的位置，那么如何实现对小车的位置和速度的估计。
首先假设小车是一个线性离散系统，那么系统可以用一个系统方程来描述：
式中是系统矩阵，是系统的外部干扰，其中系统矩阵在例子中为：
其中为采样时间间隔。这里假设我们已知小车系统做匀速直线运动。而我们能够得到的测量信息是小车的位置，那么测量方程可以描述为：
式中是观测矩阵，是测量误差，测量误差服从均值为，方差为的高斯分布。这里假设。这里观测矩阵为：
那么根据系统方程，我们假设我们可以根据上一时刻的最优估计来预测下一时刻小车所处的位置。即：
（1）
其中为小车在时刻的最优估计位置。
如果我们不知道其实际的位置和速度，那么他们中会有很多种可能的正确组合。在卡尔曼滤波算法中，通常假设变量都是随机的且服从高斯分布，即：每个变量都有一个均值和方差来表示状态的不确定性。但在小车这个例子中，我们知道速度和位置之间是存在关联的，如果运动速度慢，那么距离的无变化就小，如果运动速度快，那么距离的变化就大，这是由于这两个变量之间存在相关性。而卡尔曼滤波器的目的就是尽可能从不确定性的测量数据中提取更多的信息。而刚刚讲的速度和位置之间相关性的信息，我们用来表示。
除了估计之间的误差，我们的系统中还可能存在外部干扰。通常针对这类干扰，卡尔曼滤波器认为其服从高斯分布，即状态变量移动到一个新的且服从高斯分布的区域，协方差为。
因此，我们可以得到我们的协方差的预测方程为：
（2）
如果我们对系统的建模准确，比如我们知道小车做的是匀速运动，那么之前建立的系统其实描述的比较准确，系统误差主要来自外界干扰，其高斯分布概率如图2左侧小车的位置和速度均服从均值为0，方差为1的高斯分布；但如果小车不是做匀速运动，或者我们不知道小车的运动模式，那么上面建立的系统模型的描述就没那么准确，系统误差就包括小车的速度模型误差和外界的干扰，那么小车位置、速度的高斯分布应如概率图2右侧，小车的位置分布服从均值为0，方差为1的高斯分布，而速度分布均值为0，方差为3的正态分布。又如果我们知道小车做的是匀加速运动，那么我们可以对系统再增加一维加速度估计，这样也同样可以提高系统模型的准确性，并降低系统误差。
图2 小车状态分布与系统误差的关系
通过式（1）、（2）我们可以知道，新的时刻的预测是根据上一时刻最优估计预测加上已知的外部控制修正得到的，而新的预测协方差矩阵是通过上一时刻的最优协方差矩阵预测并加上外部环境干扰得到的。但如果系统一直只有预测，那么对小车状态估计的误差会随着时间越来越大。这时，我们通常会隔一段时间通过传感器对小车的状态进行测量，测量的信息可能是直接（位置和速度）或间接（位置）的告诉我们关于小车状态。在前文我们已经假设本例中传感器可以测量到小车的位置。
图3 小车真实位置和测量小车位置随时间的变化
如图3所示，当观测噪声时，测量的小车位置是比较杂乱的。那么，如何根据有噪声的测量信息来修正对小车状态的估计呢？
首先，根据公式（1），（2）的预测的小车状态和其协方差矩阵，我们可以知道在当前时刻的预测测量信息以及预测测量信息的协方差矩阵为:
而根据传感器在时刻的测量信息，我们可以知道小车在t时刻的位置应当符合均值为，方差为的高斯分布。而根据上一时刻-1的最优估计得到在时刻时，小车真实的位置应当符合均值为，方差为的高斯分布函数，如图4所示。其中较高较集中的曲面的为系统估计的小车位置高斯分布，较低较宽的曲面为测量信息得到的小车位置高斯分布。
图4 在t时刻根据测量信息和系统预测分别得到小车位置的高斯分布
现在我们有两个关于小车位置信息的高斯分布，分别是来自预测的测量信息以及测量信息。接下来就是重点中的重点，如何将两个高斯分布融合起来，并得到一个新的高斯分布函数。我们先以一维的高斯函数为例，具有方差为，均值为的高斯曲线可以用下面公式表示：
（3）
（4）
那么我们假设我们分别有一个均值为，方差为以及一个均值为，方差为的高斯分布如图5所示。那么我们定义一个新的高斯分布为两个高斯分布相乘：
图5 两个高斯分布
将公式（3）带入（4）中，并对参数进行归一化后，可以得到新的高斯分布：
这式子有两个相同的部分，我们提取出来：
并扩展到协方差矩阵的形式，并带入我们的小车例子中：
那么，我们可以得到关于小车真实位置信息的修正，我们分别把，可以得到：
（5）
（6）
但我们实际需要的是。因此，对公式（5）和公式（6）两边同左乘上，那么我们可以得到：
最后，我们注意到 中包含了 ，我们重新定义为：
那么，我们可以得到最终的公式：
(7)
(8)
以上，就是完整的线性卡尔曼滤波器的全部推导，我们将重要的公式提取出来，就是线性卡尔曼滤波器的全部内容:
根据上面的卡尔曼滤波器的公式迭代，我们可以得到实时的小车位置和速度如图6，图7所示。
图6 小车真实位置，小车观测位置和小车估计位置随时间变化
图7 小车真实速度和小车估计速度随时间变化
参考资料
[1] Kalman R E. A new approach to linear filtering and prediction problems[J]. 1960.
[2] Ljung L. Asymptotic behavior of the extended Kalman filter as a parameter estimator for linear systems[J]. IEEE Transactions on Automatic Control, 1979, 24(1): 36-50.
[3] Aidala V J, Nardone S C. Biased estimation properties of the pseudolinear tracking filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 1982 (4): 432-441.
[4] Wan E A, Van Der Merwe R. The unscented Kalman filter for nonlinear estimation[C]//Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No. 00EX373). Ieee, 2000: 153-158.
[5] Van Der Merwe R, Wan E A. The square-root unscented Kalman filter for state and parameter-estimation[C]//2001 IEEE international conference on acoustics, speech, and signal processing. Proceedings (Cat. No. 01CH37221). IEEE, 2001, 6: 3461-3464.
[6] Sun S L, Deng Z L. Multi-sensor optimal information fusion Kalman filter[J]. Automatica, 2004, 40(6): 1017-1023.
[7] Olfati-Saber R. Distributed Kalman filtering for sensor networks[C]//2007 46th IEEE Conference on Decision and Control. IEEE, 2007: 5492-5498.
[8]https://blog.csdn.net/gavinmiaoc/article/details/88987803?ops_request_misc=&request_id=&biz_id=102&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E5%99%A8&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-88987803.non（详解卡尔曼滤波器_telnetipc的博客-CSDN博客_卡尔曼滤波器）
[9]https://blog.csdn.net/gavinmiaoc/article/details/88987803?ops_request_misc=&request_id=&biz_id=102&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2%E5%99%A8&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-0-88987803.non（卡尔曼滤波器介绍_山水之间2018的博客-CSDN博客_卡尔曼滤波器）
-END-
本文由西湖大学智能无人系统实验室博士生郑灿伦原创
实验室网站：https://shiyuzhao.westlake.edu.cn
转自：“测绘学术资讯”微信公众号
如有侵权，请联系本站删除！
