平衡小车卡尔曼滤波算法

本帖最后由 D调的华丽于 2015-1-10 12:57 编辑

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

平衡小车卡尔曼滤波算法

猜你喜欢