iNav开源代码之Filters
1. 源由
iNav 6.1.1远航出现异常：RC断链&GPS信号丢失导致炸机。
根据当前Emergency landing tumbled crash? – inav 6.1.1 #9184最新获得的信息看，IMU姿态原始数据正常，IMU滤波数据异常，PID控制器内部数据出现了溢出。但尚不清楚是什么原因导致的该情况发生？
从事件顺序的角度看，“【传感数据】IMU姿态”滤波似乎更靠近出问题的前端。
因此，我们看下iNav的滤波器设计，当然从目前Git代码改动情况看，如果存在问题更倾向接口参数类型变更导致精度出现问题，比如：double修改为float。
2. 滤波器应用类型
- 一般滤波
- kalman滤波
- 动态gyro带通滤波
- rpm滤波
2.1 一般滤波
typedef union {
biquadFilter_t biquad;
pt1Filter_t pt1;
pt2Filter_t pt2;
pt3Filter_t pt3;
} filter_t;
void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);
细分类型：
- pt1Filter
- pt2Filter
- pt3Filter
- biquadFilter
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_PT2,
FILTER_PT3
} filterType_e;
2.1.1 pt1Filter
一阶低通滤波
- 低通
- 解耦
- 滤波器配置参数(对象)：pt1Filter_t *filter
typedef struct pt1Filter_s {
float state;
float RC;
float dT;
float alpha;
} pt1Filter_t;
void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
float pt1FilterGetLastOutput(pt1Filter_t *filter);
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
void pt1FilterReset(pt1Filter_t *filter, float input);
2.1.2 pt2Filter
二阶低通滤波
- 低通
- 解耦
- 滤波器配置参数(对象)：pt2Filter_t *filter
typedef struct pt2Filter_s {
float state;
float state1;
float k;
} pt2Filter_t;
float pt2FilterGain(float f_cut, float dT);
void pt2FilterInit(pt2Filter_t *filter, float k);
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
float pt2FilterApply(pt2Filter_t *filter, float input);
2.1.3 pt3Filter
三阶低通滤波
- 低通
- 解耦
- 滤波器配置参数(对象)：pt3Filter_t *filter
typedef struct pt3Filter_s {
float state;
float state1;
float state2;
float k;
} pt3Filter_t;
float pt3FilterGain(float f_cut, float dT);
void pt3FilterInit(pt3Filter_t *filter, float k);
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
float pt3FilterApply(pt3Filter_t *filter, float input);
2.1.4 biquadFilter
- 支持低通、带通
- 解耦
- 滤波器配置参数(对象)：biquadFilter_t *filter
typedef enum {
FILTER_LPF,
FILTER_NOTCH
} biquadFilterType_e;
typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2;
float x1, x2, y1, y2;
} biquadFilter_t;
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);
API接口：
void biquadFilterInitNotch(biquadFilter_t *filter, uint32_t samplingIntervalUs, uint16_t filterFreq, uint16_t cutoffHz);
void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs);
float biquadFilterApply(biquadFilter_t *filter, float sample);
float biquadFilterReset(biquadFilter_t *filter, float value);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
2.2 kalman滤波
void gyroKalmanInitialize(uint16_t q);
float gyroKalmanUpdate(uint8_t axis, float input);
void gyroKalmanUpdateSetpoint(uint8_t axis, float setpoint);
注：后面会重点介绍这款滤波器，详见第三章节。
2.3 动态gyro带通滤波
这里的带通滤波主要应用一般滤波biquadFilter中的FILTER_NOTCH滤波器。
2.3.1 dynamicGyroNotchFilters
代码详见：src\main\flight\dynamic_gyro_notch.c
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
/*
* Number of peaks to detect with Dynamic Notch Filter aka Matrix Filter. This is equal to the number of dynamic notch filters
*/
#define DYN_NOTCH_PEAK_COUNT 3
typedef struct dynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
float dynNotchQ;
uint32_t looptime;
uint8_t enabled;
biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
} dynamicGyroNotchState_t;
void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state);
void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]);
float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input);
2.3.2 secondaryDynamicGyroNotchFilters
代码详见：src\main\flight\secondary_dynamic_gyro_notch.c
/**
- Secondary dynamic notch filter.
- In some cases, noise amplitude is high enough not to be filtered by the primary filter.
- This happens on the first frequency with the biggest aplitude
*/
typedef struct secondaryDynamicGyroNotchState_s {
uint16_t frequency[XYZ_AXIS_COUNT];
float dynNotchQ;
uint32_t looptime;
uint8_t enabled;
biquadFilter_t filters[XYZ_AXIS_COUNT];
filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT];
} secondaryDynamicGyroNotchState_t;
void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state);
void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]);
float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input);
2.4 rpm滤波
在rpm滤波器内部实现中，仍然使用biquadFilter中的FILTER_NOTCH带通滤波器。代码详见：src\main\flight\rpm_filter.c
typedef struct rpmFilterConfig_s {
uint8_t gyro_filter_enabled;
uint8_t dterm_filter_enabled;
uint8_t gyro_harmonics;
uint8_t gyro_min_hz;
uint16_t gyro_q;
uint8_t dterm_harmonics;
uint8_t dterm_min_hz;
uint16_t dterm_q;
} rpmFilterConfig_t;
PG_DECLARE(rpmFilterConfig_t, rpmFilterConfig);
#define RPM_FILTER_UPDATE_RATE_HZ 500
#define RPM_FILTER_UPDATE_RATE_US (1000000.0f / RPM_FILTER_UPDATE_RATE_HZ)
void disableRpmFilters(void);
void rpmFiltersInit(void);
void rpmFilterUpdateTask(timeUs_t currentTimeUs);
float rpmFilterGyroApply(uint8_t axis, float input);
3. 滤波器技术类型
- pt1Filter
- pt2Filter
- pt3Filter
- biquadFilter
- kalmanFilter
3.1 常见滤波原理
关于前面四种滤波器的原理，可参考：BetaFlight模块设计之二十九：滤波模块分析。这里着重介绍下kalman滤波器。
3.2 卡尔曼滤波简介
卡尔曼滤波非常适合不断变化的系统，其优点是内存占用较小（只需保留前一个状态）、速度快，是实时问题和嵌入式系统的理想选择。
结合一个物体（飞行器）位置和速度的关系来简单的描述下kalman滤波。
在速度-位置坐标系中表示：
在物体匀速运动时：
经过变化，可以表示为：
通常来说，变速运动比较常见，因此引入加速度概念后：
经过变化，可以表示为：
然后，这个位置-速度坐标系下，可以表达为：
当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。
这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和“世界”相关的所有不确定性：
X
k
X_k
Xk的每个预测状态都可能会移动到另一点，也就是蓝色的高斯分布会移动到紫色高斯分布的位置，并且具有协方差
Q
k
Q_k
Qk。换句话说，我们把这些不确定影响视为协方差
Q
k
Q_k
Qk的噪声。
这个紫色的高斯分布拥有和原分布相同的均值，但协方差不同。表达式：
简而言之，这里：
-
新的最佳估计是基于 原最佳估计 和 已知外部影响校正后得到的预测。
-
新的不确定性是基于 原不确定性 和 外部环境的不确定性 得到的预测。
详细参考资料：
【1】卡爾曼濾波器的原理和應用
【2】尔曼滤波，一份通俗易懂的教程
注：数学上的推导过程相对更为严谨，这里也就不多码字了，请大家参考：kalman_intro
4. 参考资料
【1】iNav开源代码之严重炸机 – 危险隐患
【2】iNav开源代码之严重炸机 – FAILSAFE
【3】iNav开源代码之严重炸机 – 紧急降落
【4】iNav开源代码之EmergencyLanding
【5】BetaFlight模块设计之二十九：滤波模块分析
5190
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
