卡尔曼滤波学习笔记

Kalman滤波器的前世：

17世纪，Roger Cotes 开始研究最小均方问题。

17世纪中叶，最小均方差估计(Least squares Estimation)理论逐渐完善：Tobias Mayer在1750年将其用于月球运用的估计，Leanard Euler在1749年、Pierre Laplace在1787年分别用于土星的运动估计。Roger Boscovich在1755用最小均方估计地球的大小。

1777年，Daniel Bernoulli(伯努利)发明了最大似然估计算法。

1805年、1808年、1809年Adrien Legendre、Robert Adrain、Karl Gauss建立的递归的最小估计理论。

1880年，Thorvald Nicolai Thiele开发了Thiele滤波器，在某些情况下，Thiele滤波器与Kalman滤波器等级，Thiele提出了估计过程噪声和测量噪声中方差的方法。

19时间40年代，Wiener设计了Wiener滤波器，然而，Wiener滤波器不是在状态空间进行的（这个学过Wiener滤波的就知道，它是直接从观测空间z(n)=s(n)+w(n)进行的滤波），Wiener是稳态过程，它假设测量是通过过去无限多个值估计得到的。Wiener滤波器比Kalman滤波器具有更高的自然统计特性。这些也限制其只是更接近理想的模型，要直接用于实际工程中需要足够的先验知识（要预知协方差矩阵），美国NASA曾花费多年的时间研究维纳理论，但依然没有在空间导航中看到维纳理论的实际应用。

在1950末期，大部分工作开始对维纳滤波器中协方差的先验知识通过状态空间模型进行描述。通过状态空间表述后的算法就和今天看到的Kalman滤波已经极其相似了。Johns Hopkins大学首先将这个算法用在了导弹跟踪中，那时在RAND公司工作的Peter Swerling将它用在了卫星轨道估计，Swerling实际上已经推导出了（1959年发表的）无噪声系统动力学的Kalman滤波器，在他的应用中，他还考虑了使用非线性系统动力学和和测量方程。可以这样说，Swerling和发明Kalman滤波器是失之交臂，一线之隔。在kalman滤波器闻名于世之后，他还写信到AIAA Journal声讨要获得Kalman滤波器发明的荣誉（然而这时已经给滤波器命名Kalman了）。总结其失之交臂的原因，主要是Swerling没有直接在论文中提出Kalman滤波器的理论，而只是在实践中应用。

Rudolph Kalman在1960年发现了离散时间系统的Kalman滤波器，这就是我们在今天各种教材上都能看到的，1961年Kalman和Bucy又推导了连续时间的Kalman滤波器。Ruslan Stratonovich也在1960年也从最大似然估计的角度推导出了Kalman滤波器方程。

目前，卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

设预测过程噪声w(n)~N(0,Q)，测量噪声v(n)~N(0,R)。Kalman计算输出分为预测过程和修正过程如下：

预测:

预测值： 

x(n|n-1)为用n时刻以前的数据进行对n时刻的估计结果，x(n-1|n-1)为上次预测的最终状态向量，A为状态转移矩阵，B为控制输入矩阵，u(n)为驱动输入矩阵。

最小均方误差矩阵：

P(n|n-1)为NxN，最小预测均方误差矩阵，

修正：

误差增益：

修正值：

最小均方差误差矩阵：

从(5)~(9)中：

• x(n)：Nx1的状态矢量
• z(n)：Mx1的观测矢量，Kalman滤波器的输入
• x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果
• x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出

• P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为

• P(n|n)：NxN，修正后最小均方误差矩阵。
• K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误差”，直观含义就是用n-1预测n时刻状态的预测最小均方误差在n时刻的总误差中的比重，比重越大，说明真值接近预测值的概率越小（接近测量值的概率越大），这也可以从(8)式中看到。

Kalman滤波算法的步骤是(1)(2)->(3->(4)(5)。当然，建议找本教材复习下上面公式的推导过程，或参见Wiki上的介绍https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2