三個最簡單公式講完卡爾曼濾波算法

概述

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

---

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

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>