卡尔曼滤波

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

复制代码
#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;
}
相关推荐
橘为己有4412 小时前
STM32:I2C(1)
stm32·单片机·嵌入式硬件
少司府4 小时前
嵌入式外设篇(三)GPIO·三:MCU系统结构、总线与统一编制
linux·python·stm32·单片机·嵌入式硬件·mcu·嵌入式软件
2501_922639585 小时前
率能半导体 SS8812T双通道H桥驱动芯片:36V/1.6A,覆盖POS打印机、安防相机、办公自动化、游戏机、机器人五大应用场景
stm32·单片机·嵌入式硬件·硬件工程
ScilogyHunter5 小时前
[14] TMS570LC4357_VIM向量中断:128 通道、双 VIM 与硬件向量分发
单片机·编辑器·vim·excel·hercules·tms570
国科安芯6 小时前
FPGA上电偶发不启动:多轨电源时序、PGOOD与并联供电的FAE排查笔记——ASP4644S2B在星载智能处理板中的工程应用分析
笔记·单片机·嵌入式硬件·fpga开发·低轨卫星·抗辐射
国科安芯19 小时前
卫星控制板偶发复位:MCU供电、时钟、ECC与总线的联合定位——AS32S601ZIT2在星载控制单元中的FAE排查方法
单片机·嵌入式硬件·mcu·risc-v·抗辐射·通信芯片
Zw-awa21 小时前
姿态是怎么算出来的:互补解算、四元数和欧拉角
stm32
jianqiang.xue1 天前
把Agent塞进MCU:工具次数和上下文长度怎么卡
java·前端·网络·单片机·esp32·ai编程
创界工坊工作室1 天前
106-基于51单片机的自行车测速仪设计
stm32·单片机·嵌入式硬件·51单片机·proteus
意法半导体STM321 天前
【官方原创】LAT1726 如何通过STM32H5 Bootloader进行固件升级与运行
stm32·单片机·嵌入式硬件·stm32h5·bootloader