-
Notifications
You must be signed in to change notification settings - Fork 241
/
Copy pathKalman滤波器从原理到实现.html
672 lines (643 loc) · 53.5 KB
/
Kalman滤波器从原理到实现.html
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/html; charset=utf-8" />
<meta http-equiv="Content-Style-Type" content="text/css" />
<meta name="generator" content="pandoc" />
<title></title>
<style type="text/css">code{white-space: pre;}</style>
<style type="text/css">
table.sourceCode, tr.sourceCode, td.lineNumbers, td.sourceCode {
margin: 0; padding: 0; vertical-align: baseline; border: none; }
table.sourceCode { width: 100%; line-height: 100%; }
td.lineNumbers { text-align: right; padding-right: 4px; padding-left: 4px; color: #aaaaaa; border-right: 1px solid #aaaaaa; }
td.sourceCode { padding-left: 5px; }
code > span.kw { color: #007020; font-weight: bold; }
code > span.dt { color: #902000; }
code > span.dv { color: #40a070; }
code > span.bn { color: #40a070; }
code > span.fl { color: #40a070; }
code > span.ch { color: #4070a0; }
code > span.st { color: #4070a0; }
code > span.co { color: #60a0b0; font-style: italic; }
code > span.ot { color: #007020; }
code > span.al { color: #ff0000; font-weight: bold; }
code > span.fu { color: #06287e; }
code > span.er { color: #ff0000; font-weight: bold; }
</style>
<link rel="stylesheet" href="../stylesheets/Github.css" type="text/css" />
<title>Kalman滤波器从原理到实现</title>
</head>
<body>
<div id="header"><center>
<p class="header_titleline">
<a href="../index.html" target="_self" title="主页">主页 </a><a href="../Search.html" target="_self" title="站内搜索">站内搜索 </a><a href="../Projects.html" target="_self" title="项目研究">项目研究 </a><a href="../Archives.html" target="_self" title="文章存档">文章存档 </a><a href="../README.html" target="_self" title="分类目录">分类目录 </a><a href="../AboutMe.html" target="_self" title="关于我">关于我 </a>
</p>
</center></div>
<h1>Kalman滤波器从原理到实现</h1>
<h4>old / xiahouzuoxin</h4>
<h4>Tags: DSP</h4>
转载请注明出处: <a href="http://xiahouzuoxin.github.io/notes/">http://xiahouzuoxin.github.io/notes/</a>
<div id="TOC">
<ul>
<li><a href="#kalman滤波器的历史渊源">Kalman滤波器的历史渊源</a></li>
<li><a href="#从牛顿到卡尔曼">从牛顿到卡尔曼</a></li>
<li><a href="#一场递推的游戏">一场递推的游戏</a></li>
<li><a href="#均方误差中的门道">均方误差中的门道</a></li>
<li><a href="#matlab程序看过来">Matlab程序看过来</a></li>
<li><a href="#kalman滤波c程序">Kalman滤波C程序</a></li>
<li><a href="#参考资料">参考资料</a></li>
</ul>
</div>
<!---title:Kalman滤波器从原理到实现-->
<!---keywords:DSP-->
<!---date:old-->
<h2 id="kalman滤波器的历史渊源">Kalman滤波器的历史渊源</h2>
<blockquote>
<p>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.</p>
</blockquote>
<blockquote>
<p>—— Peter of Blois, late twelfth century</p>
</blockquote>
<p>太喜欢第一句话了，“我们是巨人肩膀上的矮人，巨人们的优雅让我么看得更比他们更远”，谁说不是呢？</p>
<p>说起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，矮子们就别管了吧！</p>
<p>在1880年，丹麦的天文学家Thorvald Nicolai Thiele在之前最小均方估计的基础上开发了一个递归算法，与Kalman滤波非常相似。在某些标量的情况下，Thiele的滤波器与Kalman滤波器时等价的，Thiele提出了估计过程噪声和测量噪声中方差的方法（过程噪声和测量噪声是Kalman滤波器中关键的概念）。</p>
<p>上面提到的这么多研究估计理论的先驱，大多是天文学家而非数学家。现在，大部分的理论贡献都源自于实际的工程。“There is nothing so practical as a good theory”，应该就是“实践是检验真理的唯一标准”之类吧。</p>
<p>现在，我们的控制论大Wiener终于出场了，还有那个叫Kolmogorov（柯尔莫戈洛夫）的神人。在19世纪40年代，Wiener设计了Wiener滤波器，然而，Wiener滤波器不是在状态空间进行的（这个学过Wiener滤波的就知道，它是直接从观测空间z(n)=s(n)+w(n)进行的滤波），Wiener是稳态过程，它假设测量是通过过去无限多个值估计得到的。Wiener滤波器比Kalman滤波器具有更高的自然统计特性。这些也限制其只是更接近理想的模型，要直接用于实际工程中需要足够的先验知识（要预知协方差矩阵），美国NASA曾花费多年的时间研究维纳理论，但依然没有在空间导航中看到维纳理论的实际应用。</p>
<p>在1950末期，大部分工作开始对维纳滤波器中协方差的先验知识通过状态空间模型进行描述。通过状态空间表述后的算法就和今天看到的Kalman滤波已经极其相似了。Johns Hopkins大学首先将这个算法用在了导弹跟踪中，那时在RAND公司工作的Peter Swerling将它用在了卫星轨道估计，Swerling实际上已经推导出了（1959年发表的）无噪声系统动力学的Kalman滤波器，在他的应用中，他还考虑了使用非线性系统动力学和和测量方程。可以这样说，Swerling和发明Kalman滤波器是失之交臂，一线之隔。在kalman滤波器闻名于世之后，他还写信到AIAA Journal声讨要获得Kalman滤波器发明的荣誉（然而这时已经给滤波器命名Kalman了）。总结其失之交臂的原因，主要是Swerling没有直接在论文中提出Kalman滤波器的理论，而只是在实践中应用。</p>
<p>Rudolph Kalman在1960年发现了离散时间系统的Kalman滤波器，这就是我们在今天各种教材上都能看到的，1961年Kalman和Bucy又推导了连续时间的Kalman滤波器。Ruslan Stratonovich也在1960年也从最大似然估计的角度推导出了Kalman滤波器方程。</p>
<p>目前，卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。</p>
<h2 id="从牛顿到卡尔曼">从牛顿到卡尔曼</h2>
<p>从现在开始，就要进行Kalman滤波器探讨之旅了，我们先回到高一，从物理中小车的匀加速直线运动开始。</p>
<p>话说，有一辆质量为m的小车，受恒定的力F，沿着r方向做匀加速直线运动。已知小车在t-ΔT时刻的位移是s(t-1)，此时的速度为v(t-1)。求：t时刻的位移是s(t)，速度为v(t)？</p>
<p>由牛顿第二定律，求得加速度：<img src="https://latex.codecogs.com/png.latex? \large a=F/m"></p>
<p>那么就有下面的位移和速度关系：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large s(t)=s(t-1)+v(t-1)\Delta{T}+\frac{1}{2}\frac{F}{m}{(\Delta{T})}^2"></p>
<p><img src="https://latex.codecogs.com/png.latex? \large v(t)=v(t-1)+\frac{F}{m}\Delta{T}"></p>
<p>如果将上面的表达式用矩阵写在一起，就变成下面这样：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \begin{pmatrix} s(t) \\ v(t) \end{pmatrix}={\begin{pmatrix} 1 & \Delta{T} \\ 0 & 1 \end{pmatrix}} {\begin{pmatrix} s(t-1) \\ v(t-1) \end{pmatrix}} + {\begin{pmatrix} \frac{(\Delta{T})^2}{2} \\ {\Delta{T}} \end{pmatrix}} {\frac{F}{m}}................................(1)"></p>
<p>卡尔曼滤波器是建立在动态过程之上，由于物理量（位移，速度）的不可突变特性，这样就可以通过t-1时刻估计（预测）t时刻的状态，其__状态空间__模型为：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{x(t)=Ax(t-1)+Bu(t)+w(t)}................................(2)"></p>
<p>对比一下(1)(2)式，长得及其相似有木有：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large A={\begin{pmatrix} 1 & \Delta{T} \\ 0 & 1 \end{pmatrix}}, B={\begin{pmatrix} \frac{(\Delta{T})^2}{2} \\ {\Delta{T}} \end{pmatrix}}"></p>
<p>匀加速直线运动过程就是卡尔曼滤波中状态空间模型的一个典型应用。下面我们重点关注(2)式，鉴于研究的计算机信号都是离散的，将(2)是表示成离散形式为：</p>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \mathbf{x(n)=Ax(n-1)+Bu(n)+w(n)}................................(3)">
</center>
<p>其中各个量之间的含义是：</p>
<ol style="list-style-type: decimal">
<li>x(n)是状态向量，包含了观测的目标（如：位移、速度）</li>
<li>u(n)是驱动输入向量，如上面的运动过程是通过受力驱动产生加速度，所以u(n)和受力有关</li>
<li>A是状态转移矩阵，其隐含指示了“n-1时刻的状态会影响到n时刻的状态（这似乎和马尔可夫过程有些类似）”</li>
<li><p>B是控制输入矩阵，其隐含指示了“n时刻给的驱动如何影响n时刻的状态”</p>
<p>从运动的角度，很容易理解：小车当前n时刻的位移和速度一部分来自于n-1时刻的惯性作用，这通过Ax(n)来度量，另一部分来自于现在n时刻小车新增加的外部受力，通过Bu(n)来度量。</p></li>
<li><p>w(n)是过程噪声，w(n)~N(0,Q)的高斯分布，过程噪声是使用卡尔曼滤波器时一个重要的量，后面会进行分析。</p></li>
</ol>
<p>计算n时刻的位移，还有一种方法：拿一把长的卷尺（嗯，如果小车跑了很长时间，估计这把卷尺就难买到了），从起点一拉，直接就出来了，设测量值为z(n)。计算速度呢？速度传感器往那一用就出来了。</p>
<p>然而，初中物理就告诉我们，“尺子是量不准的，物体的物理真实值无法获得”，测量存在误差，我们暂且将这个误差记为v(n)。这种通过直接测量的方式获得所需物理量的值构成__观测空间__：</p>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \mathbf{z(n)=H(n)x(n)+v(n)} ................................(4)">
</center>
<p>z(n)就是测量结果，H(n)是观测矢量，x(n)就是要求的物理量（位移、速度），v(n)~N(0,R)为测量噪声，同状态空间方程中的过程噪声一样，这也是一个后面要讨论的量。大部分情况下，如果物理量能直接通过传感器测量，<img src="https://latex.codecogs.com/png.latex? \small \mathbf{H(n)=I}">。</p>
<p><img src="../images/Kalman滤波器从原理到实现/img1.png" alt="img1" /> <img src="../images/Kalman滤波器从原理到实现/img2.png" alt="img2" /></p>
<p>现在就有了两种方法（如上图）可以得到n时刻的位移和速度：一种就是通过(3)式的状态空间递推计算（Prediction），另一种就是通过(4)式直接拿尺子和传感器测量（Measurement）。致命的是没一个是精确无误的，就像上图看到的一样，分别都存在0均值高斯分布的误差w(n)和v(n)。</p>
<p>那么，我最终的结果是取尺子量出来的好呢，还是根据我们伟大的牛顿第二定律推导出来的好呢？抑或两者都不是！</p>
<h2 id="一场递推的游戏">一场递推的游戏</h2>
<p>为充分利用测量值（Measurement）和预测值（Prediction），Kalman滤波并不是简单的取其中一个作为输出，也不是求平均。</p>
<p>设预测过程噪声w(n)<sub>N(0,Q)，测量噪声v(n)</sub>N(0,R)。Kalman计算输出分为预测过程和修正过程如下：</p>
<ol style="list-style-type: decimal">
<li><p>预测</p>
<p>预测值：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{\hat{x}(n|n-1)=A\hat{x}(n-1|n-1)+Bu(n)}................................(5)"></p>
<p>最小均方误差矩阵：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{P(n|n-1)=AP(n-1|n-1)A^T+Q}................................(6)"></p></li>
<li><p>修正</p>
<p>误差增益：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{K(n)=P(n|n-1)H^T(n)[R(n)+H(n)P(n|n-1)H^T(n)]^{-1}}.............(7)"></p>
<p>修正值：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{\hat{x}(n|n)=A\hat{x}(n|n-1)+K(n)[{z(n)-H(n)\hat{x}(n|n-1)}]}......................(8)"></p>
<p>最小均方误差矩阵：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{P(n|n)=[I-K(n)H(n)]P(n|n-1)}................................(9)"></p></li>
</ol>
<p>从(5)~(9)中：</p>
<ul>
<li>x(n)：Nx1的状态矢量</li>
<li>z(n)：Mx1的观测矢量，Kalman滤波器的输入</li>
<li>x(n|n-1)：用n时刻以前的数据进行对n时刻的估计结果</li>
<li>x(n|n)：用n时刻及n时刻以前的数据对n时刻的估计结果，这也是Kalman滤波器的输出</li>
<li><p>P(n|n-1)：NxN，最小预测均方误差矩阵，其定义式为</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{P(n|n-1)=E\{[x(n)-\hat{x}(n|n-1)][x(n)-\hat{x}(n|n-1)]^T\}}"></p>
通过计算最终得到(6)式。</li>
<li>P(n|n)：NxN，修正后最小均方误差矩阵。</li>
<li><p>K(n)：NxM，误差增益，从增益的表达式看，相当于“预测最小均方误差”除以“n时刻的测量误差+预测最小均方误差”，直观含义就是用n-1预测n时刻状态的预测最小均方误差在n时刻的总误差中的比重，比重越大，说明真值接近预测值的概率越小（接近测量值的概率越大），这也可以从(8)式中看到。</p></li>
</ul>
<p>Kalman滤波算法的步骤是(5)(6)->(7)->(8)(9)。当然，建议找本教材复习下上面公式的推导过程，或参见Wiki上的介绍<http://en.wikipedia.org/wiki/Kalman_filter>。</p>
<p>公式就是那么的抽象，一旦认真研究懂了却也是茅塞顿开，受益也比只知皮毛的多。尽管如此，我还算更喜欢先感性后理性。仍以上面的运动的例子来直观分析：</p>
<blockquote>
<p><strong>Example</strong>:</p>
</blockquote>
<blockquote>
<p>还可以更简单一些：设小车做匀速（而非匀加速）直线运动，方便计算，假设速度绝对的恒定（不波动，所以相关的方差都为0），则u(t)==0恒成立。设预测（过程）位移噪声w(n)~N(0,2<sup>2)，测量位移噪声v(n)~N(0,1</sup>2)，n-1状态的位移<img src="https://latex.codecogs.com/png.latex? \small \hat{x}(n-1|n-1)=50m}">，速度为v=10m/s，n时刻与n-1时刻的物理时差为ΔT=1s。同时，也用尺子测了一下，结果位移为z(n)=62m。</p>
</blockquote>
<blockquote>
<p>则A = [1 ΔT; 0 1]=[1 1; 0 1]，根据(5)，预测值为</p>
</blockquote>
<blockquote>
<p><img src="https://latex.codecogs.com/png.latex? \small \hat{x}(n|n-1)=1*\hat{x}(n-1|n-1)+v*\Delta{T}=[60m; 10m/s]}">。</p>
</blockquote>
<blockquote>
<p>现在已经有了估计值和测量值，哪个更接近真值，这就通过最小均方误差矩阵来决定！</p>
</blockquote>
<blockquote>
<p>要求已知上次的修正后的最小均方误差P(n-1|n-1)=[1 0; 0 0]（匀速，所以P(2,2)=0，右斜对角线上为协方差，值为0，P(1,1)为n-1时刻位移量的均方误差，因为要计算P(1,1)还得先递推往前计算P(n-2|n-2)，所以这里暂时假设为1），则根据(6)式，最小预测预测均方误差为P(n|n-1)=[1 0; 0 0][1 1; 0 1][1 0; 0 0]=[1 0; 0 0]。</p>
</blockquote>
<blockquote>
<p>由物理量的关系知，H(n)=[1 1]，增益K(n)=[1;0]{1+[1 1][1 0; 0 0][1; 1]}^(-1)=[1/2;0]。</p>
</blockquote>
<blockquote>
<p>所以，最后的n时刻估计值既不是用n-1得到的估计值，也不是测量值，而是：<img src="https://latex.codecogs.com/png.latex? \small \hat{x}(n|n)=[60;1]+[1/2;0]{62-[1 1][60;1]}=[60.5m; 1m/s]">，因此，最终的Kalman滤波器的输出位移是60.5m。</p>
</blockquote>
<p>从上面的递推关系知道，要估计n时刻就必须知道n-1时刻，那么n=0时刻该如何估计，因此，卡尔曼滤波要初始化的估计值x(-1|-1)和误差矩阵P(-1|-1)，设x(-1,-1)~N(Us, Cs)，则初始化：</p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{x(-1|-1)=\mu_s}"></p>
<p><img src="https://latex.codecogs.com/png.latex? \large \mathbf{P(-1|-1)=C_s}"></p>
<p>综上，借用一张图说明一下Kalman滤波算法的流程：</p>
<div class="figure">
<img src="../images/Kalman滤波器从原理到实现/img3.png" alt="img3" /><p class="caption">img3</p>
</div>
<p>图中的符号和本文符号稍有差异，主要是P的表示上。从上图也可以看出，Kalman滤波就是给定-1时刻的初始值，然后在预测（状态空间）和修正（观测空间）之间不停的递推，求取n时刻的估计x和均方误差矩阵P。</p>
<h2 id="均方误差中的门道">均方误差中的门道</h2>
<p>到这里，应该对Kalman滤波有个总体的概念了，有几个观点很重要，是建立Kalman滤波器的基础：</p>
<ol style="list-style-type: decimal">
<li>一个是n-1对n时刻估计值，一个是n时刻的测量值，估计值和测量值都存在误差，且误差都假设满足独立的高斯分布</li>
<li>Kalman滤波器就是充分结合了估计值和测量值得到n时刻更接近真值的估计结果</li>
<li>Kalman滤波器引入状态空间的目的是避免了“像Wiener滤波器一样需要对过去所有[0,n-1]时刻协方差先验知识都已知”，而直接可以通过上一时刻即n-1时刻的状态信息和均方误差信息就可递推得到n时刻的估计。尽管递推使得实际应用中方便了，但n-1对n时刻的估计实际上使用到了所有前[0,n-1]时刻的信息，只不过信息一直通过最小均方误差进行传递到n-1时刻。基于此，Kalman滤波也需要先验知识，即-1时刻的初始值。</li>
</ol>
<p>在上小节中只看到Kalman的结论，那么Kalman滤波器是如何将估计值和测量值结合起来，如何将信息传递下去的呢？这其中，“独立高斯分布”的假设条件功劳不可谓不大！测量值z(n)~N(uz,σz<sup>2)，估计值x(n)~N(ux,σx</sup>2)。</p>
<blockquote>
<p>Kalman滤波器巧妙的用“独立高斯分布的乘积”将这两个测量值和估计值进行融合！</p>
</blockquote>
<p>如下图：估计量的高斯分布和测量量的高斯分布经过融合后为绿色的高斯分布曲线。</p>
<div class="figure">
<img src="../images/Kalman滤波器从原理到实现/img4.png" alt="img4" /><p class="caption">img4</p>
</div>
<p><img src="https://latex.codecogs.com/png.latex? \large y_f=\frac{1}{\sqrt{2\pi{\sigma^2_z}}}\exp^{-\frac{(r-uz)^2}{2\sigma^2_z}}*\frac{1}{\sqrt{2\pi{\sigma^2_x}}}\exp^{-\frac{(r-ux)^2}{2\sigma^2_x}}=\frac{1}{\sqrt{2\pi{\sigma^2}}}\exp^{-\frac{(r-\mu)^2}{2\sigma^2}}"></p>
<p>稍微计算一下，通过上式求出u和σ^2，</p>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \mu=\frac{u_x\sigma^2_z+u_z\sigma^2_x}{\sigma^2_z+\sigma^2_x}=u_x+\frac{\sigma^2_x}{\sigma^2_z+\sigma^2_x}(u_z-u_x)........(10)">
</center>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \sigma^2=\frac{\sigma^2_x\sigma^2_z}{\sigma^2_x+\sigma^2_z}=\sigma^2_x(1-\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z})........(11)">
</center>
<p>现在令</p>
<center>
<img src="https://latex.codecogs.com/png.latex? \large K=\frac{\sigma^2_x}{\sigma^2_x+\sigma^2_z}........(12)">
</center>
<p>则(10)(11)变成：</p>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \mu==u_x+K(u_z-u_x)........(13)">
</center>
<center>
<img src="https://latex.codecogs.com/png.latex? \large \sigma^2==\sigma^2_x(1-K)........(14)">
</center>
<p>到这里，请将(13)-(14)与(8)-(9)式对比！标量的情况下，在小车的应用中有：A=1，H=1，正态分布的均值u就是我们要的输出结果，正态分布的方差σz^2就是最小均方误差。推广到矢量的情况，最小均方误差矩阵就是多维正态分布的协方差矩阵。</p>
<p>从(12)式也很容易看到卡尔曼增益K的含义：就是估计量的方差占总方差（包括估计方差和测量方差）的比重。</p>
<p>一切都变得晴朗起来了，然而这一切的一切，却都源自于“估计量和测量量的独立高斯分布”这条假设。进一步总结Kalman滤波器：</p>
<blockquote>
<p>假设状态空间的n-1时刻估计值和观测空间的n时刻测量值都满足独立高斯分布，Kalman滤波器就是通过高斯分布的乘积运算将估计值和测量值结合，获得最接近真值的n时刻估计。</p>
</blockquote>
<blockquote>
<p>高斯分布乘积运算的结果仍为高斯分布，高斯分布的均值对应n时刻的估计值，高斯分布的方差对应n时刻的均方误差。</p>
</blockquote>
<h2 id="matlab程序看过来">Matlab程序看过来</h2>
<p>下面的一段Matlab代码是从网上找到的，程序简单直接，但作为学习分析用很棒，</p>
<pre class="sourceCode matlab"><code class="sourceCode matlab"><span class="co">% KALMANF - updates a system state vector estimate based upon an</span>
<span class="co">% observation, using a discrete Kalman filter.</span>
<span class="co">%</span>
<span class="co">% Version 1.0, June 30, 2004</span>
<span class="co">%</span>
<span class="co">% This tutorial function was written by Michael C. Kleder</span>
<span class="co">%</span>
<span class="co">% INTRODUCTION</span>
<span class="co">%</span>
<span class="co">% Many people have heard of Kalman filtering, but regard the topic</span>
<span class="co">% as mysterious. While it's true that deriving the Kalman filter and</span>
<span class="co">% proving mathematically that it is "optimal" under a variety of</span>
<span class="co">% circumstances can be rather intense, applying the filter to</span>
<span class="co">% a basic linear system is actually very easy. This Matlab file is</span>
<span class="co">% intended to demonstrate that.</span>
<span class="co">%</span>
<span class="co">% An excellent paper on Kalman filtering at the introductory level,</span>
<span class="co">% without detailing the mathematical underpinnings, is:</span>
<span class="co">% "An Introduction to the Kalman Filter"</span>
<span class="co">% Greg Welch and Gary Bishop, University of North Carolina</span>
<span class="co">% http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html</span>
<span class="co">%</span>
<span class="co">% PURPOSE:</span>
<span class="co">%</span>
<span class="co">% The purpose of each iteration of a Kalman filter is to update</span>
<span class="co">% the estimate of the state vector of a system (and the covariance</span>
<span class="co">% of that vector) based upon the information in a new observation.</span>
<span class="co">% The version of the Kalman filter in this function assumes that</span>
<span class="co">% observations occur at fixed discrete time intervals. Also, this</span>
<span class="co">% function assumes a linear system, meaning that the time evolution</span>
<span class="co">% of the state vector can be calculated by means of a state transition</span>
<span class="co">% matrix.</span>
<span class="co">%</span>
<span class="co">% USAGE:</span>
<span class="co">%</span>
<span class="co">% s = kalmanf(s)</span>
<span class="co">%</span>
<span class="co">% "s" is a "system" struct containing various fields used as input</span>
<span class="co">% and output. The state estimate "x" and its covariance "P" are</span>
<span class="co">% updated by the function. The other fields describe the mechanics</span>
<span class="co">% of the system and are left unchanged. A calling routine may change</span>
<span class="co">% these other fields as needed if state dynamics are time-dependent;</span>
<span class="co">% otherwise, they should be left alone after initial values are set.</span>
<span class="co">% The exceptions are the observation vectro "z" and the input control</span>
<span class="co">% (or forcing function) "u." If there is an input function, then</span>
<span class="co">% "u" should be set to some nonzero value by the calling routine.</span>
<span class="co">%</span>
<span class="co">% SYSTEM DYNAMICS:</span>
<span class="co">%</span>
<span class="co">% The system evolves according to the following difference equations,</span>
<span class="co">% where quantities are further defined below:</span>
<span class="co">%</span>
<span class="co">% x = Ax + Bu + w meaning the state vector x evolves during one time</span>
<span class="co">% step by premultiplying by the "state transition</span>
<span class="co">% matrix" A. There is optionally (if nonzero) an input</span>
<span class="co">% vector u which affects the state linearly, and this</span>
<span class="co">% linear effect on the state is represented by</span>
<span class="co">% premultiplying by the "input matrix" B. There is also</span>
<span class="co">% gaussian process noise w.</span>
<span class="co">% z = Hx + v meaning the observation vector z is a linear function</span>
<span class="co">% of the state vector, and this linear relationship is</span>
<span class="co">% represented by premultiplication by "observation</span>
<span class="co">% matrix" H. There is also gaussian measurement</span>
<span class="co">% noise v.</span>
<span class="co">% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q</span>
<span class="co">% v ~ N(0,R) meaning v is gaussian noise with covariance R</span>
<span class="co">%</span>
<span class="co">% VECTOR VARIABLES:</span>
<span class="co">%</span>
<span class="co">% s.x = state vector estimate. In the input struct, this is the</span>
<span class="co">% "a priori" state estimate (prior to the addition of the</span>
<span class="co">% information from the new observation). In the output struct,</span>
<span class="co">% this is the "a posteriori" state estimate (after the new</span>
<span class="co">% measurement information is included).</span>
<span class="co">% s.z = observation vector</span>
<span class="co">% s.u = input control vector, optional (defaults to zero).</span>
<span class="co">%</span>
<span class="co">% MATRIX VARIABLES:</span>
<span class="co">%</span>
<span class="co">% s.A = state transition matrix (defaults to identity).</span>
<span class="co">% s.P = covariance of the state vector estimate. In the input struct,</span>
<span class="co">% this is "a priori," and in the output it is "a posteriori."</span>
<span class="co">% (required unless autoinitializing as described below).</span>
<span class="co">% s.B = input matrix, optional (defaults to zero).</span>
<span class="co">% s.Q = process noise covariance (defaults to zero).</span>
<span class="co">% s.R = measurement noise covariance (required).</span>
<span class="co">% s.H = observation matrix (defaults to identity).</span>
<span class="co">%</span>
<span class="co">% NORMAL OPERATION:</span>
<span class="co">%</span>
<span class="co">% (1) define all state definition fields: A,B,H,Q,R</span>
<span class="co">% (2) define intial state estimate: x,P</span>
<span class="co">% (3) obtain observation and control vectors: z,u</span>
<span class="co">% (4) call the filter to obtain updated state estimate: x,P</span>
<span class="co">% (5) return to step (3) and repeat</span>
<span class="co">%</span>
<span class="co">% INITIALIZATION:</span>
<span class="co">%</span>
<span class="co">% If an initial state estimate is unavailable, it can be obtained</span>
<span class="co">% from the first observation as follows, provided that there are the</span>
<span class="co">% same number of observable variables as state variables. This "auto-</span>
<span class="co">% intitialization" is done automatically if s.x is absent or NaN.</span>
<span class="co">%</span>
<span class="co">% x = inv(H)*z</span>
<span class="co">% P = inv(H)*R*inv(H')</span>
<span class="co">%</span>
<span class="co">% This is mathematically equivalent to setting the initial state estimate</span>
<span class="co">% covariance to infinity.</span>
function s = kalmanf(s)
<span class="co">% set defaults for absent fields:</span>
if ~isfield(s,<span class="st">'x'</span>); s.x=nan*z; end
if ~isfield(s,<span class="st">'P'</span>); s.P=nan; end
if ~isfield(s,<span class="st">'z'</span>); error(<span class="st">'Observation vector missing'</span>); end
if ~isfield(s,<span class="st">'u'</span>); s.u=<span class="fl">0</span>; end
if ~isfield(s,<span class="st">'A'</span>); s.A=eye(length(x)); end
if ~isfield(s,<span class="st">'B'</span>); s.B=<span class="fl">0</span>; end
if ~isfield(s,<span class="st">'Q'</span>); s.Q=zeros(length(x)); end
if ~isfield(s,<span class="st">'R'</span>); error(<span class="st">'Observation covariance missing'</span>); end
if ~isfield(s,<span class="st">'H'</span>); s.H=eye(length(x)); end
if isnan(s.x)
<span class="co">% initialize state estimate from first observation</span>
if diff(size(s.H))
error(<span class="st">'Observation matrix must be square and invertible for state autointialization.'</span>);
end
s.x = inv(s.H)*s.z;
s.P = inv(s.H)*s.R*inv(s.H');
else
<span class="co">% This is the code which implements the discrete Kalman filter:</span>
<span class="co">% Prediction for state vector and covariance:</span>
s.x = s.A*s.x + s.B*s.u;
s.P = s.A * s.P * s.A' + s.Q;
<span class="co">% Compute Kalman gain factor:</span>
K = s.P*s.H'*inv(s.H*s.P*s.H'+s.R);
<span class="co">% Correction based on observation:</span>
s.x = s.x + K*(s.z-s.H*s.x);
s.P = s.P - K*s.H*s.P;
<span class="co">% Note that the desired result, which is an improved estimate</span>
<span class="co">% of the sytem state vector x and its covariance P, was obtained</span>
<span class="co">% in only five lines of code, once the system was defined. (That's</span>
<span class="co">% how simple the discrete Kalman filter is to use.) Later,</span>
<span class="co">% we'll discuss how to deal with nonlinear systems.</span>
end
return</code></pre>
<p>该程序中使用的符号的含义与本文一致，函数前的注释再清晰不过了，就不多说。下面是一段测试代码：</p>
<pre class="sourceCode matlab"><code class="sourceCode matlab"><span class="co">% Define the system as a constant of 12 volts:</span>
clear all
s.x = <span class="fl">12</span>;
s.A = <span class="fl">1</span>;
<span class="co">% Define a process noise (stdev) of 2 volts as the car operates:</span>
s.Q = <span class="fl">2</span>^<span class="fl">2</span>; <span class="co">% variance, hence stdev^2</span>
<span class="co">% Define the voltimeter to measure the voltage itself:</span>
s.H = <span class="fl">1</span>;
<span class="co">% Define a measurement error (stdev) of 2 volts:</span>
s.R = <span class="fl">2</span>^<span class="fl">2</span>; <span class="co">% variance, hence stdev^2</span>
<span class="co">% Do not define any system input (control) functions:</span>
s.B = <span class="fl">0</span>;
s.u = <span class="fl">0</span>;
<span class="co">% Do not specify an initial state:</span>
s.x = nan;
s.P = nan;
<span class="co">% Generate random voltages and watch the filter operate.</span>
tru=[]; <span class="co">% truth voltage</span>
for t=<span class="fl">1</span>:<span class="fl">20</span>
tru(end+<span class="fl">1</span>) = randn*<span class="fl">2</span>+<span class="fl">12</span>;
s(end).z = tru(end) + randn*<span class="fl">2</span>; <span class="co">% create a measurement</span>
s(end+<span class="fl">1</span>)=kalmanf(s(end)); <span class="co">% perform a Kalman filter iteration</span>
end
figure
hold on
grid on
<span class="co">% plot measurement data:</span>
hz=plot([s(<span class="fl">1</span>:end-<span class="fl">1</span>).z],<span class="st">'r.'</span>);
<span class="co">% plot a-posteriori state estimates:</span>
hk=plot([s(<span class="fl">2</span>:end).x],<span class="st">'b-'</span>);
ht=plot(tru,<span class="st">'g-'</span>);
legend([hz hk ht],<span class="st">'observations'</span>,<span class="st">'Kalman output'</span>,<span class="st">'true voltage'</span>,<span class="fl">0</span>)
title(<span class="st">'Automobile Voltimeter Example'</span>)
hold off</code></pre>
<p>Kalman的参数中s.Q和s.R的设置非常重要，之前也提过，一般要通过实验统计得到，它们分布代表了状态空间估计的误差和测量的误差。</p>
<div class="figure">
<img src="../images/Kalman滤波器从原理到实现/img5.png" alt="img5" /><p class="caption">img5</p>
</div>
<p>Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。</p>
<h2 id="kalman滤波c程序">Kalman滤波C程序</h2>
<p>我就在上面公式的基础上实现了基本的Kalman滤波器，包括1维和2维状态的情况。先在头文件中声明1维和2维Kalman滤波器结构：</p>
<pre class="sourceCode c"><code class="sourceCode c"><span class="co">/*</span>
<span class="co"> * FileName : kalman_filter.h</span>
<span class="co"> * Author : xiahouzuoxin @163.com</span>
<span class="co"> * Version : v1.0</span>
<span class="co"> * Date : 2014/9/24 20:37:01</span>
<span class="co"> * Brief : </span>
<span class="co"> * </span>
<span class="co"> * Copyright (C) MICL,USTB</span>
<span class="co"> */</span>
<span class="ot">#ifndef _KALMAN_FILTER_H</span>
<span class="ot">#define _KALMAN_FILTER_H</span>
<span class="co">/* </span>
<span class="co"> * NOTES: n Dimension means the state is n dimension, </span>
<span class="co"> * measurement always 1 dimension </span>
<span class="co"> */</span>
<span class="co">/* 1 Dimension */</span>
<span class="kw">typedef</span> <span class="kw">struct</span> {
<span class="dt">float</span> x; <span class="co">/* state */</span>
<span class="dt">float</span> A; <span class="co">/* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */</span>
<span class="dt">float</span> H; <span class="co">/* z(n)=H*x(n)+w(n),w(n)~N(0,r) */</span>
<span class="dt">float</span> q; <span class="co">/* process(predict) noise convariance */</span>
<span class="dt">float</span> r; <span class="co">/* measure noise convariance */</span>
<span class="dt">float</span> p; <span class="co">/* estimated error convariance */</span>
<span class="dt">float</span> gain;
} kalman1_state;
<span class="co">/* 2 Dimension */</span>
<span class="kw">typedef</span> <span class="kw">struct</span> {
<span class="dt">float</span> x[<span class="dv">2</span>]; <span class="co">/* state: [0]-angle [1]-diffrence of angle, 2x1 */</span>
<span class="dt">float</span> A[<span class="dv">2</span>][<span class="dv">2</span>]; <span class="co">/* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */</span>
<span class="dt">float</span> H[<span class="dv">2</span>]; <span class="co">/* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2 */</span>
<span class="dt">float</span> q[<span class="dv">2</span>]; <span class="co">/* process(predict) noise convariance,2x1 [q0,0; 0,q1] */</span>
<span class="dt">float</span> r; <span class="co">/* measure noise convariance */</span>
<span class="dt">float</span> p[<span class="dv">2</span>][<span class="dv">2</span>]; <span class="co">/* estimated error convariance,2x2 [p0 p1; p2 p3] */</span>
<span class="dt">float</span> gain[<span class="dv">2</span>]; <span class="co">/* 2x1 */</span>
} kalman2_state;
<span class="kw">extern</span> <span class="dt">void</span> kalman1_init(kalman1_state *state, <span class="dt">float</span> init_x, <span class="dt">float</span> init_p);
<span class="kw">extern</span> <span class="dt">float</span> kalman1_filter(kalman1_state *state, <span class="dt">float</span> z_measure);
<span class="kw">extern</span> <span class="dt">void</span> kalman2_init(kalman2_state *state, <span class="dt">float</span> *init_x, <span class="dt">float</span> (*init_p)[<span class="dv">2</span>]);
<span class="kw">extern</span> <span class="dt">float</span> kalman2_filter(kalman2_state *state, <span class="dt">float</span> z_measure);
<span class="ot">#endif </span><span class="co">/*_KALMAN_FILTER_H*/</span></code></pre>
<p>我都给了有详细的注释，<code>kalman1_state</code>是状态空间为1维/测量空间1维的Kalman滤波器，<code>kalman2_state</code>是状态空间为2维/测量空间1维的Kalman滤波器。两个结构体都需要通过初始化函数初始化相关参数、状态值和均方差值。</p>
<pre class="sourceCode c"><code class="sourceCode c"><span class="co">/*</span>
<span class="co"> * FileName : kalman_filter.c</span>
<span class="co"> * Author : xiahouzuoxin @163.com</span>
<span class="co"> * Version : v1.0</span>
<span class="co"> * Date : 2014/9/24 20:36:51</span>
<span class="co"> * Brief : </span>
<span class="co"> * </span>
<span class="co"> * Copyright (C) MICL,USTB</span>
<span class="co"> */</span>
<span class="ot">#include "kalman_filter.h"</span>
<span class="co">/*</span>
<span class="co"> * @brief </span>
<span class="co"> * Init fields of structure @kalman1_state.</span>
<span class="co"> * I make some defaults in this init function:</span>
<span class="co"> * A = 1;</span>
<span class="co"> * H = 1; </span>
<span class="co"> * and @q,@r are valued after prior tests.</span>
<span class="co"> *</span>
<span class="co"> * NOTES: Please change A,H,q,r according to your application.</span>
<span class="co"> *</span>
<span class="co"> * @inputs </span>
<span class="co"> * state - Klaman filter structure</span>
<span class="co"> * init_x - initial x state value </span>
<span class="co"> * init_p - initial estimated error convariance</span>
<span class="co"> * @outputs </span>
<span class="co"> * @retval </span>
<span class="co"> */</span>
<span class="dt">void</span> kalman1_init(kalman1_state *state, <span class="dt">float</span> init_x, <span class="dt">float</span> init_p)
{
state->x = init_x;
state->p = init_p;
state->A = <span class="dv">1</span>;
state->H = <span class="dv">1</span>;
state->q = <span class="fl">2e2</span>;<span class="co">//10e-6; /* predict noise convariance */</span>
state->r = <span class="fl">5e2</span>;<span class="co">//10e-5; /* measure error convariance */</span>
}
<span class="co">/*</span>
<span class="co"> * @brief </span>
<span class="co"> * 1 Dimension Kalman filter</span>
<span class="co"> * @inputs </span>
<span class="co"> * state - Klaman filter structure</span>
<span class="co"> * z_measure - Measure value</span>
<span class="co"> * @outputs </span>
<span class="co"> * @retval </span>
<span class="co"> * Estimated result</span>
<span class="co"> */</span>
<span class="dt">float</span> kalman1_filter(kalman1_state *state, <span class="dt">float</span> z_measure)
{
<span class="co">/* Predict */</span>
state->x = state->A * state->x;
state->p = state->A * state->A * state->p + state->q; <span class="co">/* p(n|n-1)=A^2*p(n-1|n-1)+q */</span>
<span class="co">/* Measurement */</span>
state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
state->x = state->x + state->gain * (z_measure - state->H * state->x);
state->p = (<span class="dv">1</span> - state->gain * state->H) * state->p;
<span class="kw">return</span> state->x;
}
<span class="co">/*</span>
<span class="co"> * @brief </span>
<span class="co"> * Init fields of structure @kalman1_state.</span>
<span class="co"> * I make some defaults in this init function:</span>
<span class="co"> * A = {{1, 0.1}, {0, 1}};</span>
<span class="co"> * H = {1,0}; </span>
<span class="co"> * and @q,@r are valued after prior tests. </span>
<span class="co"> *</span>
<span class="co"> * NOTES: Please change A,H,q,r according to your application.</span>
<span class="co"> *</span>
<span class="co"> * @inputs </span>
<span class="co"> * @outputs </span>
<span class="co"> * @retval </span>
<span class="co"> */</span>
<span class="dt">void</span> kalman2_init(kalman2_state *state, <span class="dt">float</span> *init_x, <span class="dt">float</span> (*init_p)[<span class="dv">2</span>])
{
state->x[<span class="dv">0</span>] = init_x[<span class="dv">0</span>];
state->x[<span class="dv">1</span>] = init_x[<span class="dv">1</span>];
state->p[<span class="dv">0</span>][<span class="dv">0</span>] = init_p[<span class="dv">0</span>][<span class="dv">0</span>];
state->p[<span class="dv">0</span>][<span class="dv">1</span>] = init_p[<span class="dv">0</span>][<span class="dv">1</span>];
state->p[<span class="dv">1</span>][<span class="dv">0</span>] = init_p[<span class="dv">1</span>][<span class="dv">0</span>];
state->p[<span class="dv">1</span>][<span class="dv">1</span>] = init_p[<span class="dv">1</span>][<span class="dv">1</span>];
<span class="co">//state->A = {{1, 0.1}, {0, 1}};</span>
state->A[<span class="dv">0</span>][<span class="dv">0</span>] = <span class="dv">1</span>;
state->A[<span class="dv">0</span>][<span class="dv">1</span>] = <span class="fl">0.1</span>;
state->A[<span class="dv">1</span>][<span class="dv">0</span>] = <span class="dv">0</span>;
state->A[<span class="dv">1</span>][<span class="dv">1</span>] = <span class="dv">1</span>;
<span class="co">//state->H = {1,0};</span>
state->H[<span class="dv">0</span>] = <span class="dv">1</span>;
state->H[<span class="dv">1</span>] = <span class="dv">0</span>;
<span class="co">//state->q = {{10e-6,0}, {0,10e-6}}; /* measure noise convariance */</span>
state->q[<span class="dv">0</span>] = <span class="fl">10e-7</span>;
state->q[<span class="dv">1</span>] = <span class="fl">10e-7</span>;
state->r = <span class="fl">10e-7</span>; <span class="co">/* estimated error convariance */</span>
}
<span class="co">/*</span>
<span class="co"> * @brief </span>
<span class="co"> * 2 Dimension kalman filter</span>
<span class="co"> * @inputs </span>
<span class="co"> * state - Klaman filter structure</span>
<span class="co"> * z_measure - Measure value</span>
<span class="co"> * @outputs </span>
<span class="co"> * state->x[0] - Updated state value, Such as angle,velocity</span>
<span class="co"> * state->x[1] - Updated state value, Such as diffrence angle, acceleration</span>
<span class="co"> * state->p - Updated estimated error convatiance matrix</span>
<span class="co"> * @retval </span>
<span class="co"> * Return value is equals to state->x[0], so maybe angle or velocity.</span>
<span class="co"> */</span>
<span class="dt">float</span> kalman2_filter(kalman2_state *state, <span class="dt">float</span> z_measure)
{
<span class="dt">float</span> temp0 = <span class="fl">0.</span>0f;
<span class="dt">float</span> temp1 = <span class="fl">0.</span>0f;
<span class="dt">float</span> temp = <span class="fl">0.</span>0f;
<span class="co">/* Step1: Predict */</span>
state->x[<span class="dv">0</span>] = state->A[<span class="dv">0</span>][<span class="dv">0</span>] * state->x[<span class="dv">0</span>] + state->A[<span class="dv">0</span>][<span class="dv">1</span>] * state->x[<span class="dv">1</span>];
state->x[<span class="dv">1</span>] = state->A[<span class="dv">1</span>][<span class="dv">0</span>] * state->x[<span class="dv">0</span>] + state->A[<span class="dv">1</span>][<span class="dv">1</span>] * state->x[<span class="dv">1</span>];
<span class="co">/* p(n|n-1)=A^2*p(n-1|n-1)+q */</span>
state->p[<span class="dv">0</span>][<span class="dv">0</span>] = state->A[<span class="dv">0</span>][<span class="dv">0</span>] * state->p[<span class="dv">0</span>][<span class="dv">0</span>] + state->A[<span class="dv">0</span>][<span class="dv">1</span>] * state->p[<span class="dv">1</span>][<span class="dv">0</span>] + state->q[<span class="dv">0</span>];
state->p[<span class="dv">0</span>][<span class="dv">1</span>] = state->A[<span class="dv">0</span>][<span class="dv">0</span>] * state->p[<span class="dv">0</span>][<span class="dv">1</span>] + state->A[<span class="dv">1</span>][<span class="dv">1</span>] * state->p[<span class="dv">1</span>][<span class="dv">1</span>];
state->p[<span class="dv">1</span>][<span class="dv">0</span>] = state->A[<span class="dv">1</span>][<span class="dv">0</span>] * state->p[<span class="dv">0</span>][<span class="dv">0</span>] + state->A[<span class="dv">0</span>][<span class="dv">1</span>] * state->p[<span class="dv">1</span>][<span class="dv">0</span>];
state->p[<span class="dv">1</span>][<span class="dv">1</span>] = state->A[<span class="dv">1</span>][<span class="dv">0</span>] * state->p[<span class="dv">0</span>][<span class="dv">1</span>] + state->A[<span class="dv">1</span>][<span class="dv">1</span>] * state->p[<span class="dv">1</span>][<span class="dv">1</span>] + state->q[<span class="dv">1</span>];
<span class="co">/* Step2: Measurement */</span>
<span class="co">/* gain = p * H^T * [r + H * p * H^T]^(-1), H^T means transpose. */</span>
temp0 = state->p[<span class="dv">0</span>][<span class="dv">0</span>] * state->H[<span class="dv">0</span>] + state->p[<span class="dv">0</span>][<span class="dv">1</span>] * state->H[<span class="dv">1</span>];
temp1 = state->p[<span class="dv">1</span>][<span class="dv">0</span>] * state->H[<span class="dv">0</span>] + state->p[<span class="dv">1</span>][<span class="dv">1</span>] * state->H[<span class="dv">1</span>];
temp = state->r + state->H[<span class="dv">0</span>] * temp0 + state->H[<span class="dv">1</span>] * temp1;
state->gain[<span class="dv">0</span>] = temp0 / temp;
state->gain[<span class="dv">1</span>] = temp1 / temp;
<span class="co">/* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/</span>
temp = state->H[<span class="dv">0</span>] * state->x[<span class="dv">0</span>] + state->H[<span class="dv">1</span>] * state->x[<span class="dv">1</span>];
state->x[<span class="dv">0</span>] = state->x[<span class="dv">0</span>] + state->gain[<span class="dv">0</span>] * (z_measure - temp);
state->x[<span class="dv">1</span>] = state->x[<span class="dv">1</span>] + state->gain[<span class="dv">1</span>] * (z_measure - temp);
<span class="co">/* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */</span>
state->p[<span class="dv">0</span>][<span class="dv">0</span>] = (<span class="dv">1</span> - state->gain[<span class="dv">0</span>] * state->H[<span class="dv">0</span>]) * state->p[<span class="dv">0</span>][<span class="dv">0</span>];
state->p[<span class="dv">0</span>][<span class="dv">1</span>] = (<span class="dv">1</span> - state->gain[<span class="dv">0</span>] * state->H[<span class="dv">1</span>]) * state->p[<span class="dv">0</span>][<span class="dv">1</span>];
state->p[<span class="dv">1</span>][<span class="dv">0</span>] = (<span class="dv">1</span> - state->gain[<span class="dv">1</span>] * state->H[<span class="dv">0</span>]) * state->p[<span class="dv">1</span>][<span class="dv">0</span>];
state->p[<span class="dv">1</span>][<span class="dv">1</span>] = (<span class="dv">1</span> - state->gain[<span class="dv">1</span>] * state->H[<span class="dv">1</span>]) * state->p[<span class="dv">1</span>][<span class="dv">1</span>];
<span class="kw">return</span> state->x[<span class="dv">0</span>];
}</code></pre>
<p>其实，Kalman滤波器由于其递推特性，实现起来很简单。但调参有很多可研究的地方，主要需要设定的参数如下：</p>
<ol style="list-style-type: decimal">
<li>init_x：待测量的初始值，如有中值一般设成中值（如陀螺仪）</li>
<li>init_p：后验状态估计值误差的方差的初始值</li>
<li>q：预测（过程）噪声方差</li>
<li>r：测量（观测）噪声方差。以陀螺仪为例，测试方法是：保持陀螺仪不动，统计一段时间内的陀螺仪输出数据。数据会近似正态分布，按3σ原则，取正态分布的(3σ)^2作为r的初始化值。</li>
</ol>
<p>其中q和r参数尤为重要，一般得通过实验测试得到。</p>
<p>找两组声阵列测向的角度数据，对上面的C程序进行测试。一维Kalman（一维也是标量的情况，就我所知，现在网上看到的代码大都是使用标量的情况）和二维Kalman（一个状态是角度值，另一个状态是向量角度差，也就是角速度）的结果都在图中显示。这里再稍微提醒一下：状态量不要取那些能突变的量，如加速度，这点在文章“从牛顿到卡尔曼”一小节就提到过。</p>
<div class="figure">
<img src="../images/Kalman滤波器从原理到实现/img6.png" alt="img6" /><p class="caption">img6</p>
</div>
<p>Matlab绘出的跟踪结果显示：</p>
<blockquote>
<p>Kalman滤波结果比原信号更平滑。但是有椒盐突变噪声的地方，Kalman滤波器并不能滤除椒盐噪声的影响，也会跟踪椒盐噪声点。因此，推荐在Kalman滤波器之前先使用中值滤波算法去除椒盐突变点的影响。</p>
</blockquote>
<p>上面所有C程序的源代码及测试程序都公布在我的<a href="https://github.com/xiahouzuoxin/kalman_filter">Github</a>上，希望大家批评指正其中可能存在的错误。</p>
<h2 id="参考资料">参考资料</h2>
<ol style="list-style-type: decimal">
<li>Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation. Ramsey Faragher, Lecture Notes.</li>
<li>An Introduction to the Kalman Filter. Greg Welch and Gary Bishop.</li>
<li>http://robotsforroboticists.com/kalman-filtering/, 公式彩色着色，含pyhton源码</li>
<li>http://alumni.media.mit.edu/~wad/mas864/psrc/kalman.c.txt, 包含Kalman滤波的C代码</li>
<li>http://www.cs.unc.edu/~welch/kalman/, 比较全的Kalman链接</li>
</ol>
<div class="ds-thread" data-thread-key="Kalman滤波器从原理到实现" data-title="Kalman滤波器从原理到实现" data-url="xiahouzuoxin.github.io/notes/html/Kalman滤波器从原理到实现.html"></div>
<script>window._bd_share_config={"common":{"bdSnsKey":{},"bdText":"","bdMini":"2","bdMiniList":false,"bdPic":"","bdStyle":"0","bdSize":"16"},"slide":{"type":"slide","bdImg":"5","bdPos":"right","bdTop":"300"},"image":{"viewList":["qzone","tsina","tqq","renren","weixin"],"viewText":"分享到：","viewSize":"16"},"selectShare":{"bdContainerClass":null,"bdSelectMiniList":["qzone","tsina","tqq","renren","weixin"]}};with(document)0[(getElementsByTagName('head')[0]||body).appendChild(createElement('script')).src='http://bdimg.share.baidu.com/static/api/js/share.js?v=89860593.js?cdnversion='+~(-new Date()/36e5)];</script>
<!-- 多说公共JS代码 start (一个网页只需插入一次) -->
<script type="text/javascript">
var duoshuoQuery = {short_name:"xiahouzuoxin"};
(function() {
var ds = document.createElement('script');
ds.type = 'text/javascript';ds.async = true;
ds.src = (document.location.protocol == 'https:' ? 'https:' : 'http:') + '//static.duoshuo.com/embed.js';
ds.charset = 'UTF-8';
(document.getElementsByTagName('head')[0]
|| document.getElementsByTagName('body')[0]).appendChild(ds);
})();
</script>
<!-- 多说公共JS代码 end -->
<div id="footer">
<p class="footer_subline">联系邮箱: [email protected]</p>
<p class="footer_subline">声明: 本站所有文章如非特别说明均为原创，转载请注明出处！
<script type="text/javascript">var cnzz_protocol = (("https:" == document.location.protocol) ? " https://" : " http://");document.write(unescape("%3Cspan id='cnzz_stat_icon_1253219218'%3E%3C/span%3E%3Cscript src='" + cnzz_protocol + "s4.cnzz.com/z_stat.php%3Fid%3D1253219218%26show%3Dpic' type='text/javascript'%3E%3C/script%3E"));</script>
</p>
</div>
<!-- 回到顶部 -->
<script>
lastScrollY=0;
function heartBeat(){
var diffY;
if (document.documentElement && document.documentElement.scrollTop)
diffY = document.documentElement.scrollTop;
else if (document.body)
diffY = document.body.scrollTop
else
{/*Netscape stuff*/}
percent=.1*(diffY-lastScrollY);
if(percent>0)percent=Math.ceil(percent);
else percent=Math.floor(percent);
document.getElementById("full").style.top=parseInt(document.getElementById("full").style.top)+percent+"px";
lastScrollY=lastScrollY+percent;
}
suspendcode="<div id=\"full\" style='right:1px;POSITION:absolute;TOP:600px;z-index:100'><a onclick='window.scrollTo(0,0);'><img src='../images/top.png'></a><br></div>"
document.write(suspendcode);
window.setInterval("heartBeat()",1);
</script>
</body>
</html>