从入门到登峰-嵌入式Tracker定位算法全景之旅 Part 5 ｜地图匹配与轻量 SLAM：HMM/Viterbi 与简化图优化

Part 5 ｜地图匹配与轻量 SLAM：HMM/Viterbi 与简化图优化

在本章中，我们将在 ESP32-S3 这样的资源受限平台上，实现 地图匹配（Map Matching）和轻量级图优化（Lightweight SLAM）功能。通过 隐马尔可夫模型（HMM）+ Viterbi 算法，以及简化的图优化思路，校正定位轨迹，提升整体精度与一致性。

一、地图数据结构设计

1. 路网 & 室内平面图

   • 节点（Node）：路口或转折点，包含 (x, y) 坐标

   • 边（Edge）：连接两个节点，存储长度 L 与拓扑关系

2. 压缩存储

   • 将节点列表和边列表分别存入 Flash 或外部 SPI-Flash，结构示例：

     typedef struct {uint16_t id;float x, y;
     } MapNode;typedef s