如何编程实现SLAM中的扩展卡尔曼滤波器算法？

本文共计2999个文字，预计阅读时间需要12分钟。

Doreen 01 | 问题描述 | 预先知晓事物未来的状态总是很有价值的！+ 预知台风的路线可以避免或减轻巨大自然灾害的损失。+ 敌国打过来的导弹，如果能够高精度预测轨迹，就能有‘先发制人’的优势。

©作者 |Doreen

01 问题描述

预先知道事物未来的状态总是很有价值的！

√ 预知台风的路线可以避免或减轻重大自然灾害的损失。

√ 敌国打过来的导弹，如果能够高精度预测轨迹，就能有效拦截。

√ 操控无人机，需要知道下一刻飞机的方向、速度不断修正，才能精准控制、回避各种风险。

这是一个状态估计问题

如下图所描绘的，在 k 个（一个或多个）时间点上，

基于初始的状态信息

一系列观测数据

给定控制输入

以及系统的运动和观测模型，力求预测系统在每一时刻的真实状态

图 1.1: 状态估计问题示意图

以自动驾驶为例，图中符号的含义如下：

● xˇ: 设计的轨迹，比如预先计算得出的理想轨迹。

● w: 驾驶过程中各种操作引入的噪声，称为过程噪声。

● x: 在理想轨迹之上混入了过程噪声的真实轨迹。

● t 下标表示时间。

● n: 观测噪声。

● y: 观测数据：对真实轨迹的观测，其中包含观测噪声。