clearallv150目标速度v_se
sor0传感器速度t1扫描周期
xradarposito
0传感器坐标yradarposito
0
ppredzeros44Pzzzeros22Pxxzeros42xpredzeros41ypredzeros21sumx0sumy0sumxukf0sumyukf0sumxekf0sumyekf0统计的初值
L4alpha1kalpha0belta2ramda3L
azimutherror0015方位均方误差ra
geerror100距离均方误差process
oise1过程噪声均方差
taot33t2200t22t0000t33t2200t22tthei
putmatrixofprocessGt220t00t220t
a35pi180a_v5100
a_se
sor45pi180x18000初始位置
1页
如有你有帮助，请购买下载，谢谢！
fy112000
fori1200xi1xivcosatyi1yivsi
ate
d
如有你有帮助，请购买下载，谢谢！
fori1200xradarposito
0yradarposito
0
Zmeasure1iata
yiyradarposito
xixradarposito
ra
domNormal0azimutherror11Zmeasure2isqrtyiyradarposito
2xixradarposito
2ra
domNormal0ra
geerror11
xxiZmeasure2icosZmeasure1i观测值yyiZmeasure2isi
Zmeasure1i
measureerrorazimutherror200ra
geerror2processerrortaoprocess
oisevNoisesizeprocesserror1wNoisesizemeasureerror1
A1t000100001t0001A
oisesizeA1
forj12L1Wmj12LramdaWcj12Lramdae
d
Wm1ramdaLramdaWc1ramdaLramda1alpha2belta权值
ifi1xerrorra
geerror2cosZmeasure1i2Zmeasure2i2azimutherror2si
Zmeasure1i2yerrorra
geerror2si
Zmeasure1i2Zmeasure2i2azimutherror2cosZmeasure1i2xyerrorra
geerror2Zmeasure2i2azimutherror2si
Zmeasure1icosZmeasure1iPxerrorxerrortxyerrorxyerrortxerrort2xerrort2xyerrort2xyerrort2xyerrorxyerrortyerroryerrort
2页
f如有你有帮助，请购买下载，谢谢！
xyerrort2xyerrort2yerrort2yerrort2xestimateZmeasure2icosZmeasure1i0Zmeasure2isi
Zmeasure1i0e
d
chocholPLramdaforj1LxgamaP1jxestimatechojxgamaP2jxestimatechoje
dXsigmaxestimatexgamaP1xgamaP2FAXsigmapreFXsigmaxpredzerosA
oise1forj12L1xpredxpredWmjXsigmapreje
dNoise1A
oiseppredzerosNoise1Noise1forj12L1ppredppredWcjXsigmaprejxpredXsigmaprejxprede
dppredppredprocesserror
chorcholLramdappredforj1LXaugsigmaP1jxpredchorjXaugsigmaP2jxpredchorje
dXaugsigmaxpredXaugsigmaP1XaugsigmaP2
forj12L1Ysigmapre1jata
Xaugsigma3jXaugsigma1jYsigmapre2jsqrtXaugsigma1j2Xaugsigma3j2e
dypredzeros21forj12L1ypredypredWmjYsigmapreje
dPzzzeros22forj12L1PzzPzzWcjYsigmaprejypredYsigmaprejyprede
dPzzPzzmeasureerror
3r