PX4-Autopilot 中 EKF2 估计器选择状态（EstimatorSelectorStatus）uORB 消息深度解析

EKF2（扩展卡尔曼滤波器）是 PX4 飞控中负责姿态、位置与速度融合估计的核心模块。当飞控同时挂载多套 IMU（陀螺仪/加速度计）与气压计等冗余传感器时，系统会并行运行多个 EKF2 实例，由 EKF2Selector 在运行时选出最健康的实例作为主估计器。EstimatorSelectorStatus（uORB topic：estimator_selector_status）正是 EKF2Selector 对外发布的状态消息，它记录了当前主实例编号、可用实例数量、切换统计、各实例的健康/误差评分以及 IMU 一致性故障检测结果，是排查估计器故障、多 IMU 冗余切换和飞行日志分析的核心数据源。本文将以 EstimatorSelectorStatus.md 为主线，结合 EKF2Selector.cpp 与 EKF2Selector.hpp 的源码实现，逐一拆解该消息的全部字段语义、底层生成逻辑、触发条件与常见使用场景。

消息定义与字段总览

estimator_selector_status 是 EKF2 模块对外发布的 uORB 消息之一，消息定义源文件位于 msg/EstimatorSelectorStatus.msg，共包含 15 个字段，按功能可划分为四组：选择状态、设备标识、各实例评分与 IMU 故障检测。

完整字段定义表

以下为消息的完整字段定义（类型、数组长度与语义均以 msg/EstimatorSelectorStatus.msg 为准）：

字段名	类型	说明
timestamp	uint64	消息发布时间戳，自系统启动以来的微秒数（hrt_absolute_time()）
primary_instance	uint8	当前被选为主估计器的 EKF 实例编号
instances_available	uint8	当前可用的 EKF 实例数量（至少一个 IMU 实例运行）
instance_changed_count	uint32	主实例切换累计次数（计数器）
last_instance_change	uint64	最近一次主实例切换的时间戳（微秒）
accel_device_id	uint32	主实例当前使用的加速度计设备 ID
baro_device_id	uint32	主实例当前使用的气压计设备 ID
gyro_device_id	uint32	主实例当前使用的陀螺仪设备 ID
mag_device_id	uint32	主实例当前使用的磁力计设备 ID
combined_test_ratio	float32[9]	各实例的综合测试比（velocity/position/height 测试比的组合），≥1 表示失败
relative_test_ratio	float32[9]	各实例相对主实例的误差评分（累计误差增量），在 [-1, 1] 区间内
healthy	bool[9]	各实例是否健康（带迟滞滤波的状态）
accumulated_gyro_error	float32[4]	各陀螺仪相对均值一致性检查的累积角误差（弧度）
accumulated_accel_error	float32[4]	各加速度计相对均值一致性检查的累积速度误差（m/s）
gyro_fault_detected	bool	是否检测到陀螺仪故障
accel_fault_detected	bool	是否检测到加速度计故障

消息源文件中还保留着原始 C 语言定义，其结构为：

uint64 timestamp		# time since system start (microseconds)

uint8 primary_instance

uint8 instances_available

uint32 instance_changed_count
uint64 last_instance_change

uint32 accel_device_id
uint32 baro_device_id
uint32 gyro_device_id
uint32 mag_device_id

float32[9] combined_test_ratio
float32[9] relative_test_ratio
bool[9] healthy

float32[4] accumulated_gyro_error
float32[4] accumulated_accel_error
bool gyro_fault_detected
bool accel_fault_detected

数组长度的设计约束

消息中的数组长度并非随意设定，而是与 EKF2 的实际能力上限严格绑定。在 EKF2Selector.hpp 中定义了实例数量上限：

#if CONSTRAINED_MEMORY
# define EKF2_MAX_INSTANCES 2
#else
# define EKF2_MAX_INSTANCES 9
#endif

即在内存受限的平台上（CONSTRAINED_MEMORY），最多运行 2 个 EKF 实例；在常规平台（如大多数 STM32 飞控与 SITL）上最多 9 个实例。因此 combined_test_ratio、relative_test_ratio、healthy 均声明为长度 9 的数组。accumulated_gyro_error 与 accumulated_accel_error 的长度为 4，对应 sensors_status_imu 消息中 gyro_inconsistency_rad_s / accel_inconsistency_m_s_s 数组的长度（IMU_STATUS_SIZE，即最大支持 4 个 IMU 一致性比较）。源码中通过 static_assert 确保这些数组不会越界，例如：

