x0初始状态
Ri
put请输入过程噪声方差R的值测量噪声协方差
Qi
put请输入观测噪声方差Q的值过程状态协方差
tf100模拟长度
N100粒子滤波器中的粒子个数
xhatx
P2
xhatPartx
粒子滤波器初期
fori1N
xpartixsqrtPra
d
e
d
xArrx
yArrx220sqrtRra
d
xhatArrx
PArrP
xhatPartArrxhatPart
closeall
fork1tf
模拟系统
x05x25x1x28cos12k1sqrtQra
d
yx220sqrtRra
d
粒子滤波器
fori1N
xpartmi
usi05xparti25xparti1xparti28cos12k1sqrtQra
d
ypartxpartmi
usi220
vhatyypart观测和预测的差
qi1sqrtRsqrt2piexpvhat22R
e
d
平均每一个估计的可能性
qsumsumq
fori1N
qiqiqsum归一化权值
e
d
重采样
fori1N
ura
d
qtempsum0
forj1N
qtempsumqtempsumqj
ifqtempsumu
xpartixpartmi
usj
break
e
d
e
d
e
d
xhatPartmea
xpart
在图片中表示出数据
xArrxArrx
yArryArry
xhatArrxhatArrxhat
PArrPArrP
xhatPartArrxhatPartArrxhatPart
e
d
t0tf
figure
plottxArrbtxhatPartArrk
setgcaFo
tSize10setgcfColoryellow
xlabeltimestepylabelstate
lege
d真实值PF估计值
xhatRMSsqrt
ormxArrxhatArr2tf
xhatPartRMSsqrt
ormxArrxhatPartArr2tf
dispPF估计误差均方值
um2strxhatPartRMS
r