卡尔曼滤波算法代码总结

1、/*/*kalman.c*/*1-DKalmanfilterAlgorithm,usinganinclinometerandgyro/*Author:RichChiOoi*/*/*Version:1.0/*Date:30.05.2003/*Adapted*/*/Hudson(hudson)from*/Trammel*/*/*/*Compilationprocedure:/*Linux*/*/*gcc68-cXXXXXX.c(tocreateobjectfile)gcc68-oXXXXXX.hexXXXXXX.oppwa.o*/*/"Uploaddata:*/*ulfilename.t

2、xt*/*/*Inthisversion:*/*/*Thisisafreesoftware;youcanredistributeitand/ormodify/*itunderthetermsoftheGNUGeneralPublicLicenseaspublished*/*/*bytheFreeSoftwareFoundation;eitherversion2oftheLicense,/*or(atyouroption)anylaterversion./*/*/*/*thiscodeisdistributedinthehopethatitwillbeuseful,*/*butWITHOUTAN

3、YWARRANTY;withouteventheimpliedwarrantyof*/*MERCHANTABILITYorFITNESSFORAPARTICULARPURPOSE.Seethe*/*GNUGeneralPublicLicenseformoredetails./*/*/*YoushouldhavereceivedacopyoftheGNUGeneralPublicLicense*/*alongwithAutopilot;ifnot,writetotheFreeSoftware/*Foundation,Inc.,59TemplePlace,Suite330,Boston,*/*/*

4、MA02111-1307USA*/*/#include<math.h>#include"eyebot.h"/* Thestateisupdatedwithgyroratemeasurementevery20ms* changethisvalueifyouupdateatadifferentrate.*/staticconstfloatdt=0.02;/* Thecovariancematrix.Thisisupdatedateverytimestepto* determinehowwellthesensorsaretrackingtheactualstate.*

5、/staticfloatP22=1,0,0,1;/* Ourtwostates,theangleandthegyrobias.Asabyproductofcomputing* theangle,wealsohaveanunbiasedangularrateavailable.Theseare* read-onlytotheuserofthemodule.*/floatangle;floatqbias;floatrate;/* TheRrepresentsthemeasurementcovariancenoise.R=EvvT* Inthiscase,itisa1x1matrixthatsays

6、thatweexpect* 0.1radjitterfromtheinclinometer.* fora1x1matrixinthiscasev=0.1*/staticconstfloatR_angle=0.001;/* Qisa2x2matrixthatrepresentstheprocesscovariancenoise.* Inthiscase,itindicateshowmuchwetrusttheinclinometer* relativetothegyros.*/staticconstfloatQangle=0.001;staticconstfloatQ_gyro=0.0015;/

7、* state_updateiscalledeverydtwithabiasedgyromeasurement* bytheuserofthemodule.Itupdatesthecurrentangleand* rateestimate.* Thepitchgyromeasurementshouldbescaledintorealunits,but* doesnotneedanybiasremoval.Thefilterwilltrackthebias.* A=0-1* 00*/voidstateUpdate(constfloatq_m)floatq;floatPdot4;/* rate g

8、yro measurement.* H = 1 0 * because the angle measurement directly corresponds to the angle* estimate and the angle measurement has no relation to the gyro bias.*/Unbiasourgyro*/q=qm-qbias;/当前角速度：测量值-估计值/* Computethederivativeofthecovariancematrix* (equation22-1)* Pdot=A*P+P*A'+Q*/Pdot0=Q_angle-

9、P01-P10;/*0,0*/Pdot1=-P11;/*0,1*/Pdot2=-P11;/*1,0*/Pdot3=Q_gyro;/*1,1*/*Storeourunbiasgyroestimate*/rate=q;/* Updateourangleestimate* angle+=angledot*dt* +=(gyro-gyrobias)*dt* +=q*dt* /angle+=q*dt;/角速度积分累加到估计角度/*Updatethecovariancematrix*/P00+=Pdot0*dt;P01+=Pdot1*dt;P10+=Pdot2*dt;P11+=Pdot3*dt;/* ka

10、lmanupdateiscalledbyauserofthemodulewhenanew* inclinoometermeasurementisavailable.* Thisdoesnotneedtobecalledeverytimestep,butcanbeif* theaccelerometerdataareavailableatthesamerateasthevoidkalmanUpdate(constfloatincAngle)/*Computeourmeasuredangleandtheerrorinourestimate*/floatangle_m=incAngle;floata

11、ngle_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;/*constfloath1=0;*/* PrecomputePH'asthetermis

12、usedtwice* NotethatH0,1=h1iszero,sothattermisnotnotcomputed*/constfloatPHt_0=h_0*P00;/*+h_1*P01=0*/constfloatPHt_1=h_0*P10;/*+h_1*P11=0*/* Computetheerrorestimate:* (equation21-1)* E=HPH'+R* /floatE=Rangle+(h0*PHt0);/* ComputetheKalmanfiltergains:* (equation21-2)* K=PH'inv(E)*/floatK0=PHt0/E

