最近研究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                                E;                        计算的过程量         float                                K_0;                        含有卡尔曼增益的另外一个函数，用于计算最优估计值         float                                K_1;                        含有卡尔曼增益的函数，用于计算最优估计值的偏差         float                                t_0;                                         float                                t_1;         float                                Pdot[4];                Pdot[4] = {0,0,0,0};过程协方差矩阵的微分矩阵         float                                PP[2][2];                PP[2][2] = { { 1, 0 },{ 0, 1 } };协方差(covariance)         float                                Angle_Final;        后验估计最优角度值（即系统处理最终值）         float                                Gyro_Final;        后验估计最优角速度值 }KalmanCountData; void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct); void Kalman_Filter_Init(KalmanCountData * Kalman_Struct); #endif [/pre] kalman.c [pre lang="C" line="1"  file="kalman.c"] #include "kalman.h" /**   ******************************************************************************   * @file    void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)   * @author  willieon   * @version V0.1   * @date    January-2015   * @brief   卡尔曼滤波计算中间量初始化   *           *   ******************************************************************************   * @attention   *   *    *    *   ******************************************************************************   */ void Kalman_Filter_Init(KalmanCountData * Kalman_Struct) {         Kalman_Struct -> Angle_err                 = 0;         Kalman_Struct -> Q_bias                         = 0;         Kalman_Struct -> PCt_0                         = 0;         Kalman_Struct -> PCt_1                         = 0;         Kalman_Struct -> E                                 = 0;         Kalman_Struct -> K_0                         = 0;         Kalman_Struct -> K_1                         = 0;         Kalman_Struct -> t_0                         = 0;         Kalman_Struct -> t_1                         = 0;         Kalman_Struct -> Pdot[0]                 = 0;         Kalman_Struct -> Pdot[1]                 = 0;         Kalman_Struct -> Pdot[2]                 = 0;         Kalman_Struct -> Pdot[3]                 = 0;                 Kalman_Struct -> PP[0][0]                 = 1;         Kalman_Struct -> PP[0][1]                 = 0;         Kalman_Struct -> PP[1][0]                 = 0;         Kalman_Struct -> PP[1][1]                 = 1;                 Kalman_Struct -> Angle_Final         = 0;         Kalman_Struct -> Gyro_Final                 = 0; } /**   ******************************************************************************   * @file    void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)   * @author  willieon   * @version V0.1   * @date    January-2015   * @brief   卡尔曼滤波计算   *           *   ******************************************************************************   * @attention   *                Accel:加速度计数据处理后进来的角度值   *                Gyro :陀螺仪数据处理后进来的角速度值   *                Kalman_Struct:递推运算所需要的中间变量，由用户定义为全局结构体变量   *                Kalman_Struct -> Angle_Final  为滤波后角度最优值   *                Kalman_Struct -> Gyro_Final   为后验角度值   ******************************************************************************   */ void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct) {                 //陀螺仪积分角度（先验估计）                 Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt;                                   //先验估计误差协方差的微分                 Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0];                  Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1];                 Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1];                 Kalman_Struct -> Pdot[3] = Q_gyro;                                  //先验估计误差协方差的积分                 Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;                    Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;                    Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt;                 Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt;                                  //计算角度偏差                 Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;                                          //卡尔曼增益计算                 Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0];                 Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0];                                  Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0;                                  Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E;                 Kalman_Struct -> K_1 = Kalman_Struct -> PCt_1 / Kalman_Struct -> E;                                  //后验估计误差协方差计算                 Kalman_Struct -> t_0 = Kalman_Struct -> PCt_0;                 Kalman_Struct -> t_1 = C_0 * Kalman_Struct -> PP[0][1];                 Kalman_Struct -> PP[0][0] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_0;                                  Kalman_Struct -> PP[0][1] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_1;                 Kalman_Struct -> PP[1][0] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_0;                 Kalman_Struct -> PP[1][1] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_1;                 Kalman_Struct -> Angle_Final += Kalman_Struct -> K_0 * Kalman_Struct -> Angle_err;         //后验估计最优角度值                 Kalman_Struct -> Q_bias        += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;                 //更新最优估计值的偏差                 Kalman_Struct -> Gyro_Final   = Gyro - Kalman_Struct -> Q_bias;                                                 //更新最优角速度值 } [/pre] 代码可以放在实际工程中使用，也可以用VS等C编译工具进行实验学习。在VS中的main()实例使用如下 [pre lang="C" line="1" file="main.c"] #include "kalman.h" #include "stdio.h" #include "stdlib.h" void main(void) {         KalmanCountData k;         //定义一个卡尔曼运算结构体         Kalman_Filter_Init(&k);         //讲运算变量初始化         int m,n;                    for(int a = 0;a<80;a++)         //测试80次         {                 //m,n为1到100的随机数                 m = 1+ rand() %100;                 n = 1+ rand() %100;                 //卡尔曼滤波，传递2个测量值以及运算结构体                  Kalman_Filter((float)m,(float)n,&k);                 //打印结果                 printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0);                  } } [/pre]