1.一種變分貝葉斯的非線性卡爾曼濾波器的設計方法,其特征在于包括下述步驟:
第一步:根據貝葉斯濾波理論獲取非線性系統狀態的估計值,并作為迭代初始值;詳細步驟如下:
步驟1.1:根據先驗信息,狀態演化信息和前一時刻的量測,計算狀態預測,量測預測,狀態預測誤差協方差,量測預測誤差協方差及狀態預測與量測預測的交互協方差;
在迭代優化的過程中,非線性貝葉斯濾波器作為內核濾波執行器為迭代優化提供初始值,非線性貝葉斯濾波器的方程如下:
xk|k-1=fk(xk-1) (1)
zk|k-1=h(xk|k-1) (2)
Pk|k-1=E[(xk-xk|k-1)(xk-xk|k-1)Τ] (3)
Pk,xz=E[(xk-xk|k-1)(zk-zk|k-1)Τ] (4)
Pk,zz=E[(zk-zk|k-1)(zk-zk|k-1)Τ] (5)
其中,xk-1表示k-1時刻的狀態值,xk|k-1和zk|k-1分別表示k-1時刻到k時刻的狀態預測值和量測預測值,fk(g)和hk(g)分別表示狀態轉移函數和量測函數,Pk|k-1,Pk,xz和Pk,zz分別表示k-1時刻到k時刻的狀態預測誤差協方差,狀態預測與量測預測之間的交互誤差協方差,量測預測誤差協方差,E[g]表示求期望;
步驟1.2:根據傳感器當前時刻傳感器量測信息zk和Kalman濾波的更新公式,計算濾波增益Kk,狀態估計xk|k和狀態估計誤差協方差Pk|k:
Kk=Pk,xz(Pk,zz)-1 (6)
xk|k=xk|k-1+Kk(zk-zk|k-1) (7)
Pk|k=Pk|k-1-KkPk,zzKkΤ (8)
第二步:根據變分貝葉斯框架和KL散度建立迭代優化函數詳細步驟如下:
步驟2.1:根據量測和狀態之間的貝葉斯公式和Jensen不等式定理建立變分置信下界,并將置信下界作為目標函數;
變分貝葉斯的置信下界為目標函數矩陣在以xk為隱變量和zk為觀測變量的狀態空間模型中,k時刻的邊緣概率密度函數表示為:
其中,p(zk,xk)=p(zk|xk)p(xk|xk-1)為k時刻的聯合分布,p(xk|xk-1)為從k-1到k時刻是狀態轉移概率,定義ψk@(xk|k,Pk|k),其中,xk|k和Pk|k分別表示待估計變量的狀態估計及其誤差協方差,公式(9)滿足Jensen不等式定理,因此公式(9)可化為公式(10):
置信優化的下界定義為:
其中,是的簡化形式,p(xk)和q(xk|ψk)分別表示狀態的真實分布和其變分分布,p(zk|xk)表示條件似然函數;
最大化L(ψk)即實現將積分問題到優化問題的轉化:
步驟2.2:引入KL散度作為約束項
其中,i表示迭代次數,βi表示懲罰因子,表示變分分布q(xk|ψk)和變分分布之間的KL散度;
在高斯假設條件下,表示xk服從均值為xk|k方差為Pk|k的高斯分布N(xk|xk|k,Pk|k),假設k-1時刻的狀態估計后驗概率密度函數已知p(xk)~N(xk|xk|k-1,Pk|k-1),因此公式(13)中第二項可進一步表示為:
第三步:在變分分布條件下線性化迭代優化函數詳細步驟如下:
步驟3.1:在處線性化量測矩陣hk(xk),以計算似然函數對數期望Eq[logp(zk|xk)]的一階梯度和二階梯度,進而根據泰勒公式線性化對數似然函數的期望Eq[logp(zk|xk)],詳細步驟如下:
在高斯假設條件下對優化函數矩陣進行線性化,定義參數和分別為Eq[logp(zk|xk|k)]的一階梯度和二階梯度,則:
其中,和分別表示計算關于xk|k和Pk|k的梯度;
根據Bonnet定理和Price定理,一階梯度和二階梯度可分別表示為:
其中,表示量測噪聲方差Rk的逆,公式(16)中表示迭代過程中信息的期望,采用線性化近似和采樣近似兩種近似方法:
1)當采用線性化近似的方法,則有:
由公式(18)得到公式(19):
將公式(19)帶入公式(13)中使用;
2)若采用采樣近似的方法,則有:
其中,N表示狀態估計樣本個數;
由公式(20)得到公式(21):
將公式(21)帶入公式(13)中;
步驟3.2:在變分分布服從高斯分布的條件下,計算狀態預測的概率密度函數和第i+1次變分分布之間的KL散度:
假設維數為D的變量ξ1服從均值為μ1方差為C1的高斯分布N(ξ1|μ1,C1),相同維數變量ξ2服從均值為μ2方差為C2的高斯分布N(ξ2|μ2,C2),則兩者的KL散度可化為如下形式:
因此,第i次迭代的變分分布和第i+1次迭代變分分布之間的KL散度為:
Dx表示服從高斯分布的變量x的維數;
同理,公式(14)可化為:
步驟3.3:結合公式(13)-(15)和公式(23)-(24),優化函數矩陣可化為:
第四步:將線性化后的迭代優化函數求偏導和計算狀態估計和估計誤差協方差,并由此設計卡爾曼濾波器,詳細計算步驟如下:
步驟4.1:將線性化后的迭代優化函數在處求偏導,分別計算Φ(xk|k,Pk|k)對xk|k和Pk|k的偏導矩陣:
步驟4.2:令偏導等于0,以獲取第i+1次迭代的狀態估計
其中bi=1/(1+βi);
步驟4.3:將線性化后的迭代優化函數在處求偏導;
步驟4.4:令偏導等于0,以獲取第i+1次迭代的狀態估計誤差協方差
第五步:第四步所設計的卡爾曼濾波器的自適應迭代終止條件如下:
其中,ε為迭代終止門限。