卡尔曼滤波算法-简易版

代码实现：

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

滤波算法excel实现

卡尔曼滤波算法-简易版相关推荐

node.js——麻将算法（六）简易版麻将出牌AI1.0
普通麻将的出牌AI如果不是要求特别高的话,其实蛮容易实现的,毕竟大多数人打牌都只是看自己的手牌. 所以作为简易版的AI,出牌的策略只要奔着胡牌去就可以了.我们能想到的就是把相邻或相同的牌凑到一起,把单 ...
智能停车场[简易版]
智能停车场[简易版] 项目简介开发环境功能简介停车场收费管理停车场停车位管理网页端功能实现 Socket通信数据库相关知识本地数据库操作 pymysql数据操作 web开发中的数据库操 ...
JS正则表达式-附身份证号码验证（简易版）
文章目录 JS正则表达式匹配与搜索一.方括号,圆括号二.修饰符三.元字符四.量词身份证号码验证(简易版) 身份证号码规则: 1.地址码 2.年份码 3.月份码 4.日期码 5.顺序码 6. ...
简易版 useState 实现
基础版 useState 简单实现:只是数组通过数组实现,初始化的时候,创建两个数组:states 与 setters ,设置光标 cursor 为 0 第一次调用 useState 时,创建一个 ...
互联网公司实习岗位选择与简易版职业发展规划
文章目录 1.互联网公司的实习岗位 1.1 互联网公司有哪些? 大厂,中厂,小厂的界定是什么? 1.2 实习岗位方向都有哪些? 1.3 在哪里投递简历?哪里找实习信息? 1.4 参考资料 2.入职门槛 ...
连连看连接算法Javascript版
继连连看连接算法Python版后,我将该算法移植到了Javascript上,为在浏览器版连连看做准备. 功能及使用方法参照另外一篇:连连看连接算法Python版. 值得一提的是由于Javascript ...
c语言按给定成绩查询,C语言学生成绩管理系统（简易版）
#include #include #include int readstudents(struct students stu[]); //读取学生信息 int readsexcode(struct ...
Matlab简易版美图秀秀（GUI界面实现）
该项目为数字图像处理课程的期末大项目,主要内容是用matlab软件实现一些美图秀秀相关功能,比如对图像进行水平垂直翻转,旋转,裁剪,抠图等基础功能:对图像添加浮雕,艺术噪声,灰度胶片,动感模糊,素描, ...
基于C++的简易版《球球大作战》游戏设计
全套资料下载地址:https://download.csdn.net/download/sheziqiong/85602628 全套资料下载地址:https://download.csdn.net/d ...

卡尔曼滤波算法-简易版

滤波算法excel实现

卡尔曼滤波算法-简易版相关推荐

最新文章

热门文章