卡爾曼濾波算法代碼總結(jié)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;/當(dāng)前角速度：測(cè)量值-估計(jì)值/* 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;/角速度積分累加到估計(jì)角度/*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;一一現(xiàn)在智能小車(chē)上用的卡爾曼濾波算法。由于做平衡小車(chē)，然后對(duì)那段濾波

14、算法很疑惑，然后網(wǎng)上講的又比較少，我看了書(shū)。這是小弟的對(duì)這段卡爾曼濾波程序的一點(diǎn)理解，因?yàn)榛A(chǔ)薄弱(大二)錯(cuò)的請(qǐng)多多包涵。先上程序，這是抄的不知道誰(shuí)的代碼。抱歉了。不過(guò)這程序好像都寫(xiě)的差不多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;段時(shí)間的,有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個(gè)方程X(k|k-1)=AX(k-1|k-1)+BU(k).(1)/先驗(yàn)估計(jì)P(k|k-1)=AP(k-1|k-1)A+Q(2)協(xié)

16、方差矩陣的預(yù)測(cè)Kg(k)=P(k|k-1)H/(HP(k|k-1)H+R)(3)/計(jì)算卡爾曼增益X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1)(4)通過(guò)卡爾曼增益進(jìn)行修正P(k|k)=(I-Kg(k)H)P(k|k-1)(5)跟新協(xié)方差陣5個(gè)式子比較抽象，現(xiàn)在直接用實(shí)例來(lái)說(shuō)對(duì)于角度來(lái)說(shuō)，我們認(rèn)為此時(shí)的角度可以近似認(rèn)為是上一時(shí)刻的角度值加上上一時(shí)刻陀螺儀測(cè)得的角加速度值乘以時(shí)間，因?yàn)閐dt,角度微分等于時(shí)間的微分乘以角速度。但是陀螺儀有個(gè)靜態(tài)漂移(而且還是變化的)，靜態(tài)漂移就是靜止了沒(méi)有角速度然后陀螺儀也會(huì)輸出一個(gè)值，這個(gè)值肯定是沒(méi)有意義的，計(jì)算時(shí)要把它減去。由此我們得

17、到了當(dāng)前角度的預(yù)測(cè)值A(chǔ)ngleAngle=Angle+(Gyro-Q_bias)*dt;其中等號(hào)左邊Angle為此時(shí)的角度，等號(hào)右邊Angle為上一時(shí)刻的角度，Gyro為陀螺儀測(cè)的角速度的值，dt是兩次濾波之間的時(shí)間間隔。floatdt=0.005;這是程序中的定義同時(shí)Q_bias也是一個(gè)變化的量。但是就預(yù)測(cè)來(lái)說(shuō)認(rèn)為現(xiàn)在的漂移跟上一時(shí)刻是相同的即Q_bias=Q_bias將兩個(gè)式子寫(xiě)成矩陣的形式得到上式，這個(gè)式子對(duì)應(yīng)于卡爾曼濾波的第一個(gè)式子X(jué)(k|k-1)=AX(k-1|k-1)+BU(k).(1)/先驗(yàn)估計(jì)X(k|k-1)為2維列向量Angle,A為2維方陣1出，X(k-1|k-1)為2維列

18、向量Qbias01,B為2維列向量dtU(k)為Gyro0,這里是卡爾曼濾波的第二個(gè)式子接著是預(yù)測(cè)方差陣的預(yù)測(cè)值，這里首先要給出兩個(gè)值，一個(gè)是漂移的噪聲，一個(gè)是角度值的噪聲，(所謂噪聲就是數(shù)據(jù)的方差值)P(k|k-1)=AP(k-1|k-1)A+Q這里的Q為向量AngleQ_bias的協(xié)方差矩陣，即cov(Angle,Q_bias)=。因?yàn)槠圃肼曔€有角度噪聲是相互獨(dú)立的，則cov(Q_bias,Angle)=。又由性質(zhì)可知cov(x,x)=D(x)即方差，所以得到的矩陣如下D(Angle)00D(Q_bias),這里的兩個(gè)方差值是開(kāi)始就給出的常數(shù)程序中的定義如下floatQ_angle=0.

19、001;floatQ_gyro=0.003;接著是這一部分AP(k-1|k-1)A， 其中的(P(k-1)|P(k-1)為上一時(shí)刻的預(yù)測(cè)方差陣卡爾曼濾波的目標(biāo)就是要讓這個(gè)預(yù)測(cè)方差陣最小。其中P(k-1|k-1)設(shè)為ab，第一式已知A為1dtcd01則計(jì)算AP(k-1|k-1)A+Q(就是個(gè)矩陣乘法和加法，算算吧)結(jié)果如下2_acdtbdtd.(dt)D(Angle)bddtAngleQbiasdt1AngleQbiasdtGyro0AngleQ_biascov(Angle,Angle)cov(Q_bias,Angle)cov(Angle,Q_bias)cov(Q_bias)cddtd2.d.(

20、dt)很小為了計(jì)算簡(jiǎn)便忽略不計(jì)。于是得到 IdtD(Angle)bddtddtda,b,c,d分別和矩陣的P00,P01,P10,P11計(jì)算過(guò)程轉(zhuǎn)化為如下程序，代換即可Pdot0=Q_angle-PP01-PP10;Pdot1=-PP11;Pdot2=-PP11;Pdot3=Q_gyro;PP00+=Pdot0*dt;PP01+=Pdot1*dt;PP10+=Pdot2*dt;PP11+=Pdot3*dt;三，這里是卡爾曼濾波的第三個(gè)式子Kg(k)=P(k|k-1)H/(HP(k|k-1)H+R)(3)/計(jì)算卡爾曼增益即計(jì)算卡爾曼增益，這是個(gè)二維向量設(shè)為0H=1k1,這里的一P(K|K-1)+R,這里又有一個(gè)常數(shù)R,程序中的定義如下floatR