加速度计与陀螺仪融合一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.docx

一阶互补

//a=tau/(tau+looptime)

//newAngle=anglemeasuredwithatan2usingtheaccelerometer

//加速度传感器输出值

//newRate=anglemeasuredusingthegyro

//looptime=looptimeinmillis()

floattau=0.075;

floata=0.0;

floatComplementary(floatnewAngle,floatnewRate,intlooptime)

{

floatdtC=float(looptime)/1000.0;

a=tau/(tau+dtC);

//下行代码更改成白色，下载后恢复成其他颜色即可看到

x_angleC=;

returnx_angleC;

}

二阶互补

//newAngle=anglemeasuredwithatan2usingtheaccelerometer

//newRate=anglemeasuredusingthegyro

//looptime=looptimeinmillis()

floatComplementary2(floatnewAngle,floatnewRate,intlooptime)

{

floatk=10;

floatdtc2=float(looptime)/1000.0;

x1=(newAngle-x_angle2C)*k*k;

y1=dtc2*x1+y1;

x2=;//此行代码更改成白色，下载后恢复成其他颜色即可看到

x_angle2C=dtc2*x2+x_angle2C;

returnx_angle2C;

}Heretoowejusthavetosetthe?kand?magically?weget?theangle.

卡尔曼滤波

//KasBotV1-Kalmanfiltermodule

float

float

float

floatx_bias=0;

floatP_00=0,P_01=0,P_10=0,P_11=0;

floaty,S;

floatK_0,K_1;

//newAngle=anglemeasuredwithatan2usingtheaccelerometer

//newRate=anglemeasuredusingthegyro

//looptime=looptimeinmillis()

floatkalmanCalculate(floatnewAngle,floatnewRate,intlooptime)

{

floatdt=float(looptime)/1000;

x_angle+=dt*(newRate-x_bias);

//以下代码更改成白色，下载后恢复成其他颜色即可看到

P_00+=;

P_01+=;

P_10+=;

P_11+=;

y=newAngle-x_angle;

S=P_00+R_angle;

K_0=P_00/S;

K_1=P_10/S;

x_angle+=K_0*y;

x_bias+=K_1*y;

P_00-=K_0*P_00;

P_01-=K_0*P_01;

P_10-=K_1*P_00;

P_11-=K_1*P_01;

returnx_angle;

}To?gettheanswer,?youhavetoset?3parameters:?Q_angle,?R_angle,R_gyro.

详细卡尔曼滤波

/*-*-indent-tabs-mode:T;c-basic-offset:8;tab-width:8;-*-vi:setts=8:

*$Id:tilt.c,v1.12003/07/0918:23:29johnExp$

*

*1dimensionaltiltsensorusingadualaxisaccelerometer

*andsingleaxisangularrategyro.Thetwosensorsarefused

*viaatwostateKalmanfilter,withonestatebeingtheangle

*andthe