EKFUKFPF的三个算法r
clearr
ticr
x01初始状态r
x_estimate1状态的估计r
e_x_estimatex_estimateEKF的初始估计r
u_x_estimatex_estimateUKF的初始估计r
p_x_estimatex_estimatePF的初始估计r
Q10i
put请输入过程噪声方差Q的值过程状态协方差r
R1i
put请输入测量噪声方差R的值测量噪声协方差r
P5初始估计方差r
e_PPUKF方差r
u_PPUKF方差r
pf_PPPF方差r
tf50模拟长度r
x_arrayx真实值数组r
e_x_estimate_arraye_x_estimateEKF最优估计值数组r
u_x_estimate_arrayu_x_estimateUKF最优估计值数组r
p_x_estimate_arrayp_x_estimatePF最优估计值数组r
u_k1微调参数r
u_symmetry_
umber4对称的点的个数r
u_total_
umber2u_symmetry_
umber1总的采样点的个数r
li
ear05r
N500粒子滤波的粒子数r
closeallr
r
粒子滤波初始N个粒子r
fori1Nr
p_xpartip_x_estimatesqrtpf_Pra
d
r
e
dr
r
fork1tfr
模拟系统r
xli
earx25x1x28cos12k1sqrtQra
d
状态值r
yx220sqrtRra
d
观测值r
r
r
扩展卡尔曼滤波器r
进行估计第一阶段的估计r
e_x_estimate_1li
eare_x_estimate25e_x_estimate1e_x_estimate28cos12k1r
e_y_estimatee_x_estimate_1220这是根据k1时估计值为1得到的观测值；只是这个由我估计得到的第24行的y也是观测值不过是由加了噪声的真实值得到的r
相关矩阵r
e_Ali
ear251e_x_estimate21e_x_estimate22传递矩阵r
e_He_x_estimate_110观测矩阵r
估计的误差r
e_p_estimatee_Ae_Pe_AQr
扩展卡尔曼增益r
e_Ke_p_estimatee_He_He_p_estimatee_HRr
进行估计值的更新第二阶段r
e_x_estimate_2e_x_estimate_1e_Kye_y_estimater
更新后的估计值的误差r
e_p_estimate_updatee_p_estimatee_Ke_He_p_estimater
进入下一次迭代的参数变化r
e_Pe_p_estimate_updater
e_x_estimatee_x_estimate_2r
r
r
粒子滤波器r
粒子滤波器r
fori1Nr
p_xpartmi
usi05p_xparti25p_xparti1p_xparti28cos12k1sqrtQra
d
这个式子比下面一行的效果好r
xpartmi
usi05xparti25xparti1xparti28cos12k1r
p_ypartp_xpartmi
usi220预测值r
p_vhatyp_ypart观测和预测的差r
p_qi1sqrtRsqrt2piexpp_vhat22R各个粒子的权值r
e
dr
平均每一个估计的可能性r
p_qsumsump_qr
fori1Nr
p_qip_qip_qsum各个粒子进行权值归一化r
e
dr
r