卡尔曼滤波的实现

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

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

具体来说对于一维的卡尔曼滤波, 如果认为控制量为零的话卡尔曼滤波公式可以简化为以下 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)
	}
}