第三章 传感器与状态估计：IMU原理、GPS/RTK、视觉里程计、扩展卡尔曼滤波（EKF）基础
各位同学，欢迎来到第三章。
说实话，这一章是整个课程里最“硬核”的部分之一。你想想看，无人机在天上飞，它怎么知道自己在哪里？姿态怎么样？速度多快？
答案就是——传感器。
但单个传感器都有毛病。IMU会飘，GPS会丢星，视觉会受光照影响。所以我们需要把它们融合起来。这就是状态估计要做的事。
我个人习惯把这一章分成四块：IMU、GPS/RTK、视觉里程计，以及把它们揉在一起的EKF。咱们一个一个来。
3.1 IMU原理：加速度计与陀螺仪
IMU，惯性测量单元。说白了就是两个东西：加速度计和陀螺仪。
加速度计测什么？测比力。注意，不是直接测重力，也不是直接测加速度。它测的是“物体受到的惯性力减去重力”。这个区别很关键，我当年刚入行时就在这里栽过跟头。
陀螺仪测角速度。三轴的，每秒能转多少度。
核心公式：
加速度计模型：a_measured = a_true - g + b_a + n_a
陀螺仪模型：ω_measured = ω_true + b_g + n_g其中 b 是零偏，n 是噪声。零偏会随时间缓慢变化，这就是IMU会飘的根本原因。
我在项目中遇到过一件事：有一次飞无人机做航测，起飞前IMU校准通过了，但飞了十分钟后姿态开始明显漂移。查了半天，发现是温度变化导致零偏漂移。从那以后，我每次做IMU初始化都会等它热机稳定。
避坑指南：
我曾经在实验室里用廉价IMU做实验，发现数据噪声大得离谱。后来总结出经验：
- 消费级IMU（如MPU6050）：适合入门，但别指望精度
- 工业级IMU（如ADIS系列）：零偏稳定性好很多
- 战术级IMU：价格感人，但效果确实好
选型时别只看参数表，一定要看“零偏不稳定性”这个指标。
3.2 GPS/RTK：从米级到厘米级
GPS大家都熟悉。但你真的了解它吗？
普通GPS精度在2-5米左右。为什么？因为电离层延迟、卫星钟差、多路径效应这些误差源太多了。
RTK（实时动态差分）技术，说白了就是在地面放一个基准站，它知道自己的精确位置，然后计算误差修正量，发给无人机。这样精度能到厘米级。
| 定位方式 | 精度 | 适用场景 |
|---|---|---|
| 普通GPS | 2-5米 | 粗略导航、航线飞行 |
| DGPS | 0.5-1米 | 农业植保 |
| RTK | 1-3厘米 | 测绘、精准降落 |
| PPK | 1-3厘米 | 后处理测绘 |
嗯，这里要注意：RTK需要基准站和移动站之间有通信链路。如果飞得太远，信号断了，就会降级到普通GPS模式。我见过有人因为这个撞了树。
重要提醒：
GPS信号容易被遮挡。在城市峡谷、树林密集的地方，千万别完全依赖GPS。我建议至少保留一个视觉或激光的备用定位方案。
3.3 视觉里程计：让无人机“看”路
视觉里程计（VO），就是通过摄像头图像序列来估计运动。
原理其实不复杂：
- 提取特征点（比如ORB、SIFT）
- 匹配相邻帧的特征点
- 用对极几何或PnP解算位姿
但实际做起来坑很多。光照变化、运动模糊、纹理缺失，随便一个都能让VO崩掉。
我记得有一次在傍晚测试，光线越来越暗，VO直接失效了。后来加了曝光时间自适应调整，才勉强能用。
VO vs VIO：
纯视觉里程计只靠图像，容易飘。视觉惯性里程计（VIO）把IMU和视觉融合，效果就好很多。现在主流方案基本都是VIO，比如VINS-Mono、ORB-SLAM3。
3.4 扩展卡尔曼滤波（EKF）：融合的艺术
好了，前面讲了三种传感器，各有各的毛病。怎么把它们结合起来？
EKF就是答案。
卡尔曼滤波原本是线性的。但无人机运动是非线性的，所以要用扩展卡尔曼滤波——说白了就是把非线性模型线性化，然后套用卡尔曼滤波的框架。
EKF的核心流程：
- 预测步：用IMU数据推算下一时刻的状态
- 更新步：用GPS或视觉观测修正预测结果
代码实现大概长这样：
# 预测步
x_pred = f(x_prev, u) # 状态预测
P_pred = F * P_prev * F.T + Q # 协方差预测
# 更新步
K = P_pred * H.T * (H * P_pred * H.T + R)^-1 # 卡尔曼增益
x_est = x_pred + K * (z - h(x_pred)) # 状态更新
P_est = (I - K * H) * P_pred # 协方差更新你想想看，这里面最关键的是什么？是噪声协方差矩阵Q和R的调参。Q代表模型的不确定性，R代表观测的不确定性。
调参经验：
我曾经花了两周时间调EKF参数，怎么调都不对。后来发现是Q矩阵设得太小，导致滤波器过于相信IMU预测，GPS观测基本没用。把Q调大一些后，效果立竿见影。
我的建议是：先让滤波器跑起来，然后看残差（innovation）的统计特性。如果残差均值不为零，说明模型有偏差；如果方差太大，说明噪声参数不对。
3.5 实战：一个简单的EKF融合示例
咱们来写一个简单的二维位置估计。假设无人机在平面上运动，IMU提供加速度，GPS提供位置观测。
import numpy as np
# 状态向量: [x, y, vx, vy]
# 观测向量: [x_gps, y_gps]
dt = 0.01 # 时间步长
# 状态转移矩阵
F = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 观测矩阵
H = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0]
])
# 噪声协方差
Q = np.eye(4) * 0.1 # 过程噪声
R = np.eye(2) * 1.0 # 观测噪声
# 初始化
x = np.array([0, 0, 0, 0])
P = np.eye(4)
# 模拟数据
for t in range(1000):
# 预测
x_pred = F @ x
P_pred = F @ P @ F.T + Q
# 模拟GPS观测（带噪声）
z = np.array([x[0] + np.random.randn()*0.5,
x[1] + np.random.randn()*0.5])
# 更新
y = z - H @ x_pred # 残差
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
x = x_pred + K @ y
P = (np.eye(4) - K @ H) @ P_pred这段代码虽然简单，但包含了EKF的全部精髓。你可以在上面加IMU的加速度输入，加视觉的位姿观测，原理是一样的。
注意：
实际工程中，EKF的维度可能高达20-30维（位置、速度、姿态、陀螺零偏、加速度计零偏等）。矩阵运算要小心数值稳定性。我建议用平方根滤波或UKF来避免数值问题。
3.6 本章小结
传感器融合不是简单的“取平均”。
IMU提供高频但会漂移的估计，GPS提供低频但有界的观测，视觉提供相对运动的约束。EKF把它们按各自的置信度加权融合，得到最优估计。
我个人觉得，理解EKF的关键不在于公式推导，而在于理解“预测-观测-修正”这个闭环。你想想看，这不就是我们做决策的方式吗？先凭经验预测，再根据实际反馈修正。
下一章我们会讲路径规划。但在那之前，请务必把状态估计搞扎实。因为规划再好，不知道自己在哪里，一切都是白搭。
好了，这一章就到这里。有问题欢迎交流。