卡尔曼滤波算法代码总结20150311

1、*/*kalman.c/*1-DKalmanfilterAlgorithm,usinganinclinometerandgyro/* Thecovariancematrix.Thisisupdatedateverytimestepto*/*/*Author:RichChiOoi/*Version:1.0/*Date:30.05.2003/*AdaptedfromTrammelHudson()/*/*Compilationprocedure:*/*/*/*/*Linux*/*/*/gcc68-cXXXXXX.c(tocreateobjectfile)gcc68-oXXXXXX.hexXXXXXX

2、.oppwa.o*/*/*Uploaddata:*/*ul*/*/*Inthisversion:/*/*/*Thisisafreesoftware;youcanredistributeitand/ormodify/*itunderthetermsoftheGNUGeneralPublicLicenseaspublished*/*/*bytheFreeSoftwareFoundation;eitherversion2oftheLicense,/*or(atyouroption)anylaterversion.*/*/*/*thiscodeisdistributedinthehopethatitw

3、illbeuseful,*/*butWITHOUTANYWARRANTY;withouteventheimpliedwarrantyof*/*MERCHANTABILITYorFITNESSFORAPARTICULARPURPOSE.Seethe*/*GNUGeneralPublicLicenseformoredetails./*/*/*YoushouldhavereceivedacopyoftheGNUGeneralPublicLicense*/*alongwithAutopilot;ifnot,writetotheFreeSoftware/*Foundation,Inc.,59Temple

4、Place,Suite330,Boston,/*MA02111-1307USA/mmmmm*/L*/*/*/#include#includeeyebot.h* Thestateisupdatedwithgyroratemeasurementevery20ms* changethisvalueifyouupdateatadifferentrate.*/staticconstfloatdt=0.02;/*/* determinehowwellthesensorsaretrackingtheactualstate.*/staticfloatP22=1,0,0,1;/* Ourtwostates,th

5、eangleandthegyrobias.Asabyproductofcomputing* theangle,wealsohaveanunbiasedangularrateavailable.Theseare* read-onlytotheuserofthemodule.*/floatangle;floatqbias;floatrate;/* TheRrepresentsthemeasurementcovariancenoise.R=EvvT* Inthiscase,itisa1x1matrixthatsaysthatweexpect* 0.1radjitterfromtheinclinome

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

7、romeasurement* bytheuserofthemodule.Itupdatesthecurrentangleand* rateestimate.* Thepitchgyromeasurementshouldbescaledintorealunits,but* doesnotneedanybiasremoval.Thefilterwilltrackthebias.* A=0-1* 00*/floatq;floatPdot4;/一二Unbiasourgyro*/q=qm-qbias;/当前角速度：测量值-估计值/* Computethederivativeofthecovariance

8、matrix* (equation22-1)* Pdot=A*P+P*A+Q*/Pdot0=Q_angle-P01-P10;/*0,0*/Pdot1=-P11;/*0,1*/Pdot2=-P11;/*1,0*/Pdot3=Q_gyro;/*1,1*/*Storeourunbiasgyroestimate*/rate=q;/* Updateourangleestimate* angle+=angle_dot*dt* +=(gyro-gyro_bias)*dt* +=q*dt* /angle+=q*dt;/角速度积分累加到估计角度/*Updatethecovariancematrix*/P00+=

9、Pdot0*dt;P01+=Pdot1*dt;P10+=Pdot2*dt;P11+=Pdot3*dt;)/* kalmanupdateiscalledbyauserofthemodulewhenanew* inclinoometermeasurementisavailable.* Thisdoesnotneedtobecalledeverytimestep,butcanbeif* theaccelerometerdataareavailableatthesamerateasthe* rategyromeasurement.* H=10*becausetheanglemeasurementdir

10、ectlycorrespondstotheangle*estimateandtheanglemeasurementhasnorelationtothegyro二bias.*/三Computeourmeasuredangleandtheerrorinourestimate*/floatangle_m=incAngle;floatangle_err=angle_m-angle;/1.12zk-H*xk_dot/*一一一* h_0showshowthestatemeasurementdirectlyrelatesto* thestateestimate.* H=h0h1* Theh_1showsth

11、atthestatemeasurementdoesnotrelate* tothegyrobiasestimate.Wedontactuallyusethis,so* wecommentitout.* /floath_0=1;/*constfloath1=0;*/* PrecomputePHasthetermisusedtwice* NotethatH0,1=h1iszero,sothattermisnotnotcomputed*/constfloatPHt_0=h_0*P00;/*+h_1*P01=0*/constfloatPHt_1=h_0*P10;/*+h_1*P11=0*/* Comp

