基于扩展卡尔曼滤波的slam

xingyu_cxy

已于 2022-01-26 10:21:34 修改

阅读量1.6k

点赞数

CC 4.0 BY-SA版权

分类专栏： SLAM公式文章标签： slam 卡尔曼滤波算法计算机视觉

于 2022-01-07 19:45:57 首次发布

本文链接：https://blog.youkuaiyun.com/xingyu_cxy/article/details/122370846

SLAM公式专栏收录该内容

2 篇文章

订阅专栏

本文详细介绍了卡尔曼滤波及其扩展形式——扩展卡尔曼滤波的工作原理。从初始估计开始，通过状态转移矩阵、测量矩阵和不确定性矩阵，逐步阐述了预测和更新步骤。同时，提到了不变扩展卡尔曼滤波，强调了其在非线性系统估计中的应用。内容涵盖了方程推导和算法实现流程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

Kalman Filter：

Extended Kalman Filter：

Choose an initial estimate $X^0∣0\hat{X}_{0 \mid 0}$ and uncertainty matrix $P0∣0P_{0 \mid 0}$ .

Define $F_{n}, G_{n}$ , and $H_{n}$ :
$en∣n−1=Fnen−1∣n−1+GnwnYn−b(X^n∣n−1)=Hnen∣n−1+Vn \begin{aligned} e_{n \mid n-1} &=F_{n} e_{n-1 \mid n-1}+G_{n} w_{n} \\ Y_{n}-b\left(\hat{X}_{n \mid n-1}\right) &=H_{n} e_{n \mid n-1}+V_{n} \end{aligned}$
Define $Q_{n}$ as $Cov⁡(wn)\operatorname{Cov}\left(w_{n}\right)$ and $Rn\mathrm{R}_{n}$ as $Cov⁡(Vn)\operatorname{Cov}\left(V_{n}\right)$

$zn=Yn−b(X^n∣n−1)Sn=HnPn∣n−1HnT+RnKn=Pn∣n−1HnTSn−1 \begin{aligned} & z_{n}=Y_{n}-b\left(\hat{X}_{n \mid n-1}\right)\\ &S_{n}=H_{n} P_{n \mid n-1} H_{n}^{T}+R_{n} \\ &K_{n}=P_{n \mid n-1} H_{n}^{T} S_{n}^{-1} \end{aligned}$

loop:
Propagation
$X^n∣n−1=f(X^n−1∣n−1,un,0)Pn∣n−1=FnPn−1∣n−1FnT+GnQnGnT \begin{aligned} &\hat{X}_{n \mid n-1}=f\left(\hat{X}_{n-1 \mid n-1}, u_{n}, 0\right) \\ &P_{n \mid n-1}=F_{n} P_{n-1 \mid n-1} F_{n}^{T}+G_{n} Q_{n} G_{n}^{T} \end{aligned}$
Measurement update
$Pn∣n=[I−KnHn]Pn∣n−1X^n∣n=X^n∣n−1+Knzn \begin{aligned} & P_{n \mid n}=\left[I-K_{n} H_{n}\right] P_{n \mid n-1} \\ &\hat{X}_{n \mid n}=\hat{X}_{n \mid n-1}+K_{n} z_{n} \end{aligned}$ end loop

Invariant Extended Kalman Filter

Choose initial $χ^0∣0∈G\hat{\chi}_{0 \mid 0} \in G$ and $P0∣0∈Rd×d=P_{0 \mid 0} \in \mathbb{R}^{d \times d}=$ $Cov⁡(ξ0∣0)\operatorname{Cov}\left(\xi_{0 \mid 0}\right)$ .
Define $H_{n}$ ,
$F_{n}$ : $g(exp⁡(ξ),un)=exp⁡(Fnξ)g\left(\exp (\xi), u_{n}\right)=\exp \left(F_{n} \xi\right)$
Define $Q_{n}$ as $Cov⁡(wn)\operatorname{Cov}\left(w_{n}\right)$ and $R_{n}$ as $Cov⁡(Vn)\operatorname{Cov}\left(\mathrm{V}_{n}\right)$
Compute $zn=Yn−b(χ^n∣n−1)z_{n}=Y_{n}-b\left(\hat{\chi}_{n \mid n-1}\right)$
$Sn=HnPn∣n−1HnT+RnS_{n}=H_{n} P_{n \mid n-1} H_{n}^{T}+R_{n}$
$Kn=Pn∣n−1HnTSn−1K_{n}=P_{n \mid n-1} H_{n}^{T} S_{n}^{-1}$
$Pn∣n=[I−KnHn]P_{n \mid n}=\left[I-K_{n} H_{n}\right]$

loop
Propagation
$χ^n∣n−1=f(χ^n−1∣n−1,un)Pn∣n−1=FnPn−1∣n−1Fn−1+Qn \begin{aligned} &\hat{\chi}_{n \mid n-1}=f\left(\hat{\chi}_{n-1 \mid n-1}, u_{n}\right) \\ &P_{n \mid n-1}=F_{n} P_{n-1 \mid n-1} F_{n}^{-1}+Q_{n} \end{aligned}$
Measurement update
$Pn∣n−1χ^n∣n=χ^n∣n−1exp⁡(Knzn) \begin{aligned} &P_{n \mid n-1}\hat{\chi}_{n \mid n}=\hat{\chi}_{n \mid n-1} \exp \left(K_{n} z_{n}\right) \end{aligned}$
end loop