static_assert(EKF2_MAX_INSTANCES <= sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof(
              estimator_selector_status_s::combined_test_ratio[0]),
              "increase estimator_selector_status_s::combined_test_ratio size");

如果未来提升实例或 IMU 上限，必须同步扩大消息数组，否则编译将直接失败。

谁在发布与消费这条消息

发布方：EKF2Selector

EKF2Selector 继承自 ModuleParams 与 px4::ScheduledWorkItem，以 10ms（FILTER_UPDATE_PERIOD）周期运行，其成员中包含 estimator_selector_status 的 uORB 发布器：

uORB::Publication<estimator_selector_status_s> _estimator_selector_status_pub{ORB_ID(estimator_selector_status)};

消息的组装与发布集中在 PublishEstimatorSelectorStatus() 函数（见 EKF2Selector.cpp）：

void EKF2Selector::PublishEstimatorSelectorStatus()
{
	estimator_selector_status_s selector_status{};
	selector_status.primary_instance = _selected_instance;
	selector_status.instances_available = _available_instances;
	selector_status.instance_changed_count = _instance_changed_count;
	selector_status.last_instance_change = _last_instance_change;
	selector_status.accel_device_id = _instance[_selected_instance].accel_device_id;
	selector_status.baro_device_id = _instance[_selected_instance].baro_device_id;
	selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id;
	selector_status.mag_device_id = _instance[_selected_instance].mag_device_id;
	selector_status.gyro_fault_detected = _gyro_fault_detected;
	selector_status.accel_fault_detected = _accel_fault_detected;

	for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
		selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
		selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
		selector_status.healthy[i] = _instance[i].healthy.get_state();
	}

	for (int i = 0; i < IMU_STATUS_SIZE; i++) {
		selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
		selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
	}

	selector_status.timestamp = hrt_absolute_time();
	_estimator_selector_status_pub.publish(selector_status);
	_last_status_publish = selector_status.timestamp;
}

注意：primary_instance 直接取自内部成员 _selected_instance（初始为 INVALID_INSTANCE，即 UINT8_MAX）；四个 *_device_id 均取自被选中的主实例对应的传感器 ID，而非当前正在使用的传感器（当前使用传感器由 sensor_selection 消息另行发布，参见 sensor_selection.msg）。

消息的发布时机

尽管 EKF2Selector 每 10ms 调度一次，estimator_selector_status 并非固定频率发布，而是采用条件触发 + 定期兜底的策略（见 EKF2Selector.cpp）：

• 当实例健康状态发生变化、主实例切换计数变化、检测到 IMU 故障时，将 _selector_status_publish 置位，下一次调度立即发布；
• 当某个备选实例的 relative_test_ratio 正在逼近切换阈值（预示着可能的实例切换）时，会提高发布频率以便下游及时感知；
• 若长时间无事件触发，则以兜底周期发布，保证日志与订阅方始终能拿到最新状态（_last_status_publish 间隔受 hrt_elapsed_time 控制）。

消费方：Commander 健康检查

estimator_selector_status 最核心的消费方是 Commander 的估计器健康检查模块 estimatorCheck.cpp：

estimator_selector_status_s estimator_selector_status;
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
	bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance)
				&& _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance)
				&& _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
	...
}

即：Commander 跟随 primary_instance 同步切换自己订阅的 estimator_status、estimator_sensor_bias、estimator_status_flags 等消息的实例号，从而始终检查当前主估计器的健康状况并据此执行解锁/安全逻辑。

另一个典型消费方是 MAVLink 的 ESTIMATOR_STATUS 流（见 ESTIMATOR_STATUS.hpp），它订阅 estimator_selector_status，一旦发现 primary_instance 与当前订阅实例不一致，就调用 ChangeInstance() 切换跟随的估计器数据源。

字段语义详解：从源码看每一组字段

选择状态组：primary_instance 与 instances_available

• primary_instance：当前被选中作为“主 EKF”的实例号。主实例负责向全系统发布 vehicle_attitude、vehicle_local_position、vehicle_global_position、vehicle_odometry、wind 等被下游飞行控制、导航逻辑广泛依赖的消息。Commander 与 MAVLink 流通过它切换订阅源。
• instances_available：可用实例数量。它在 EKF2Selector.cpp 中动态增长：

if ((i + 1) > _available_instances) {
	_available_instances = i + 1;
	updated = true;
}

每当发现更高编号的实例也开始发布 estimator_status，可用数量就随之扩展。这反映了多 IMU 系统的实际场景：例如飞控检测到第二块 IMU 有效后，EKF2 会启动对应实例并逐步纳入选择池。

