最近研究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] |
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
