[發(fā)明專利]基于加性潛變量擴(kuò)維卡爾曼濾波的狀態(tài)估計(jì)方法

在審

• 申請?zhí)枺?/td>: 202011513729.0
• 申請日: 2020-12-21
• 公開（公告）號: CN112731372A
• 公開（公告）日: 2021-04-30
• 發(fā)明（設(shè)計(jì)）人: 林志鵬;鮑中新;袁洢苒;文成林;徐曉濱
• 申請（專利權(quán)）人: 杭州電子科技大學(xué)
• 主分類號: G01S13/72
• 分類號: G01S13/72;G01S13/00
• 代理公司: 浙江千克知識產(chǎn)權(quán)代理有限公司 33246
• 代理人: 周希良

1.基于加性潛變量擴(kuò)維卡爾曼濾波的狀態(tài)估計(jì)方法，該方法包括以下步驟：

步驟1：建立符合卡爾曼濾波框架的狀態(tài)模型與觀測模型；

x(k+1)＝A(k)x(k)+B(k)f(x(k),k)+w₁(k) (1)

z(k+1)＝C(k+1)x(k+1)+v(k+1) (2)

其中，k為離散時間，x(k)是n維狀態(tài)向量，A(k)是狀態(tài)模型線性部分的轉(zhuǎn)移矩陣，f(x(k),k)是基本函數(shù)組成的n維非線性向量，B(k)是非線性向量的系數(shù)矩陣；z(k+1)是觀測向量，C(k+1)是觀測矩陣；w₁(k)和v(k+1)分別為已知的高斯型的過程噪聲和觀測噪聲向量；

上式(1)、(2)中滿足以下兩個假設(shè)：

假設(shè)1：w₁(k)和v(k+1)為不相關(guān)的白噪聲序列，且滿足統(tǒng)計(jì)特性E{w₁(k)}＝0,E{v(k+1)}＝0，E{w(k)w^T(j)}＝Q(k)δ_k,j，E{v(k)v^T(j)}＝R(k)δ_k,j，其中δ_k,j為克羅迪克函數(shù)，當(dāng)k＝j(luò)時，δ_k,j＝1；否則，δ_k,j＝0；j為離散時刻；Q(k)是對稱的非負(fù)定矩陣，R(k+1)是對稱的正定矩陣；

假設(shè)2：狀態(tài)初值x(0)和過程噪聲、觀測噪聲之間互相獨(dú)立，滿足E{w(k)v^T(k)}＝0，E{x(0)v^T(k)}＝0，E{x(0)w^T(k)}＝0；且P₀≥0為半正定矩陣；

步驟2：非線性函數(shù)的擬線性化表示；

將式(1)中的非線性向量f(x(k),k)中的各個分量f_i(x(k),k)定義為潛變量

β_i(k)＝f_i(x(k)),i＝1,2,…,n (3)

基于式(3)，狀態(tài)模型(1)的擬線性化形式為

x(k+1)＝A(k)x(k)+B(k)β(k)+w₁(k) (4)

其中，β(k)＝[β₁(k),…,β_i(k),…,β_n(k)],i＝1,2,…,n

步驟3：非線性函數(shù)的線性化表示；

建立加性潛變量函數(shù)向量β(k)的動態(tài)關(guān)聯(lián)模型

β(k+1)＝S(k)x(k)+G(k)β(k)+w₂(k) (5)

式中S(k)是狀態(tài)向量的系數(shù)矩陣，G(k)是潛變量函數(shù)向量的系數(shù)矩陣，w₂(k)為高斯型白噪聲向量；

若記擴(kuò)維后的狀態(tài)向量為：

X(k)＝[x^T(k),β^T(k)]^T (6)

再結(jié)合式(4)和式(5)，狀態(tài)模型(1)基于擴(kuò)維后的狀態(tài)向量(6)的線性化系統(tǒng)和觀測模型的線性化模型分別為

其中

步驟4：設(shè)計(jì)狀態(tài)向量與潛變量聯(lián)合估計(jì)的擴(kuò)維卡爾曼濾波器；

對于擴(kuò)維后的線性狀態(tài)模型(7)和觀測模型(8)，假設(shè)已知初始值和初始誤差協(xié)方差矩陣P(k|k)，則濾波器設(shè)計(jì)的具體步驟如下：

1)狀態(tài)一步預(yù)測

2)狀態(tài)一步預(yù)測協(xié)方差

式中Q₀(k)是對稱的非負(fù)定矩陣；

3)濾波增益矩陣

式中R₀(k+1)是對稱的正定矩陣；

4)新息序列

5)狀態(tài)估計(jì)

6)估計(jì)誤差協(xié)方差