连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法1.2卡尔曼滤波算法对于离散化后获得的系统状态方程（5）和量测方程（6），我们可以直接运用前面建立的离散时间系统卡尔曼滤波算法，从而得X(k +1k) = Φ(k + 1,k)X(Kk)(16)(17)Pp( + 1|) = (k +1, k)Pp(K|k)T ( + 1, k) +T(k)QD(k)T (k)KD(k + 1) = PD(k + 1|k)HT (k + 1)[H(k + 1)Pp(k + 1|k)HT (k + 1) + RD(k + 1)-1= P(k + 1k + 1)HT (k + 1)RD'(k + 1)(18)X( +1|k + 1) = X(k + 1|k) + Kp(k + 1)[Y(k + 1) - H(k + 1)X(k +1|k)(19)(20)Pp(k + 1|k +1) =[I - KD(k +1)H(k + 1)]Pp(k + 1|k)X(0|0) = Xo, Pp(0|0) = Po(21)10/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 1.2 卡尔曼滤波算法 对于离散化后获得的系统状态方程 (5) 和量测方程 (6)，我们可以直接 运用前面建立的离散时间系统卡尔曼滤波算法，从而得 Xˆ(k + 1|k) = Φ(k + 1, k)Xˆ(k|k) (16) PD(k + 1|k) = Φ(k + 1, k)PD(k|k)ΦT (k + 1, k) + Γ(k)QD(k)ΓT (k) (17) KD(k + 1) = PD(k + 1|k)H T (k + 1)[H(k + 1)PD(k + 1|k)H T (k + 1) + RD(k + 1)]−1 = PD(k + 1|k + 1)H T (k + 1)R −1 D (k + 1) (18) Xˆ(k + 1|k + 1) = Xˆ(k + 1|k) + KD(k + 1)[Y (k + 1) − H(k + 1)Xˆ(k + 1|k)] (19) PD(k + 1|k + 1) = [I − KD(k + 1)H(k + 1)]PD(k + 1|k) (20) Xˆ(0|0) = X¯0, PD(0|0) = P0 (21) Prof. Yuan-Li Cai 10/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法考虑到X(t+△tlt + At) - X(tit)dx(tit)limdtAt△t-→0Φ(t + △t,t)X(tlt) - X(tlt) )= lim△t△t-→0Kp(t+△t,t)[Y(t + △t) - H(t + At)X(t + △tlt)] + limAtAt→0=F(t)X(t/t) + Pp(t)t)HT(t)R-1(t)[Y(t) -H(t)X(tt)如果令(22)K(t) = Pp(t|t)HT (t)R-I(t)11/45Prof. Yuan-Li CaiXian Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 考虑到 dXˆ(t|t) dt = lim ∆t→0 Xˆ(t + ∆t|t + ∆t) − Xˆ(t|t) ∆t = lim ∆t→0 ( Φ(t + ∆t, t)Xˆ(t|t) − Xˆ(t|t) ∆t ) + + lim ∆t→0 KD(t + ∆t, t) ∆t [Y (t + ∆t) − H(t + ∆t)Xˆ(t + ∆t|t)] = F(t)Xˆ(t|t) + PD(t|t)H T (t)R −1 (t)[Y (t) − H(t)Xˆ(t|t)] 如果令 K(t) = PD(t|t)H T (t)R −1 (t) (22) Prof. Yuan-Li Cai 11/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法那么dx(tit)= F(t)X(tt) + K(t)[Y(t) - H(t)X(t)t))(23)dt另外，由(20)、（17)、（13）可知Pp(t+△tt+△t)=[I-Kp(t+△t)H(t+△t)]Pp(t+△tt)=[I - KD(t + △t)H(t +△t)][(t +△t,t)Pp(t}t)T (t +△t,t) +G(t)Q(t)GT (t)At)代入（11)，并注意到Kp(t+△t,t)K(t +△t) =△t△t→012/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 那么 dXˆ(t|t) dt = F(t)Xˆ(t|t) + K(t)[Y (t) − H(t)Xˆ(t|t)] (23) 另外，由 (20)、(17)、(13) 可知 PD(t + ∆t|t + ∆t) = [I − KD(t + ∆t)H(t + ∆t)]PD(t + ∆t|t) = [I − KD(t + ∆t)H(t + ∆t)][Φ(t + ∆t, t)PD(t|t)ΦT (t + ∆t, t) + G(t)Q(t)G T (t)∆t] 代入 (11)，并注意到 K(t + ∆t) = KD(t + ∆t, t) ∆t ∆t→0 Prof. Yuan-Li Cai 12/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法可导出Pp(t+△tt+△t)=Pp(tlt)+F(t)Pp(tt)At+Pp(tlt)FT(t)△t-K(t +△t)H(t +△t)Pp(tlt)△t +G(t)Q(t)GT(t)At +o(△t)2所以得dP() = F(t) Po() + Pp(1) F (t - (t)(1) P(1) + ()Q()C (t)dt(24)如果令P(t)=Pp(tt)，X(t)=X(tt)，我们便由 (23)、(22)、(24)得到了连续时间系统的卡尔曼滤波算法，如表1所示。13/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 可导出 PD(t + ∆t|t + ∆t) = PD(t|t) + F(t)PD(t|t)∆t + PD(t|t)F T (t)∆t −K(t + ∆t)H(t + ∆t)PD(t|t)∆t + G(t)Q(t)G T (t)∆t + o(∆t) 2 所以得 dPD(t|t) dt = F(t)PD(t|t) + PD(t|t)F T (t − K(t)H(t)PD(t|t) + G(t)Q(t)G T (t) (24) 如果令 P(t) = PD(t|t)，Xˆ(t) = Xˆ(t|t)，我们便由 (23)、(22)、(24) 得 到了连续时间系统的卡尔曼滤波算法，如表1 所示。 Prof. Yuan-Li Cai 13/45 Xi’an Jiaotong University
连续时间系统滤波与平滑算法线性系统卡尔曼滤波算法表1：连续时间系统卡尔曼滤波算法（1）状态方程X(t) = F(t)X(t) +G(t)W(t)量测方程Y(t) =H(t)X(t) +V(t)x(0) = Ex(0), P(0) = var[X(0)滤波初值滤波增益K(t) = P(t)HT(t)R-1(t)dx( = F(t)X(t) + K(t)[Y(t) - H(t)X(t)状态估计dtP=FP+PFT-PHTR-1HP+GQGT协方差阵14/45Prof. Yuan-Li CaiXi'an Jiaotong University
1 线性系统卡尔曼滤波算法 『连续时间系统滤波与平滑算法』 表 1: 连续时间系统卡尔曼滤波算法 (1) 状态方程 X˙ (t) = F(t)X(t) + G(t)W(t) 量测方程 Y (t) = H(t)X(t) + V (t) 滤波初值 Xˆ(0) = EX(0), P(0) = var[X(0)] 滤波增益 K(t) = P(t)HT (t)R−1 (t) 状态估计 dXˆ(t) dt = F(t)Xˆ(t) + K(t)[Y (t) − H(t)Xˆ(t)] 协方差阵 P˙ = F P + P F T − PHT R−1HP + GQGT Prof. Yuan-Li Cai 14/45 Xi’an Jiaotong University