第9课扩展卡尔曼滤波(EKF)

掌握EKF处理非线性系统的方法，理解雅可比矩阵与线性化误差。

1. 为什么需要EKF

线性: x = A·x + B·u  → KF直接适用
非线性: x = f(x,u)     → EKF线性化

f(x) ≈ f(x̂) + F·(x-x̂)   F = ∂f/∂x (雅可比)
h(x) ≈ h(x̂) + H·(x-x̂)   H = ∂h/∂x

2. EKF算法

预测步

x̂⁻ = f(x̂,u)　P⁻ = F·P·F^T+Q

更新步

K = P⁻·H^T·(H·P⁻·H^T+R)^-1

x̂ = x̂⁻ + K·(z-h(x̂⁻))　P = (I-K·H)·P⁻

3. 15态无人机EKF

x = [pos_N, pos_E, pos_D,      # 位置(3)
     vel_N, vel_E, vel_D,      # 速度(3)
     qw, qx, qy, qz,          # 四元数(4)
     gyro_bx, gyro_by, gyro_bz, # 陀螺零偏(3)
     accel_bx, accel_by]        # 加计零偏(2) = 15态

4. 雅可比矩阵计算

def attitude_jacobian(q, omega):
    """姿态运动学雅可比"""
    w,x,y,z = q
    return np.array([
        [0, -omega[0], -omega[1], -omega[2]],
        [omega[0], 0, omega[2], -omega[1]],
        [omega[1], -omega[2], 0, omega[0]],
        [omega[2], omega[1], -omega[0], 0]
    ]) * 0.5

5. 仿真验证

6. EKF局限性

方法	原理	优势	代价
UKF	无迹变换	无需雅可比,三阶精度	计算稍大
IEKF	迭代更新	减小线性化误差	多次迭代
粒子滤波	蒙特卡洛	任意非高斯	计算量大

7. 小结

✅ EKF通过雅可比线性化处理非线性

✅ 15态EKF：位置+速度+姿态+零偏

✅ 大角度机动注意线性化误差

扩展阅读与实践

关键公式回顾

本课涉及的核心公式和算法需要反复练习才能真正掌握。建议通过修改仿真参数、添加扰动等方式深入理解每个参数对系统行为的影响。在实际飞行中，这些参数的选择往往需要在理论分析的基础上结合实验微调。

🔧 工程实践要点

实际部署时注意数值稳定性(归一化/限幅/防除零)
实时性要求: 控制环1kHz+，导航环50-100Hz
传感器数据时间戳对齐至关重要
异常处理: 传感器失效需安全降级策略
日志记录: 飞行数据全程记录便于事后分析
固件升级: 始终使用稳定版本，升级前备份参数

📊 性能指标参考

指标	消费级	工业级	研究级
姿态精度	±2°	±0.5°	±0.1°
位置精度(GPS)	±2m	±0.5m	±2cm(RTK)
悬停精度	±1m	±0.3m	±0.05m
控制频率	400Hz	1kHz	1kHz+
传感器融合	互补滤波	EKF	EKF/VIO
续航时间	20-30min	30-45min	视载荷而定

10. 深度工程实践

EKF的雅可比矩阵推导是工程实现中最容易出错的环节。常见错误包括：坐标系混淆(惯性系vs机体系)、旋转方向反号、角速度与姿态导数的混淆。建议：(1)用数值差分验证解析雅可比的正确性；(2)单元测试每个雅可比矩阵；(3)在SITL中用已知轨迹验证EKF精度。

关键参数速查

参数	典型值	影响
控制频率	400-1000Hz	稳定性/延迟
EKF频率	200-500Hz	估计精度
导航频率	10-50Hz	路径跟踪
传感器延迟	5-200ms	需时间补偿
电池警戒	25%→RTH	安全返航

常见故障排查

飞行振荡→减小PID增益或增大微分项
漂移→检查传感器校准和零偏估计
突然倾斜→检查电机/电调/桨是否有损坏
定位跳变→检查GPS质量/多径干扰
高度异常→检查气压计通风和温度补偿

深入专题：算法实现与优化

算法复杂度分析

在实际飞控系统中，算法必须在有限的计算资源下实时运行。以下为本课核心算法的复杂度分析：

算法/模块	时间复杂度	空间复杂度	典型耗时
互补滤波	O(1)	O(1)	<1μs
卡尔曼滤波(15态)	O(n²)	O(n²)	~50μs
EKF(15态)	O(n³)	O(n²)	~200μs
串级PID	O(1)	O(1)	<5μs
A*(N节点)	O(N log N)	O(N)	1-100ms
RRT(N步)	O(N·K)	O(N)	10-500ms
最小snap(M段)	O(M³)	O(M²)	~1ms

实时系统设计原则

无人机飞控是典型的实时嵌入式系统，必须满足严格的时序约束：

硬实时：姿态控制环必须在1ms内完成(1kHz)，否则可能失控
软实时：EKF更新应在5ms内完成(200Hz)，偶尔超时可接受
非实时：路径规划可在后台线程运行，100ms-1s响应均可

# 典型飞控任务优先级(高→低)
# 1. IMU采样+姿态估计 (1kHz, 优先级最高)
# 2. 角速度PID控制     (1kHz)
# 3. 姿态PID控制       (500Hz)
# 4. EKF状态更新       (200Hz)
# 5. 位置/速度控制      (100Hz)
# 6. 路径规划/避障      (10-50Hz)
# 7. 通信/日志          (1-10Hz)

