代码实现:
/**
******************************************************************************
* @file bsp_kalman.h
* @author Zxp
* @version V1.0.0
* @date 2021-06-08
* @brief 卡尔曼滤波算法头文件
******************************************************************************
*/
#ifndef __BSP_KALMAN_H_
#define __BSP_KALMAN_H_
#ifdef __cplusplus
extern "C"
{
#endif
#include "stddef.h"
#include "stdint.h"
typedef struct tagKalmanParam
{
float covQ; ///< 过程噪声协方差,Q增大,动态响应变快,收敛稳定性变坏
float covR; ///< 观测噪声协方差,R增大,动态响应变慢,收敛稳定性变好
float x_last; //上一时刻的最佳估计
float p_last; //上一时刻的最优偏差
} KalmanTypeDef;
int32_t BSP_InitKalmanParameter(KalmanTypeDef* pKalman, float covR, float covQ);
float BSP_CaluKalman(KalmanTypeDef* pKalman, float measure);
#ifdef __cplusplus
}
#endif
#endif // #ifndef __BSP_KALMAN_H_
/**
******************************************************************************
* @file bsp_kalman.c
* @author Zxp
* @version V1.0.0
* @date 2021-06-08
* @brief 此文件实现了卡尔曼滤波算法
参考博客:
https://www.zhihu.com/question/22422121
******************************************************************************
*/
#include "bsp_kalman.h"
/**
* @brief 初始化卡尔曼滤波的参数
* @param pKalman: 要设置的卡尔曼参数结构的指针
* @param covR: 观测噪声协方差
* @param covQ: 过程噪声协方差
* @return 状态
* @retval 0: 设置成功
* @retval -1: 指针为空
*/
int32_t BSP_InitKalmanParameter(KalmanTypeDef* pKalman, float covR, float covQ)
{
if (pKalman == NULL)
{
return -1;
}
pKalman->covR = covR;
pKalman->covQ = covQ;
pKalman->x_last = 0;
pKalman->p_last = pKalman->covQ;
return 0;
}
/**
* @brief 计算滤波值
* @param pKalman: 要计算的卡尔曼参数结构的指针
* @param measure: 测量到的原始数据
* @return 滤波值,若pKalman为空,则返回0
*/
float BSP_CaluKalman(KalmanTypeDef* pKalman, float measure)
{
float x_mid, kg, p_mid;
float x_now, p_now;
if (pKalman == NULL)
{
return 0;
}
x_mid = pKalman->x_last;//上一时刻的值 //公式1
p_mid = pKalman->p_last + pKalman->covQ; //公式2
kg = p_mid / (p_mid + pKalman->covR); //公式3
x_now = x_mid + kg * (measure - x_mid);//当前时刻的最优值 //公式4
p_now = (1 - kg) * p_mid; //当前时刻的最优值偏差 公式5
pKalman->p_last = p_now;
pKalman->x_last = x_now;
return x_now;
}
滤波算法excel实现