@zwh8800
2016-04-30T01:33:21.000000Z
字数 859
阅读 327106
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)
}
}