[TOC]
一阶低通滤波
一阶低通滤波，又叫 一阶惯性滤波，其算法公式为：
\[Y_k = \alpha X_k + (1-\alpha) Y_{k-1}\]根据安卓开发者文档中提供的低通滤波算法：
// https://developer.android.com/guide/topics/sensors/sensors_motion#java
public void onSensorChanged(SensorEvent event){
// In this example, alpha is calculated as t / (t + dT),
// where t is the low-pass filter's time-constant and
// dT is the event delivery rate.
final float alpha = 0.8;
// Isolate the force of gravity with the low-pass filter.
gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0];
gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1];
gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2];
// Remove the gravity contribution with the high-pass filter.
linear_acceleration[0] = event.values[0] - gravity[0];
linear_acceleration[1] = event.values[1] - gravity[1];
linear_acceleration[2] = event.values[2] - gravity[2];
}
我实现了加速度的一阶低通滤波算法：
# https://github.com/cggos/sensor_imu/blob/master/script/sensor_imu.py#L285-L293
alpha = 0.8
self.AccTmp[0] = alpha * self.AccTmp[0] + (1-alpha) * msg.linear_acceleration.x
self.AccTmp[1] = alpha * self.AccTmp[1] + (1-alpha) * msg.linear_acceleration.y
self.AccTmp[2] = alpha * self.AccTmp[2] + (1-alpha) * msg.linear_acceleration.z
msg.linear_acceleration.x = self.AccTmp[0]
msg.linear_acceleration.y = self.AccTmp[1]
msg.linear_acceleration.z = self.AccTmp[2]
self.imu_pub_filter.publish(msg)
效果如图所示：
可以看到滤波效果明显，并且具有滞后性。