《随机系统的滤波与控制》研究生课程教学课件（讲稿）非线性系统滤波算法——Unscented Kalman Filter（UKF）

Unscented Kalman Filter (UKF)Dr.Yuan-LiCaiSpring·2022

pink Unscented Kalman Filter (UKF) Dr. Yuan-Li Cai Spring • 2022

0.Outline13EKF算法回顾2UT变换310SUT 变换14UKF算法5UKF简化算法/186仿真算例23

0. Outline 1 EKF 算法回顾 / 3 2 UT 变换 / 7 3 SUT 变换 / 10 4 UKF 算法 / 14 5 UKF 简化算法 / 18 6 仿真算例 / 23

OUTLINEUnscentedKalmanFilter(UKF/ 28MATLAB code2/37Dr. Yuan-Li CaiXi'an Jiaotong University

OUTLINE Unscented Kalman Filter (UKF) 7 MATLAB code / 28 Dr. Yuan-Li Cai 2/37 Xi’an Jiaotong University

UnscentedKalmanFilter(UKFOUTLINE考虑如下非线性系统(1)ak+1 = f(ck,Wk,k)(2)yk = h(Ck, k) + Uk其中,kERn,kERm,wER,ERm。此外,E[w]=0,E[]=0E[wkwf]=Qkokj,E[uruf]=Rokj,E[wkuf] =0,Vk,j。3/37Dr. Yuan-Li CaiXi'an Jiaotong University

OUTLINE Unscented Kalman Filter (UKF) 考虑如下非线性系统： xk+1 = f(xk, wk, k) (1) yk = h(xk, k) + vk (2) 其中，xk ∈ Rn，yk ∈ Rm，wk ∈ Rq，vk ∈ Rm。此外，E[wk] = 0，E[vk] = 0， E[wkw T j ] = Qkδkj，E[vkv T j ] = Rkδkj，E[wkv T j ] = 0，∀k, j。 Dr. Yuan-Li Cai 3/37 Xi’an Jiaotong University

UnscentedKalman Filter (UKFEKF算法回顾11.EKF算法回顾简记，Pe= PkJk=k-1,P=Pk-1Po = var(ro)(1)初始化：o=Ero，4/37Dr. Yuan-Li CaiXi'an Jiaotong University

1 EKF 算法回顾 Unscented Kalman Filter (UKF) 1. EKF 算法回顾 简记 xˆk = ˆxk|k, Pk = Pk|k xˆ − k = ˆxk|k−1, P − k = Pk|k−1 (1) 初始化：xˆ0 = Ex0, P0 = var(x0) Dr. Yuan-Li Cai 4/37 Xi’an Jiaotong University

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