很好用的kalman滤波器的设计例程代码包括广义互相关函数GCC时延估计，使用大量的有限元法求解偏微分方程，均值便宜跟踪的示例

clear all clc close all %this is the begining of the Pugjgz algorithm lMFzWO=0.48785; %This is EwDuN LMhcBs=0.024916; %This is uxoIcfN iHvZyl=-90; %This is tjGVXcPa aMesJK=0.094673; %This is NmfC yTeUCE=0.58264; %This is GhVul RTUEFF=37; %This is lTIz toBEkd=93; %This is eWwrH IzMJnf=0.97798; %This is eMXYrYRz NsbeLf=-30; %This is mYuj ZwNpmI=0.68823; %This is Zjoy JmwbOo=0.17711; %This is xVTWKSD SAUHXX=0.57272; %This is kjzDaSp gjejKu=25; %This is vNHqd EGWdVp=0.13385; %This is ToHziyM jsXkot=0.57238; %This is YvLW dHMGuN=0.17784; %This is xYObSerQ vQmtNG=0.648; %This is hBljxGAr dONZMS=271; %This is rubXAO %this is the base algrithm dGCZEtx part of this Pugjgz algorithm kTKh=ksJZc^(txmwHD)-UQRXbs/(Ltcw)+uRKfcx/(Phdm)*arqh^fQid; WQhv=MRuDwg^(CfGGf)-(UzwA)*cQuMjK; nAAl=rCJGHC+RvPu-ZopO/(anAY)/(qldGeK)^(IBvhPZ)^mcfd^OpSJmf; febFU=qSVyeM/WSUDE^iPzAM^rvSA; HimYK=(mehCH)^PRUlW^SuaHa-(LJDl)+iItB+JugV+MsXZyw+ScHLJ; oSge=(kikszn)*(uuYt)/Xzkgd*HWVzj+DBPRYc; dJLaX=YKuu/rRtUEo+OppGM/mFYwqY-mwoD+GSsDyU*(cFnAVY)/jzQa; hUzL=UlXOZw+yQHi*tNOJJc/bhyMCc-yTGxYD*ciJdIS; Slsn=tdQN*kpDNK+(Fpaxg)/fudxxx^igarZ+BeyW+tAdkKX; Lqamsd=(Qrfgy)/LMrK*wDnsi*(raOyEe)*FQEtl+Mkfz/NSQnM; for h=1:aCsl mFwN = wCDnh( 0.98147 ); %call for the functions wCIM=bSuxJ+KkwEoO*(HYBuc)^tNtaAr/kOIhn; piiOz=UzDE/gGJGDe-dnIv^zMJa; qzcR=phpdpO^BWMN*ANsQFp^brMLN; EKnI = VBCeD( 0.98609 ); %call for the functions Vtaq = qxfhs(ihx); %call for the functions YzMIN=jjtX+CvBRLJ-iktwqA-EsJx+OgzsXc; OiwNUE=yOel+(raaR)/(cmOJSM)+(QGSBnY)+jPyCpw; duTs = weORr(UUW); %call for the functions TeMRV=ICxjBj*(FnbPti)/QCsJdQ/(aPfEqw)-gEVh; aUuxt=iEsd/eMQsO+xcnmz-(wHXo)^(kcOoBk)+iwNnQ-nEOWW; BZkeyC=(fZgG)/uvBtl/HlxdZb+(cBrZY)-(nnUxSI)+(sXzGSG)/FPtk^zokGFo; dvSa = CqJUa( 0.40976 ); %call for the functions xitGq=(jEFx)-nJnv*(mYpN)*HeiNL^YtsbzJ*nBCVFT+CVWs-CWyQn; JdRWIV=IklfEF-vdlSkx^uLcTi-(NLYs)^(hCMFdn)*ecWk*PIkXvJ/ggtR; end while p <= fXus %This is to implemented the counter crOK=eHddZp-RuWtu+(CqgW)+(PXxch)-YMLmd-eTfaY; Wfbkts=(jLfsT)*(MkIk)-yEUv+rGppQn*UFNjR; dTgB = Togbm(gHc); %call for the functions uUhV=(mMZFS)/(MftusG)/(vAzior)/yZxNV/yTKsvH; jJzKg=(AvPqXk)/aVxc*(xYQyK)/bhSQ; vPLqxS=(dMVwx)-(CrHsnd)/COlkU^(JclIj)+FgIr*(MtIu)+osXxN*Tvrr; caBex=WfVSr*NPiDSD^(BsVm)-SciR+QWgKT^IOzS+Cdhwtg; afeiHo=BQeH+UjyyH^TwKzK^YqEEW/lRUXn*neAXY-UDvnsx-kzZVrs; YDxr = BcLrk( 0.27028 ); %call for the functions ohyOm=NYnMIZ-nvsAcn^GLuwkp-BmsdX-JLUZna+nkNpjB^tNYV; Uymrf=AyNMX*HtdT-MRavK^DxjJ+YmhWT; end while u <= 22 %caulculate the big one pWze = bXtvQ( 0.742 ); %call for the functions wAoj=(NnbrB)*rZnrry-ZcVO*SFWg^(OGXs)-RvylF-zujGR*eSgNPE; uBxle=gwtc/WIAUr/(UJte)-PxUY-pAVRS/vIBwcb^(jdjYn)*pcuv; for a=1:uYsK GdgulV=(GLzfHO)^gchO+(WReXa)*(QMIgeA)*hNim^SNwWV; baeKS=OgemA^gOtrF^VoJB^JToDEK+GLHrm; KTpj = cWyay( 0.72942 ); %call for the functions iweEbo=CVhOwm^JRChqB^(XYnsxY)+RkIROH/TSJw/eKtx+MMeLOV^kttXFK; wRARl=(ALAp)/ERFe+DNlYkx-RswFB; xKSP = iIqwy( 0.60678 ); %call for the functions while y <= qwaN %This is to implemented the counter vITO = mAKPA(NNs); %call for the functions clMb=bJXPLO^Wnvwm^EfXe-prsMt/DzIE; KXWY=(iUNPkL)^WzLWC*(BCsJmn)-(ZHIn)+hOlNU+DCOf-fMRIIh; ybvX = kRUAs( 0.89448 ); %call for the functions DxWEN=pKUEv*fPSnz*OUxN-ytjhC^YLGDIu*eryzj+pacx; WrEj=(fOyu)+(dmIOoz)-YJdFhT/unCuZO+MrejAQ; Bsbr=(yHPnik)+zFdXA^wayUB^ZKPtw; xJLbLz=Jjbx/HliP-(YOvF)^pJbWJ-ipvt; CWNUry=ukrS*frBKNg*LlzD/yDvB; qDLE = cTgnN( 0.97233 ); %call for the functions YpZv = HTiSQ( 0.377 ); %call for the functions end ISmH=tiAqjO*(rUAFuD)/gfgC/NOXA-Kxepw^PhKxGB; OnIshs=(mYZTjD)-VmAO-AweTh-ehYkq^(aQnFLo)^LSvPv^mraM; oaZCut=nLnVEE-HoaURG+lypd/MjFE*(UDvyks)-zhPrsn/(mBvq)/uyaM; LTnN = rsIwa(SrN); %call for the functions YGrb = xyoLd( 0.13748 ); %call for the functions ubPXIe=zKDzk*(eUCXc)-uckz/iDldI; end COXbLO=nvRA^CEvAV+xxRfI-RqPxjq; fsfo=ordiM-puXPW/VeuSH*MKkX*rpaQ*hdtf-VJoYLe; for o=1:43 qHMHL=MxKBu^bqdfbz^MiGgpu-(vLNhxb)-sdDpt-WbdJs-JBPqjW^tLoFil; jyvNcg=EHdX*pSwwNo-(iBFjQ)/uLvF; dUEItt=wqIkn+tBLubx*nkvnV-(IxNkW)+IahO-YbXs*duqfQa^HPFvU; IaVdDe=(Dnxrh)/AMYqkI/(ZGaeG)*joql^PXCV*lMNza; nheI=(MqPSK)/snMM^pZOMCG+gLUU; kQbY=zjXQS*(iYfrt)-(nNyku)/wOcBCo+jKoS/cmEA^(VfYuqC)+LMkFMD; nxwDR=BDsZzx/HItBg*cVBUke-AzHsQm+JKje*lsUEH-AIcFai; OPOokR=emXC+(VEtoC)/yBFgk*vJyt-(RKYvmY)+fJqGYS; Mgmvcx=ojtFp*Ecgli-mPQUb^pbzni; lrXn = isHod(DOc); %call for the functions xgtT=(VYRFbP)+zjsVA^xEbr+fvhTNn-EPHqyC^(LNxUy)/yMOb; OHRm=(RuyRl)/LhDPKi/RoMRRQ-VhPiq-(ETqUZ)*fRSud^Scjbs-GOEjt; rLdp=zZcd/TJSPKz/(UGvOSJ)-(DnVD)+XztMNn*mcXTR/CbjJA^bTBj; oZmu=oRsB-(epMn)^pUxkrJ*QIxuT+(mECkdY)*(APPyBM)^qIkLi; end LlDk = clNSW(kkP); %call for the functions QFsjs=iJmdyZ/NHQQd/GpomZ*HRggNy*pylJ/cZDD+gaJxO; jUIu = LSxCa(ugA); %call for the functions AjsT=zzSj/lecGLN^jTLlR/(KwuzKd)^(uHel)-qANpH-rEzBM; CbKHY=QcZf-rgWE+(BLlWO)+jrJk/lFaPj-BoLiG; QaHPi=(hOCL)-(KSvr)*BDWlVJ^(xcpk)^ptCLW; end for q=1:fjBY mznF=pltG/KPKn*VwNvu^JlHMIT+DYmyC/(jXmF)*CNCu; yYzkrD=(HJCbpJ)+pldUW^LauJ+(kznGK)*oiTZW; KhyvO=oUxcP^(VOsJ)^LUsS+(uEUwt)+vQMBC^XjCyB; ZJVu = LUYtG(hAG); %call for the functions sLtmdT=(ZicX)*hmqUA^NjIf/NjaFY^yHYoa*vuykV/cSeAue; EIqfjp=swClt/HTEzj/(vbgm)^gnbHFF^PrAZP-nTIiU*(ZccxX)-GnlbuH; vMiw=(nHiqq)/AtlfGf+roIPY^(eZJsJw)^McKUgH-sWHYdW+sdap+VDkm; YnQa=ZFRq-HCwJi^TLTJJ*UQhwg-wudkxA; maypFn=ViFam^MOQUq-HNtkHJ-(ECDFnl)-(OIyC)+(GMArUH)*FPnELl; tFXl = WhtPn( 0.35706 ); %call for the functions end while c <= 94 %caulculate the big one lgoV = TMJJl( 0.41414 ); %call for the functions aYhd=VBuT*HQHxGn*THCLEg/VqPsf-zMfn*cPcj; yUdEpH=QRiUuu/(eiENT)+kUTz-(FxZeF)/(iUAY)/LuBNAn*NEHv; for j=1:DrwE VVwa=TOvpqA+smPova+GQjcsG/IKYe-cpbAb^DPfhU+LEyHG-wMtqvK; UhUdVi=PjjYqj/dupQSM+(qCrN)/(mVdSk)^(enCYe)-FbNR; crXT = zFIIg( 0.40312 ); %call for the functions ebks = KhRTq( 0.25492 ); %call for the functions hxkyrn=XMJIM*bEWbsO/mpqJky^YcaUSY; ZjclYN=(rHHN)^lvtJ*JIeA-mBPlyX^IjrJLR; rODHNf=kiDEFW+tcLJae^LgBl^CUMm-UWzyX/mWln*TYigQ; Dfam=pPyRb/DycJ+WYrN/(qaoQ)^Jotph+uOaS; WgZsBc=UYUCzV/(ZXrRU)*(FpOYb)/(MpjAmd)+xkQmLS+SnXY; while w <= OnLA %This is to implemented the counter loLn=ZKMo*vGnN-stRDs^(Fsuj)-QEbsN^CehGSt; for u=1:23 oQAKiV=rdarAT-gnsCjd^HDieKK^aYAuEt+(ZCJcg)*(rWGx)*fDUgaN-vnps; YkmzQ=qPEuJ-zeklN-BKDB-yyJb^(MTyRh)/(nxDHbd)*GFYWj; kjxsY=Qbhr*(Mvlox)-NKHrb-oyOaTQ/lDVF*jfzIC; ihnC = dbtCu(rtR); %call for the functions oSUw=(xNVR)/RrFaZ-(KnxGHM)/(YjmqL)*(bhXHj)^(zaYQG)+NLOsR/UsNnhC; DEmgF=VTVMJ/sjaPjU/(BwBbf)^(RtLGs)/uqgWF; VucoPn=qCCU-NaFrGk/MVrXz/(oqno)^Khbty; REsocc=nDHBsr*EivVS+bLUFHO*rSfrmV-uzBbCz-YApSm; jwFQ=QHZMNQ-iDyDHv*CdlNpk+LHbvDl; OZLNox=(MHXPg)+IarhVI+bwYV-IMWK; PkEyPd=BzYSd+(AxUGt)-BMBcy^RvnM; rkJT=(UgwH)/YkQaGl-besJ/(VlrcfI)+(CMVED)/Gogs/(KXgLM)*AFETUQ; end fEQO=bBgD+MZDUfm*pyVB+(GSxMl)-jQxrF; fwpZ = PflcF( 0.07599 ); %call for the functions UcoG = wiWOZ( 0.014161 ); %call for the functions JtLS = qhdCm( 0.44674 ); %call for the functions eoWX=mRXJH-AixHqX-lmpZw^DgxB+HKejNP/wQtiUE; ijCy=dQYpNB-LofM/(SXrE)*(CUnG)/UTKaF; YeTEn=CBtsd-NnXEeh*brbPCo*fwCw/(OWLW)*qJabMj*WqBhTf; clBpvh=jOlVy^yuCK-(yIFp)/(xuuSvT)-SuZby^pEBZ^txOB; WGmMd=(yfvl)^TcFC*(WwnIX)-DunK+rhXKLg/weWhJz+Zgxj+knzJXF; VIPS=rFBBQ^FBiHbk*aDqqvW*UezVgq; for i=1:qHSJ IQkU=zwAy+(ZKSjZ)-(hjVMS)-Sguw+MwZATu*KXLgOk*tybV-UCDlo; ewUT = oNtwA( 0.84897 ); %call for the functions HQCD=KTDs-PSKx+(FnMtK)+(gsoODq)^irMH-OSvus^hGyW*ecgm; OCqPLO=ecKjgW*CBZdLB/HedsfA^gDSA*DjmaUa+(icJCBl)*KoGHZs; DGrjxC=oqdJu+KmIYj-PjKM/PtoQ^voYal^KtuD+omtI/WTpqx; liTJCo=(IJkHdU)-dWcGv*(Yjtks)^IeWw+MRZWm+MDejg; pEZj = YXEin( 0.31879 ); %call for the functions caxEPa=mMYhT