自动驾驶 - 滤波算法

目前比较常用的滤波算法有：

平均值滤波算法
中位值滤波算法
一阶滤波算法
卡尔曼滤波算法

1. 平均值滤波算法

1.1. 算法介绍

平均值滤波算法是比较常用，也比较简单的滤波算法。在滤波时，将N个周期的采样值计算平均值，算法非常简单。当N取值较大时，滤波后的信号比较平滑，但是灵敏度差；相反N取值较小时，滤波平滑效果差，但灵敏度好。

优点：算法简单，对周期性干扰有良好的抑制作用，平滑度高，适用于高频振动的系统。

缺点：对异常信号的抑制作用差，无法消除脉冲干扰的影响。

1.2. 实现代码

下面的代码是平均值滤波的示例代码。

float data[10];float averageFilter(float in_data)
{float sum = 0;for(int i=0; i<9; i++){data[i]=data[i+1];sum = sum + data[i];}data[9] = in_data;sum = sum + data[9];return(sum/10);}

在代码中，data[]为全局变量，它用来记录10个周期的采样值，averageFilter()为滤波函数，它的输入为新采集到的数据，函数中，首先将data[]中的数据进行移位，并将新采集到的数据保存到data[]中，同时计算data[]中10个数据的和，最后返回10个数据和的平均值。

2. 中位值滤波算法

2.1. 算法介绍

中位值滤波算法的实现方法是采集N个周期的数据，去掉N个周期数据中的最大值和最小值，取剩下的数据的平均值。中位值滤波算法特别适用于会偶然出现异常值的系统。中位值滤波算法应用比较广泛，比如用于一些比赛的评分，经常是去掉一个最高分去掉一个最低分，将其他评分取平均值作为选手的最终得分。

优点：相比于平均值滤波算法，中位值滤波算法能够有效滤除偶然的脉冲干扰。

缺点：与平均值滤波算法相同，中位值滤波算法也存在反应速度慢、滞后的问题。

2.2. 实现代码

下面的代码是中位值滤波的示例代码。

float data[10];float middleFilter(float in_data)
{float sum = 0;float temp[10];float change;int i,j;//记录数据for(i=0; i<9; i++){data[i]=data[i+1];}data[9] = in_data;//复制数据for(i=0; i<10; i++)temp[i] = data[i];//冒泡法排序for(i=1; i<10; i++)for(j=0; j<10-i; j++){if(temp[j] > temp[j+1]){change = temp[j];temp[j] = temp[j+1];temp[j+1] = change;}}//求和for(i=1; i<9; i++)sum = sum + temp[i];//返回平均值return(sum/8);}

在上面的代码中，分为几个步骤：

步骤1：读取新数据，并更新数据数组；

步骤2：复制数据到临时数组，以便保持原始数据的顺序不变；

步骤3：对临时数组进行排序；

步骤4：计算中位平均值。

3. 一阶滤波算法

3.1. 算法介绍

一阶滤波算法是比较常用的滤波算法，它的滤波结果=a*本次采样值+（1-a）*上次滤波结果，其中，a为0~1之间的数。一阶滤波相当于是将新的采样值与上次的滤波结果计算一个加权平均值。a的取值决定了算法的灵敏度，a越大，新采集的值占的权重越大，算法越灵敏，但平顺性差；相反，a越小，新采集的值占的权重越小，灵敏度差，但平顺性好。

优点：对周期干扰有良好的抑制作用，适用于波动频率比较高的场合，它不用记录历史数据。

缺点：滞后、灵敏度低。

3.2. 实现代码

下面的代码是一阶滤波的示例代码。

float final=0;
float a=0.25;float firstOrderFilter(float in_data)
{final = a*in_data + (1-a)*final; return(final);
}

4. 卡尔曼滤波算法

4.1. 算法介绍

卡尔曼滤波是一个神奇的滤波算法，应用非常广泛，它是一种结合先验经验、测量更新的状态估计算法。

完整卡尔曼滤波算法

有了上面的推导，我们在下面列出来完成卡尔曼滤波的公式，卡尔曼滤波分为预测过程和更新过程两个过程，在公式中，我们又引入了缩放系数h，和协方差q。

4.2. 实现代码

下面我们通过c++代码来实现卡尔曼滤波算法，所实现的算法为一维滤波算法。首先定义卡尔曼滤波的参数：

typedef struct{float filterValue;//滤波后的值float kalmanGain;//Kalamn增益float A;//状态矩阵float H;//观测矩阵float Q;//状态矩阵的方差float R;//观测矩阵的方差float P;//预测误差float B;float u;
}KalmanInfo;

下面是卡尔曼滤波器的初始化函数，在这个函数中，info为卡尔曼滤波参数的指针。初始化的参数是针对一个车速滤波过程的设置。

void Kalm::initKalmanFilter(KalmanInfo *info)
{info->A = 1;info->H = 1;info->P = 0.1;info->Q = 0.05;info->R = 0.1;info->B = 0.1;info->u = 0;info->filterValue = 0;
}

卡尔曼滤波过程函数，函数的输入info为卡尔曼滤波参数的指针，new_value为新的测量值，函数返回滤波后的估计值。

float Kalm::kalmanFilterFun(KalmanInfo *info, float new_value)
{float predictValue = info->A*info->filterValue+info->B*info->u;//计算预测值info->P = info->A*info->A*info->P + info->Q;//求协方差info->kalmanGain = info->P * info->H /(info->P * info->H * info->H + info->R);//计算卡尔曼增益info->filterValue = predictValue + (new_value - predictValue)*info->kalmanGain;//计算输出的值info->P = (1 - info->kalmanGain* info->H)*info->P;//更新协方差return info->filterValue;
}