切换统计组：instance_changed_count 与 last_instance_change

这两个字段由 SelectInstance()（见 EKF2Selector.cpp）维护：

_selected_instance = ekf_instance;
_instance_changed_count++;
_last_instance_change = sensor_selection.timestamp;
_instance[ekf_instance].time_last_selected = _last_instance_change;

• instance_changed_count：自启动以来的主实例切换次数，可用于在日志分析中快速判断是否存在“主估计器频繁切换（churn）”的异常现象。
• last_instance_change：最近一次切换发生的时刻（微秒时间戳）。它与切换计数配合，可以计算两次切换之间的间隔，识别抖动。

在切换时，EKF2Selector 还会通过 PrintInstanceChange() 输出警告日志，例如 primary EKF changed 0 -> 1，并立即更新 sensor_selection 消息（切换 accel/gyro 设备 ID），同时清零所有实例的 relative_test_ratio，避免旧评分影响新一轮选择。

设备标识组：四个 *_device_id

accel_device_id、baro_device_id、gyro_device_id、mag_device_id 记录了当前主实例正在融合的四类传感器的硬件 ID。这些 ID 是传感器驱动注册时分配的唯一标识（SENSOR_*_DEVICE_ID），在日志分析中用于：

• 确认主估计器实际使用了哪一颗物理传感器（在多套冗余传感器下尤其重要）；
• 将 EKF 状态与传感器原始数据（sensor_accel、sensor_gyro、sensor_baro、sensor_mag）按设备 ID 关联比对；
• 与 gyro_fault_detected / accel_fault_detected 配合，定位故障传感器。

需要强调的是，*_device_id 在 EKF2Selector.cpp 中来源于 estimator_status 消息（即 EKF2 内部使用的传感器 ID），而“当前选中的传感器”则由 sensor_selection 消息发布，二者在切换瞬间可能略有差异，分析时应结合两条消息。

实例评分组：combined_test_ratio 与 relative_test_ratio

这是 EKF2Selector 决策逻辑的核心数据。

combined_test_ratio（综合测试比）：在 EKF2Selector.cpp 中由每个实例的 estimator_status 计算而来：

// test ratios are invalid when 0, >= 1 is a failure
if (!PX4_ISFINITE(status.vel_test_ratio) || (status.vel_test_ratio <= 0.f)) {
	status.vel_test_ratio = 1.f;
}
if (!PX4_ISFINITE(status.pos_test_ratio) || (status.pos_test_ratio <= 0.f)) {
	status.pos_test_ratio = 1.f;
}
...
float combined_test_ratio = fmaxf(0.5f * (status.vel_test_ratio + status.pos_test_ratio), status.hgt_test_ratio);

_instance[i].combined_test_ratio = combined_test_ratio;

const bool healthy = (status.filter_fault_flags == 0) && (combined_test_ratio > 0.f);
_instance[i].healthy.set_state_and_update(healthy, status.timestamp);

_instance[i].warning = (combined_test_ratio >= 1.f);

解读要点：

• 综合测试比取“速度/位置测试比均值的一半”与“高度测试比”两者的较大值，本质是对 EKF2 内部多个创新量（innovation）测试比的保守聚合；
• 测试比 ≥ 1 表示该实例对应的量测出现异常（warning = true）；测试比为 0 或非有限值被视为无效（强制置 1，即视为失败）；
• healthy 判定要求 filter_fault_flags == 0（滤波器无故障标志）且综合测试比 > 0，并通过 1 秒的迟滞（hysteresis）平滑（healthy.set_hysteresis_time_from(false, 1_s)），防止健康状态在边界处高频抖动。

relative_test_ratio（相对误差评分）：这是一个带记忆的“累计分”，在 EKF2Selector.cpp 中每次主实例更新时对每个备选实例累加：

const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio;

// reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes
const float threshold = _gyro_fault_detected ? 0.0f : fmaxf(_param_ekf2_sel_err_red.get(), 0.05f);

if (error_delta > 0 || error_delta < -threshold) {
	_instance[i].relative_test_ratio += error_delta;
	_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim);
	...
}

