Unscented Kalman Filter(UKF1EKF算法回顾(2）时间修正（timeupdate)：(3)+1 = f(全,0,k)(4)PR+1=FrPFT+GkQGT5/37Dr.Yuan-Li CaiXi'an Jiaotong University
1 EKF 算法回顾 Unscented Kalman Filter (UKF) (2) 时间修正（time update）： xˆ − k+1 = f(ˆxk, 0, k) (3) P − k+1 = FkPkF T k + GkQkG T k (4) Dr. Yuan-Li Cai 5/37 Xi’an Jiaotong University
Unscented Kalman Filter(UKF1EKF算法回顾(3）量测修正（measurementupdate）：(5)9k+1 = h(+1,k + 1)P1 = Hk+1P+H+I + Rk+1(6)Pi = P+H+1(7)Kk+1 = P(P1)-1(8)(9)k+1=+1+K+1(yk+1-9元+1)Pk+1 = P+ Kk+1P, K+1(10)6/37Dr. Yuan-Li CaiXi'an Jiaotong University
1 EKF 算法回顾 Unscented Kalman Filter (UKF) (3) 量测修正（measurement update）： yˆ − k+1 = h(ˆx − k+1, k + 1) (5) P yy k+1 = Hk+1P − k+1H T k+1 + Rk+1 (6) P xy k+1 = P − k+1H T k+1 (7) Kk+1 = P xy k+1(P yy k+1) −1 (8) xˆk+1 = ˆx − k+1 + Kk+1(yk+1 − yˆ − k+1) (9) Pk+1 = P − k+1 − Kk+1P yy k+1KT k+1 (10) Dr. Yuan-Li Cai 6/37 Xi’an Jiaotong University
UnscentedKalmanFilter(UKFEKF算法回顾1其中:Of(rk,Wk,k)Fk=arkWk=0Of(rk, wk,k)Gkawkk=k,wk=0Oh(rk+1,k +1)Hk+1=Ork+1l2k+1=x+1按简记符号，+1、9+1分别表示系统状态及量测的（一步）预测估计。7/37Dr. Yuan-Li CaiXi'an Jiaotong University
1 EKF 算法回顾 Unscented Kalman Filter (UKF) 其中： Fk = ∂f(xk, wk, k) ∂xk xk=ˆxk,wk=0 Gk = ∂f(xk, wk, k) ∂wk xk=ˆxk,wk=0 Hk+1 = ∂h(xk+1, k + 1) ∂xk+1 xk+1=ˆx − k+1 按简记符号，xˆ − k+1、yˆ − k+1 分别表示系统状态及量测的（一步）预测估计。 Dr. Yuan-Li Cai 7/37 Xi’an Jiaotong University
UT变换UnscentedKalman Filter (UKF22.UT 变换UT:UnscentedTransformation考虑非线性映射：y=h(r)(11)其中,rERn~N(,P),yERm。8/37Dr.Yuan-LiCaiXi'an JiaotongUniversity
2 UT 变换 Unscented Kalman Filter (UKF) 2. UT 变换 UT: Unscented Transformation 考虑非线性映射： y = h(x) (11) 其中，x ∈ Rn ∼ N(¯x, Px)，y ∈ Rm。 Dr. Yuan-Li Cai 8/37 Xi’an Jiaotong University
UT变换UnscentedKalman Filter(UKF2构造如下加权sigma点集K(12)=0XO=Zwo2n+k1(13)Xi=+(V(n+k)P),W=2=1.....n2（n+）1(14)Xi=-(V(n+)P)i-ni=n+1....2mWi2(n +k)其中，是可调参数。当为正态分布时，=3-n。9/37Dr. Yuan-Li CaiXian JiaotongUniversity
2 UT 变换 Unscented Kalman Filter (UKF) 构造如下加权 sigma 点集： χ0 = ¯x, w0 = κ n + κ , i = 0 (12) χi = ¯x + (p (n + κ)Px)i , wi = 1 2(n + κ) , i = 1, · · · , n (13) χi = ¯x − ( p (n + κ)Px)i−n, wi = 1 2(n + κ) , i = n + 1, · · · , 2n (14) 其中，κ 是可调参数。当 x 为正态分布时，κ = 3 − n。 Dr. Yuan-Li Cai 9/37 Xi’an Jiaotong University