经过看各种博客和文章，让我最清楚明白的，是xiahouzuoxin 的博客，之后又看了一些国外的文献进行自己的理解-20161116
首先得明白P Q R这些矩阵的含义与来源
Q：过程激励噪声的协方差矩阵。翻译成这个名字是由时间序列信号模型的观点，平稳随机序列可以看成是由典型噪声源激励线性系统产生，故译作过程激励噪声。
R：观测噪声的协方差矩阵
P：不断迭代计算的估计误差的协方差矩阵
kalman滤波的过程：
滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法。
典型的公式中，前两个是时间更新方程，后三个是状态更新方程（公式中（ − 代表先验，ˆ代表估计）
Paste_Image.png
Paste_Image.png
参数解释和公式
这里有两个空间，状态空间n*1,观测空间m*1，往往观测量和状态量为同一个量，比如我观测机器人的位置(x,y)，同时我要估计的也是机器人的位置(x,y)，此时H观测模型矩阵为单位矩阵。但是也存在不同的情况，比如我观测的是机器人的速度和姿态，我估计的是机器人的位置，此时H观测模型矩阵需要根据不同情况自己设定。
Paste_Image.png
Paste_Image.png
最后把让我茅塞顿开的博文转载过来
Kalman滤波器的历史渊源
We are like dwarfs on the shoulders of giants, by whose grace we see farther than they. Our study of the works of the ancients enables us to give fresh life to their finer ideas, and rescue them from time’s oblivion and man’s neglect.
—— Peter of Blois, late twelfth century
太喜欢第一句话了，“我们是巨人肩膀上的矮人，巨人们的优雅让我么看得更比他们更远”，谁说不是呢？
说起Kalman滤波器的历史，最早要追溯到17世纪，Roger Cotes开始研究最小均方问题。但由于缺少实际案例的支撑（那个时候哪来那么多雷达啊啥的这些信号啊），Cotes的研究让人看着显得很模糊，因此在估计理论的发展中影响很小。17世纪中叶，最小均方估计（Least squares Estimation）理论逐步完善，Tobias Mayer在1750年将其用于月球运动的估计，Leonard Euler在1749年、Pierre Laplace在1787分别用于木星和土星的运动估计。Roger Boscovich在1755用最小均方估计地球的大小。1777年，77岁的Daniel Bernoulli（大名鼎鼎的伯努利）发明了最大似然估计算法。递归的最小均方估计理论是由Karl Gauss建立在1809年（好吧，他声称在1795年就完成了），当时还有Adrien Legendre在1805年完成了这项工作，Robert Adrain在1808年完成的，至于到底谁是Boss，矮子们就别管了吧！
在1880年，丹麦的天文学家Thorvald Nicolai Thiele在之前最小均方估计的基础上开发了一个递归算法，与Kalman滤波非常相似。在某些标量的情况下，Thiele的滤波器与Kalman滤波器时等价的，Thiele提出了估计过程噪声和测量噪声中方差的方法（过程噪声和测量噪声是Kalman滤波器中关键的概念）。
上面提到的这么多研究估计理论的先驱，大多是天文学家而非数学家。现在，大部分的理论贡献都源自于实际的工程。“There is nothing so practical as a good theory”，应该就是“实践是检验真理的唯一标准”之类吧。
现在，我们的控制论大Wiener终于出场了，还有那个叫Kolmogorov（柯尔莫戈洛夫）的神人。在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开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。
从牛顿到卡尔曼
从现在开始，就要进行Kalman滤波器探讨之旅了，我们先回到高一，从物理中小车的匀加速直线运动开始。
话说，有一辆质量为m的小车，受恒定的力F，沿着r方向做匀加速直线运动。已知小车在t-ΔT时刻的位移是s(t-1)，此时的速度为v(t-1)。求：t时刻的位移是s(t)，速度为v(t)？
由牛顿第二定律，求得加速度：
img1
img2
现在就有了两种方法（如上图）可以得到n时刻的位移和速度：一种就是通过(3)式的状态空间递推计算（Prediction），另一种就是通过(4)式直接拿尺子和传感器测量（Measurement）。致命的是没一个是精确无误的，就像上图看到的一样，分别都存在0均值高斯分布的误差w(n)和v(n)。
那么，我最终的结果是取尺子量出来的好呢，还是根据我们伟大的牛顿第二定律推导出来的好呢？抑或两者都不是！
一场递推的游戏
为充分利用测量值（Measurement）和预测值（Prediction），Kalman滤波并不是简单的取其中一个作为输出，也不是求平均。
设预测过程噪声w(n)N(0,Q)，测量噪声v(n)N(0,R)。Kalman计算输出分为预测过程和修正过程如下：
预测
预测值：
img3
图中的符号和本文符号稍有差异，主要是P的表示上。从上图也可以看出，Kalman滤波就是给定-1时刻的初始值，然后在预测（状态空间）和修正（观测空间）之间不停的递推，求取n时刻的估计x和均方误差矩阵P。
均方误差中的门道
到这里，应该对Kalman滤波有个总体的概念了，有几个观点很重要，是建立Kalman滤波器的基础：
一个是n-1对n时刻估计值，一个是n时刻的测量值，估计值和测量值都存在误差，且误差都假设满足独立的高斯分布
Kalman滤波器就是充分结合了估计值和测量值得到n时刻更接近真值的估计结果
Kalman滤波器引入状态空间的目的是避免了“像Wiener滤波器一样需要对过去所有[0,n-1]时刻协方差先验知识都已知”，而直接可以通过上一时刻即n-1时刻的状态信息和均方误差信息就可递推得到n时刻的估计。尽管递推使得实际应用中方便了，但n-1对n时刻的估计实际上使用到了所有前[0,n-1]时刻的信息，只不过信息一直通过最小均方误差进行传递到n-1时刻。基于此，Kalman滤波也需要先验知识，即-1时刻的初始值。
在上小节中只看到Kalman的结论，那么Kalman滤波器是如何将估计值和测量值结合起来，如何将信息传递下去的呢？这其中，“独立高斯分布”的假设条件功劳不可谓不大！测量值z(n)N(uz,σz^2)，估计值x(n)N(ux,σx^2)。
Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！
如下图：估计量的高斯分布和测量量的高斯分布经过融合后为绿色的高斯分布曲线。
img4
Paste_Image.png
该程序中使用的符号的含义与本文一致，函数前的注释再清晰不过了，就不多说。下面是一段[测试]
Paste_Image.png
Kalman的参数中s.Q和s.R的设置非常重要，之前也提过，一般要通过实验统计得到，它们分布代表了状态空间估计的误差和测量的误差。
img5
Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。
Kalman滤波C程序
我就在上面公式的基础上实现了基本的Kalman滤波器，包括1维和2维状态的情况。先在头文件中声明1维和2维Kalman滤波器结构：
Paste_Image.png
我都给了有详细的注释，kalman1_state
是状态空间为1维/测量空间1维的Kalman滤波器，kalman2_state
是状态空间为2维/测量空间1维的Kalman滤波器。两个结构体都需要通过初始化函数初始化相关参数、状态值和均方差值。
Paste_Image.png
其实，Kalman滤波器由于其递推特性，实现起来很简单。但调参有很多可研究的地方，主要需要设定的参数如下：
init_x：待测量的初始值，如有中值一般设成中值（如陀螺仪）
init_p：后验状态估计值误差的方差的初始值
q：预测（过程）噪声方差
r：测量（观测）噪声方差。以陀螺仪为例，测试方法是：保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，按3σ原则，取正态分布的(3σ)^2作为r的初始化值。
其中q和r参数尤为重要，一般得通过实验测试得到。
找两组声阵列测向的角度数据，对上面的C程序进行测试。一维Kalman（一维也是标量的情况，就我所知，现在网上看到的代码大都是使用标量的情况）和二维Kalman（一个状态是角度值，另一个状态是向量角度差，也就是角速度）的结果都在图中显示。这里再稍微提醒一下：状态量不要取那些能突变的量，如加速度，这点在文章“从牛顿到卡尔曼”一小节就提到过。
img6
Matlab绘出的跟踪结果显示：
Kalman滤波结果比原信号更平滑。但是有椒盐突变噪声的地方，Kalman滤波器并不能滤除椒盐噪声的影响，也会跟踪椒盐噪声点。因此，推荐在Kalman滤波器之前先使用中值滤波算法去除椒盐突变点的影响。
上面所有C程序的源代码及测试程序都公布在我的Github上，希望大家批评指正其中可能存在的错误。
网友评论