iNav开源代码之Filters

1. 源由
2. 滤波器应用类型
- 2.1 一般滤波
- 2.1.1 pt1Filter
- 2.1.2 pt2Filter
- 2.1.3 pt3Filter
- 2.1.4 biquadFilter
- 2.2 kalman滤波
- 2.3 动态gyro带通滤波
- - 2.3.1 dynamicGyroNotchFilters
  - 2.3.2 secondaryDynamicGyroNotchFilters
- 2.4 rpm滤波
3. 滤波器技术类型
- 3.1 常见滤波原理
- 3.2 卡尔曼滤波简介
4. 参考资料

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 一般滤波

代码详见：src\main\common\filter.c

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滤波

代码详见：src\main\flight\kalman.c

【1】Kalman filter

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);