卡爾曼濾波及非線性系統估計兼談KF、EKF、UKF

預備知識

1，n維狀態$x_i$有Gauss分佈協方差$P_i$，有如下線性變換
$x_k=Ax_{k-1}$
則，$x_k$亦有Gauss分佈，且協方差爲$AP_{k-1}A^T$，
$A:(x_{k-1},P_{k-1}) \to (Ax_{k-1},AP_{k-1}A^T)$
2，兩個Gauss分佈的融合：
$(u_2,\sigma_2)= \begin{cases} (u_0,\sigma_0) \\ (u_1,\sigma_1) \end{cases} \\ K=\sigma_0(\sigma_0+\sigma_1)^{-1} \\ u_2=u_0+K(u_1-u_0) \\ \sigma_2=\sigma_0\sigma_1(\sigma_0+\sigma_1)^{-1}=(1-k)\sigma_0$
3，構造sigma點
採用對稱採樣策略，對分佈爲$(\bar{x},P_x)$的$n$維隨機變量$x$產生$2n+1$個Gauss分佈列向量，如下：
$x^i= \begin{cases} \bar{x},i=0 \\ \bar{x}+[\sqrt{(n+\kappa)P_x}]_i,i=1,2,\cdots,n \\ \bar{x}-[\sqrt{(n+\kappa)P_x}]_i,i=n+1,n+2,\cdots,2n \end{cases}$
$\kappa$是尺度參數，調節其可以逼近精度。
均值權重$W_i^m$與方差權重$W_i^c$如下求取：
$\begin{cases} W_0^m=\kappa / (n+\kappa) \\ W_0^c=\kappa / (n+\kappa)+(1-\alpha^2+\beta) \\ W_i^m=W_i^c=\kappa / [2(n+\kappa)],i=1,2,\cdots,2n \end{cases}$
其中，$\kappa=\alpha^2(n+\lambda)-n$

• $\alpha$確定$\bar{x}$周圍sigma點的分佈，通常設爲較小的正數$(1\gt \alpha \ge 1e^{-4} )$
• $\lambda$是第二個尺度參數，通常設置爲$0$或$3-n$
• $\beta$是狀態分佈參數，對於Gauss分佈$\beta=2$是最優的，如果狀態是單變量，則$\beta=0$最佳
• 對於Gauss分佈，考慮到4階距的統計量，通常取值$n+\kappa=3$

卡爾曼濾波KF

狀態轉移方程：
$x_k=F_kx_{k-1}+B_ku_k,Q_k \in GaussNoise$
$P_k=F_kP_{k-1}F_k^T+Q_k$
觀測方程：
$z_k=H_kx_k,R_k \in GaussNoise$
經過狀態轉移與觀測方程，得到Gauss分佈：
$(H_k\hat{x}_k, H_k\hat{P}_kH_k^T)$
由傳感器觀測，得到Gauss分佈：
$(z_k, R_k)$
融合這兩個分佈：
$K_k= H_k\hat{P}_kH_k^T( H_k\hat{P}_kH_k^T+R_k)^{-1}$
$H_kx_k=H_k\hat{x}_k+K_k(z_k-H_k\hat{x}_k)$
$H_kP_kH_k^T=H_k\hat{P}_kH_k^T-K_kH_k\hat{P}_kH_k^T$
$K_k^\prime= \hat{P}_kH_k^T( H_k\hat{P}_kH_k^T+R_k)^{-1}$
得到：
$x_k=\hat{x}_k+K_k^\prime(z_k-H_k\hat{x}_k)$
$P_k=\hat{P}_k-K_k^\prime H_k\hat{P}_k$

擴展卡爾曼濾波EKF

對於非線性系統，不能用KF，很自然的可以利用Taylor展開，轉爲近似線性系統，套用KF，$F_k$與$H_k$需要用一階Taylor展開的Jacobi代替。

非線性系統濾波原理

系統方程如下：
$\begin{cases} x_k=f(x_{k-1},u_k, v_k) \\ z_k=g(x_k,w_k) \end{cases}$
系統狀態轉移後的統計量：
$\begin{cases} \bar{x}_k=E\{x_k|Y^{k-1}\} \\ \bar{P}_k=E\{(x_k- \bar{x}_k)(x_k- \bar{x}_k)^T|Y^{k-1}\} \end{cases}$
系統觀測的統計量：
$\begin{cases} \bar{y}_k=E\{y_k|Y^{k-1}\} \\ P_{y,k}=E\{(y_k-g( \bar{x}_k))(y_k- g( \bar{x}_k))^T|Y^{k-1}\} \\ P_{xy,k}=E\{(x_k- \bar{x}_k)(y_k-g( \bar{x}_k))^T|Y^{k-1}\} \\ K_k=P_{xy,k}P_{y,k}^{-1} \end{cases}$
系統狀態更新：
$\begin{cases} \hat{x}_k=\bar{x}_k+K_k(y_k-\bar{y}_k) \\ \hat{P}_k= \bar{P}_k-K_kP_{y,k}K_k^T \end{cases}$

無跡卡爾曼濾波UKF

已知分佈$(\hat{x}_{k-1},\hat{P}_{k-1})$
取點並狀態轉移：
$\{sigma:x_{k-1}^i\},i\in0,1,2,\cdots,2n-1,2n \\ f:x_{k-1}^i\mapsto x_{k|k-1}^i$
$x_{k|k-1}^i\implies \begin{cases} W^m\mapsto \hat{x}_{k|k-1}=\sum_{i=0}^{2n}W_i^m x_{k|k-1}^i \\ W^c\mapsto \hat{P}_{k|k-1}= \sum_{i=0}^{2n}W_i^c (x_{k|k-1}^i-\hat{x}_{k|k-1})(x_{k|k-1}^i-\hat{x}_{k|k-1})^T+Q_k \end{cases}$
取點並觀測轉移：
$\{sigma:\hat{x}_{k|k-1}^i\},i\in0,1,2,\cdots,2n-1,2n \\ g:\hat{x}_{k|k-1}^i\mapsto z_k^i$
$z_k^i\implies \begin{cases} W^m\mapsto \hat{z}_k=\sum_{i=0}^{2n}W_i^m z_k^i \\ W^c\mapsto \hat{P}_{z_k}= \sum_{i=0}^{2n}W_i^c (z_k^i-\hat{z}_k)(z_k^i-\hat{z}_k)^T+R_k \\ W^c\mapsto \hat{P}_{x_kz_k}=\sum_{i=0}^{2n}W_i^c (\hat{x}_{k|k-1}^i-\hat{x}_{k|k-1})(z_k^i-\hat{z}_k)^T \end{cases} \\ K_k=\hat{P}_{x_kz_k}\hat{P}_{z_k}^{-1}$
狀態更新：
$\hat{x}_k=\hat{x}_{k|k-1}+K_k(z_k-\hat{z}_k) \\ \hat{P}_k=\hat{P}_{k|k-1}-K_k\hat{P}_{z_k}K_k^T$