# EKF-SLAM新地标协方差如何初始化

> 原文链接：https://robotics.hellobike.com/pitfalls/2023-11-16-robotics-se-4599
> 频道：pitfalls | 更新日期：2023-11-16 | 标签：auto, stackexchange, slam, ekf, errors, mapping
> 核心摘要：EKF-SLAM新增地标时不要置零或随意设大，应用逆观测模型雅可比初始化协方差块。

# EKF-SLAM新地标协方差如何初始化

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[the uncertainty of initializing new landmark in EKF-SLAM](https://robotics.stackexchange.com/questions/4599/the-uncertainty-of-initializing-new-landmark-in-ekf-slam)（Robotics Stack Exchange #4599）

**状态**：已解决

## 问题

在基于特征地图的 EKF-SLAM 中，新观测地标加入状态向量后，若协方差相关项设为零，地标估计误差不随时间变化；若设很大，重观测会改善但误差趋于固定值。

## 环境

SLAM 建图与定位系统

## 解决办法

1. 使用逆观测模型 `g^{-1}(x, z)` 根据当前状态估计和观测生成新地标状态。
2. 分别计算 `g^{-1}` 对状态 `x` 和观测 `z` 的雅可比 `G_x`、`G_z`。
3. 扩展状态为 `[x; g^{-1}(x, z)]`。
4. 按块初始化协方差：右上为 `P_k G_x^T`，左下为 `G_x P_k`，右下为 `G_x P_k G_x^T + G_z R_k G_z^T`，其中 `R_k` 为观测方差或协方差。

原问题作者：CroCo；采纳答案作者：kamek。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。
:::

## 原始参考来源 (Sources)

- [Robotics Stack Exchange #4599：the uncertainty of initializing new landmark in EKF-SLAM](https://robotics.stackexchange.com/questions/4599/the-uncertainty-of-initializing-new-landmark-in-ekf-slam)