# RTOS调度: 优先级抢占 + 时间片轮转

代码质量与测试

飞控软件是安全关键系统，代码质量要求极高：

编码规范：MISRA C/C++，禁止动态内存分配
静态分析：Coverity/Cppcheck扫描所有警告
单元测试：每个算法模块100%覆盖率
SIL测试：软件在环仿真验证功能
HIL测试：硬件在环仿真验证时序
飞行测试：从悬停开始逐步扩大飞行包线

# 单元测试示例
def test_pid_output():
    pid = PID(kp=1.0, ki=0, kd=0)
    assert pid.update(1.0, 0.01) == 1.0  # P=1*1.0

def test_pid_integral_limit():
    pid = PID(kp=0, ki=1.0, kd=0, i_limit=5.0)
    for _ in range(1000):
        pid.update(1.0, 0.01)  # 大量积分
    assert abs(pid.integral) <= 5.0  # 不超过限幅

def test_kalman_convergence():
    kf = KalmanFilter(...)
    for _ in range(100):
        kf.predict()
        kf.update(measurement)
    assert kf.P[0,0] < initial_P  # 协方差下降

开源飞控架构对比

特性	PX4	ArduPilot	Betaflight
定位	研究/工业	工业/爱好者	竞速穿越
代码量	~500K行	~800K行	~200K行
支持的机型	多旋翼/固定翼/VTOL	多旋翼/固定翼/直升机/车/船	多旋翼/固定翼
导航能力	强(全面)	强(最全面)	弱(仅自稳)
实时性	NuttX RTOS	ChibiOS/RTOS	Bare-metal
仿真支持	SITL/Gazebo/AirSim	SITL/Gazebo	有限
社区活跃度	高	最高	高(竞速圈)

安全关键系统设计

无人机系统属于安全关键(Safety-Critical)系统，设计时必须遵循以下原则：

Fail-safe设计：任何单点故障不应导致灾难性后果
冗余设计：关键传感器(IMU/GPS)至少双冗余
看门狗：硬件看门狗检测软件挂起，超时复位
状态机：明确的飞行状态转换，禁止非法状态
地理围栏：硬编码的飞行区域限制
最大倾斜角限制：防止翻机失控
电池保护：低压自动返航/降落
信号丢失保护：遥控信号丢失5s自动RTH

实战案例与行业应用

典型应用场景

本课所学技术在以下场景中直接应用：

电力巡检：自主飞行沿输电线路巡检，自动识别绝缘子缺陷、连接点过热、树障距离不足等问题。每天可完成30-50基铁塔的巡检任务，效率是人工的10倍以上。
应急救援：灾区快速测绘，10分钟内生成1km2正射影像，为救援决策提供实时地图。红外热成像搜索受困人员，夜间也能有效搜救。
精准农业：多光谱相机获取作物NDVI指数，识别病虫害区域，指导变量施药。相比全田喷洒节约30-50%农药。
建筑监测：定期无人机巡检生成三维模型，对比分析建筑变形、沉降、裂缝发展。精度可达毫米级。
物流配送：山区/海岛等交通不便区域的医疗物资、快递配送。无人机30分钟内可覆盖20km半径范围。

系统集成经验

将本课模块集成到完整系统时的注意事项：

传感器数据流：IMU 1kHz -> 姿态估计 500Hz -> 控制 400Hz -> 电机PWM 400Hz
时间戳同步：所有传感器数据必须使用同一时间基准(通常为飞控启动后的微秒计数)
数据延迟补偿：GPS延迟约100ms，需在EKF中用IMU前向传播补偿
传感器切换：GPS可用时用GPS，室内切换到光流/VIO，需平滑过渡
故障检测：EKF创新序列(innovation)监测传感器异常，过大则降低该传感器权重
参数存储：所有PID/滤波器参数持久化到Flash，上电自动恢复

开发环境搭建指南

要实践本课内容，需要搭建以下开发环境：

# 1. Python环境(仿真开发)
sudo apt install python3 python3-pip
pip3 install numpy scipy matplotlib

# 2. PX4开发环境(飞控开发)
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
make px4_sitl jmavsim  # 验证安装

# 3. ArduPilot开发环境
git clone https://github.com/ArduPilot/ardupilot.git --recursive
pip3 install pymavlink MAVProxy dronekit

# 4. 仿真环境
# Gazebo: 3D物理仿真
sudo apt install gazebo11 libgazebo11-dev
# AirSim: 高保真视觉仿真
# 从GitHub Releases下载预编译版本

# 5. 地面站
# QGroundControl: sudo ./QGroundControl.AppImage
# Mission Planner: Windows平台

学习路径建议

根据不同的学习目标，建议以下路径：

目标	重点课程	实践项目
飞控算法工程师	1-9,25-26	自定义PID+姿态估计算法
导航规划工程师	10-18,27	A*+RRT避障巡检系统
集群系统工程师	19-24	多机编队协同任务
全栈无人机工程师	全部30课	完整自主巡检系统

关键参考文献

全权《多旋翼设计与控制》- 四旋翼系统设计必读
Thrun等《Probabilistic Robotics》- 卡尔曼滤波与SLAM理论
LaValle《Planning Algorithms》- 路径规划算法大全
PX4开发文档: https://docs.px4.io/
ArduPilot文档: https://ardupilot.org/
MATLAB无人机工具箱: 官方仿真与代码生成

🚁 第9课：扩展卡尔曼滤波(EKF)

📚 本课目标