卡尔曼滤波算法-简易版

卡尔曼滤波算法-简易版 2022-08-30 982

代码实现：

/**
 ******************************************************************************
 * @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;
}