卡尔曼滤波的实现

blog 算法

卡尔曼滤波在数据采集中十分有用。本文介绍一下卡尔曼滤波的理论，然后给出一个实现。

具体的理论性介绍可以在 这里 找到

具体来说对于一维的卡尔曼滤波, 如果认为控制量为零的话卡尔曼滤波公式可以简化为以下 5 条:

X(k|k-1)=X(k-1|k-1) ……… (6)
P(k|k-1)=P(k-1|k-1) +Q ……… (7)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)

其中, X(k|k-1) 这次的估计值, X(k-1|k-1) 是上次的最优值 (也就是上次计算结果)

由于我们没有控制量, 所以假设这次的值和上次的相等, 所以令第 k 次估计值 = 第 k-1 次最优值

公式 7 中 P(k|k-1) 是这次估计值的偏差 (协方差), 令其等于上次最优值的偏差加上一个参数 (因为系统总是越运行偏差越大, 所以每次都加上一个 Q)

X(k|k)是本次的最优值 (也就是结果), Z(k) 是本次的测量值, Kg 是卡尔曼增益, 根据 (9) 计算出

由 (8), (9) 可以计算出结果

另外, 需要计算出这次最优值的偏差, 以便下次回归 (递归)

代码写出来很简单, 但是完成的功能却很强大:

volatile uint16_t adc_values[3];        //三个前方传感器
#define Q 1e-6f
#define R 1e-1f
volatile float kalman_value[3];
volatile float kalman_p[3];
void kalman_filter()
{
    int i;
    float p;
    float x;
    float kg;
    for (i = 0; i < 3; ++i) 
    {
        x = kalman_value[i];        //(6)
        p = kalman_p[i] + Q;        //(7)
        kg = p / (p + R);           //(9)
        kalman_p[i] = (1 - kg) * p; //(10)
        kalman_value[i] = x + kg * (adc_values[i] - x);     //(8)
    }
}