线性最优滤波小结线性最优滤波小结蔡远利XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 1 线性最优滤波小结 蔡远利
2线性最优滤波小结卡尔曼滤波算法可以从不同的角度审视和推导，通常可以更加方便地建立滤波公式，同时也有利于快速记忆。首先，对于一步预测，即时间修正，不涉及新的量测信息，相对简单。这里不再复述标准假设。设已知(αk，Pk），由系统的状态方程(1)Xk+1= Φk+1,kXk+k+1,kuk+TkWkXUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 2 卡尔曼滤波算法可以从不同的角度审视和推导，通常可以更加 方便地建立滤波公式，同时也有利于快速记忆。首先，对于一步 预测，即时间修正，不涉及新的量测信息，相对简单。 这里不再复述标准假设。 设已知(𝑥̂𝑘, 𝑃𝑘) ，由系统的状态方程 𝑥𝑘+1 = Φ𝑘+1,𝑘𝑥𝑘 + Ψ𝑘+1,𝑘𝑢𝑘 + Γ𝑘𝑤𝑘 (1)
3线性最优滤波小结可知Xk+1]k = E[xk+1/Yk] = Φk+1,kE[x/Yk] + Y+1,kk(2)=Φk+1,kXk+Yk+1,kuk由(1)和(2)式可导出(3)Pk+1|k = Φk+1,kPkΦT+1,k + IiQkrT(2)和(3)便构成了最优一步预测公式（时间修正）。XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 3 可知 𝑥̂𝑘+1|𝑘 = 𝐸[𝑥𝑘+1|𝒀1 𝑘 ] = Φ𝑘+1,𝑘𝐸[𝑥𝑘|𝒀1 𝑘 ] + Ψ𝑘+1,𝑘𝑢𝑘 = Φ𝑘+1,𝑘𝑥̂𝑘 + Ψ𝑘+1,𝑘𝑢𝑘 (2) 由(1)和(2)式可导出 𝑃𝑘+1|𝑘 = Φ𝑘+1,𝑘𝑃𝑘Φ𝑘+1,𝑘 T + Γ𝑘𝑄𝑘Γ𝑘 𝑇 (3) (2)和(3)便构成了最优一步预测公式（时间修正）
4线性最优滤波小结1.融合估计与KF当获得k+1次量测，即(4)Vk+1 ~ (0,Rk+1)Yk+1=Hk+1Xk+1+Vk+1,可以建立此时的一个最优估计(最小二乘）(+1=(H+1R1Hk+1)-1H+1R+1yk+1(5)(P+1 = (Ht+1Rk+1Hk+1)-1XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 4 1. 融合估计与 KF 当获得𝑘 + 1次量测，即 𝑦𝑘+1 = 𝐻𝑘+1𝑥𝑘+1 + 𝑣𝑘+1 , 𝑣𝑘+1 ∼ (0, 𝑅𝑘+1 ) (4) 可以建立此时的一个最优估计(最小二乘) { 𝑥̂𝑘+1 𝑎 = (𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝐻𝑘+1 ) −1𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝑦𝑘+1 𝑃𝑘+1 𝑎 = (𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝐻𝑘+1 ) −1 (5)
5线性最优滤波小结将(x+1,P+1)与(Xk+1/k,Pk+1/k)进行融合，则得k+1时刻状态的最优估计如下：(Xk+1=Pk+1(Pk+1kXk+1/k+H+1Rk+1yk+1)(6)(Pk+1=(Pk+1/k+H+1Rk+1Hk+1)-1上式可以变换为其他等价形式。考虑到卡尔曼滤波的结构为XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 5 将(𝑥̂𝑘+1 𝑎 , 𝑃𝑘+1 𝑎 )与(𝑥̂𝑘+1|𝑘, 𝑃𝑘+1|𝑘)进行融合，则得𝑘 + 1时刻状态 的最优估计如下： { 𝑥̂𝑘+1 = 𝑃𝑘+1(𝑃𝑘+1|𝑘 −1 𝑥̂𝑘+1|𝑘 + 𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝑦𝑘+1) 𝑃𝑘+1 = (𝑃𝑘+1|𝑘 −1 + 𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝐻𝑘+1) −1 (6) 上式可以变换为其他等价形式。 考虑到卡尔曼滤波的结构为
6线性最优滤波小结Xk+1=+1/k +Kk+1(y+1-H+1+1/k)(7)= (I - Kk+1Hk+1)Xk+1/k + Kk+1Yk+1比较(6)与(7)，可知Kk+1 = Pk+1H+1Rk+1(8)(9)Pk+1 = (I - Kk+1Hk+1)Pk+1/k(7)、(8)、(9)是常见的卡尔曼滤波公式。XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 6 𝑥̂𝑘+1 = 𝑥̂𝑘+1|𝑘 + 𝐾𝑘+1(𝑦𝑘+1 − 𝐻𝑘+1𝑥̂𝑘+1|𝑘) = (𝐼 − 𝐾𝑘+1𝐻𝑘+1 )𝑥̂𝑘+1|𝑘 + 𝐾𝑘+1𝑦𝑘+1 (7) 比较(6)与(7)，可知 𝐾𝑘+1 = 𝑃𝑘+1𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 (8) 𝑃𝑘+1 = (𝐼 − 𝐾𝑘+1𝐻𝑘+1 )𝑃𝑘+1|𝑘 (9) (7)、(8)、(9)是常见的卡尔曼滤波公式
线性最优滤波小结2.最小二乘与KF在k+1时刻，我们有(10)Xk+1/k=Xk+1+Ek+1Ek+1 ~ (0, Pk+1|k)(11)Vk+1 ~ (0, Rk+1)Yk+1=Hk+1Xk+1+Vk+1将(10)也视为量测方程之一，于是XUTUPROF.YUAN-LI CAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 7 2. 最小二乘与 KF 在𝑘 + 1时刻，我们有 𝑥̂𝑘+1|𝑘 = 𝑥𝑘+1 + 𝜖𝑘+1 , 𝜖𝑘+1 ∼ (0, 𝑃𝑘+1|𝑘) (10) 𝑦𝑘+1 = 𝐻𝑘+1𝑥𝑘+1 + 𝑣𝑘+1 , 𝑣𝑘+1 ∼ (0, 𝑅𝑘+1 ) (11) 将(10)也视为量测方程之一，于是
8线性最优滤波小结Ek+1HM7[Pk+1]k00Pk+1/kEVVTdef W-W0Rk+1Rk+]0由加权最小二乘估计可得Xk+1/kXk+1 = Xk+1/k+1 = (HTWH)-1HTИYk+1Pk+1 = (HTWH)-1XUTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 8 ℋ = [ 𝐼𝑛×𝑛 𝐻𝑘+1 ], 𝒱 = [ 𝜖𝑘+1 𝑣𝑘+1 ] 𝐸𝒱𝒱 𝑇 = [ 𝑃𝑘+1|𝑘 0 0 𝑅𝑘+1 ] ≝ 𝒲−1 , ⇒ 𝒲 = [ 𝑃𝑘+1|𝑘 −1 0 0 𝑅𝑘+1 −1 ] 由加权最小二乘估计可得 𝑥̂𝑘+1 = 𝑥̂𝑘+1|𝑘+1 = (ℋ𝑇𝒲ℋ) −1ℋ𝑇𝒲 [ 𝑥̂𝑘+1|𝑘 𝑦𝑘+1 ] 𝑃𝑘+1 = (ℋ𝑇𝒲ℋ) −1
9线性最优滤波小结其中-10Pk+1|k[Pk+1]/kHk+1Rk+1]HTW =[InxnHRk+10InxnHTWH =[Pk+1]k H+1R+1]P+1/k + H+1Rk+1Hk+1Hk+1因此XUTUPROF.YUAN-LI CAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 9 其中 ℋ𝑇𝒲 = [𝐼𝑛×𝑛 𝐻𝑘+1 𝑇 ][ 𝑃𝑘+1|𝑘 −1 0 0 𝑅𝑘+1 −1 ] = [𝑃𝑘+1|𝑘 −1 𝐻𝑘+1𝑅𝑘+1 −1 ] ℋ𝑇𝒲ℋ = [𝑃𝑘+1|𝑘 −1 𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 ][ 𝐼𝑛×𝑛 𝐻𝑘+1 ] = 𝑃𝑘+1|𝑘 −1 + 𝐻𝑘+1 𝑇 𝑅𝑘+1 −1 𝐻𝑘+1 因此
10线性最优滤波小结(12)Xk+1 =Pk+1(Pk+1)kXk+1/k + Hk+1Rk+1yk+1)其他讨论同上小节，不再赘述。XJTUPROF.YUAN-LICAI
XJTU PROF. YUAN-LI CAI 线性最优滤波小结 10 𝑥̂𝑘+1 = 𝑃𝑘+1 (𝑃𝑘+1|𝑘 −1 𝑥̂𝑘+1|𝑘 + 𝐻𝑘+1𝑅𝑘+1 −1 𝑦𝑘+1 ) (12) 其他讨论同上小节，不再赘述