其中 _rel_err_score_lim 为 1.0（评分钳位在 <a href="https://link.gitcode.com/i/9d67782caf0db1454660da2355890a83" target="_blank">-1, 1]），_rel_err_thresh 为 0.5（见 [EKF2Selector.hpp）。含义是：

• 当备选实例的综合测试比低于主实例（误差更小）且差值超过阈值 EKF2_SEL_ERR_RED（默认 0.2，见 params_selector.yaml）时，相对评分向负方向累积，表示“该实例正在变得比主实例更好”；
• 评分越高（接近 1）说明该实例相对主实例更差，越低（接近 -1）说明相对更优；
• 主实例一旦切换，所有实例的 relative_test_ratio 立即清零，重新开始积分。

IMU 故障检测组：accumulated_*_error 与 *_fault_detected

这四个字段来自 EKF2Selector 对 sensors_status_imu（IMU 状态聚合消息）的一致性分析，逻辑集中在 UpdateErrorScores()（见 EKF2Selector.cpp）。

累积误差计算（以陀螺仪为例）：

const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get());
const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get());

for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) {
	if (sensors_status_imu.gyro_device_ids[i] != 0) {
		n_gyros++;
		_accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s;
		_accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f);

		if (_accumulated_gyro_error[i] > angle_threshold) {
			n_gyro_exceedances++;
		}
		...
	} else {
		_accumulated_gyro_error[i] = NAN;  // 该槽位无传感器
	}
}

即：将“该陀螺仪与多 IMU 均值的一致性偏差（rad/s）”超出阈值的部分按时间步长积分，得到累积角误差（accumulated_gyro_error，单位弧度）；当累积误差超过角度阈值时记为一次超限。加速度计同理，将加速度偏差（m/s²）超出 EKF2_SEL_IMU_ACC 的部分积分成累积速度误差（accumulated_accel_error，单位 m/s），超过 EKF2_SEL_IMU_VEL 阈值记为超限。

故障判定（见 EKF2Selector.cpp）：

if (n_gyro_exceedances > 0) {
	if (n_gyros >= 3) {
		// If there are 3 or more sensors, the one with the largest accumulated error is faulty
		_gyro_fault_detected = true;
		faulty_gyro_id = sensors_status_imu.gyro_device_ids[largest_gyro_error_index];
	} else if (n_gyros == 2) {
		// A fault is present, but the faulty sensor identity cannot be determined
		_gyro_fault_detected = true;
	}
}

• 当冗余传感器 ≥ 3 个时，可将累计误差最大的那个判定为故障传感器，并记录其设备 ID；
• 当只有 2 个传感器时，只能确认“存在故障”但无法定位故障源（无法判断是谁偏离了均值），此时 gyro_fault_detected = true 且 faulty_gyro_id 保持为 0；
• 故障标志置位后，EKF2Selector.cpp 会无条件将使用了该故障传感器的 EKF 实例标记为不健康（healthy = false），从而促使选择器切换到使用其他传感器组合的实例。

实例切换决策：这些字段如何驱动行为

EKF2Selector 在选择下一个主实例时（见 EKF2Selector.cpp）会同时维护多个候选：

• 综合测试比最小的健康实例（best_ekf / best_test_ratio）；
• 排除“持续告警”实例后的最优选择（best_ekf_no_sustained_warning，持续告警定义为 warning 且超过 1 秒无正常状态）；
• 使用不同 IMU 的备选实例（best_ekf_different_imu），以在故障时规避故障传感器；
• 相对评分优于主实例且超过阈值 _rel_err_thresh（0.5）且距上次被选中超过 10 秒的实例（lower_error_available，避免主实例频繁来回切换）。

切换策略还包括：

• 主实例健康时优先保持当前选择，避免不必要的切换（churn）；
• 主实例短暂不健康时先“挺过”（ride out）短暂故障（_selected_unhealthy_since 机制），避免切到已发散实例；但若主实例超时冻结（timeout，默认 50ms 无 estimator_status 更新，见 EKF2Selector.cpp），则无延迟降级到备选实例；
• 当 _gyro_fault_detected / _accel_fault_detected 为真时，切换阈值降为 0（threshold = 0.0f），允许更激进的切换以快速规避故障传感器。

参数配置：影响评分与故障判定的 5 个 EKF2_SEL_* 参数

EKF2Selector 的评分与故障检测行为完全由以下 5 个参数决定（定义见 params_selector.yaml）：

参数名	默认值	单位	作用
EKF2_SEL_ERR_RED	0.2	-	相对误差降低阈值：备选实例必须比主实例好至少该值，其相对评分才会下降，用于抑制无意义切换
EKF2_SEL_IMU_RAT	7.0	deg/s	陀螺仪角速率一致性阈值：与均值偏差超过该值的部分开始累积角误差
EKF2_SEL_IMU_ANG	15.0	deg	陀螺仪累积角误差上限：超过即判定该陀螺仪故障
EKF2_SEL_IMU_ACC	1.0	m/s²	加速度计一致性阈值：与均值偏差超过该值的部分开始累积速度误差
EKF2_SEL_IMU_VEL	2.0	m/s	加速度计累积速度误差上限：超过即判定该加速度计故障

