三个最简单公式讲完卡尔曼滤波算法

概述

首先明确一下卡尔曼滤波的基本概念：可参考知乎诸位大神的“如何通俗易懂地描述卡尔曼滤波“，这里我也稍微说明一下。
所谓卡尔曼滤波就是当你在测量一个值时，同时拥有模型估计和直接测量两种方式，但是两种方式都不太准确，于是就可以用卡尔曼增益系数来分配两种方式的可信度权重，以得出新的估计值，并以新估计值为基础，更新卡尔曼增益系数重新分配权重，逐步逼近真实值。卡尔曼增益系数却决于两种方式的方差，哪个更靠谱就更相信哪个。
以下三个公式用倒推的方法说明如何实现卡尔曼滤波。

---

公式一：用上一步的估计值更准确地求这一步的估计值

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();
}