【学习心得】MPU6050数据处理实验二——尝试卡尔曼
葱拌豆腐 于 2013-4-10 09:03 编辑
================2013-04-10更新==========
昨天晚上回去试了一下“P00=(1-KgH)P10(1-KgH)+KgRKg;”的效果，没有发现任何不同，Kg在第四个循环后就已经收敛到了0.42。
====================================================
自从接触自衡小车开始就发现卡尔曼是个必须过的坎儿，翻遍了坛子里的和卡尔曼有关的帖子，中文的外文的资料看了一大堆，还是没有真正搞明白卡尔曼的原理，没到这个时候心里那个后悔啊，当初在学校为啥不好好学习呢，早知今日要做自衡小车，当初真应该好好学习一下。好在最近照葫芦画瓢的写了一个简单的融合算法，先贴出来，自己做实验时，感觉效果很一般，没有预期中的牛逼，还在进一步研究，下面先贴代码。
ARDUINO 代码复制打印下载
#include <I2Cdev.h>#include <MPU6050.h>#include <Wire.h>float ax,az,gy;//get acceleration_x,acceleration_z,rotation_ylong OffsetGy,OffsetSum;float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotationlong LastTime,NowTime,TimeSpan,SampleTime;////===========kalman filter parameter define============float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;MPU6050 mpu;void setup(){//============initial kalman parameter======A=1.0;H=1.0;Q=0.003;R=0.01;P00=5.0;P10=0.0;X00=0.0;//===============end initial===========Wire.begin();Serial.begin(9600);Serial.println("start");mpu.initialize();Serial.println(mpu.getRotationY());Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");//compute the gyroscope offset automaticfor(int i=0;i<200;i++){ Serial.println(mpu.getRotationY()); OffsetSum=OffsetSum+mpu.getRotationY(); delay(50); }OffsetGy=OffsetSum/200;pid_SP=0.0;Kp=10.0;Kd=2.0;Ki=0.0;}void loop(){NowTime=millis();TimeSpan=NowTime-LastTime;LastTime=NowTime;//=================kalman filter==========================ax=mpu.getAccelerationX()/16384.00;az=mpu.getAccelerationZ()/16384.00;AngleAcc=(-1.0)atan2(ax,az)180/PI;gy=(mpu.getRotationY()-OffsetGy)/131.00;AngleRotation=AngleRotation+gyTimeSpan/1000.00;//=============kalman calculate============B=TimeSpan/1000.0;U=gy;Z=AngleAcc;//一下按照卡尔曼滤波的五个公式写的，融合角度作为状态，陀螺仪的角速度作为控制量，加速度的角度作为观测值//所以A是1，B是采样周期X10=AX00+BU; //=======formula 1P10=AP00(A)+Q;//=============formula 2Kg=P10(H)/(HP10(H)+R);//=======formula 3X00=X10+Kg(Z-HX10);//========formula 4P00=(1-KgH)P10;//=============formula 5//P00=(1-KgH)P10(1-KgH)+KgRKg;//======根据维基百科的说法，当Kg不是最优时采用这个公式AngleMerge=X00;Serial.print(AngleRotation);Serial.print(",");Serial.print(AngleMerge);Serial.print(",");Serial.print(AngleAcc);Serial.print(",");Serial.print(0.8AngleRotation+0.2AngleAcc);Serial.print(",");Serial.print(P00);Serial.print(",");Serial.println(Kg);delay(100);}#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
float ax,az,gy;//get acceleration_x,acceleration_z,rotation_y
long OffsetGy,OffsetSum;
float AngleAcc,AngleRotation,AngleMerge;//angel computed by acceleration and rotation
long LastTime,NowTime,TimeSpan,SampleTime;//
//===========kalman filter parameter define============
float A,B,H,Q,R,P00,P10,X00,X10,Z,U,Kg;
MPU6050 mpu;
void setup()
{
//============initial kalman parameter======
A=1.0;
H=1.0;
Q=0.003;
R=0.01;
P00=5.0;
P10=0.0;
X00=0.0;
//===============end initial===========
Wire.begin();
Serial.begin(9600);
Serial.println("start");
mpu.initialize();
Serial.println(mpu.getRotationY());
Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
//compute the gyroscope offset automatic
for(int i=0;i<200;i++)
{
Serial.println(mpu.getRotationY());
OffsetSum=OffsetSum+mpu.getRotationY();
delay(50);
}
OffsetGy=OffsetSum/200;
pid_SP=0.0;
Kp=10.0;
Kd=2.0;
Ki=0.0;
}
void loop()
{
NowTime=millis();
TimeSpan=NowTime-LastTime;
LastTime=NowTime;
//=================kalman filter==========================
ax=mpu.getAccelerationX()/16384.00;
az=mpu.getAccelerationZ()/16384.00;
AngleAcc=(-1.0)atan2(ax,az)180/PI;
gy=(mpu.getRotationY()-OffsetGy)/131.00;
AngleRotation=AngleRotation+gyTimeSpan/1000.00;
//=============kalman calculate============
B=TimeSpan/1000.0;
U=gy;
Z=AngleAcc;
//一下按照卡尔曼滤波的五个公式写的，融合角度作为状态，陀螺仪的角速度作为控制量，加速度的角度作为观测值
//所以A是1，B是采样周期
X10=AX00+BU; //=======formula 1
P10=AP00(A)+Q;//=============formula 2
Kg=P10(H)/(HP10(H)+R);//=======formula 3
X00=X10+Kg(Z-HX10);//========formula 4
P00=(1-KgH)P10;//=============formula 5
//P00=(1-KgH)P10(1-KgH)+KgRKg;//======根据维基百科的说法，当Kg不是最优时采用这个公式
AngleMerge=X00;
Serial.print(AngleRotation);
Serial.print(",");
Serial.print(AngleMerge);
Serial.print(",");
Serial.print(AngleAcc);
Serial.print(",");
Serial.print(0.8AngleRotation+0.2AngleAcc);
Serial.print(",");
Serial.print(P00);
Serial.print(",");
Serial.println(Kg);
delay(100);
}
监视Kg的变化，发现几个周期之内就不再变化了，滤波效果图如下：
2013-4-9 10:55 上传
(51.11 KB)
滤波效果对比
对卡尔曼滤波的研究还在继续，后面会继续更新本文。
via - 极客工坊