第4章状态估计入门

第4章：状态估计入门：卡尔曼滤波思想、线性卡尔曼滤波、扩展卡尔曼滤波、传感器融合

各位同学，欢迎来到状态估计的世界。说实话，我刚开始做无人机那会儿，最头疼的就是传感器数据——GPS飘得离谱，IMU（惯性测量单元）又一直在漂。你想想看，一个无人机在天上飞，你连它到底在哪儿都搞不清楚，这还怎么控制？

后来我明白了，我们需要一个「大脑」，能把一堆不靠谱的传感器数据揉在一起，猜出最靠谱的状态。这个大脑，就是卡尔曼滤波器。今天咱们就把它聊透。

4.1 卡尔曼滤波的核心思想：猜，然后修正

卡尔曼滤波说白了就两步：预测和更新。我习惯把它比作「开车时的预判」。

预测：你根据上一秒的位置和速度，猜下一秒在哪儿。这叫「先验估计」。
更新：你低头看了一眼GPS，发现实际位置跟猜的不一样。于是你修正一下自己的猜测。这叫「后验估计」。

关键来了：你有多相信自己的预测？又有多相信传感器的测量？卡尔曼滤波用一个叫「卡尔曼增益」的东西来动态调整这个信任度。

核心思想一句话：用模型预测未来，用测量修正预测，两者加权平均，权重由不确定性决定。

我在项目中遇到过一个问题：如果GPS信号突然变差，卡尔曼增益会自动降低测量权重，更多地依赖IMU的预测。这就是它聪明的地方——自适应。

4.2 线性卡尔曼滤波（LKF）

线性卡尔曼滤波假设系统是线性的，噪声是高斯白噪声。说白了，就是状态变化可以用一个矩阵乘法搞定。

4.2.1 状态空间模型

我们用一个简单的例子：一维位置和速度估计。

// 状态向量：x = [位置, 速度]^T
// 状态转移方程：x_k = A * x_{k-1} + B * u_k + w_k
// 测量方程：z_k = H * x_k + v_k

// 其中：
// A = [1, dt; 0, 1]   // 状态转移矩阵
// H = [1, 0]          // 测量矩阵（只测量位置）
// w_k ~ N(0, Q)       // 过程噪声
// v_k ~ N(0, R)       // 测量噪声

嗯，这里要注意：Q和R是调参的关键。Q越大，说明你越不相信模型；R越大，说明你越不相信传感器。我刚开始调的时候，把Q设得太小，结果滤波器反应迟钝，无人机飞起来像喝醉了酒。

4.2.2 五个核心公式

线性卡尔曼滤波就五个公式，我建议你背下来：

步骤	公式	说明
预测状态	x_pred = A * x_est + B * u	根据模型猜下一步
预测协方差	P_pred = A * P_est * A^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	修正后的不确定性

个人经验：调参时，先把R固定，然后慢慢增大Q。你会发现滤波器从「反应迟钝」变成「反应灵敏」，再变成「噪声敏感」。找到中间那个平衡点，就是最佳参数。

4.3 扩展卡尔曼滤波（EKF）

现实世界哪有那么多线性系统？无人机飞行时，角度和位置的关系是非线性的。这时候线性卡尔曼滤波就歇菜了。

扩展卡尔曼滤波的思想很简单：把非线性函数线性化。怎么线性化？用泰勒展开，取一阶近似。

4.3.1 核心变化

状态转移：不再是A*x，而是f(x)，一个非线性函数
测量方程：不再是H*x，而是h(x)，也是一个非线性函数
雅可比矩阵：用f对x的偏导数（雅可比矩阵）代替原来的A和H

// 假设状态 x = [位置, 速度, 角度]^T
// 非线性状态转移：f(x) = [位置 + 速度*cos(角度)*dt, 
//                          速度 + 加速度*dt, 
//                          角度 + 角速度*dt]

// 计算雅可比矩阵 F = df/dx
// F = [[1, cos(角度)*dt, -速度*sin(角度)*dt],
//      [0, 1, 0],
//      [0, 0, 1]]

// 然后就用这个F代替原来的A，继续套用五个公式

避坑指南：我曾经在EKF里忘记更新雅可比矩阵，结果滤波器发散得一塌糊涂。记住：每次预测都要重新计算雅可比，因为状态变了，线性化点也变了。

4.3.2 EKF的局限性

说实话，EKF有个大问题：线性化误差。如果系统非线性很强，一阶近似就不够用了。这时候可以考虑UKF（无迹卡尔曼滤波）或者粒子滤波，但那是进阶内容了。

4.4 传感器融合实战

好了，理论讲完了，咱们来点实际的。无人机上最常见的传感器融合就是GPS + IMU。

4.4.1 为什么需要融合？

GPS：低频（10Hz），有漂移，但长期稳定
IMU：高频（100Hz以上），短期精准，但长期积分会漂

你想想看，如果只用IMU，飞10秒后位置可能偏了10米。如果只用GPS，更新太慢，控制跟不上。融合之后，用IMU做高频预测，用GPS做低频修正，完美互补。

4.4.2 融合架构

// 伪代码：GPS + IMU 融合
// 初始化
x = [0, 0, 0, 0]  // [位置x, 位置y, 速度x, 速度y]
P = eye(4) * 100   // 初始不确定性很大

// 主循环（100Hz）
while (true) {
    // 1. IMU数据到达（高频）
    accel = readIMU()  // 加速度
    gyro = readGyro()  // 角速度
    
    // 预测步骤
    x_pred = f(x, accel, gyro, dt)
    P_pred = F * P * F^T + Q
    
    // 2. GPS数据到达（低频，10Hz）
    if (newGPSData()) {
        gps_pos = readGPS()
        z = gps_pos
        
        // 更新步骤
        K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
        x = x_pred + K * (z - H * x_pred)
        P = (I - K * H) * P_pred
    } else {
        // 没有GPS，直接用预测值
        x = x_pred
        P = P_pred
    }
    
    // 输出融合后的状态
    outputState(x)
}

关键点：IMU的预测频率远高于GPS的更新频率。在两次GPS更新之间，我们完全依赖IMU做预测。GPS来了，就修正一次。这就是「松耦合」融合。

4.4.3 调参心得

我调过很多次这个融合滤波器，有几个坑你一定要避开：

Q矩阵别设太小：IMU有零偏，如果Q太小，滤波器会过于相信IMU，导致位置漂移无法被修正
R矩阵要动态调整：GPS在城市峡谷里噪声很大，这时候R应该设大一些。我习惯根据GPS的HDOP（水平精度因子）来动态调整R
初始化很重要：第一次上电时，P矩阵设大一些，让滤波器快速收敛。我曾经因为P设太小，导致滤波器花了10分钟才收敛

进阶技巧：如果你发现EKF在强非线性下表现不好，可以试试「迭代EKF」（IEKF）。它在更新步骤里多迭代几次，能显著降低线性化误差。我在一个四旋翼项目中用过，效果立竿见影。

4.5 本章小结

状态估计是无人机导航的基石。说白了，就是用模型预测，用传感器修正，用不确定性来权衡。线性卡尔曼滤波适合简单系统，扩展卡尔曼滤波能处理非线性，传感器融合则把多个传感器的优势结合起来。

最后送你一句话：没有完美的传感器，只有聪明的算法。卡尔曼滤波就是那个让不完美的传感器变得有用的算法。下一章，咱们聊聊路径规划，看看怎么让无人机自己找到最优路线。

📚 课程目录