54 lines
1.6 KiB
C
54 lines
1.6 KiB
C
/**
|
||
* @file filter.h
|
||
* @author xxx
|
||
* @date 2023-08-08 22:59:46
|
||
* @brief
|
||
* @copyright Copyright (c) 2023 by xxx, All Rights Reserved.
|
||
*/
|
||
|
||
#ifndef __FILTER_H__
|
||
#define __FILTER_H__
|
||
#include "lib.h"
|
||
|
||
typedef struct
|
||
{
|
||
float32 x; // 卡尔曼滤波器的估计值
|
||
float32 a; // 状态转移矩阵（1，表示没有动态变化）
|
||
float32 h; // 观测矩阵（1，表示直接观测）
|
||
float32 q; // 过程噪声协方差
|
||
float32 r; // 观测噪声协方差
|
||
float32 p; // 估计误差协方差
|
||
float32 gain; // 卡尔曼增益
|
||
|
||
float32 change_max; // 允许的最大变化量，用于判断观测值是否异常
|
||
} kalman_t; // 卡尔曼滤波器结构
|
||
|
||
typedef struct
|
||
{
|
||
BOOL fisrt_flag; // 第一次标志位
|
||
float32 alpha; // 滤波系数 0~1
|
||
float32 last_value; // 上次滤波结果
|
||
} lpf_t; // 一阶低通滤波器
|
||
|
||
typedef struct
|
||
{
|
||
uint16_t size; // 滑动窗口大小
|
||
float32 *window; // 滑动窗口
|
||
volatile float32 sum; // 滑动窗口和
|
||
volatile float32 out; // 滤波结果
|
||
uint16_t index; // 滑动窗口索引
|
||
} lpf_window_t; // 滑动窗口滤波器
|
||
|
||
void kalman_init(kalman_t *cfg, float32 change_max);
|
||
float32 kalman_update(kalman_t *cfg, float32 input);
|
||
|
||
void lpf_init(lpf_t *cfg);
|
||
float32 lpf_update(lpf_t *cfg, float32 input);
|
||
void lpf_reset(lpf_t *cfg);
|
||
|
||
void lpf_window_init(lpf_window_t *cfg, uint16_t size);
|
||
void lpf_window_dinit(lpf_window_t *cfg);
|
||
float32 lpf_window_update(lpf_window_t *cfg, float32 input);
|
||
void lpf_window_reset(lpf_window_t *cfg);
|
||
#endif // __FILTER_H__
|