一阶卡尔曼滤波C语言

洪恒远

已于 2024-01-12 16:29:26 修改

阅读量461

点赞数 8

分类专栏：其他文章标签： c语言单片机

于 2024-01-12 16:26:21 首次发布

本文链接：https://blog.csdn.net/VIFIN/article/details/135556308

版权

其他专栏收录该内容

2 篇文章 0 订阅

订阅专栏

本文介绍了卡尔曼滤波算法在C语言中的实现，包括头文件中的数据类型定义、卡尔曼滤波器结构体以及初始化和一阶卡尔曼滤波函数的代码片段。

摘要由CSDN通过智能技术生成

一、头文件定义

typedef unsigned char Kalman_U8;
typedef unsigned short int Kalman_U16;
typedef unsigned long int Kalman_U32;
typedef float Kalman_F8;

typedef enum
{
	KalmanResult_NULL,
	KalmanResult_OK,
	KalmanResult_ERROR,
}
KalmanResult_Typedef;

typedef struct
{
	Kalman_F8 LastP;		//上次协方差
	Kalman_F8 NowP;			//当前协方差
	Kalman_F8 X_hat;		//卡尔曼滤波计算值
	Kalman_F8 Kg;			//卡尔曼增益系数
	Kalman_F8 Q;			//过程噪声
	Kalman_F8 R;			//测量噪声
}
Kalman_Typedef;

/*
 * 功能：初始化
 * 输入：p_Kalman：卡尔曼结果体指针
 * 		KalmanParameter：卡尔曼初始化参数
 * 输出：KalmanResult_ERROR：卡尔曼结果体指针无效
 *		KalmanResult_OK：初始化完成
 */
KalmanResult_Typedef KalmanInit(Kalman_Typedef* p_Kalman, Kalman_Typedef KalmanParameter);

/*
 * 功能：一阶卡尔曼滤波
 * 输入：p_Kalman：卡尔曼结果体指针
 * 输出：KalmanResult_ERROR：卡尔曼结果体指针无效
 *		KalmanResult_OK：计算完成
 * 备注：p_Kalman->X_hat存放最新的最优值
 */
KalmanResult_Typedef KalmanFilter(Kalman_Typedef* p_Kalman, Kalman_F8 Value);

二、工程文件

需额外包含头文件“stddef.h”

/*
 * 功能：初始化
 * 输入：p_Kalman：卡尔曼结果体指针
 * 		KalmanParameter：卡尔曼初始化参数
 * 输出：KalmanResult_ERROR：卡尔曼结果体指针无效
 *		KalmanResult_OK：初始化完成
 */
KalmanResult_Typedef KalmanInit(Kalman_Typedef* p_Kalman, Kalman_Typedef KalmanParameter)
{
	if(p_Kalman == NULL)
	{
		return KalmanResult_ERROR;
	}
	else
	{
		*p_Kalman = KalmanParameter;

		return KalmanResult_OK;
	}
}

/*
 * 功能：一阶卡尔曼滤波
 * 输入：p_Kalman：卡尔曼结果体指针
 * 输出：KalmanResult_ERROR：卡尔曼结果体指针无效
 *		KalmanResult_OK：计算完成
 * 备注：p_Kalman->X_hat存放最新的最优值
 */
KalmanResult_Typedef KalmanFilter(Kalman_Typedef* p_Kalman, Kalman_F8 Value)
{
	if(p_Kalman == NULL)
	{
		return KalmanResult_ERROR;
	}
	else
	{
		p_Kalman->NowP = p_Kalman->LastP + p_Kalman->Q;
		p_Kalman->Kg = p_Kalman->NowP / (p_Kalman->NowP + p_Kalman->R);
		p_Kalman->X_hat = p_Kalman->Kg * (Value - p_Kalman->X_hat) + p_Kalman->X_hat;
		p_Kalman->LastP = (1 - p_Kalman->Kg) * p_Kalman->NowP;

		return KalmanResult_OK;
	}
}