这些参数在 EKF2Selector.hpp 中通过 DEFINE_PARAMETERS 注册并读取。调参指引（基于源码语义推断）：

• 若多 IMU 系统频繁误报故障，可适当放宽 EKF2_SEL_IMU_ANG 与 EKF2_SEL_IMU_VEL（提高累积误差上限）或 EKF2_SEL_IMU_RAT / EKF2_SEL_IMU_ACC（提高触发累积的偏差门槛）；
• 若主实例切换过于频繁（churn），可增大 EKF2_SEL_ERR_RED，使备选实例更难压低相对评分；
• 若希望故障时切换更快，可将 EKF2_SEL_ERR_RED 调低——注意源码中强制下限为 0.05（fmaxf(_param_ekf2_sel_err_red.get(), 0.05f)），且陀螺/加速度计故障期间阈值强制为 0，不受该参数影响。

典型使用场景

1. 多 IMU 冗余与故障切换监控

在装配双 IMU 或更多冗余传感器的飞控上，通过订阅 estimator_selector_status 可以实时观察：

• instances_available 是否达到预期冗余数（如 2 或更多）；
• healthy[i] 哪些实例健康、哪些被降级；
• gyro_fault_detected / accel_fault_detected 是否触发、accumulated_gyro_error[i] / accumulated_accel_error[i] 哪个传感器累积误差在增长；
• primary_instance、instance_changed_count、last_instance_change 确认切换发生的时间与频次。

2. 飞行日志（ulog）故障排查

estimator_selector_status 是 ekf2 模块默认记录的话题之一（见 logged_topics.cpp 中 estimator_selector_status 的注册）。在 Flight Review 等日志分析工具中，通过 primary_instance 变化曲线与 combined_test_ratio / relative_test_ratio 波形，可以还原故障时间线：测试比持续抬升 → 实例被标记 warning → 触发切换 → 新实例接管。若日志中 instance_changed_count 频繁递增，说明选择器处于不稳定状态，应结合 IMU 原始数据定位硬件问题。

3. 上游模块跟随主实例

任何需要“跟随当前主估计器”的模块（Commander、MAVLink ESTIMATOR_STATUS 流、车辆姿态/加速度模块等）都应像 estimatorCheck.cpp 与 ESTIMATOR_STATUS.hpp 那样，订阅 estimator_selector_status 并在 primary_instance 变化时调用 ChangeInstance() 切换自己的订阅实例，避免读到陈旧或错误实例的数据。

相关消息与联动关系

estimator_selector_status 与以下 uORB 消息紧密联动，分析时可交叉引用：

• EstimatorStatus.msg：各 EKF 实例内部的状态与测试比（vel_test_ratio、pos_test_ratio、hgt_test_ratio、filter_fault_flags 等），是 combined_test_ratio 与 healthy 的原始输入；
• SensorSelection.msg：当前选中的传感器设备 ID（accel/gyro），由选择器在切换时同步更新；
• SensorsStatusImu.msg：多 IMU 一致性数据（gyro_inconsistency_rad_s、accel_inconsistency_m_s_s 及对应设备 ID），是 accumulated_*_error 与 *_fault_detected 的输入源；
• EstimatorStatusFlags.msg：实例的故障标志明细，配合健康检查使用。

EKF2 的单元测试 EKF2SelectorTest.cpp 对选择器行为进行了自动化验证，例如验证“两个健康实例时选择器稳定在实例 0 且不抖动”（检查 status.primary_instance == 0、status.instances_available == 2、status.healthy[0] && status.healthy[1]），以及“测试比被顶满的实例会被降级”等场景，可作为理解各字段语义与切换行为的行为参考。

小结

EstimatorSelectorStatus 是 PX4 EKF2 多实例冗余选择机制的“体检报告”：primary_instance / instances_available 描述选择器当前状态，instance_changed_count / last_instance_change 记录切换历史，四个 *_device_id 标识主实例使用的传感器，combined_test_ratio / relative_test_ratio / healthy 给出每个实例的质量评分，accumulated_*_error 与 *_fault_detected 则暴露 IMU 一致性与故障信息。无论是调试多 IMU 切换、排查估计器故障，还是编写跟随主估计器的下游模块，这条消息都是必须掌握的接口。