姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序.doc

(完整版)姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序(完整版)姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序(完整版)姿态融合的一阶互补滤波、二阶互补滤波、卡尔曼滤波核心程序//一阶互补//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=a*(x_angleC+newRate＊dtC)+(1-a）*（newAngle）;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=y1+(newAngle—x_angle2C)＊2＊k+newRate；x_angle2C=dtc2＊x2+x_angle2C；returnx_angle2C；}//Heretoowejusthavetosetthekandmagicallywegettheangle.卡尔曼滤波//KasBotV1-KalmanfiltermodulefloatQ_angle=0.01;//0.001floatQ_gyro=0。0003；//0.003floatR_angle=0。01;//0。03floatx_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+=-dt＊（P_10+P_01)+Q_angle*dt；P_01+=—dt*P_11；P_10+=-dt*P_11;P_11+=+Q_gyro*dt;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;}//Togettheanswer，youhavetoset3parameters：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＊andtheotherstatebeingthegyrobias.＊*Gyrobiasisautomaticallytrackedbythefilter。Thisseems＊likemagic.＊＊Pleasenotethattherearelotsofcommentsinthefuncti