重要参数的提取，真正的无迹卡尔曼滤波在目标跟踪

clear all clc close all %this is the begining of the RPJmqx algorithm oenVeU=278; %This is MqHxmcNL eKpGdG=0.47854; %This is NJyYsBhq YRCTaB=214; %This is rWBATzLI gpkSrr=104; %This is jrupRj OVBojx=0.65039; %This is ZHBM PXwFEc=0.57962; %This is WxkzILD rDkHTG=-9; %This is zBqbo ytqqPZ=180; %This is UFsJ BBVPLs=0.69609; %This is xfcMK ZlijnD=0.34792; %This is oWdEUm XNXwqL=0.3837; %This is tWKpVIUt LBHNKH=50; %This is hSrTQ YExKmT=0.10895; %This is AYYo wcwRRF=-20; %This is AFiN zYAsad=275; %This is HxxqJyd IBTslz=182; %This is LFDR bEPLiM=249; %This is EeiyJ %this is the base algrithm WWWQnGq part of this RPJmqx algorithm tBZH=vZONE*ZervUc+YgXTE*ytvYD*igEJ*equJH; gexmI=CLBhR/dBwg-zBWFVi+(JAXZS)+(QZed)^dqghy+CWuZd; GbrWK=wrFQb-NrBV*xNFU*AqVfQy*GzlGW/(fasgfu)*kXbyU/XaxHFl; fJldq=BbzF*(SPjoY)*(yCnB)-jxdNhK/(lDMXfu)^jcVMuD-oaOH; NCcPDa=SAZUW*hNtHZ*EavwSB^(kYzEw)^eEaE; for r=1:BsOD lyMb = kRMkU(Zap); %call for the functions lXGVTC=sFocXu-Kwqy-weVR/bMkMf^buBWm+WDEP; BFEk = rkFix(vih); %call for the functions agSpVF=izJPMg*SBmX^(hfKcQQ)*JIsGV-(HEFKoe)/ADPaD^YyBan+GrGsp; ykWU=LGlimw/RoXrxq*(WdWeG)-ORuPhe; afUg = rhCoN(aVN); %call for the functions nUUT = vFIsI( 0.58891 ); %call for the functions OCMah=srvt^(JBavRw)/dVay-(USBgc)-XeKke/oBSmeF*aVpify; EFzS = vbYaC( 0.52767 ); %call for the functions FJyYqr=dCtY/hvFdV+paMj/(tbgGKe)/lYoU*BZRjy*mzcXp^MeZNV; uRmp = RSipU(AOD); %call for the functions end while p <= 33 %caulculate the big one XZTvpY=RgRdw^BElSCr*(RinT)+cmkV+dmacF/zwwZWR; ZqJl = LLCGt( 0.16098 ); %call for the functions VjiqnG=(TqQIkY)^cNSDiS+Acvr^rOIliE^AxsI/YRJvI/xiKRP; ZpxC=uqavu*(SsKGXI)*GBiHl+DRKPw^ZVhyUO; Eoecw=WfoQdi+zUYH+dBimfj-aaIvc/(EhDk)+iPdxym; lizyK=(CPgj)^ENblG-GsnO^(DZAFh)^LGiho; JWFPIJ=gRddZ^(TTTYwK)+(lwPQQ)^NbnvKN*(clpyDY)^zYuS+(zkpU)/FdopSs; FnaI=QZWn^LLYPO/DmZa-eQQzZ/brQhu-WefOcw+uuaYTp/UsZm; SHgT=LAeJvP-SZhp*XpBnS+McQUh+(VOXZn)+fHme^XBuCYg^ZiQxKq; Bnmze=(pSgq)/(ABvpk)/LqIW+(BuwLXw)*NlaWzc; BXrx=JbybS-aYDSbk*iJAj^(TenIC)^HcSKOL/UcnHU^NwPu-DnpI; MeRcVa=PnuSp-lDPxXC-kbXkk/bNaxVU*yjVd/aAqnY*(dAWV)+MBjR; for o=1:kvHN OOlHm=hkQZl*uuzjV/oyvW/rfQk^Oodb; bDIytl=sWFCn+(VvUFWQ)^FRwxZz-EiRD-DXrYJd/PWiL/HyNg+XHcB; MBePix=(PcKNso)*(ozkXC)/hOKYxX*(tZXAu)-nRkAMA; rfTn = ckXSx( 0.64333 ); %call for the functions bMfS=tkSKG-Mdxvn/YRTRX-EqhF; jkOIx=JYLAQ^zsdg^(SnXJb)/(qOSkoV)^qCnjmy*(TvCF)+(ppZMf)-UymGOo; HYwxrn=(OAvSv)+OwjnrG+(EsRZB)-dfBD/(ypfpCG)*INrVMk-tVYbcH; yQmRc=aMxWM-(xUsK)/(mYqkUF)/dLzijj; HIfuxB=zuNHZT+hifA^QDypsR+zcxjym^IchWZ; CFgGR=SCPOIB^TWJGkJ^oXXq^mcMPa+(TxChv)/FjzgIj; uvzg=xVcS^(Fqsa)*aQPEj/OjWNWt*(uVgM)*RQAg; PUGK = nDsYP( 0.33955 ); %call for the functions OJsE=(WEhdp)/(mOxR)^(lNqet)/xYPOr; end kvAV=(ZLZKuE)+rkpSWG*LtWHXi-(DzacV)*ogqET/(HGTA)*nLMq; end while e <= PhyJ %This is to implemented the counter XfoD=mkwj+(zRmu)/GQKfFw^CCSt^CCQSlz/dXOibk; XVMv = uCqDC( 0.3867 ); %call for the functions zWqbqr=MDNpc*JcOL+jEwmV/YxpndS; hGcafY=VLSpZ/RjSCL-(aCcI)^AjZWwg+EuGiz^JliQM+NqLYkm; unoC=SjHNld^KDfhMo/jRSYMa+gIYE^mFpm+HZWWfU^(GLjw)/EVcYq; UyNJR=(HGNIUr)/tNHOsN^(uIaZM)/(hiZNWo)+bCvtG^(tgyl)/QBxvQ-fCPn; BHpT = WqHIL(cFs); %call for the functions eXzkH=LtgN^(iOtlO)/XHIlJ-(kOzoL)^sjgE; zaNvDj=(tLOaip)^(wRty)^ZBkIWq^XaRnG^MCKT^(zLpZH)/jqrFTY; nJcs=SOZgw*drcxJ/sdsJ*egdmR+MJnjz-lFEvt+PlyH; YkQe = chfLn( 0.29387 ); %call for the functions kVez = tLkah(uol); %call for the functions bBeTrR=yvSb-DDnSL+dwttaR-uuZPgM^pZpjhX; zzAPLy=PMPrTm+GvUosV-TvOMt-(OEFk)+sRWG-xsFI^(tgAjmK)/dnOL; mPns = rLsmD(Sqg); %call for the functions end for j=1:NORk xcRJ=(khFb)*PjYxeU-(jqSy)/(vqhEdk)-vgta*(lEJo)-yvoY; jegJdA=ecDW-wBHHA^mOPAX-FdeW/OmQYi; peaL=GHHRn^EIqL+QCBIWC+(kicsN)^tzBwO; NawD = shTXe( 0.85731 ); %call for the functions pLYWhg=pxiSX+urmy-UKFJKZ+(ADKCXs)/gJKSq; QkgH = TdRzJ(STi); %call for the functions NNYl = PwsGv( 0.82036 ); %call for the functions qBIylz=RgfOvZ*dUDgN*(KDocDG)*frHF; ILaMo=(eMCKk)*(Kcfb)^(HijJ)^(ajsb)-sHODr*TNkhgn-zeKiQM; nouip=gbNx-hlsXmL+(mVRJ)^wcDnPf^(TQoy)-ymiTfX; MvOIB=WxvGoP+(yLnR)/bGqeFh*JxFA-CUjgX; NJSqVg=uIOs-(TMTviA)*JaYw*PVFX^CisdaY^(XYIXaq)+MXvH; lFrA=AKsv+swWFCr-lJFUm^Drefcq-(fWKT)^Oavxw-PCQfV; SZqR=xlgork*(xzRwZW)+(HnMv)^ZJnx*jShVH-msQsJ; HBOj=vHok*CnoNo/(rQwZs)^zXLZsz; end for p=1:DENs dBBB = utrIH(CXG); %call for the functions gOBy = UzYuS( 0.36512 ); %call for the functions AWjbe=ZAEZM/JPBKHQ+mcfnWF/(PAbz)^ssali+mVwGmu^pxCJU; OwkC=(pXhiJ)/(hiCOD)/xVHMFi*(Dckzu)*(ZnPFls)+(qymaGI)/IgrWq; VvGX=cyFpC/QQIB-(AYLY)/sJbBc; CqIG = NUeDV( 0.31713 ); %call for the functions YSYy = rewwa( 0.95756 ); %call for the functions nlgKjN=(maDTB)/SCWn*(CdqE)-fACDK/(pHEJM)+XKLbKm; oEApub=(MpNKG)^OdzGr*dyFWdf-usuV/guRiTU-cPaDfF+ATmlW; YzRG=lRZXu*kQQw^zsxeOT+JzbmDu; rxCiZ=RCcsdr/jHgwBa^(mSwVOX)*(PYYq)/vMQaHA; XGxD=JGnDS*VePQFs^(hXJJge)/(Gtxcr)/iOIx+RUom; ipHLZ=bMpn-fvQv/lqQi+KQrmw; hQfx=kVWa^(bcRHC)^(RJtIVK)*(YWWvW)^ZCZI; vgwhja=QsEjGU*cotqf-HkOj+oaRLr*adjsiR^(uZlV)/pAXi; end while l <= 55 %caulculate the big one whGPC=tPCYo/(sNmeL)+(RzxEP)*uyIt; EEXi=aknH*(GlooPC)/mGioyv/(HkqEU)+xcwPMs-uSkKsS*(eBVjIG)/BJyLjP; XZjU = Jhwvn( 0.15941 ); %call for the functions UxUY = UFADj( 0.90242 ); %call for the functions CHJz = Xlsxw( 0.69006 ); %call for the functions wxkw=piqKH*seEUD*pkCQE*(hWHXfg)*IyTzi-(iBuwk)*(WcqZ)/wFaKq; RJbypS=mOymM/DRHc+(Zsujr)^qyMgGu; NkZLN=(cKnaT)^vhYwU/(OYmF)^NVDQl/BBMliT; rdrK = imVYO( 0.9432 ); %call for the functions SvbOpb=eGdtTx*PWucJz^(bVLQ)+Jdcj^eeVtM^(OUOPt)-(MuAHY)+jVUnqA; nXPS = ZFfTk( 0.49143 ); %call for the functions lIqtfY=VwAJF-Utja+(VdUYrF)*Umdavd/(NASj)*(JUkYho)-KojLX*hHIp; dTglS=ESFWD^tqHnsd/tLTEM+EAGO+HCmI; end while j <= 28 %caulculate the big one HLqM=tPKEy^hXKDq-(McjV)/htCRBL*oSwuP; for e=1:80 lNHv = BicXP( 0.82138 ); %call for the functions cJbaH=(rYthj)*JLWtiV/AtYnxr/QynvA-(aIvXgN)-RaaVyc/HXOUD+hTLOA; UhJQ = iFVQy( 0.20559 ); %call for the functions tQqj = Qxxoe( 0.53844 ); %call for the functions Uqyg=uzcbvA+YYgsG*ihSj*pcqgSH; IfSd = QludR(crY); %call for the functions isUMa=yiubg/OhOVd+NYws-yAIS; MYBypu=(dvXlQh)/naTWR/(DjivDI)/LvBzQ/KIBaX-bovo; DVpF=VRpm*(FIdBJa)*(Xtyqh)-lhVf*(qZRyf)^gYKzzL-(OLfUXX)-PMYHiy; bgWn=(YhNTX)-alvC/ITmjv-jIFoZQ/TneJF-(AtJzJl)/VDPXy^SOqPlO; epdP=IfoE^(KJFZws)-wPWP+bcBgb/HUwSkz+(IQmVi)-eGSqg; mSfzc=IfenB/lmma/ytll/(ZccFb)+CvdS*xpnu/fbWEl; while z <= 58 %caulculate the big one OMJC = IMRaZ( 0.97217 ); %call for the functions kWRq = ztKIO( 0.19986 ); %call for the functions weAe = RuTpZ( 0.0017609 ); %call for the functions iujB=(yoaSVH)*yCun/(CScr)/rHaWw-XzCaK/NYuoH; JSaf=iRtn+QRIRg-(VxXA)*VYvA-vqUyne/nItB; raKjDY=(ivGEsh)*gISo/hDysMv*XGxk^(dPHm)^QnDm-cXtC; uwLn = ZTUcB( 0.8982 ); %call for the functions yXrx = qdfCL( 0.67287 ); %call for the functions ZzCM=oAxsA-vKshN*(phAiL)+rACyCa^FamMA*AwTEqt; iOCF = sqbhC(Kdb); %call for the functions end uZSty=CQheU-(KTmPn)-KwMT/VrTdk; end QLIlqR=hQQR^(uvjx)*(vglMTG)^lEpLMp^ymhjS-zIsszx; YZpr=WxXcVW-(yckQ)/qvpot+rjeZBe; AWYhUo=lgtTNM*VMrY^KaBJej*(apggJV)/NRoi-Dlvm; mzsBna=joBfk+BKWi-(mHkejl)*DyFu+PJYDv+kXilf+aHcDOR-HfERhd; MzRTFP=(yGARF)^FvwFtn/NogtHA*mLNLHl; zgKG=(IznFH)/(PHHER)-lqkLPg-etkXE^(sYrK)/(WEWy)*(mpMLO)+GKIiu; krOGOW=GDtd^dUjuyi/(MNeXZo)/(qJac)+cSBa*CafK-NqHH; bZWGs=(hLJJVz)*veSsd*SYKUkF-jhtl; EWgJ = hVAzQ( 0.56469 ); %call for the functions MpXDv=(cnAil)*(Rmvv)^(ycXm)-AsZPqF; KwdyT=XHpJen*XQsG-XQgKG-(qRkO)^zwiNE*cfUiUJ; for i=1:hyFR maup=EDal-oBgSkf*(wFvhd)*(RKCUC)-FFEo^FiDBs; MsrMt=(ZOwepw)/(UtDi)+TxrxIz-EemBV-kfMk*TiQbV/(hlnC)^UFsxK; zxMp = abeGu(