12、utetheerrorestimate:* (equation21-1)* E=HPH+R* /floatE=Rangle+(h0*PHt0);/* ComputetheKalmanfiltergains:* (equation21-2)* K=PHinv(E)*/floatK_0=PHt_0/E;floatK1=PHt1/E;/* Updatecovariancematrix:* (equation21-3)* P=P-KHP三Let*/floatY_0=PHt_0;/*h_0*P00*/floatY_1=h_0*P01;：P00-=K_0*Y_0;P01-=K_0*Y_1;P10-=K1*

13、Y0;P11-=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;一一现在智能小车上用的卡尔曼滤波算法。由于做平衡小车，然后对那段滤波

14、算法很疑惑，然后网上讲的又比较少，我看了书。这是小弟的对这段卡尔曼滤波程序的一点理解，因为基础薄弱(大二)错的请多多包涵。先上程序，这是抄的不知道谁的代码。抱歉了。不过这程序好像都写的差不多voidKalman_Filter(floatGyro,floatAccel)(Angle+=(Gyro-Q_bias)*dt;Pdot0=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-An

15、gle;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;Angle+=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)协

16、方差矩阵的预测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个式子比较抽象，现在直接用实例来说对于角度来说，我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角加速度值乘以时间，因为ddt,角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的)，静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得

17、到了当前角度的预测值AngleAngle=Angle+(Gyro-Q_bias)*dt;其中等号左边Angle为此时的角度，等号右边Angle为上一时刻的角度，Gyro为陀螺仪测的角速度的值，dt是两次滤波之间的时间间隔。floatdt=0.005;这是程序中的定义同时Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的即Q_bias=Q_bias将两个式子写成矩阵的形式得到上式，这个式子对应于卡尔曼滤波的第一个式子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维列

18、向量Qbias01,B为2维列向量dtU(k)为Gyro0,这里是卡尔曼滤波的第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声，(所谓噪声就是数据的方差值)P(k|k-1)=AP(k-1|k-1)A+Q这里的Q为向量AngleQ_bias的协方差矩阵，即cov(Angle,Q_bias)=。因为漂移噪声还有角度噪声是相互独立的，则cov(Q_bias,Angle)=。又由性质可知cov(x,x)=D(x)即方差，所以得到的矩阵如下D(Angle)00D(Q_bias),这里的两个方差值是开始就给出的常数程序中的定义如下floatQ_angle=0.

19、001;floatQ_gyro=0.003;接着是这一部分AP(k-1|k-1)A， 其中的(P(k-1)|P(k-1)为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中P(k-1|k-1)设为ab，第一式已知A为1dtcd01则计算AP(k-1|k-1)A+Q(就是个矩阵乘法和加法，算算吧)结果如下2_acdtbdtd.(dt)D(Angle)bddtAngleQbiasdt1AngleQbiasdtGyro0AngleQ_biascov(Angle,Angle)cov(Q_bias,Angle)cov(Angle,Q_bias)cov(Q_bias)cddtd2.d.(

20、dt)很小为了计算简便忽略不计。于是得到 IdtD(Angle)bddtddtda,b,c,d分别和矩阵的P00,P01,P10,P11计算过程转化为如下程序，代换即可Pdot0=Q_angle-PP01-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)/计算卡尔曼增益即计算卡尔曼增益，这是个二维向量设为0H=1k1,这里的一P(K|K-1)+R,这里又有一个常数R,程序中的定义如下floatR