概述

首先明確一下卡爾曼濾波的基本概念：可參考知乎諸位大神的“如何通俗易懂地描述卡爾曼濾波“，這里我也稍微說明一下。
所謂卡爾曼濾波就是當你在測量一個值時，同時擁有模型估計和直接測量兩種方式，但是兩種方式都不太準確，于是就可以用卡爾曼增益系數來分配兩種方式的可信度權重，以得出新的估計值，并以新估計值為基礎，更新卡爾曼增益系數重新分配權重，逐步逼近真實值。卡爾曼增益系數卻決于兩種方式的方差，哪個更靠譜就更相信哪個。
以下三個公式用倒推的方法說明如何實現卡爾曼濾波。

---

公式一：用上一步的估計值更準確地求這一步的估計值

X[i+1]=(1-K[i+1])x[i]+K[i+1]*z[i+1]
x即估計值，z即測量值。
可以看出，這次的估計值是由上次的估計值和這次的測量值共同決定的，這兩者所占權重由K決定，K就是我們說的卡爾曼增益系數。

---

公式二：卡爾曼增益系數怎么來

K[i+1]=(P[i]+Q)/(P[i]+Q+R)
P是上一次估計值的方差，Q是高斯噪聲的方差，R是測量值的方差，Q和R都是常數。
可以看出，K是由兩類方差的比值決定的，因為Q，R都是常數，所以上一次估計值的方差起決定性作用，如果上一次估計完以后發現方差很大，說明估計的不太靠譜，在這個式子中易知K會隨之變大更接近1，這樣的話結合上一個公式，測量值所占比值就會更大，也就是說這一次的估計值更信任測量值。

---

公式三：估計值的方差怎么來

P[i+1]=(1-K[i+1])P[i]
這一次的方差由上一次的方差和這次的增益系數決定

---

總結

只要滿足卡爾曼濾波使用條件（各種狀態符合正態分布），給出第一個估計值，第一個估計方差，高斯噪聲方差，測量值方差，將這三個公式放進一個迭代循環里，估計值x就能一步一步逼近真實值。

代碼

附一個C語言實現卡爾曼濾波的簡單例子的代碼。
代碼是http://blog.csdn.net/sinat_20265495/article/details/51006311這篇博客提供的，自己按理解添加了很多注釋。

#include "stdio.h" #include "stdlib.h" #include "math.h"double frand() {return 2 * ((rand() / (double)RAND_MAX) - 0.5);//隨機噪聲(-1~1之間)：rand()/RAND_MAX為0~1之間 }void main() {float x_last = 0;//初始估計值float p_last = 0.02;//初始估計值方差float Q = 0.018;//高斯噪聲方差float R = 0.542;//測量方差，反應測量的精度float kg;//卡爾曼增益float x_mid;float x_now;float p_mid;float p_now;float z_real = 0.56;//實際值（要測量的目標）float z_measure;float summerror_kalman = 0;//測量累積誤差float summerror_measure = 0;//估計值累積誤差int i;x_last = z_real + frand()*0.03;x_mid = x_last;for (i = 0; i < 100;i++){x_mid = x_last;//上一個卡爾曼估計值p_mid = p_last + Q;//上一個估計值方差加上噪聲方差，即實際方差kg = p_mid / (p_mid + R);//新的卡爾曼增益系數，新估計值方差與新估計值方差加測量方差的比值。如果測量方差很大，顯然卡爾曼系數變小，計算新估計值時，分配給測量值的權重就小一點z_measure = z_real + frand()*0.03;//新的測量值,系統隨機生成x_now = x_mid + kg*(z_measure - x_mid);//即x[i]=(1-kg)*x[i-1]+kg*z[i]，由測量值和上一次估計值共同決定這次估計值，信誰多一點由卡爾曼增益系數決定，卡爾曼增益系數又由每一輪的新方差決定p_now = (1 - kg)*p_mid;//新的估計方差，原方差的卡爾曼信任度算得printf("真實值:%6.3f\n", z_real);printf("測量值:%6.3f [差值（絕對值）:%.3f]\n", z_measure, fabs(z_real - z_measure));printf("卡爾曼值: %6.3f [差值（絕對值）:%.3f]\n", x_now, fabs(z_real - x_now));printf("\n\n");summerror_kalman += fabs(z_real - x_now);//卡爾曼誤差累積summerror_measure += fabs(z_real - z_measure);//測量誤差累積p_last = p_now;x_last = x_now;}printf("總體測量誤差累積 :%f\n", summerror_measure);printf("總體卡爾曼濾波誤差累積:%f\n", summerror_kalman);printf("卡爾曼誤差所占比例(越大越好咯):%d%%\n", 100 - (int)((summerror_kalman / summerror_measure) * 100));getchar(); }

• 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

<link href="https://csdnimg.cn/release/phoenix/mdeditor/markdown_views-2b43bc2447.css" rel="stylesheet"></div>

總結

以上是生活随笔為你收集整理的三个最简单公式讲完卡尔曼滤波算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。