基于扩展卡尔曼滤波的slam

xingyu_cxy

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

阅读量1.5k

点赞数

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

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

本文链接：https://blog.csdn.net/xingyu_cxy/article/details/122370846

版权

SLAM公式专栏收录该内容

2 篇文章 0 订阅

订阅专栏

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

摘要由CSDN通过智能技术生成

Kalman Filter：

Extended Kalman Filter：

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

Define $F_{n}, G_{n}$ , and $H_{n}$ :
$\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 $\operatorname{Cov}\left(w_{n}\right)$ and $\mathrm{R}_{n}$ as $\operatorname{Cov}\left(V_{n}\right)$

$\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
$\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
$\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 $\hat{\chi}_{0 \mid 0} \in G$ and $P_{0 \mid 0} \in \mathbb{R}^{d \times d}=$ $\operatorname{Cov}\left(\xi_{0 \mid 0}\right)$ .
Define $H_{n}$ ,
$F_{n}$ : $g\left(\exp (\xi), u_{n}\right)=\exp \left(F_{n} \xi\right)$
Define $Q_{n}$ as $\operatorname{Cov}\left(w_{n}\right)$ and $R_{n}$ as $\operatorname{Cov}\left(\mathrm{V}_{n}\right)$
Compute $z_{n}=Y_{n}-b\left(\hat{\chi}_{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}$
$P_{n \mid n}=\left[I-K_{n} H_{n}\right]$

loop
Propagation
$\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
$\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