三種卡爾曼濾波理論

1、精選優(yōu)質(zhì)文檔-傾情為你奉上經(jīng)典卡爾曼濾波動態(tài)系統(tǒng)的卡爾曼濾波數(shù)學模型包括狀態(tài)方程和觀測方程，對于線性系統(tǒng)，其離散形式為Xk= k,k-1Xk-1+ Gk-1Wk-1Lk= HkXk+ Vk其中，Xk為系統(tǒng)在tk時刻的n×1維狀態(tài)向量，k,k-1為系統(tǒng)從tk-1時刻到tk時刻的n×n維狀態(tài)轉(zhuǎn)移矩陣，Wk-1為tk-1時刻的r×1動態(tài)噪聲，Gk-1為tk-1時刻的n×r動態(tài)噪聲矩陣，Lk為系統(tǒng)在tk時刻的m×1維觀測向量，Hk為系統(tǒng)在tk時刻的m×n維觀測矩陣，Vk為系統(tǒng)在tk時刻的m×1維觀測噪聲。根據(jù)最小二乘原則，可推得卡爾

2、曼濾波遞推公式為：1）狀態(tài)向量一步預(yù)測值為 Xk,k-1=k,k-1 Xk-1,k-12）狀態(tài)向量一步預(yù)測值方差矩陣為Pk,k-1= k,k-1 Pk-1,k-1k,k-1T+Gk-1Qk-1Gk-1T 其中，Qk為動態(tài)噪聲方差矩陣。3）狀態(tài)向量估計值為Xk,k= Xk,k-1+JkLk- HkXk,k-14）狀態(tài)向量估計值方差矩陣為Pk,k= I- JkHkPk,k-1其中，Jk為濾波增益矩陣，具體形式如下Jk= Pk,k-1HkTHkPk,k-1HkT+ Rk-1Rk為觀測噪聲方差矩陣?；诜讲钛a償?shù)淖赃m應(yīng)卡爾曼濾波假定Wk和Vk是正態(tài)序列，X0是正態(tài)向量。則定義l步預(yù)測殘差是Vk+l=

3、Lk+l- Lk+l,k其中， Lk+l，Lk+l,k分別為第k+l期觀測值和它的最佳觀測值Lk+l,k= Hk+lk+l,kXk,k，則Vk+l為正態(tài)向量。Vk+l的方差陣Svv為：Svv= Hk+lk+l,kPkk+l,kTHk+lT+Rk+l+ i=1lHk+lk+l,k+iGk+l,k+i-1Qk+i-1Gk+l,k+i-1Tk+l,k+iTHk+lT記Hk+lk+l,k+iGk+l,k+i-1= Ak+l,k+i= ahjk+l,k+i，式中h=1,2,m；j=1,2,r。假定Qk+i-1在觀測時間段tk+1，tk+2，tk+N上為常值對角陣，即Qk+i-1= 11200rr2=Q并

4、記diagQ= 112,222,rr2T。根據(jù)EVk+lTVk+l=trVk+lVk+lT=trSvv，于是記Vk+lTVk+l=trSvv+ k+l其中，k+l為零均值隨機變量，l=1,2,N。令Ek+l= Vk+lTVk+l-trHk+lk+l,kPkk+l,kTHk+lT-trRk+l又記E= Ek+l,Ek+NT= k+l,k+NTA= Ak+l,Ak+NT則有E=AdiagQ+ 上式就是關(guān)于diagQ的線性方程組。當Nr時，有唯一解。記diagQ的LS估計為diagQ= ATA-1ATE根據(jù)上面各式求得任意長度時間段上的Q，把它作為動態(tài)噪聲協(xié)方差陣的實時估計?；跇O大驗后估計的自適應(yīng)

5、卡爾曼濾波動態(tài)噪聲向量Wk、觀測噪聲向量Vk是互相獨立的正態(tài)白噪聲向量，滿足EWk=q,EVk=r，若噪聲的統(tǒng)計特性已知，可由極大驗后估計原理得到自適應(yīng)卡爾曼濾波的遞推方程為：1）狀態(tài)向量一步預(yù)測值為 Xk,k-1=k,k-1 Xk-1,k-1+ qk-12）狀態(tài)向量一步預(yù)測值方差矩陣為Pk,k-1= k,k-1 Pk-1,k-1k,k-1T+Qk-1 3）狀態(tài)向量估計值為Xk,k= Xk,k-1+JkLk- HkXk,k-14）狀態(tài)向量估計值方差矩陣為Pk,k= I- JkHkPk,k-1其中，Jk為濾波增益矩陣，具體形式如下Jk= Pk,k-1HkTHkPk,k-1HkT+ Rk-1Rk為觀測噪聲方差矩陣。若噪聲均值向量q,r和協(xié)方差陣Qk，Rk未知時，可以利用濾波估值Xj,j和預(yù)報值Xj,j-1近似代替計算較復(fù)雜的平滑估值Xk,j和Xk,j-1。由于地鐵站口基坑的開挖施工，對