【无人机】使用扩展卡尔曼滤波 （EKF） 算法来处理传感器测量，各传感器的参数设置，高度数据融合、不同传感器融合模式
目录
#1、IMU
#2、磁力计
#3、高度
#典型配置
#4、气压计
#静压位置误差修正
#气压计偏置补偿
#5、全球导航系统/全球定位系统--GNSS/GPS
#位置和速度测量
#偏航测量
#GPS 速度的偏航
#双接收器
#GNSS 性能要求
#6、测距
#条件范围辅助-Conditional range aiding
#范围高度融合
#测距仪障碍物检测
#7、空速
#8、合成侧滑
#9、使用阻力比力的多旋翼风力估计
#10、光流
#11、外部视觉系统
使用扩展卡尔曼滤波 （EKF） 算法来处理传感器测量并提供状态的估计
参考：Using the ECL EKF | PX4 Guide (v1.15)
EKF使用哪些传感器测量？
EKF 具有不同的作模式，允许不同的传感器测量组合。 启动时，过滤器检查传感器的最小可行组合，在完成初始倾斜、偏航和高度对准后，进入提供旋转、垂直速度、垂直位置、IMU 角度偏差和 IMU 速度偏差估计的模式。
此模式需要 IMU 数据、偏航源（磁力计或外部视觉）和高度数据源。 所有 EKF作模式都需要这个最小数据集。然后，可以使用其他传感器数据来估计其他状态。
#1、IMU
- 三轴体以最小 100Hz 的速率固定惯性测量单元 delta 角度和 delta 速度数据。 注意：在 EKF 使用 IMU delta 角度数据之前，应将其应用于 IMU delta 角度数据。
#2、磁力计
需要最小速率为 5Hz 的三轴体固定磁力计数据（或外部视觉系统姿态数据）。
磁力计数据可以通过两种方式使用：
- 磁力计测量值使用倾斜估计和磁偏角转换为偏航角。 然后，偏航角被 EKF 用作观测值。
- 这种方法不太准确，并且不允许学习体坐标系场偏移，但它对磁异常和大型启动陀螺仪偏差更可靠。
- 这是启动和接地期间使用的默认方法。
- XYZ 磁力计读数用作单独的观测值。
- 这种方法更准确，但需要正确估计磁力计偏置。
- 当无人机旋转时可以观察到偏差，当无人机加速（线性加速）时可以观察到真实航向。
- 由于偏差可以变化并且仅在移动时才能观察到，因此在不移动时切换回航向融合会更安全。
- 它假设地球磁场环境变化缓慢，当存在明显的外部磁异常时，性能较差。
- 这是车辆移动时使用的默认方法。
- 这种方法更准确，但需要正确估计磁力计偏置。
用于选择这些模式的 logic 由 EKF2_MAG_TYPE 参数设置。 建议使用默认的 “自动 ”模式（），因为它在地面上使用更坚固的磁力计偏航，并在移动时使用更精确的 3 轴磁力计。 始终设置 '3-axis' 模式（）更容易出错，并且需要所有 IMU 都经过良好校准。EKF2_MAG_TYPE=0EKF2_MAG_TYPE=2
该选项可以在没有磁力计的情况下运行，方法是使用双天线 GPS 的偏航来替换磁力计，或使用 IMU 测量和 GPS 速度数据来估计无人机运动的偏航。
#3、高度
高度数据源 - GPS、气压、测距仪、外部视觉或这些的组合，最小速率为 5Hz。
如果不存在任何选定的测量，则 EKF 将不会启动。 当检测到这些测量值时，EKF 将初始化状态并完成倾斜和偏航对齐。 当倾斜和偏航对准完成时，EKF 可以过渡到其他作模式，从而允许使用额外的传感器数据：
每个高度源都可以使用其专用的控制参数来启用/禁用：
- GNSS/GPS：EKF2_GPS_CTRL
- 气压计：EKF2_BARO_CTRL
- 测距仪：EKF2_RNG_CTRL
- 外部视觉：当 EKF2_HGT_REF 设置为 “视觉” 时启用
从长远来看，高度估计遵循高度数据的“参考来源”。 此引用由 EKF2_HGT_REF 参数定义。
#典型配置
| EKF2_GPS_CTRL | EKF2_BARO_CTRL | EKF2_RNG_CTRL | EKF2_HGT_REF | |
|---|---|---|---|---|
| 室外 （默认） | 7 （纬度/纬度/alt/vel） | 1 （已启用） | 1 （有条件的) | 1 个 （GNSS） |
| 室内（非平坦地形） | 0 （禁用） | 1 （已启用） | 1 （有条件的) | 2 （范围） |
| 室内（平坦地形） | 0 （禁用） | 1 （已启用） | 2 （始终启用) | 2 （范围） |
| 外部视野 | 根据需要 | 根据需要 | 根据需要 | 3 （视野） |
#4、气压计
启用/禁用使用 EKF2_BARO_CTRL 作为高度数据源。
请注意，即使有多个气压计可用，也只会融合来自一个气压计的数据。 首先选择具有最高优先级值的气压计CAL_BAROx_PRIO，如果检测到传感器故障，则回退到下一个最高优先级的气压计。 如果气压计的优先级相等，则使用第一个检测到的气压计。 通过将气压计的值设置为 （disabled），可以完全禁用气压计作为可能的源。CAL_BAROx_PRIO0
请参阅 高度 有关高度源配置的更多详细信息。
#静压位置误差修正
气压高度受车辆风、相对速度和方向引起的空气动力学干扰产生的误差的影响。 这在航空学中称为静压位置误差。 使用 ECL/EKF2 估计器库的 EKF2 模块提供了一种补偿这些误差的方法，前提是风速状态估计处于活动状态。
对于在固定翼模式下运行的飞行器，风速状态估计需要启用 Airspeed 和/或 Synthetic Sideslip 融合。
对于多旋翼，可以启用和调整阻力比力 的融合，以提供所需的风速状态估计。
EKF2 模块将误差建模为一个物体固定的椭球体，它指定了在气压中增加/减去的动压分数 - 在转换为高度估计之前。
按如下方式获得良好的调优：
-
在位置模式下在静止和最大速度之间反复向前/向后/向左/向右/向上/向下飞行一次（在静止条件下进行此测试时获得最佳结果）。
-
例如，使用以下方法提取日志文件：
.ulgQGroundControl： 分析 > 日志下载（打开新窗口）注意
相同的日志文件可用于调整多旋翼风力估算器。
-
将日志与baro_static_pressure_compensation_tuning.py （打开新窗口）Python 脚本来获取最佳参数集。
调优参数：
- EKF2_PCOEF_XP
- EKF2_PCOEF_XN
- EKF2_PCOEF_YP
- EKF2_PCOEF_YN
- EKF2_PCOEF_Z
#气压计偏置补偿
由于环境压力环境的变化或传感器温度的变化，恒定高度的气压计的测量值会发生漂移。 为了补偿这个测量误差，EKF2 使用 GNSS 高度（如果有）一个 “非漂移 ”参考来估计偏差。 无需调整。
#5、全球导航系统/全球定位系统--GNSS/GPS
#位置和速度测量
如果满足以下条件，则 GPS 测量值将用于位置和速度：
- 通过设置 EKF2_GPS_CTRL 参数来启用 GPS 使用。
- GPS 质量检查已通过。 这些检查由 EKF2_GPS_CHECK 和 参数 控制。
EKF2_REQ_*
有关高度源配置的更多详细信息，请单击此处。
#偏航测量
一些 GPS 接收器（例如Trimble MB-Two RTK GPS 接收器 （打开新窗口）可用于提供航向测量，以取代磁力计数据的使用。 当在存在大型磁异常的环境中工作时，或者在地球磁场具有较高倾角的纬度上工作时，这可能是一个显著的优势。 通过在 EKF2_GPS_CTRL 参数中将位位置 3 设置为 1（加 8）来启用 GPS 偏航测量。
#GPS 速度的偏航
EKF 在内部运行一个额外的多假设滤波器，该滤波器使用多个 3 态扩展卡尔曼滤波器 （EKF），其状态为 NE 速度和偏航角。 然后使用高斯和滤波器 （GSF） 组合这些单独的偏航角估计值。 单个 3 态 EKF 使用 IMU 和 GPS 水平速度数据（加上可选的空速数据），并且不依赖于偏航角或磁力计测量的任何先验知识。 这为主滤波器的偏航提供了备份，并在起飞后导航丢失表明磁力计的偏航估计很糟糕时，用于重置主 24 态 EKF 的偏航。 这将在 GCS 上产生消息信息消息。Emergency yaw reset - magnetometer use stopped
当 ekf2 重放记录使能时，来自此估计器的数据将被记录下来，并且可以在消息中查看。 来自单个 3 状态 EKF 偏航估计器的单个偏航估计值在字段中。 GSF 综合偏航估计值正在现场。 GSF 偏航估计值的方差在字段中。 所有角度均以弧度为单位。 GSF 对单个 3 态 EKF 输出施加的权重在字段中。yaw_estimator_statusyawyaw_compositeyaw_varianceweight
这也使得在没有任何磁力计数据或双天线 GPS 接收器的情况下运行成为可能，前提是在起飞后可以进行一些水平移动以使偏航变得可观察。 要使用此功能，请将 EKF2_MAG_TYPE 设置为 （5） 以禁用磁力计。 一旦飞行器执行了足够的水平运动以使偏航可观察，主的 24 态 EKF 就会将其偏航与 GSF 估计对齐并开始使用 GPS。none
#双接收器
来自 GPS 接收器的数据可以使用一种算法进行混合，该算法根据报告的精度对数据进行加权（如果两个接收器以相同的速率输出数据并使用相同的精度，则效果最佳）。 如果来自接收器的数据丢失，该机制还提供自动故障转移（例如，它允许使用标准 GPS 作为更准确的 RTK 接收器的备份）。 这由 SENS_GPS_MASK 参数控制。
SENS_GPS_MASK 参数默认设置为禁用混合并始终使用第一个接收器，因此必须设置它以选择使用哪些接收器精度指标来决定每个接收器输出对混合解决方案的贡献。 在使用不同的接收器模型时，将 SENS_GPS_MASK 参数设置为使用两个接收器都支持的准确性指标的值非常重要。 例如，不要将位位置 0 设置为 ，除非两个接收方的驱动程序在消息的字段中发布可比较的值。 由于定义精度的方式不同，例如CEP与1-sigma等，来自不同制造商的接收器可能很难做到这一点。trues_variance_m_svehicle_gps_position
在设置过程中应检查以下项目：
- 验证第二个接收器的数据是否存在。 这将被记录为，也可以在使用 命令 通过 nsh 控制台连接时进行检查。 需要正确设置 GPS_2_CONFIG 参数。
vehicle_gps_position_1listener vehicle_gps_position -i 1 - 检查来自每个接收器的 和 数据，并确定可以使用哪些准确度指标。 如果两个接收器都输出敏感和数据，并且 GPS 垂直位置不直接用于导航，则建议将 SENS_GPS_MASK 设置为 3。 如果只有数据可用且两个接收器都不输出数据，请将 SENS_GPS_MASK 设置为 2。 仅当使用 EKF2_HGT_REF 参数选择 GPS 作为参考高度源并且两个接收器都输出敏感数据时，才会设置位位置 2。
s_variance_m_sephepvs_variance_m_sephephs_variance_m_sepv - 混合接收器数据的输出记录为 ，并且可以在通过 nsh 终端连接时使用命令 进行检查。
ekf_gps_positionlistener ekf_gps_position - 当接收器以不同的速率输出时，混合输出将以较慢的接收器的速率。 在可能的情况下，接收器应配置为以相同的速率输出。
#GNSS 性能要求
要使 ECL 接受 GNSS 数据进行导航，需要在一段时间内满足某些最低要求，由 EKF2_REQ_GPS_H 定义（默认为 10 秒）。
最小值在 EKF2REQ* 参数中定义，每个检查都可以使用 EKF2_GPS_CHECK 参数启用/禁用。
下表显示了根据 GNSS 数据直接报告或计算的不同指标，以及 ECL 要使用的数据所需的最小值。 此外，“平均值”列显示了可能从标准 GNSS 模块（例如 u-blox M8 系列）合理获得的典型值 - 即被认为良好/可接受的值。
| 度量 | 最低要求 | 平均值 | 单位 | 笔记 |
|---|---|---|---|---|
| 弗 | < 3 （EKF2_REQ_EPH) | 0.8 | m | 水平位置误差的标准差 |
| EPV | < 5 （EKF2_REQ_EPV) | 1.5 | m | 垂直位置误差的标准差 |
| 卫星数 | ≥6 （EKF2_REQ_NSATS) | 14 | - | |
| 萨克 | < 0.5 （EKF2_REQ_SACC) | 0.2 | 米/秒 | 水平速度误差的标准偏差 |
| 修复类型 | ≥ 3 | 4 | - | 0-1：无固定，2：2D 固定，3：3D 固定，4：RTCM 代码差分，5：实时运动，浮点，6：实时运动，固定，8：外推 |
| PDOP | < 2.5 （EKF2_REQ_PDOP) | 1.0 | - | 精度的位置稀释 |
| HPOS 漂移率 | < 0.1 （EKF2_REQ_HDRIFT) | 0.01 | 米/秒 | 根据报告的 GNSS 位置计算的漂移率（静止时）。 |
| VPOS 漂移率 | < 0.2 （EKF2_REQ_VDRIFT) | 0.02 | 米/秒 | 根据报告的 GNSS 高度计算的漂移率（静止时）。 |
| HSPD | < 0.1 （EKF2_REQ_HDRIFT) | 0.01 | 米/秒 | 报告的 GNSS 水平速度的过滤幅度。 |
| VSPD | < 0.2 （EKF2_REQ_VDRIFT) | 0.02 | 米/秒 | 报告的 GNSS 垂直速度的过滤量级。 |
注意
的 和 是在 10 秒的时间内计算的，并发布在主题中。 请注意，它不会被记录下来！
hpos_drift_ratevpos_drift_ratehspdekf2_gps_driftekf2_gps_drift
#6、测距
测距仪到地面的距离由单个状态过滤器用于估计地形相对于高度基准面的垂直位置。
融合作模式由EKF2_RNG_CTRL控制：
- 条件范围辅助-Conditional range aiding
- 范围高度融合-Range height fusion
有关高度源配置的更多详细信息，请单击此处。
#条件范围辅助-Conditional range aiding
条件测距仪融合（又名条件测距辅助）在低速/低空作期间（除了其他主动高度源）激活测距仪融合以进行高度估计。
如果测距仪设置为参考高度源（使用 EKF2_HGT_REF），则其他有效高度源（例如气压和 GNSS 高度）将调整其测量值，以匹配测距仪随时间变化的读数。
当不满足启动范围辅助的条件时，将自动选择次级引用。
注意
在高度参考之间切换会导致绝对高度估计随时间漂移。 在位置模式下飞行时，这不是问题，但如果无人机应该在特定的 GNSS 高度执行任务，则可能会出现问题。 如果不需要绝对高度漂移，建议将 GNSS 高度设置为高度参考，即使使用条件范围辅助也是如此。
它主要用于起飞和着陆，在气压计设置导致旋翼清洗干扰过大并可能破坏 EKF 状态估计的情况下。
当车辆静止时，也可以使用辅助测距装置来改善高度保持。
提示
建议使用 Terrain Hold 而不是 Range Aid 进行地形保持。
这是因为地形保持使用普通的 ECL/EKF 估计器来确定高度，在大多数情况下，这通常比距离传感器更可靠。
通过设置 EKF2_RNG_CTRL = “Enabled （conditional mode）” （1） 来启用条件范围辅助。
它可以使用参数进一步配置：EKF2_RNG_A_
- EKF2_RNG_A_VMAX：最大水平速度，超过该速度时将禁用测距辅助。
- EKF2_RNG_A_HMAX：最大高度，超过该高度将禁用测距辅助。
- EKF2_RNG_A_IGATE：Range Aid 一致性检查 “gate” （禁用 Range Aid 之前的误差度量）。
#范围高度融合
PX4 允许您连续融合测距仪作为高度源（在任何飞行模式/车辆类型中）。 这对于保证无人机仅在近乎平坦的表面（例如室内）上飞行的应用可能很有用。
使用距离传感器作为高度源时，乘客应注意：
-
飞越障碍物可能会导致估计器拒绝测距仪数据（由于内部数据一致性检查），这可能导致估计器完全依赖加速度计估计时高度保持不佳。
注意
当车辆以离地面近乎恒定的高度爬上斜坡时，可能会发生这种情况，因为测距仪高度不会发生变化，而从加速度计估计的高度会发生变化。
EKF 执行创新一致性检查，考虑测量和当前状态之间的误差，以及状态的估计方差和测量本身的方差。 如果检查失败，测距仪数据将被拒绝，并且将根据加速度计和其他选定的高度源（GNSS、气压、视觉）（如果启用且可用）估计高度 如果距离传感器是活动源 oh height 数据，则在 5 秒的数据不一致后，估算器会重置高度状态以匹配当前距离传感器数据。
如果一个或多个其他高度源处于活动状态，则测距仪被宣布为故障，估算器继续使用其他传感器估计其高度。 测量值也可能再次变得一致，例如，如果飞机下降，或者如果估计的高度漂移以匹配测得的测距仪高度。
-
本地 NED 原点将随地面上下移动。
-
在不平坦的表面（例如树木）上的测距仪性能可能非常差，从而导致数据嘈杂和不一致。 这再次导致高度保持不佳。
通过将 EKF2_RNG_CTRL 设置为 “Enabled” （2） 来启用该功能。 要使测距仪在激活时成为高度参考，请将：EKF2_HGT_REF设置为“距离传感器”。
提示
要仅在无人机静止时启用测距仪融合（以便在起飞和降落期间获得更好的高度估计），但在其余时间不熔断测距仪，请使用 EKF2_RNG_CTRL 的条件模式 （1）。
#测距仪障碍物检测
EKF 可以使用垂直速度估计和测距仪数据的数值导数之间的运动学一致性检查来检测测距仪对地路径是否被阻挡（可能被有效载荷）。 如果测距仪在统计上与 EKF2 不一致，则传感器在剩余的飞行过程中被拒绝，除非统计测试以 0.5m/s 或更高的垂直速度再次通过至少 1 秒。
仅当测距仪未用作主要高度源时，才会启用该检查，并且仅在机体未水平移动时激活（因为它假定静态地面高度）。
为了有效地进行障碍物检测，需要使用飞行数据严格调整测距仪噪声参数。 然后可以调整运动学一致性门参数以获得所需的故障检测灵敏度。
调优参数：
- EKF2_RNG_NOISE
- EKF2_RNG_K_GATE
#7、空速
等效空速 （EAS） 数据可用于估计风速，并通过将 EKF2_ARSP_THR 设置为正值来减少 GPS 丢失时的漂移。 当空速数据超过EKF2_ARSP_THR正值设置的阈值且机体类型不是旋翼时，将使用空速数据。
#8、合成侧滑
固定翼平台可以利用假设的零侧滑观测来改进风速估计，还可以在没有空速传感器的情况下进行风速估计。 通过将 EKF2_FUSE_BETA 参数设置为 1 来启用此功能。
#9、使用阻力比力的多旋翼风力估计
多旋翼平台可以利用空速和沿 X 和 Y 体轴的阻力之间的关系来估计风速的北/东分量。 这可以使用 EKF2_DRAG_CTRL 来启用。
沿 X 和 Y 体轴的空速和比力（IMU 加速度计测量值）之间的关系由 EKF2_BCOEF_X、EKF2_BCOEF_Y 和 EKF2_MCOEF 参数控制，这些参数分别设置在 X 和 Y 方向上飞行的弹道系数，以及螺旋桨产生的动量阻力。 特定力观察噪声的量由 EKF2_DRAG_NOISE 参数设置。
按如下方式获得良好的调优：
- 在位置模式下在静止和最大速度之间反复向前/向后/向左/向右/向上/向下飞行一次（在静止条件下进行此测试时获得最佳结果）。
- 例如，使用QGroundControl： 分析 > 日志下载（打开新窗口）
注意
相同的 .ulg 日志文件也可用于调整静压位置误差系数。
- 将日志与mc_wind_estimator_tuning.py （打开新窗口）Python 脚本来获取最佳参数集。
#10、光流
如果满足以下条件，将使用光流数据：
- 提供有效的测距仪数据。
- EKF2_OF_CTRL 已设置。
- 流量传感器返回的质量指标大于 EKF2_OF_QMIN 参数设置的最低要求。
#11、外部视觉系统
可以使用来自外部视觉系统（例如 Vicon）的位置、速度或方向测量。
通过将 EKF2_EV_CTRL 的相应位设置为 ：true
0：水平位置数据1：垂直位置数据。 高度源还可以使用 EKF2_HGT_REF 进行配置（参见 高度 部分）。2：速度数据3：偏航数据
请注意，如果使用偏航数据（位 3），则航向是相对于外部视觉框架的;否则，航向是相对于北方的。
EKF 考虑了视觉姿态估计中的不确定性。 这些不确定性信息可以通过 MAVLink 中的协方差字段发送里程计 （打开新窗口）消息，也可以通过参数 EKF2_EVP_NOISE、 EKF2_EVV_NOISE 和 EKF2_EVA_NOISE 进行设置。 您可以使用 EKF2_EV_NOISE_MD 选择不确定性的来源。