2. 坐标系与状态估计基础：世界坐标系、机体坐标系、欧拉角与四元数、IMU与GPS数据融合简介
各位同学，欢迎来到第二讲。
说实话，做无人机飞控这么多年，我见过太多新手一上来就调PID、写控制律，结果飞机在天上乱飘，炸机了都不知道为什么。问题出在哪？说白了，就是坐标系没搞明白，状态估计一塌糊涂。
你想想看，无人机自己都不知道自己“在哪”、“朝哪”，它怎么飞？所以这一章，咱们把地基打牢。坐标系、姿态表示、传感器融合，这三板斧砍好了，后面的路径规划、避障才有意义。
核心观点： 无人机所有的决策，都建立在“我在哪”和“我朝向哪”这两个问题上。坐标系是描述位置的标尺，姿态是描述朝向的语言，而传感器融合是让这些数据变得可信的手段。
2.1 世界坐标系与机体坐标系
先聊坐标系。我习惯把世界坐标系想象成“上帝视角”。它固定在地面上，通常遵循右手定则：X轴指向东，Y轴指向北，Z轴指向天。这个坐标系是绝对的，我们规划航线、设定目标点，用的都是世界坐标。
那机体坐标系呢？它是“无人机视角”。原点在飞机重心，X轴指向机头，Y轴指向右翼，Z轴指向下。这个坐标系是跟着飞机一起动的。
嗯，这里要注意一个坑：传感器测量的数据，默认都是在机体坐标系下的。比如IMU测的加速度、角速度，都是相对于飞机本身的。你要想把这些数据用到导航里，就必须转换到世界坐标系下。
个人经验： 我在做PX4移植的时候，发现很多新手把加速度计的数据直接积分算速度，结果越算越离谱。为什么？因为加速度计测的是机体坐标系的加速度，而重力方向是朝下的。你如果不先旋转到世界坐标系，重力分量就会污染你的速度估计。
2.2 欧拉角与四元数
好，坐标系搞定了，那怎么描述飞机从机体坐标系到世界坐标系的旋转呢？这就引出了姿态表示。
欧拉角是最直观的。它用三个角度来描述旋转：偏航角（Yaw，绕Z轴转）、俯仰角（Pitch，绕Y轴转）、横滚角（Roll，绕X轴转）。
但欧拉角有个致命问题——万向锁。当俯仰角达到±90度时，偏航和横滚会失去一个自由度，导致姿态解算崩溃。我在做特技飞行时遇到过这个问题，飞机翻过来后姿态直接跳变，差点炸机。
所以，实际工程中我们几乎都用四元数。它用四个数（q0, q1, q2, q3）来表示旋转，没有奇点，计算效率也高。
// 四元数转欧拉角的示例代码（C++）
// 假设四元数为 q = [w, x, y, z]
float roll = atan2(2.0f * (q.w * q.x + q.y * q.z),
1.0f - 2.0f * (q.x * q.x + q.y * q.y));
float pitch = asin(2.0f * (q.w * q.y - q.z * q.x));
float yaw = atan2(2.0f * (q.w * q.z + q.x * q.y),
1.0f - 2.0f * (q.z * q.z + q.y * q.y));
避坑指南： 我曾经在ROS里直接用tf库的quaternion_to_rpy函数，结果发现输出的偏航角范围是[-π, π]，而我的控制器期望的是[0, 2π]。这种细节不注意，飞控就会在边界处来回震荡。
2.3 IMU与GPS数据融合简介
有了坐标系和姿态，接下来就是“状态估计”的重头戏了。说白了，就是怎么把IMU和GPS的数据揉在一起，得到靠谱的位置和速度。
IMU（惯性测量单元）包含加速度计和陀螺仪。它更新快（几百Hz），但误差会随时间累积。你想想看，加速度积分一次得速度，再积分一次得位置，积分两次，误差就爆炸了。
GPS呢？它更新慢（5-10Hz），没有累积误差，但容易受遮挡和多路径效应影响。在树荫下、高楼间，GPS信号飘得你怀疑人生。
所以，工程上的标准做法是：用IMU做高频预测，用GPS做低频修正。这就是卡尔曼滤波的核心思想。
我简单画个流程图，帮大家理解这个融合过程：
这个图很直观：IMU负责“快但会飘”，GPS负责“准但会慢”。卡尔曼滤波器就是那个裁判，它根据两者的置信度（协方差），动态地决定更相信谁。
关键点： 在实际工程中，GPS的置信度不是固定的。当飞机在室内或高楼间，GPS的协方差会变大，卡尔曼滤波器就会自动降低GPS的权重，更多地依赖IMU。反之，在开阔地带，GPS权重增加，IMU的漂移被修正。
2.4 实战中的融合策略
我个人习惯在ROS里用robot_localization包来做融合。它支持多传感器输入，而且内置了扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）。
配置起来其实不复杂，核心就是告诉它：
- 哪些传感器提供什么测量量（位置、速度、姿态）
- 每个传感器的噪声方差是多少
- 传感器之间的时间延迟怎么对齐
# robot_localization 的 EKF 配置示例 (部分)
ekf_filter_node:
frequency: 30
sensor_timeout: 0.1
two_d_mode: false
# IMU 提供姿态和角速度
imu0: /imu/data
imu0_config: [false, false, false, # 位置
true, true, true, # 速度 (角速度)
true, true, true, # 姿态
false, false, false] # 加速度
# GPS 提供位置
odom0: /gps/fix
odom0_config: [true, true, true, # 位置
false, false, false, # 速度
false, false, false, # 姿态
false, false, false] # 加速度
避坑指南： 我曾经在配置IMU时，忘了把加速度计的偏置估计打开。结果飞机悬停时，位置会慢慢漂移。后来加了imu0_remove_gravitational_acceleration: true和偏置估计，问题才解决。嗯，细节决定成败。
2.5 小结
这一章我们聊了坐标系、姿态表示、传感器融合。说白了，就是让无人机知道自己“在哪”和“朝哪”。
世界坐标系是绝对参考，机体坐标系是相对参考。欧拉角直观但有万向锁，四元数稳定无奇点。IMU和GPS融合，就是取长补短，用卡尔曼滤波把两者的优势结合起来。
下一章我们会把这些知识用到实际的路径规划中。但在此之前，我建议你把这一章的内容吃透。坐标系搞错了，后面全是白搭。