卡尔曼滤波算法-简易版

news/2025/1/12 6:00:27/

在这里插入图片描述

代码实现：

/********************************************************************************* @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;//上一时刻的值 //公式1p_mid = pKalman->p_last + pKalman->covQ;  //公式2kg = p_mid / (p_mid + pKalman->covR); //公式3x_now = x_mid + kg * (measure - x_mid);//当前时刻的最优值 //公式4p_now = (1 - kg) * p_mid;  //当前时刻的最优值偏差 公式5pKalman->p_last = p_now;pKalman->x_last = x_now;return x_now;
}