目录
1. 高度融合原理
气压计与 GPS 高度特性对比
融合目标
2. 高度融合 EKF 设计
状态向量
状态方程（预测）
观测方程（更新）
观测矩阵：
观测噪声协方差矩阵：
3. 代码实现（STM32 HAL 库）
4. 集成到无人机悬停控制
数据读取与融合流程
5. 参数调优与测试建议
参数初始值
调优步骤
我们在现有的GPS + 光流水平位置融合 EKF基础上，增加一个高度融合 EKF，融合气压计和GPS 高度数据，从而实现无人机在三维空间的精准悬停。目标精度：
- 水平位置：±0.5 米（GPS + 光流融合）
- 高度：±0.2 米（气压计 + GPS 高度融合）
1. 高度融合原理
气压计与 GPS 高度特性对比
| 传感器 | 优势 | 劣势 |
|---|---|---|
| 气压计 | 高刷新率（50~200Hz），噪声小（±0.1 米） | 存在漂移（温度、天气影响） |
| GPS 高度 | 全局绝对高度，无漂移 | 噪声大（±1~5 米），刷新率低（1~10Hz） |
融合目标
- 短期：用气压计的高刷新率数据保持高度跟踪稳定性。
- 长期：用 GPS 高度修正气压计的漂移。
- 结果：输出一个低噪声、高刷新率、无漂移的高度估计。
2. 高度融合 EKF 设计
状态向量
状态向量定义为：X=[zvz]其中：
- z：高度（米）
- vz：垂直速度（米 / 秒）
状态方程（预测）
基于匀速模型：Xk∣k−1=F⋅Xk−1∣k−1+wk状态转移矩阵：F=[10dt1]过程噪声协方差矩阵 Q：Q=[σz200σvz2]
观测方程（更新）
有两个观测源：
- 气压计：直接观测高度 zbaro
- GPS：直接观测高度 zgps
观测矩阵：
H=[10]
观测噪声协方差矩阵：
- 气压计：Rbaro=σbaro2
- GPS：Rgps=σgps2
3. 代码实现（STM32 HAL 库）
#include <math.h> // 高度 EKF 状态向量 typedef struct { float z; // 高度 (m) float vz; // 垂直速度 (m/s) } HeightEKF_State; // 高度 EKF 协方差矩阵 typedef struct { float P[2][2]; } HeightEKF_Covariance; // 传感器数据 typedef struct { float z; // 气压计高度 (m) } Baro_Data; typedef struct { float z; // GPS 高度 (m) } GPS_Height_Data; // 高度 EKF 参数 typedef struct { HeightEKF_State state; HeightEKF_Covariance cov; float dt; // 采样时间 (s) // 过程噪声 float Q_z; // 高度过程噪声方差 float Q_vz; // 速度过程噪声方差 // 观测噪声 float R_baro; // 气压计噪声方差 float R_gps; // GPS 高度噪声方差 } HeightEKF; // 初始化高度 EKF void HeightEKF_Init(HeightEKF *ekf, float dt) { ekf->dt = dt; ekf->Q_z = 0.01f; // 高度过程噪声 ekf->Q_vz = 0.1f; // 速度过程噪声 ekf->R_baro = 0.01f; // 气压计噪声方差 (0.1m)^2 ekf->R_gps = 4.0f; // GPS 高度噪声方差 (2m)^2 // 初始状态 ekf->state.z = 0.0f; ekf->state.vz = 0.0f; // 初始协方差 ekf->cov.P[0][0] = 1.0f; // 初始高度不确定度 ekf->cov.P[1][1] = 1.0f; // 初始速度不确定度 ekf->cov.P[0][1] = 0.0f; ekf->cov.P[1][0] = 0.0f; } // 预测步骤 void HeightEKF_Predict(HeightEKF *ekf) { // 状态预测 ekf->state.z += ekf->state.vz * ekf->dt; // 状态转移矩阵 F float F[2][2] = { {1, ekf->dt}, {0, 1} }; // 过程噪声 Q float Q[2][2] = { {ekf->Q_z, 0}, {0, ekf->Q_vz} }; // P = F*P*F^T + Q float P_temp[2][2]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 2; k++) { P_temp[i][j] += F[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { ekf->cov.P[i][j] = 0.0f; for (int k = 0; k < 2; k++) { ekf->cov.P[i][j] += P_temp[i][k] * F[j][k]; } ekf->cov.P[i][j] += Q[i][j]; } } } // 更新步骤（气压计） void HeightEKF_Update_Baro(HeightEKF *ekf, Baro_Data *baro) { // 观测矩阵 H = [1, 0] float H[1][2] = {1, 0}; // 观测噪声 R float R = ekf->R_baro; // 残差 z - H*x float z = baro->z; float z_pred = ekf->state.z; float y = z - z_pred; // S = H*P*H^T + R float S = 0.0f; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { S += H[0][i] * ekf->cov.P[i][j] * H[0][j]; } } S += R; // 卡尔曼增益 K = P*H^T*S^{-1} float K[2]; for (int i = 0; i < 2; i++) { K[i] = 0.0f; for (int j = 0; j < 2; j++) { K[i] += ekf->cov.P[i][j] * H[0][j]; } K[i] /= S; } // 更新状态 ekf->state.z += K[0] * y; ekf->state.vz += K[1] * y; // 更新协方差 P = (I - K*H)*P float I_KH[2][2]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { I_KH[i][j] = (i == j) ? 1.0f : 0.0f; I_KH[i][j] -= K[i] * H[0][j]; } } float P_temp[2][2]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 2; k++) { P_temp[i][j] += I_KH[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { ekf->cov.P[i][j] = P_temp[i][j]; } } } // 更新步骤（GPS 高度） void HeightEKF_Update_GPS(HeightEKF *ekf, GPS_Height_Data *gps_height) { // 观测矩阵 H = [1, 0] float H[1][2] = {1, 0}; // 观测噪声 R float R = ekf->R_gps; // 残差 z - H*x float z = gps_height->z; float z_pred = ekf->state.z; float y = z - z_pred; // S = H*P*H^T + R float S = 0.0f; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { S += H[0][i] * ekf->cov.P[i][j] * H[0][j]; } } S += R; // 卡尔曼增益 K = P*H^T*S^{-1} float K[2]; for (int i = 0; i < 2; i++) { K[i] = 0.0f; for (int j = 0; j < 2; j++) { K[i] += ekf->cov.P[i][j] * H[0][j]; } K[i] /= S; } // 更新状态 ekf->state.z += K[0] * y; ekf->state.vz += K[1] * y; // 更新协方差 P = (I - K*H)*P float I_KH[2][2]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { I_KH[i][j] = (i == j) ? 1.0f : 0.0f; I_KH[i][j] -= K[i] * H[0][j]; } } float P_temp[2][2]; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { P_temp[i][j] = 0.0f; for (int k = 0; k < 2; k++) { P_temp[i][j] += I_KH[i][k] * ekf->cov.P[k][j]; } } } for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { ekf->cov.P[i][j] = P_temp[i][j]; } } }4. 集成到无人机悬停控制
数据读取与融合流程
HeightEKF height_ekf; Baro_Data baro; GPS_Height_Data gps_height; void Hover_Control_Loop(void) { float dt = 0.01f; // 100Hz // 1. 读取传感器数据 read_baro(&baro); // 气压计高度 (m) read_gps_height(&gps_height); // GPS 高度 (m) // 2. 高度 EKF 预测 HeightEKF_Predict(&height_ekf); // 3. 高度 EKF 更新（气压计每次循环，GPS 每秒一次） HeightEKF_Update_Baro(&height_ekf, &baro); static uint32_t gps_height_update_count = 0; if (gps_height_update_count++ >= 100) { // 100Hz * 0.01s = 1s HeightEKF_Update_GPS(&height_ekf, &gps_height); gps_height_update_count = 0; } // 4. 位置环 PID（使用高度 EKF 输出） pid_pos_z.setpoint = target_pos.z; pid_pos_z.feedback = height_ekf.state.z; PID_Update(&pid_pos_z, dt); // 5. 姿态环与电机控制（同上） ... }5. 参数调优与测试建议
参数初始值
- 过程噪声：
Q_z = 0.01：高度过程噪声较小，假设无人机垂直运动平稳。Q_vz = 0.1：速度过程噪声较大，允许垂直速度变化。
- 观测噪声：
R_baro = 0.01：气压计噪声方差（0.1m）。R_gps = 4.0：GPS 高度噪声方差（2m）。
调优步骤
- 室内测试：关闭 GPS 高度更新，仅用气压计，调整
Q和R_baro使高度跟踪稳定。 - 室外测试：开启 GPS 高度更新，调整
R_gps使 GPS 修正不过于频繁或剧烈。 - 最终验证：悬停时高度误差应控制在±0.2 米以内。
✅ 我已经给你一个完整的三维位置融合方案，包括：
- 水平位置：GPS + 光流 EKF（±0.5 米）
- 高度：气压计 + GPS 高度 EKF（±0.2 米）
这样无人机可以在三维空间中实现高精度悬停。
