[發明專利]處理飛行時間測距數據的自適應卡爾曼濾波的方法

有效

• 申請號: 201410557259.6
• 申請日: 2014-10-20
• 公開（公告）號: CN104316905A
• 公開（公告）日: 2015-01-28
• 發明（設計）人: 李奇越;吳忠;王建平;黎潔;孫偉
• 申請（專利權）人: 合肥工業大學
• 主分類號: G01S5/02
• 分類號: G01S5/02
• 代理公司: 合肥和瑞知識產權代理事務所(普通合伙) 34118
• 代理人: 任崗生

1.一種處理飛行時間測距數據的自適應卡爾曼濾波的方法，包括對移動節點和錨節點之間飛行時間測距數據的卡爾曼濾波處理，其特征在于主要步驟如下：

步驟1，建立目標狀態方程和有色噪聲量測方程

x_k+1＝Ax_k+Bα_k，

v_k＝Cx_k+β_k，

β_k＝N_k-1β_k-1+γ_k，

其中，

A為狀態矩陣，B為誤差系數矩陣，C為測量矩陣，

x_k為目標在k(k＝1，2，3，……n)時刻的二維狀態向量，其中的r_k為待估計的飛行時間值、為其一階導數，

α_k為系統噪聲，是一個高斯白噪聲序列，協方差用Q表示，

v_k為一維測量向量，表示k時刻的測距數據，

β_k為k時刻的測量誤差，協方差矩陣為R_k，

N_k-1為自回歸系數，

γ_k為高斯白噪聲序列；

步驟2，根據原始卡爾曼濾波算法，由k-1時刻的濾波結果得到目標的k時刻的預測值以及k時刻的新息值z_k，計算公式為

x^k/k-1=Ax^k-1/k-1,]]>

zk=vk-Cx^k/k-1,]]>

其中，為k-1時刻的濾波結果；

步驟3，求出測量噪聲協方差k時刻的估計值

先計算參數S_k，其由下式來估計得到

S^k=1k-1Σi=1k(zi-z‾i)(zi-z‾i)T,]]>

其中，

是前i次新息值的均值，計算為

z‾i=1iΣj=1izj,]]>

再按照下兩式得出測量噪聲協方差在k時刻的估計值

P_k，k-1＝AP_k-1，k-1A^T+BQB^T，

R^k=S^k-CPk,k-1CT,]]>

其中，P_k，k-1是k時刻預測誤差方差陣，P_k-1，k-1是k-1時刻估計誤差方差陣；

步驟4，計算N_k-1

根據β_k-1和γ_k獨立，且有協方差的關系得出N_k-1的計算式為

Nk-1=(R^k-var(γk))/R^k-1,]]>

其中，var(γ_k)是γ_k的方差；

步驟5，計算k時刻的門限值m_k

計算前k次測距數據的方差取門限值為方差值，即

步驟6，更新卡爾曼濾波增益

將k時刻的新息值與門限值進行比較，若新息值大于門限值，則將卡爾曼增益置為零，否則按以下兩步計算濾波增益G_k，

H_k-1＝[CA-N_k-1C]，

Gk=(APk-1,k-1Hk-1T+BQBTCT)·(Hk-1Pk-1,k-1Hk-1T+CBQBTCT+R^k-1)-1;]]>

步驟7，更新估計誤差方差陣

由P0=([var(x0)]-1+CTR~0-1C)-1]]>得出估計誤差方差陣的初值，

其中，

x₀取為第一個測距數據，為誤差協方差初值，取作前幾次試驗測距數據的方差，

估計誤差方差陣更新表達式為

P_k，k＝(A-G_kH_k-1)·P_k-1，k-1A^T+(I-G_kC)BQB^T；

步驟8，根據k時刻濾波輸出方程輸出k時刻的濾波結果，濾波方程為

x^k/k=Ax^k-1/k-1+Gk·(vk-Nk-1vk-1-Hk-1x^k-1/k-1),]]>

如此循環往復，輸出1到n時刻的濾波結果，得到對飛行時間測距數據的處理結果。