EKF融合编码器与激光测距实现机器人精确定位

markdown复制## 1. 项目概述与核心需求

在移动机器人定位领域，单纯依赖编码器或激光测距仪都存在明显缺陷。编码器通过轮速积分计算位移（航迹推算），但会累积误差；激光测距仪能获取环境特征点距离，但受遮挡和噪声影响。这个项目演示了如何用扩展卡尔曼滤波器（EKF）融合两类传感器的优势——将编码器的连续运动预测与激光测距仪的离散环境观测相结合，实现更鲁棒的二维定位。

核心解决三个问题：
- 运动模型非线性：机器人转向时航向角与位移的关系需线性化处理
- 多源数据异步：编码器高频输出与激光雷达低频扫描的时间对齐
- 误差特性差异：编码器误差随时间累积，激光测距误差呈瞬时爆发性

> 实测表明，单独使用编码器定位10分钟后误差可达1.5米，而纯激光定位在特征稀疏区域会失效。EKF融合后误差稳定在0.2米内。

## 2. EKF理论基础与模型构建

### 2.1 扩展卡尔曼滤波器原理
标准KF假设系统为线性，而EKF通过一阶泰勒展开处理非线性：
1. 状态预测：`x_k = f(x_{k-1}, u_k) + w_k`  
   （f为非线性运动模型，w为过程噪声）
2. 观测预测：`z_k = h(x_k) + v_k`  
   （h为非线性观测模型，v为观测噪声）

关键步骤：
- 在当前估计点线性化f和h，得到雅可比矩阵F和H
- 预测协方差时使用线性化模型：`P_k = F P_{k-1} F^T + Q`

### 2.2 移动机器人状态模型
定义状态向量：`x = [px, py, theta, v, w]^T`  
（位置x/y、航向角、线速度、角速度）

运动模型（差分驱动）：

px_k = px_{k-1} + v*cos(theta)dt
py_k = py_{k-1} + vsin(theta)dt
theta_k = theta_{k-1} + wdt

code复制雅可比矩阵F需对f求偏导，例如：
```matlab
F = [1, 0, -v*sin(theta)*dt, cos(theta)*dt, 0;
     0, 1,  v*cos(theta)*dt, sin(theta)*dt, 0;
     0, 0,  1,               0,             dt;
     0, 0,