平衡小车卡尔曼滤波算法

最近研究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