『连续时间系统滤波与平滑算法]Prof.Yuan-Li CaiSpring2023
pink 『连续时间系统滤波与平滑算法』 Prof. Yuan-Li Cai Spring 2023
0.Outline2线性系统卡尔曼滤波算法非线性系统滤波算法/283/37连续时间系统平滑算法
0. Outline 1 线性系统卡尔曼滤波算法 / 2 2 非线性系统滤波算法 / 28 3 连续时间系统平滑算法 / 37
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法1.线性系统卡尔曼滤波算法1.1从离散时间系统到连续时间系统考虑连续时间动态系统X(t) = F(t)X(t) +G(t)W(t)(1)(2)Y(t) = H(t)X(t) +V(t)2/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 1. 线性系统卡尔曼滤波算法 1.1 从离散时间系统到连续时间系统 考虑连续时间动态系统 X˙ (t) = F(t)X(t) + G(t)W(t) (1) Y (t) = H(t)X(t) + V (t) (2) Prof. Yuan-Li Cai 2/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法其中，W(t)～(0,Q(t))与V(t)～(O,R(t))是互不相关的白噪声过程，它们与初始状态X(0)～(Xo,Po)不相关。另外，设Q(t)≥0,R(t)>0,它们分别是系统过程噪声和量测噪声的功率谱密度矩阵。由线性系统理论可知(3)X(t) = Φ(t,to)X(to) +Φ(t, T)G()W()dT其中(t,to) = F(t)Φ(t,to),Φ(to,to) = I(4)称为系统转移矩阵。3/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 其中，W(t) ∼ (0, Q(t)) 与 V (t) ∼ (0, R(t)) 是互不相关的白噪声过程，它 们与初始状态 X(0) ∼ (X¯ 0, P0) 不相关。另外，设 Q(t) ≥ 0, R(t) > 0，它 们分别是系统过程噪声和量测噪声的功率谱密度矩阵。 由线性系统理论可知 X(t) = Φ(t, t0)X(t0) + Z t t0 Φ(t, τ )G(τ )W(τ )dτ (3) 其中 Φ(˙ t, t0) = F(t)Φ(t, t0), Φ(t0, t0) = I (4) 称为系统转移矩阵。 Prof. Yuan-Li Cai 3/45 Xi’an Jiaotong University
【连续时间系统滤波与平滑算法]线性系统卡尔曼滤波算法由此可以将（1)、（2）离散化为(5)X(k +1) =Φ(k + 1, k)X(k) +T(k)WD(k)(6)Y(k +1)= H(k+1)X(k +1) + Vp(k+1)式中，X(k）=X(tk)：ts=k△t，△t为采样时间间隔。下面研究△t很小时，（1)、(2）与(5)、（6）之间的关系。首先，由（4不难发现(7)Φ(t+△t,t) ~ I +F(t)△t如果t=k△t，近似地（忽略△t二阶以上小量）(8)Φ( + 1, k) ~ I +F(k)△t4/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 由此可以将 (1)、(2) 离散化为 X(k + 1) = Φ(k + 1, k)X(k) + Γ(k)WD(k) (5) Y (k + 1) = H(k + 1)X(k + 1) + VD(k + 1) (6) 式中，X(k) = X(tk)，tk = k∆t，∆t 为采样时间间隔。 下面研究 ∆t 很小时，(1)、(2) 与 (5)、(6) 之间的关系。首先，由 (4) 不难发现 Φ(t + ∆t, t) ≈ I + F(t)∆t (7) 如果 t = k∆t，近似地（忽略 ∆t 二阶以上小量） Φ(k + 1, k) ≈ I + F(k)∆t (8) Prof. Yuan-Li Cai 4/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法而由(3)可得t+At(9)X(t + △t) = Φ(t + △t,t)X(t) +Φ(t+△t.T)G(T)W(T)dT即(5)．其中t+△tr(k)Wp(k) =(t + △t, T)G()W(T)di为了保证离散后系统的过程噪声和原系统噪声具有相同的特性，要求EWD(K)=0(10)5/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 而由 (3) 可得 X(t + ∆t) = Φ(t + ∆t, t)X(t) + Z t+∆t t Φ(t + ∆t, τ )G(τ )W(τ )dτ (9) 即 (5)，其中 Γ(k)WD(k) = Z t+∆t t Φ(t + ∆t, τ )G(τ )W(τ )dτ 为了保证离散后系统的过程噪声和原系统噪声具有相同的特性，要求 EWD(k) = 0 (10) Prof. Yuan-Li Cai 5/45 Xi’an Jiaotong University
【连续时间系统滤波与平滑算法]线性系统卡尔曼滤波算法而且E[(k)WD(k)][T(k)WD(k))T = (k)QD(k)rT (k)+Att+△t(t + △t, T)G()W()WT(A)GT(A)T(t + △t, A)dTd)E+Att+△t(t + △t, T)G(T)Q(T)S(T - )GT()T(t + △t, A)dTd)t+At(t + △t, A)G()Q(A)GT()T(t + △t, )d)近似地（忽略△t二阶以上小量）(k)QD(k)rT(k) = G(t)Q(t)GT(t)△t(11)6/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 而且 E[Γ(k)WD(k)][Γ(k)WD(k)]T = Γ(k)QD(k)ΓT (k) = E Z t+∆t t Z t+∆t t Φ(t + ∆t, τ )G(τ )W(τ )WT (λ)G T (λ)ΦT (t + ∆t, λ)dτ dλ = Z t+∆t t Z t+∆t t Φ(t + ∆t, τ )G(τ )Q(τ )δ(τ − λ)G T (λ)ΦT (t + ∆t, λ)dτ dλ = Z t+∆t t Φ(t + ∆t, λ)G(λ)Q(λ)G T (λ)ΦT (t + ∆t, λ)dλ 近似地（忽略 ∆t 二阶以上小量） Γ(k)QD(k)ΓT (k) = G(t)Q(t)G T (t)∆t (11) Prof. Yuan-Li Cai 6/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法如果取(12)T(k) = G(k△t)那么(13)QD(k) = Q(t)At同样地，为了保证离散后系统的量测噪声和原量测噪声具有相同的特性，要求(14)EVp(k) = 0而且t+At+△tEVD(k +T)V(k)dT = EV(t ++)VT(t)dT7/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 如果取 Γ(k) = G(k∆t) (12) 那么 QD(k) = Q(t)∆t (13) 同样地，为了保证离散后系统的量测噪声和原量测噪声具有相同的特 性，要求 EVD(k) = 0 (14) 而且 E Z t+∆t t VD(k + τ )V T D (k)dτ = E Z t+∆t t V (t + τ )V T (t)dτ Prof. Yuan-Li Cai 7/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法即(15)Rp(k) =R(t)从量纲分析不难看出（13）与（15）的合理性。8/45Prof. Yuan-Li CaiXi'an JiaotongUniversity
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 即 RD(k) = 1 ∆t R(t) (15) 从量纲分析不难看出 (13) 与 (15) 的合理性。 Prof. Yuan-Li Cai 8/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法7Q(k) = Q(t)At1Ro(k)=云R()注:(1)EW(t)WT()=Q(t)s(t-T）的量纲是：(unit)2/s2,(t)的量纲是1/s:因此Q(t）的量纲是：(unit)/s.而Qp(k）的量纲是：(unit)2(2)EV(t)VT()=R(t)s(t-T)的量纲是：(unit)2, 因此R(t)的量纲是：（unit)?.s.而Rp(k）的量纲是：(unit)2.9/45Prof.Yuan-LiCaiXian JiaotongUniversity
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 QD(k) = Q(t)∆t RD(k) = 1 ∆t R(t) 注： (1) EW(t)WT (τ ) = Q(t)δ(t − τ ) 的量纲是:(unit) 2/s 2 , δ(t) 的量纲是： 1/s; 因此 Q(t) 的量纲是:(unit) 2/s. 而 QD(k) 的量纲是:(unit) 2 . (2) EV (t)V T (τ ) = R(t)δ(t − τ ) 的量纲是：(unit) 2 , 因此 R(t) 的量纲 是：(unit) 2 · s. 而 RD(k) 的量纲是：(unit) 2 . Prof. Yuan-Li Cai 9/45 Xi’an Jiaotong University