聯邦卡爾曼濾波
聯邦Kalman濾波
利用Kalman濾波技術對多感測器資料進行最優融合有兩種途徑:集中式Kalman濾波和分散化濾波。集中式Kalman濾波利用一個濾波器來集中處理所有子系統的資訊。分散化濾波利用子濾波器處理子系統的資訊通過全域性濾波器實現所有子系統的資訊融合。
聯邦濾波器一般結構
各子濾波器的估計不相關時的融合演算法
定理1:若有
N
N
N個區域性狀態估計
X
1
^
\hat{X_1}
X1^,
X
2
^
\hat{X_2}
X2^,
.
.
.
...
...,
X
N
^
\hat{X_N}
XN^和相應的估計誤差協方差陣
P
11
P_{11}
P11,
P
22
P_{22}
X
g
^
=
P
g
∑
i
=
1
N
P
i
i
−
1
X
i
^
\hat{X_g}=P_{g} \sum_{i=1}^{N}P_{ii}^{-1} \hat{X_i}
Xg^=Pgi=1∑NPii−1Xi^
其中
P
g
=
(
∑
i
=
1
N
P
i
i
−
1
)
−
1
P_g=(\sum_{i=1}^{N}P_{ii}^{-1})^{-1}
由
P
i
i
−
1
X
i
^
P_{ii}^{-1} \hat{X_i}
Pii−1Xi^知,當
X
i
^
\hat{X_i}
Xi^的估計精度差,即
P
i
i
P_{ii}
Pii大時,
X
i
^
\hat{X_i}
Xi^在全域性估計的作用比較小。
各子濾波器估計相關條件下的聯邦濾波演算法
在一般情況下,各子濾波器是估計是相關的。為了解決這個問題,採用方差上界技術,對濾波過程進行適當的改造,使得區域性估計實際上不相關,從而進行濾波。
假設各子濾波器的狀態估計表示為:
X
i
^
=
[
X
a
i
^
X
b
i
^
]
\hat{X_i}=\begin{bmatrix}\hat{X_{ai}}\\\hat{X_{bi}}\end{bmatrix}
其中,
X
a
i
^
\hat{X_{ai}}
Xai^是各子濾波器的公共狀態
X
c
X_c
Xc的估計,如導航中的位置,速度,和姿態等誤差狀態的估計;
X
b
i
^
\hat{X_{bi}}
Xbi^是第
i
i
i個濾波器專有的狀態估計,如GPS誤差狀態的估計。這裡只對公共狀態估計進行融合以得到其全域性估計。
資訊分配原則
系統中有兩類資訊
-
狀態運動方程資訊
狀態方程的資訊量是與系統噪聲的方差成反比的,因此可用 Q − 1 Q^{-1} Q−1表示狀態狀態方程資訊量;
-
量測方程資訊
量測方程的資訊可用量測噪聲協方差的逆 R − 1 R^{-1} R−1表示。
將系統噪聲總的資訊
Q
−
1
Q^{-1}
Q−1分配到各個子濾波器和主濾波中:
Q
−
1
=
∑
i
=
1
N
Q
i
−
1
+
Q
m
−
1
Q^{-1}=\sum_{i=1}^{N}Q_i^{-1}+Q_m^{-1}
Q−1=i=1∑NQi−1+Qm−1
其中
Q
i
=
β
i
−
1
Q
Q_i=\beta_i^{-1}Q
Qi=βi−1Q,
Q
m
=
β
m
−
1
Q
Q_m=\beta_m^{-1}Q
Qm=βm−1Q。
由資訊守恆原理可知:
∑
i
=
1
N
β
i
+
β
m
=
1
\sum_{i=1}^N\beta_i+\beta_m=1
i=1∑Nβi+βm=1
由子濾波器與主濾波器合成的全域性估計值
X
g
^
\hat{X_g}
Xg^及其相應的估計誤差方差陣
P
g
P_g
Pg被
β
i
−
1
P
g
\beta_i^{-1}P_g
βi−1Pg後再反饋到子濾波器,重置子濾波器的估計值。
X
i
^
=
X
g
^
\hat{X_i}=\hat{X_g}
Xi^=Xg^
P i i = β i − 1 P g P_{ii}=\beta_i^{-1}P_g Pii=βi−1Pg
其中 β i − 1 \beta_i^{-1} βi−1表示資訊分配係數。
為了使
P
i
j
(
i
≠
j
)
P_{ij}(i\neq j)
Pij(i=j)恆等於零,則需要對濾波過程進行改造。先構造一個增廣系統,它的狀態向量由
N
N
N個區域性濾波子系統和主濾波子系統的狀態組合而成:
KaTeX parse error: Got function '\bar' with no arguments as subscript at position 36: …_1\\\vdots\\ X_\̲b̲a̲r̲{N}\end{bmatrix…
式中:
N
ˉ
=
N
+
1
\bar{N}=N+1
Nˉ=N+1
每個子系統的狀態向量為:
X
i
=
[
X
c
X
b
i
]
X_i=\begin{bmatrix}X_c\\X_{bi}\end{bmatrix}
Xi=[XcXbi]
其中,
X
c
X_c
Xc是公共狀態向量;
X
b
i
X_{bi}
Xbi是第
i
i
i個子系統的專有狀態。
在這個增廣系統的狀態向量中含有公共狀態,但是不影響理論分析。增廣系統的狀態方程為
KaTeX parse error: Got function '\bar' with no arguments as subscript at position 34: …_1\\\vdots\\ X_\̲b̲a̲r̲{N}\end{bmatrix…
E [ W i W i T ] = Q E{\begin{bmatrix}W_iW_i^T\end{bmatrix}}=Q E[WiWiT]=Q
增廣系統總體濾波誤差方差陣一般可表示為
P
=
[
P
11
⋯
P
1
N
ˉ
⋮
⋮
P
N
ˉ
1
⋯
P
N
ˉ
N
ˉ
]
P={\begin{bmatrix}P_{11}&\cdots&P_{1\bar{N}}\\\vdots&&\vdots\\P_{\bar{N}1}&\cdots&P_{\bar{N}\bar{N}}\end{bmatrix}}
P=⎣⎢⎡P11⋮PNˉ1⋯⋯P1Nˉ⋮PNˉNˉ⎦⎥⎤
式中:
P
j
i
(
j
,
i
=
1
,
2
,
⋯
,
N
ˉ
)
P_{ji}(j,i=1,2,\cdots,\bar{N})
Pji(j,i=1,2,⋯,Nˉ)表示區域性濾波之間的相關性。
考慮到集中濾波的時間更新,由狀態方程可知
P
j
i
=
ϕ
j
j
P
j
i
∗
ϕ
i
i
T
+
G
j
Q
G
i
T
P_{ji}=\phi_{jj}P_{ji}^*\phi_{ii}^T+G_jQG_i^T
Pji=ϕjjPji∗ϕiiT+GjQGiT
由上式知,由於公共噪聲
Q
Q
Q的存在,即使
P
j
i
∗
=
0
P_{ji}^*=0
Pji∗=0,
P
j
i
P_{ji}
Pji也不會等於0,即時間更新過程將引入各子濾波器的估計相關。因此需要利用方差上屆技術來進行消除。將過程噪聲改寫為
[
G
1
⋮
G
N
ˉ
]
Q
[
G
1
T
⋯
G
N
ˉ
T
]
=
[
G
1
⋱
G
N
ˉ
]
[
Q
⋯
Q
⋮
⋮
Q
⋯
Q
]
[
G
1
T
⋱
G
N
ˉ
T
]
\begin{bmatrix}G_1\\\vdots\\G_{\bar{N}}\end{bmatrix}Q\begin{bmatrix}G_1^T&\cdots&G_{\bar{N}}^T\end{bmatrix}=\begin{bmatrix}G_1&&\\&\ddots&\\&&G_{\bar{N}}\end{bmatrix}\begin{bmatrix}Q&\cdots&Q\\\vdots&&\vdots\\Q&\cdots&Q\end{bmatrix}\begin{bmatrix}G_1^T&&\\&\ddots&\\&&G_{\bar{N}}^T\end{bmatrix}
⎣⎢⎡G1⋮GNˉ⎦⎥⎤Q[G1T⋯GNˉT]=⎣⎡G1⋱GNˉ⎦⎤⎣⎢⎡Q⋮Q⋯⋯Q⋮Q⎦⎥⎤⎣⎡G1T⋱GNˉT⎦⎤
由矩陣理論可知:
[
Q
⋯
Q
⋮
⋮
Q
⋯
Q
]
≤
[
γ
1
Q
⋯
0
⋮
⋮
0
⋯
γ
N
ˉ
Q
]
\begin{bmatrix}Q&\cdots&Q\\\vdots&&\vdots\\Q&\cdots&Q\end{bmatrix}\leq\begin{bmatrix}\gamma_1Q&\cdots&0\\\vdots&&\vdots\\0&\cdots&\gamma_{\bar{N}}Q\end{bmatrix}
⎣⎢⎡Q⋮Q⋯⋯Q⋮Q⎦⎥⎤≤⎣⎢⎡γ1Q⋮0⋯⋯0⋮γNˉQ⎦⎥⎤
1 γ 1 + ⋯ + 1 γ N ˉ = 1 , 0 ≤ 1 γ i ≤ 1 \frac{1}{\gamma_1}+\cdots+\frac{1}{\gamma_{\bar{N}}}=1,0\leq\frac{1}{\gamma_i}\leq1 γ11+⋯+γNˉ1=1,0≤γi1≤1
由此可見,選擇等號放大估計誤差方差矩陣,
P
i
i
=
ϕ
i
i
P
i
i
∗
ϕ
i
i
T
+
γ
i
G
i
Q
G
i
T
P_{ii}=\phi_{ii}P_{ii}^*\phi_{ii}^T+\gamma_iG_iQG_i^T
Pii=ϕiiPii∗ϕiiT+γiGiQGiT
P j i = ϕ j j P j i ∗ ϕ i i T = 0 , P j i ∗ = 0 P_{ji}=\phi_{jj}P_{ji}^*\phi_{ii}^T=0,P_{ji}^*=0 Pji=ϕjjPji∗ϕiiT=0,Pji∗=0
通過式中表明,只要將各子濾波器的自身的初始方差矩陣在放大些就可以忽略各子濾波器初始方差之間的相關項。這樣得到的區域性濾波結果也是保守的。
聯邦濾波器的設計步驟
- 將子濾波器和主濾波器的初始估計誤差方差矩陣設定為組合系統初始估計誤差方差的 γ i ( i = 1 , 2 , ⋯ , N ) \gamma_i(i=1,2,\cdots,N) γi(i=1,2,⋯,N)倍。
- 將子濾波器和主濾波器的過程噪聲方差矩陣設定為組合系統過程噪聲方差矩陣的 γ i \gamma_i γi倍。
- 各子濾波器處理自己的觀測資訊,進而獲得區域性估計。
- 在得到各子濾波器的區域性估計和主濾波器的估計後,按定理1進行最優合成。
- 用全域性最優濾波解來重置各子濾波器和主濾波器的濾波值和估計誤差方差矩陣。
聯邦濾波器的結構
根據資訊分配策略不同,聯邦濾波演算法有4種實現模式:零復位模式;變比例模式;無反饋模式;融合—反饋模式。
- 零復位模式( β m = 1 , β i = 0 \beta_m=1,\beta_i=0 βm=1,βi=0零化式重置)
優缺點:
- 主濾波器分配到全部資訊;
- 故障檢測和隔離能力強;
- 減少了資料通訊量,計算簡單,解算精度不高。
- 變比例模式( β m = β i = 1 / ( N + 1 ) \beta_m=\beta_i=1/(N+1) βm=βi=1/(N+1)有重置)
優缺點:
- 主濾波器與子濾波器之間平均分配資訊;
- 融合後全域性濾波精度高,區域性濾波因為有全域性濾波反饋,精度也提高了;
- 一個子系統發生故障後,主濾波器受汙染,隔離後必須重新初始化主濾波器。
- 無反饋模式( β m = 0 , β i = 1 / N \beta_m=0,\beta_i=1/N βm=0,βi=1/N無重置)
- 主濾波器狀態方程無資訊分配,主濾波器不需要進行濾波,只起簡單的融合作用;
- 各區域性濾波器獨立濾波,沒有反饋重置帶來的相互影響提高了容錯效能;
- 由於沒有全域性估計的反饋重置,區域性估計精度不高。
- 融合—反饋模式( β m = 0 , β i = 1 / N \beta_m=0,\beta_i=1/N βm=0,βi=1/N有重置)
優缺點:
- 具有反饋作用,精度較高;
- 濾波器的故障檢測與隔離能力差。