13、;floatK_1=PHt_1/E;一/* Updatecovariancematrix:* (equation21-3)* 1*Let* Y=HP*/floatY0=PHt0;/*h0*P00*/floatY_1=h_0*P01;JP00-=K0*Y0;P01-=K_0*Y_1;P10-=K_1*Y_0;P11-=K_1*Y_1;/* Updateourstateestimate:* Xnew=X+K*error* errisameasurementofthedifferenceinthemeasuredstate* andtheestimatestate.Inourcase,itisjus

14、tthedifference* betweentheinclinometermeasuredangleandtheestimatedangle.*/angle+=K_0*angle_err;q_bias+=K_1*angle_err;一一段时间的,有由于做平衡小车，然后对那段滤波算法很疑惑，然后网上讲的又比较少，我看了书。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱(大二)错的请多多包涵。先上程序，这是抄的不知道谁的代码。抱歉了。不过这程序好像都写的差不多voidKalman_Filter(floatGyro,110atAccel)Angle+=(Gyro-Q_bias)*dt;Pd

15、ot0=Q_angle-PP01-PP10;/Pdot1=-PP11;Pdot2=-PP11;/Pdot3=Q_gyro;PP00+=Pdot0*dt;PP01+=Pdot1*dt;PP10+=Pdot2*dt;PP11+=Pdot3*dt;Angle_err=Accel-Angle;PCt_0=C_0*PP00;PCt_1=C_0*PP10；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*PP01；PP00-=K_0*t_0;PP01-=K_0*t_1;PP10-=K_1*t_0;PP11-=K_1*t_1;Angl

16、e+=K_0*Angle_err;Q_bias+=K_1*Angle_err;Gyro_x=Gyro-Q_bias;首先是卡尔曼滤波的5个方程X(k|k-1)=AX(k-1|k-1)+BU(k).(1)/先验估计P(k|k-1)=AP(k-1|k-1)A'+Q(2)协方差矩阵的预测Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R)(3)/计算卡尔曼增益X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1)(4)通过卡尔曼增益进行修正P(k|k)=(I-Kg(k)H)P(k|k-1)(5)跟新协方差阵5个式子比较抽象，现在直接用实例来说对于

17、角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为d9=dtxco,角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值AngleAngle=Angle+(Gyro-Q_bias)*dt;其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。floatdt=0.005;这是程序中的定义同时Q_bias也是一个变化

18、的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_bias=Q_bias将两个式子写成矩阵的形式AngleQ bias- dt1AngleQ biasdt十 Gyro0得到上式，这个式子对应于卡尔曼滤波的第一个式子X(k|k-1)=AX(k-1|k-1)+BU(k).(1)/先验估计X(k|k-1)为2维列向量Angle,A为2维方阵1一出，X(k-1|k-1)为2维列向量Qbias01AngleQ biasB为2维列向量dtU(k)为 Gyro,这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，(所谓噪声就是数据的方差值

19、)cov(Angle,Angle)cov(Angle,Q_bias)cov(Q_bias,Anglecov(Q_bias)P(k|k-1)=AP(k-1|k-1)A'+Q这里的Q为向量AngleQ_bias的协方差矩阵，即cov(Angle,Q_bias)=0因为漂移噪声还有角度噪声是相互独立的，则cov(Q_bias,Angle)=。又由性质可知cov(x,x)=D(x)即方差，所以得到的矩阵如下D(Angle)00D(Q_bias),这里的两个方差值是开始就给出的常数程序中的定义如下floatQ_angle=0.001;floatQ_gyro=0.003;接着是这一部分AP(k-1

20、|k-1)A'，其中的(P(k-1)|P(k-1)为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中P(k-1|k-1)设为，第一式已知A为1 dtc d0 1则计算AP(k-1|k-1)A'+Q(就是个矩阵乘法和加法，算算吧)结果如下.2.aCMdtbdt+d.(dt)+D(Angle)bddtcddt2d.(dt)很小为了计算简便忽略不计。于是得到IacmdtbMdt+D(Angle)bddtc-dxdtda,b,c,d分别和矩阵的P00,P1,P10,P11计算过程转化为如下程序，代换即可Pdot0=Q_angle-PP1-PP10;Pdot1=-PP11;Pdot2=-PP11;Pdot3=Q_gyro;PP00+=Pdot0*dt;PP01+=Pdot1*dt;PP10+=Pdot2*dt;PP11+=Pdot3*dt;三，这里是卡尔曼滤波的第三个式子Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R)(3)/计算卡尔曼增益即计算卡尔曼增益，这是个二维向量设为k0H 11 0为由此kg=P(K|K-1)+R ,这里又有一个常数R,程序中的定义