|
|
发表于 2014-1-17 22:23:08
|
显示全部楼层
最近我也在学习卡尔曼滤波！从网上移植了个卡尔曼滤波，结果可以很好的跟踪角度，目前只能实现单个角度融合哈哈
所以今天和大家一起分享一下源码
/* IMU.c file
编写者：chushibin
网址：xinjiangdaxue
作者E-mail：419500541@qq.com
编译环境：MDK-Lite Version: 4.23
初版时间: 2014-0-13
测试： 本程序已在第七实验室的[Captain 飞控板]上完成测试
难点：（1）实现卡尔曼滤波
（2）实现在动态下准确的测量姿态
占用STM32 资源：
1. 使用Tim7定时器 产生us级的系统时间
功能：
姿态解算 IMU
将传感器的输出值进行姿态解算。得到目标载体的俯仰角和横滚角 和航向角
个人学习：（1）陀螺仪输出的单位是弧度，需要转换成度每秒
（2）
------------------------------------
*/
/************************************************
***************卡尔曼滤波程序********************
*************************************************
*/
#include "IMU.h"
volatile uint32_t lastUpdate, now;
volatile uint16_t sysytem_time_ms=0;
volatile float IMU_GYROx, IMU_GYROy, IMU_GYROz;
volatile float IMU_Pitch, IMU_Roll, IMU_Yaw;
volatile float incAngle[3];
volatile float g=-9.8f;
volatile float mygetzhi[9];
volatile float gyrox,gyroy,gyroz;
static float P[2][2] = {{ 1, 0 },
{ 0, 1 }};//预测方差矩阵
float angle; //下一时刻角度的估计值
float q_bias=0; //陀螺仪的偏差，即角度的测量误差
float rate; //修正后的陀螺仪测量量即角速度
//R=[0.001]
static const float R_angle = 0.001;//R代表观测协方差矩阵，
//这里我们估计的单一的角度
//Q=[0.001 0 ],
// [0 0.0015] 过程协方差
static const float Q_angle = 0.001;
static const float Q_gyro = 0.0015;
//A=[0 -1],
// [0 0];
void IMU_init(void)
{
MPU6050_initialize();
HMC5883L_SetUp();
MS561101BA_init();
delay_ms(50);
MPU6050_initialize();
HMC5883L_SetUp();
lastUpdate = micros();//更新时间
now = micros();
}
/**************************实现函数********************************************
*函数原型: void IMU_getValues(float * values)
*功 能: 读取加速度 陀螺仪 磁力计 的当前值
输入参数： 将结果存放的数组首地址
输出参数：没有
*******************************************************************************/
#define new_weight 0.35f
#define old_weight 0.65f
void IMU_getValues(float * values)
{
int16_t accgyroval[6];
static float lastacc[3]= {0,0,0};
int i;
//读取加速度和陀螺仪的当前ADC
//accgyroval[]所对应实际存储的内容是ax/ay/az/（加速度计的测量值）, gx/gy/gz(陀螺仪的测量值)
MPU6050_getMotion6(&accgyroval[0], &accgyroval[1], &accgyroval[2], &accgyroval[3], &accgyroval[4], &accgyroval[5]);
for(i = 0; i<6; i++)
{
if(i < 3)
{ //当前的加速度计当前测量值*0.35+上次加速度计的测量值*0.65=当前时刻加速度计的值
//其意义在于：当飞机没有起飞的时候，此时的飞机处于静止状态，测得的加速度计的值最为准确
//为了保证飞机在起飞后加速度的偏离的不那么块，所以前一次的测量值我们更相信一些
//也就是说更相信上一次的测量值
values = (float) accgyroval * new_weight +lastacc * old_weight ;
lastacc = values;
}
else
{
//将陀螺仪的测量值换算成度每秒
//根据陀螺仪编程手册我们可以看到，量程在2000度每秒时，陀螺仪的测量值应当除以16.4
//不同的量程对应不同的分辨率
values = ((float) accgyroval) / 16.4f; //转成度每秒
//这里已经将量程改成了 2000度每秒 16.4 对应 1度每秒
}
}
HMC58X3_mgetValues(&values[6]); //读取磁力计的ADC值
//values[6]是首地址，也就是说磁强计的测量值被存储在了values的6、7、8中
//以下对应的是陀螺仪的测量值
//注意此时的测量值时弧度每秒
IMU_GYROx = accgyroval[3];
IMU_GYROy = accgyroval[4];
IMU_GYROz = accgyroval[5];
//（横滚）卡尔曼滤波直接是对角度误差滤波
//这里需要将从陀螺仪读出的弧度每秒的单位转换成度每秒
gyrox=values[3]*180/M_PI;
gyroy=values[4]*180/M_PI;
gyroz=values[3]*180/M_PI;
}
/**********实现函数************************************
函数原型：void stateUpdate(const float q_m)
功能 ：更新状态并计算陀螺仪测量值得到的角度
状态更新，主要是计算陀螺仪的测量得到的角度
同时还更新误差方程矩阵P
输入 ：当前陀螺仪的测量值（已经转换成弧度）
*/
void stateUpdate(const float q_m)
{
volatile float halfT;
float q; //修正的角速度
float Pdot[4]; //
now = micros(); //读取时间
if(now < lastUpdate)
{ //定时器溢出过了。
halfT = ((float)(now + (0xffffffff- lastUpdate)) / 2000000.0f);
lastUpdate = now;
//return ;
}
else
{
halfT = ((float)(now - lastUpdate) / 2000000.0f);
}
lastUpdate = now; //更新时间
q = q_m - q_bias;//修正的角速度=当前陀螺仪测量值-估计的偏差
//将协方差做局部线性化，因此可以继续使用标准的卡尔曼滤波
//对协方差求导进行线近似。Pdot=A*P+p*AT+Q,此处的AT是A的转置举证
//以下计算就是根据Pdot=A*P+p*AT+Q展开计算而得
Pdot[0] = Q_angle - P[0][1] - P[1][0];
Pdot[1] = - P[1][1];
Pdot[2] = - P[1][1];
Pdot[3] = Q_gyro;
rate = q;//存储修正后的角速度
angle += q *2*halfT ;//计算由陀螺仪得到的角度
//更新协方差矩阵
P[0][0] += Pdot[0] * 2*halfT;
P[0][1] += Pdot[1] * 2*halfT;
P[1][0] += Pdot[2] * 2*halfT;
P[1][1] += Pdot[3] * 2*halfT;
}
/**********函数实现****************************
函数原型：void incAngleUpdate(float values[1] )
功能 ：计算加速度计和磁强计测量得到的角度
输入参数：values即加速度计的测量值
输出 ：无
个人说明：利用加速度计计算得到俯仰角和横滚角，在已知俯仰和横滚角度时
利用磁强计计算可得航向角
*/
void incAngleUpdate(float *shuzhi)
{
float accpitch,accroll;
float accyaw;
float acc[3];
float N,M;
//加速度计在+-2G的范围内工作，其分辨率为16384.0f
acc[0]=shuzhi[0]/16384.0f;
acc[1]=shuzhi[1]/16384.0f;
acc[2]=shuzhi[2]/16384.0f;
if(acc[0]>1.0) acc[0]=1.0;
if(acc[0]<-1.0) acc[0]=-1.0;
if(acc[1]>1.0) acc[1]=1.0;
if(acc[1]<-1.0) acc[1]=-1.0;
if(acc[2]>1.0) acc[2]=1.0;
if(acc[2]<-1.0) acc[2]=-1.0;
accpitch=asin(acc[0])*180/M_PI;
//将加速度计计算得到的角度转换成度单位，所以需要乘以180/M_PI
accroll=-asin(acc[1]/cos(accpitch))*180/M_PI;
//N和M分别是计算航向的分子和分母
//其中N=my*cos(accroll)+mz*sin(accroll)
//其中M=mx*cos(accpitch)+my*sin(accpitch)*sin(accroll)-mz*cos(accpitch)*sin(accroll)
N=shuzhi[7]*cos(accroll)+shuzhi[8]*sin(accroll);
M=shuzhi[6]*cos(accpitch)+shuzhi[7]*sin(accpitch)*sin(accroll)-shuzhi[8]*cos(accpitch)*sin(accroll);
accyaw=N/M;
incAngle[2]=accroll;
incAngle[1]=accpitch;
incAngle[0]=accyaw;
}
/**********函数实现*************************
函数原型：void kalmanUpdate( float accAngle)
功能 ：进行卡尔曼滤波，得出最优估计
输入 ：加速计测量得到的角度incAngle
输出 ：无
*/
void kalmanUpdate(float *accAngle)
{
float angle_m = accAngle[1];//加速度计得到的角度
float angle_err = angle_m - angle;//角度误差=加速度计得到的角度-陀螺仪得到的角度
//H=[h_0 h_1]
float h_0=1.0;
float h_1=0.0;
const float PHt_0 = h_0*P[0][0];
const float PHt_1 = h_0*P[1][0];
//E=HPH'+R
float E = R_angle +(h_0 * PHt_0);
//K=PH'/E
float K_0 = PHt_0 / E;
float K_1 = PHt_1 / E;
//P=P-KHP
//let Y=HP
float Y_0 = PHt_0;
float Y_1 = h_0 * P[0][1];
P[0][0] -= K_0 * Y_0;
P[0][1] -= K_0 * Y_1;
P[1][0] -= K_1 * Y_0;
P[1][1] -= K_1 * Y_1;
angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
}
/**************************实现函数********************************************
*函数原型: void IMU_getYawPitchRoll(float * angles)
*功 能: 更新四元数 返回当前解算后的姿态数据
输入参数： 无
输出参数：没有
*******************************************************************************/
void IMU_getYawPitchRoll(float * angles)
{
IMU_getValues(mygetzhi);
stateUpdate(gyrox);
incAngleUpdate(mygetzhi);
kalmanUpdate(incAngle);
angles[0] =incAngle[0];//航向
angles[1] =angle;
angles[2] =incAngle[2];//横滚
IMU_Yaw =angles[0];//航向
IMU_Pitch =angles[1];//俯仰
IMU_Roll =angles[2];//横滚
}
|
|
楼主
