杭州新网站建设方案_黄页网站大全在线看免费_成功营销十大经典案例_win7系统优化大师

看完这篇卡尔曼滤波原理，我被惊到了！ - 微波基础知识 - 微波射频网

#ifndef __COM_FILTER_H__
#define __COM_FILTER_H__
#include "stdint.h"/* 卡尔曼滤波器结构体 */
typedef struct
{float LastP; // 上一时刻的状态方差（或协方差）float Now_P; // 当前时刻的状态方差（或协方差）float out;   // 滤波器的输出值，即估计的状态float Kg;    // 卡尔曼增益，用于调节预测值和测量值之间的权重float Q;     // 过程噪声的方差，反映系统模型的不确定性float R;     // 测量噪声的方差，反映测量过程的不确定性
} KalmanFilter_Struct;extern KalmanFilter_Struct kfs[3];
int16_t Com_Filter_LowPass(int16_t newValue, int16_t preFilteredValue);double Com_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input);#endif /* __COM_FILTER_H__ */#include "Com_Filter.h"#define ALPHA 0.15 /* 一阶低通滤波 指数加权系数 */
/*** @description: 一阶低通滤波*  是一种常用的滤波器，用于去除高频噪声或高频成分，保留信号中的低频成分。*  在单片机应用中，一种简单且常见的低通滤波器是一阶无限脉冲响应（IIR）低通滤波器，*  通常实现为指数加权移动平均滤波器。* @param {int16_t} newValue 需要滤波的值* @param {int16_t} preFilteredValue 上一次滤波过的值* @return {*}*/
int16_t Com_Filter_LowPass(int16_t newValue, int16_t preFilteredValue)
{return ALPHA * newValue + (1 - ALPHA) * preFilteredValue;
}/* 卡尔曼滤波参数 */
KalmanFilter_Struct kfs[3] = {{0.02, 0, 0, 0, 0.001, 0.543},{0.02, 0, 0, 0, 0.001, 0.543},{0.02, 0, 0, 0, 0.001, 0.543}};
double Com_Filter_KalmanFilter(KalmanFilter_Struct *kf, double input)
{kf->Now_P = kf->LastP + kf->Q;kf->Kg    = kf->Now_P / (kf->Now_P + kf->R);kf->out   = kf->out + kf->Kg * (input - kf->out);kf->LastP = (1 - kf->Kg) * kf->Now_P;return kf->out;
}