卡爾曼濾波算法代碼總結(jié)

/***************************************************************************/

/*kalman.c

*/

/*1-DKalmanfilterAlgorithm,usinganinclinometerandgyro

*/

/*Author:RichChiOoi

*/

/*Version:1.0

*/

/*Date:30.05.2003

*/

/*AdaptedfromTrammelHudson(hudson@)

*/

/*-------------------------------

*/

/*Compilation

procedure:

*/

/*

Linux

*/

/*

gcc68-c

XXXXXX.c(tocreateobjectfile)

*/

/*

gcc68-o

XXXXXX.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,

*/

/*MA

02111-1307

USA

*/

/***************************************************************************/

#include<math.h>

#include"eyebot.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]

*

[0

0]

*/

voidstateUpdate(constfloatq_m){

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][1]-=K_0*t_1; PP[1][0]-=K_1*t_0; PP[1][1]-=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)//協(xié)方差矩陣的預測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)//跟新協(xié)方差陣5個式子比較抽象，現(xiàn)在直接用實例來說—，對于角度來說，我們認為此時的角度可以近似認為是上一時刻的角度值加上上一時刻陀螺儀測得的角加速度值乘以時間，因為，角度微分等于時間的微分乘以角速度。但是陀螺儀有個靜態(tài)漂移（而且還是變化的），靜態(tài)漂移就是靜止了沒有角速度然后陀螺儀也會輸出一個值，這個值肯定是沒有意義的，計算時要把它減去。由此我們得到了當前角度的預測值AngleAngle=Angle+(Gyro-Q_bias)*dt;其中等號左邊Angle為此時的角度，等號右邊Angle為上一時刻的角度，Gyro為陀螺儀測的角速度的值，dt是兩次濾波之間的時間間隔。floatdt=0.005; 這是程序中的定義同時Q_bias也是一個變化的量。但是就預測來說認為現(xiàn)在的漂移跟上一時刻是相同的即Q_bias=Q_bias將兩個式子寫成矩陣的形式得到上式，這個式子對應于卡爾曼濾波的第一個式子X(k|k-1)=AX(k-1|k-1)+BU(k)………..(1)//先驗估計X(k|k-1)為2維列向量，A為2維方陣，X(k-1|k-1)為2維列向量，B為2維列向量，U(k)為Gyro二，這里是卡爾曼濾波的第二個式子接著是預測方差陣的預測值，這里首先要給出兩個值，一個是漂移的噪聲，一個是角度值的噪聲，（所謂噪聲就是數(shù)據(jù)的方差值）P(k|k-1)=AP(k-1|k-1)A’+Q這里的Q為向量的協(xié)方差矩陣，即因為漂移噪聲還有角度噪聲是相互獨立的，則=0；=0又由性質(zhì)可知cov（x，x）=D（x）即方差，所以得到的矩陣如下，這里的兩個方差值是開始就給出的常數(shù)程序中的定義如下floatQ_angle=0.001;floatQ_gyro=0.003; 接著是這一部分AP(k-1|k-1)A’,其中的（P（k-1）|P(k-1)）為上一時刻的預測方差陣卡爾曼濾波的目標就是要讓這個預測方差陣最小。其中P(k-1|k-1)設為,第一式已知A為 則計算AP(k-1|k-1)A’+Q（就是個矩陣乘法和加法，算算吧）結(jié)果如下很小為了計算簡便忽略不計。于是得到a,b,c,d分別和矩陣的P[0][0],P[0][1],P[1][0],P[1][1]計算過程轉(zhuǎn)化為如下程序，代換即可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;三，這里是卡爾曼濾波的第三個式子Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)………(3)//計算卡爾曼增益即計算卡爾曼增益，這是個二維向量設為，這里的=為由此kg=P(K|K-1)+R，這里又有一個常數(shù)R，程序中的定義如下floatR_angle=0.5;這個指的是角度測量噪聲值，則式子的分母=P[0][0]+R_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