1 一阶低通滤波（IIR）

$$Y(n) = \alpha X(n)+(1-\alpha)Y(n-1)$$
其中 $Y(n)$ 表示当前系统的输出，$X(n)$ 则为系统当前的输入；$\alpha\in(0,1)$ 为滤波系数，而 $Y(n-1)$ 为前一次的输出。

1
2
3
4
5
int filter_out; 
static int filter_prev; 
float filter_coeff = 0.7; 
filter_out = (filter_coeff * filter_prev) + ((1-filter_coeff) * val); 
filter_prev = filter_out;

其中 filter_prev 对应前一次执行时的输出项，static 修饰保证了其能够继承 filter_prev = filter_out 的效果，进行下一轮运算。

filter_coeff 即滤波系数，假设系统输入为 val ，那么 filter_out ，即当前系统输出的计算形式就对应上文中的公式。最后只需要更新 filter_prev 就可以了。

2 卡尔曼滤波

总体来说是一种预测-修正的过程，用上一次得到的最优结果做先验估计，然后根据实际测量的值对估计值进行修正，不对数学原理做过多探究。

2.1 预测

$$\hat{x_i^-}=F\hat{x_{i-1}}+B\hat{u_{i-1}}\space(Estimate)$$
预测模型表示，先验估计结果由上一次的 后验估计结果 以及上一时刻的 系统控制输入 组成，并且受到状态转移矩阵和控制输入矩阵的影响。

$$P_i^-=FP_{i-1}F^T+Q\space (Covariance\space Estimate)$$

协方差模型表示，预测阶段的误差协方差由 上一时刻的不确定性与 噪声协方差 组成。

2.2 更新（融合）

$$K_{i} = P_i^-H^T(HP_i^-H^T+R)^{-1}$$

卡尔曼增益的计算依赖 预测误差协方差、观测矩阵及其转置 以及 测量噪声协方差。
$$x_i = \hat{x_i^-} + K_i (z_i − H \hat{x_i^-})$$

得到后验估计结果。

$$P_i = (1 − K_i H) P_i^-$$

同时，更新后验的协方差以便下一次使用。

2.3 代码实现（一维卡尔曼）

一维的情况下，矩阵相关运算基本都简化了。

1
2
3
4
5
6
7
8
typedef struct
{
	float X; // 当前时刻先验估计值
	float P; // 先验估计误差协方差
	float Q; // 过程噪声协方差
	float R; // 测量噪声协方差
	float K; // 卡尔曼增益
}Kalman_t

初始化滤波器结构体：

1
2
3
4
5
6
7
8
void Kalman_Init(Kalman_t *filter, float Q, float R, float init_val)
{
	filter->X = init_val;
	filter->P = 1.0f;
	filter->Q = Q;
	filter->R = R;
	filter->K = 0.0f
}
1
2
3
4
5
6
7
8
9
10
11
12
13
float Kalman_Updata(Kalman_t *filter, float measurement)
{
    /* 预测 */
	float x_pred = filter->X; //先验
	float p_pred = filter->P + filter->Q //协方差模型
	
	/* 更新 */
	filter->K = p_pred / (p_pred + filter->R); // 增益计算
	filter->X = x_pred + filter->K * (measurement-x_pred); //后验估计计算
	filter->P = (1.0f - filter->K) * p_pred; // 后验协方差计算
	
	return filter->X;
}
1
2
3
4
5
6
7
8
Kalman_t filter;
Kalman_Init(&filter....);
while(1)
{
	float mea = GetXXX(...);
	float filtered = Kalman_Update(&filter,mea); //滤波结果
	...
}

不难看出一维应用的场景下，确定 Q 和 R 也就是噪声项是比较重要的。

  • Q（过程噪声协方差）：反映系统模型的不确定性

    • Q越大，表示越不信任模型预测，更依赖测量值
    • Q越小，表示模型很准确，滤波器响应较慢

  • R（测量噪声协方差）：反映传感器测量的不确定性

    • R越大，表示测量值不可靠，更依赖模型预测
    • R越小，表示测量很准确，滤波器响应较快

一般可以在静态系统环境下记录多次传感器数据后记录方差作为 R 的参考值, 然后调节 Q （影响系统响应速度）的值来观察效果。