加權卡爾曼濾波

加權卡爾曼濾波融合，它通過給不同傳感器或估計結果分配不同的權重，來提高狀態估計的精度和可靠性。

一、卡爾曼濾波

1.狀態方程

$X_{k} =\phi _{k,k-1}X_{k-1}+\tau _{k-1}W_{k-1}$

? ? ? ? ? ?

2.觀測方程

$Z_{k}=H_{k}X_{k}+V_{k}$

其中：

$E[W_{k}]=E[V_{k}]=0,cov[W_{k},W_{j}]=Q_{k}\delta _{kj},cov[V_{k},V_{j}]=R_{k}\delta _{kj},$

$cov[W_{k},V_{j}]=0,\delta _{kj}=1(k=j),\delta _{kj}=0(k\neq j)$

基本方程

①狀態一步預測

$\hat{X}_{k/k-1}=\phi _{k,k-1}\hat{X}_{k-1}$

②狀態估計

$\hat{X}_{k}=\hat{X}_{k/k-1}+K_{k}(Z_{k}-H_{k}\hat{X}_{k/k-1})$

③濾波增益

$K_{k}=P_{k/k-1}H_{k}^{T}(H_{k}P_{k/k-1}H_{k}^{T}+R_{k})^{-1}$

④一步預測均方差

$P_{k/k-1}=\phi _{k,k-1}P_{k-1}\phi _{k,k-1}^{T}+\tau _{k-1}Q_{k-1}\tau _{k-1}^{T}$

⑤估計均方誤差

$P_{k}=(I-K_{k}H_{k})P_{k/k-1}(I-K_{k}H_{k})^{T}+K_{k}R_{k}K_{k}^{T}$

$=(I-K_{k}H_{k})P_{k/k-1} =(P_{k/k1}^{-1}+H_{k}^{T}R_{k}^{-1}H_{k})^{-1}$

二、加權卡爾曼濾波

對于多個傳感器，分別構成的卡爾曼濾波器，進行加權：

$\left\{\begin{matrix} \hat{x} = \sum w_{i} \hat{x}_{i} & \\ \sum w_{i} = 1& \end{matrix}\right.$

對于加權系數的求解

$\tilde{x} = x - \hat{x} = \sum w_{i} \tilde{x}_{i}$

$P = E[\tilde{x} \tilde{x}^{T}] = \sum w_{i}w_{j}P_{ij}$

當P最小時(即 $tr[P]_{min}$ )，所得到的誤差最小，即為最優估計；

運用拉格朗日構造法，令：

$J = tr[P]$

$\psi = J + \lambda (\sum w_{i} - 1)$

求導 $\frac{\partial \psi }{ \partial w_{i}} = 0$ ；即：

$2 \sum trP_{ij}w_{i} + \lambda = 0$

根據上式得

$\begin{pmatrix} \phi & I \\ I^{T}& 0 \end{pmatrix} \begin{pmatrix} w\\ \lambda /2 \end{pmatrix} = \begin{pmatrix} 0 \\ 1 \end{pmatrix}$

其中? $\phi = \sum trP_{ij}$

$\begin{pmatrix} w\\ \lambda /2 \end{pmatrix} =\begin{pmatrix} \phi & I \\ I & 0 \end{pmatrix}^{-1} \begin{pmatrix} 0\\ 1 \end{pmatrix} = \frac{1}{-I^{T} \phi ^{-1} I}\begin{pmatrix} \phi ^{-1}I\\ 1 \end{pmatrix}$

得到

$w = \frac{\phi ^{-1} I}{ I^{T} \phi ^{-1} I}$

三、濾波過程

①對n個傳感器分別建立卡爾曼濾波器

$\left\{\begin{matrix} X_{k} =\phi _{k,k-1}X_{k-1}+\tau _{k-1}W_{k-1}\\ Z_{k}=H_{k}X_{k}+V_{k}\end{matrix}\right.$

②求得對應得估計及其方差，以及相互間得協方差

$\left\{\begin{matrix} \hat{x}_{k} = x_{k|k-1} + K_{k}(z_{k} - H_{k}x_{k|k-1}) \\P_{k} = (I -K_{k}H_{k})P_{k|k-1} \\ P_{ij}(k) = (I-K_{i}H_{i})(\phi P_{ij}(k-1)\phi ^{T} +Q) (I-K_{j}H_{j})^{T}\end{matrix}\right.$

③計算加權系數

$w = \frac{\phi ^{-1} I}{ I^{T} \phi ^{-1} I}$

其中， $\phi = \sum trP_{ij}$

④計算加權最優估計

$\hat{x} = \sum w_{i} \hat{x}_{i}$

參考文獻：

[1]

X. Ding, Z. Wang, L. Zhang and C. Wang, "Longitudinal Vehicle Speed Estimation for Four-Wheel-Independently-Actuated Electric Vehicles Based on Multi-Sensor Fusion," in?IEEE Transactions on Vehicular Technology, vol. 69, no. 11, pp. 12797-12806, Nov. 2020, doi: 10.1109/TVT.2020.3026106. keywords: {Estimation;Sensors;Tires;Wheels;Kalman filters;Vehicle dynamics;Roads;Multi-sensor fusion;longitudinal vehicle speed estimation;road gradient estimation;Kalman filter;four-wheel-independently-actuated electric vehicles},

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/news/916397.shtml
繁體地址，請注明出處：http://hk.pswp.cn/news/916397.shtml
英文地址，請注明出處：http://en.pswp.cn/news/916397.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！