平衡小车卡尔曼滤波算法

最新推荐文章于 2024-07-31 16:49:23 发布

o0o0o0D

最新推荐文章于 2024-07-31 16:49:23 发布

阅读量9.8k

点赞数 18

本文链接：https://blog.csdn.net/o0o0o0d/article/details/53535623

版权

本文详细探讨了如何使用卡尔曼滤波算法优化平衡小车的稳定性。通过理论分析与实践结合，阐述了卡尔曼滤波在估计小车状态中的关键作用，包括姿态角度和速度的精确估计，从而提升小车的平衡控制能力。

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

最近研究STM32的自平衡小车，发现有两座必过的大山，一为卡尔曼滤波，二为PID算法。
网上看了很多关于卡尔曼滤波的代码，感觉写得真不咋地。一怒之下，自己重写，不废话，贴代码

[pre lang="C" line="1" file="kalman.h"]/**
  ******************************************************************************
  * @file kalman.h
  * @author  willieon
  * @version V0.1
  * @date January-2015
  * @brief 卡尔曼滤波算法
  *
  *
  ******************************************************************************
  * @attention
  *本人对卡尔曼的粗略理解：以本次测量角速度（陀螺仪测量值）的积分得出的角度值
  * 与上次最优角度值的方差产生一个权重来衡量本次测量角度（加速度测量值）
  * 与上次最优角度值，从而产生新的最优角度值。好吧，比较拗口，有误处忘指正。
  *
  ******************************************************************************
  */

#ifndef __KALMAN_H__
#define __KALMAN_H__

#define Q_angle 0.001 角度过程噪声的协方差
#define Q_gyro 0.003 角速度过程噪声的协方差
#define R_angle 0.5 测量噪声的协方差（即是测量偏差）
#define dt 0.01 卡尔曼滤波采样频率
#define C_0 1

/**************卡尔曼运算变量定义**********************
*
***由于卡尔曼为递推运算，结构体需定义为全局变量
***在实际运用中只需定义一个KalmanCountData类型的变量即可
***无需用户定义多个中间变量，简化函数的使用
*/
typedef struct
{
float Q_bias; 最优估计值的偏差，即估计出来的陀螺仪的漂移量
float Angle_err; 实测角度与陀螺仪积分角度的差值
float PCt_0;
float PCt_1;
float