搜索
/***************************************************************************//*kalman.c*//*1-DKalmanfilterAlgorithm,usinganinclinometerandgyro*//*Author:RichChiOoi*//*Version:1.0*//*Date:30.05.2003*//*AdaptedfromTrammelHudson(hudson@rotomotion.com)*//*-------------------------------*//*Compilationprocedure:*//*Linux*//*gcc68-cXXXXXX.c(tocreateobjectfile)*//*gcc68-oXXXXXX.hexXXXXXX.oppwa.o*//*Uploaddata:*//*ulfilename.txt*//***************************************************************************//*Inthisversion:*//***************************************************************************//*Thisisafreesoftware;youcanredistributeitand/ormodify*//*itunderthetermsoftheGNUGeneralPublicLicenseaspublished*//*bytheFreeSoftwareFoundation;eitherversion2oftheLicense,*//*or(atyouroption)anylaterversion.*//**//*thiscodeisdistributedinthehopethatitwillbeuseful,*//*butWITHOUTANYWARRANTY;withouteventheimpliedwarrantyof*//*MERCHANTABILITYorFITNESSFORAPARTICULARPURPOSE.Seethe*//*GNUGeneralPublicLicenseformoredetails.*//**//*YoushouldhavereceivedacopyoftheGNUGeneralPublicLicense*//*alongwithAutopilot;ifnot,writetotheFreeSoftware*//*Foundation,Inc.,59TemplePlace,Suite330,Boston,*//*MA02111-1307USA*//***************************************************************************/#includemath.h#includeeyebot.h/**Thestateisupdatedwithgyroratemeasurementevery20ms*changethisvalueifyouupdateatadifferentrate.*/staticconstfloatdt=0.02;/**Thecovariancematrix.Thisisupdatedateverytimestepto*determinehowwellthesensorsaretrackingtheactualstate.*/staticfloatP[2][2]={{1,0},{0,1}};/**Ourtwostates,theangleandthegyrobias.Asabyproductofcomputing*theangle,wealsohaveanunbiasedangularrateavailable.Theseare*read-onlytotheuserofthemodule.*/floatangle;floatq_bias;floatrate;/**TheRrepresentsthemeasurementcovariancenoise.R=E[vvT]*Inthiscase,itisa1x1matrixthatsaysthatweexpect*0.1radjitterfromtheinclinometer.*fora1x1matrixinthiscasev=0.1*/staticconstfloatR_angle=0.001;/**Qisa2x2matrixthatrepresentstheprocesscovariancenoise.*Inthiscase,itindicateshowmuchwetrusttheinclinometer*relativetothegyros.*/staticconstfloatQ_angle=0.001;staticconstfloatQ_gyro=0.0015;/**state_updateiscalledeverydtwithabiasedgyromeasurement*bytheuserofthemodule.Itupdatesthecurrentangleand*rateestimate.**Thepitchgyromeasurementshouldbescaledintorealunits,but*doesnotneedanybiasremoval.Thefilterwilltrackthebias.**A=[0-1]*[00]*/voidstateUpdate(constfloatq_m){floatq;floatPdot[4];/*Unbiasourgyro*/q=q_m-q_bias;//当前角速度：测量值-估计值/**Computethederivativeofthecovariancematrix*(equation22-1)*Pdot=A*P+P*A'+Q**/Pdot[0]=Q_angle-P[0][1]-P[1][0];/*0,0*/Pdot[1]=-P[1][1];/*0,1*/Pdot[2]=-P[1][1];/*1,0*/Pdot[3]=Q_gyro;/*1,1*//*Storeourunbiasgyroestimate*/rate=q;/**Updateourangleestimate*angle+=angle_dot*dt*+=(gyro-gyro_bias)*dt*+=q*dt*/angle+=q*dt;//角速度积分累加到估计角度/*Updatethecovariancematrix*/P[0][0]+=Pdot[0]*dt;P[0][1]+=Pdot[1]*dt;P[1][0]+=Pdot[2]*dt;P[1][1]+=Pdot[3]*dt;}/**kalman_updateiscalledbyauserofthemodulewhenanew*inclinoometermeasurementisavailable.**Thisdoesnotneedtobecalledeverytimestep,butcanbeif*theaccelerometerdataareavailableatthesamerateasthe*rategyromeasurement.**H=[10]**becausetheanglemeasurementdirectlycorrespondstotheangle*estimateandtheanglemeasurementhasnorelationtothegyro*bias.*/voidkalmanUpdate(constfloatincAngle){/*Computeourmeasuredangleandtheerrorinourestimate*/floatangle_m=incAngle;floatangle_err=angle_m-angle;//1.12zk-H*xk_dot/**h_0showshowthestatemeasurementdirectlyrelatesto*thestateestimate.**H=[h_0h_1]**Theh_1showsthatthestatemeasurementdoesnotrelate*tothegyrobiasestimate.Wedon'tactuallyusethis,so*wecommentitout.*/floath_0=1;/*constfloath_1=0;*//**PrecomputePH'asthetermisusedtwice*NotethatH[0,1]=h_1iszero,sothattermisnotnotcomputed*/constfloatPHt_0=h_0*P[0][0];/*+h_1*P[0][1]=0*/constfloatPHt_1=h_0*P[1][0];/*+h_1*P[1][1]=0*//**Computetheerrorestimate:*(equation21-1)**E=HPH'+R*/floatE=R_angle+(h_0*PHt_0);/**ComputetheKalmanfiltergains:*(equation21-2)**K=PH'inv(E)*/floatK_0=PHt_0/E;floatK_1=PHt_1/E;/**Updatecovariancematrix:*(equation21-3)**P=P-KHP*Let*Y=HP*/floatY_0=PHt_0;/*h_0*P[0][0]*/floatY_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;/**Updateourstateestimate:**Xnew=X+K*error**errisameasurementofthedifferenceinthemeasuredstate*andtheestimatestate.Inourcase,itisjustthedifference*betweentheinclinometermeasuredangleandtheestimatedangle.*/angle+=K_0*angle_err;q_bias+=K_1*angle_err;}现在智能小车上用的卡尔曼滤波算法。由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了一段时间的书。。。。。。。。。。。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱（大二），有错的请多多包涵。先上程序，这是抄的不知道谁的代码。。。抱歉了。。不过这程序好像都写的差不多voidKalman_Filter(floatGyro,floatAccel){Angle+=(Gyro-Q_bias)*dt;Pdot[0]=Q_angle-PP[0][1]-PP[1][0];/Pdot[1]=-PP[1][1];Pdot[2]=-PP[1][1];/Pdot[3]=Q_gyro;PP[0][0]+=Pdot[0]*dt;PP[0][1]+=Pdot[1]*dt;PP[1][0]+=Pdot[2]*dt;PP[1][1]+=Pdot[3]*dt;Angle_err=Accel-Angle;PCt_0=C_0*PP[0][0];PCt_1=C_0*PP[1][0];E=R_angle+C_0*PCt_0;K_0=PCt_0/E;K_1=PCt_1/E;t_0=PCt_0;t_1=C_0*PP[0][1];PP[0][0]-=K_0*t_0;PP[0