卡尔曼滤波算法代码总结(20150311)VIP免费

/***************************************************************************//*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*//***************************************************************************/#include#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]*[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){/*Computeo...