本帖最后由 葱拌豆腐 于 2013-4-10 09:03 编辑
================2013-04-10更新==========
昨天晚上回去试了一下“P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;”的效果,没有发现任何不同,Kg在第四个循环后就已经收敛到了0.42。
====================================================
自从接触自衡小车开始就发现卡尔曼是个必须过的坎儿,翻遍了坛子里的和卡尔曼有关的帖子,中文的外文的资料看了一大堆,还是没有真正搞明白卡尔曼的原理,没到这个时候心里那个后悔啊,当初在学校为啥不好好学习呢,早知今日要做自衡小车,当初真应该好好学习一下。好在最近照葫芦画瓢的写了一个简单的融合算法,先贴出来,自己做实验时,感觉效果很一般,没有预期中的牛逼,还在进一步研究,下面先贴代码。
[pre lang="arduino" line="1" file="Kalman"]
#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+gy*TimeSpan/1000.00;
//=============kalman calculate============
B=TimeSpan/1000.0;
U=gy;
Z=AngleAcc;
//一下按照卡尔曼滤波的五个公式写的,融合角度作为状态,陀螺仪的角速度作为控制量,加速度的角度作为观测值
//所以A是1,B是采样周期
X10=A*X00+B*U; //=======formula 1
P10=A*P00*(A)+Q;//=============formula 2
Kg=P10*(H)/(H*P10*(H)+R);//=======formula 3
X00=X10+Kg*(Z-H*X10);//========formula 4
P00=(1-Kg*H)*P10;//=============formula 5
//P00=(1-Kg*H)*P10*(1-Kg*H)+Kg*R*Kg;//======根据维基百科的说法,当Kg不是最优时采用这个公式
AngleMerge=X00;
Serial.print(AngleRotation);
Serial.print(",");
Serial.print(AngleMerge);
Serial.print(",");
Serial.print(AngleAcc);
Serial.print(",");
Serial.print(0.8*AngleRotation+0.2*AngleAcc);
Serial.print(",");
Serial.print(P00);
Serial.print(",");
Serial.println(Kg);
delay(100);
}
[/code]
监视Kg的变化,发现几个周期之内就不再变化了,滤波效果图如下:
对卡尔曼滤波的研究还在继续,后面会继续更新本文。
|