EKF2.CPP
/****************************************************************************
*
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "EKF2.hpp"
using namespace time_literals;
using math::constrain;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
pthread_mutex_t ekf2_module_mutex = PTHREAD_MUTEX_INITIALIZER;
static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
#if !defined(CONSTRAINED_FLASH)
static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
#endif // !CONSTRAINED_FLASH
/*
EKF2::EKF2 构造函数用于初始化 EKF2 类的实例。该函数接受三个参数，并根据这些参数和内部参数进行初始化设置。以下是对该构造函数的详细解释：
该构造函数接受三个参数：
multi_mode: 布尔值，表示是否处于多模式。
config: 配置结构体，包含工作队列配置。
replay_mode: 布尔值，表示是否处于重放模式。
*/
EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, config),
_replay_mode(replay_mode && !multi_mode),
_multi_mode(multi_mode),
_instance(multi_mode ? -1 : 0),
_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
_global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
_odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
_params(_ekf.getParamHandle()),
/*初始化列表，使用了多个初始化参数来初始化成员变量。这些成员变量包括传感器延迟、噪声、创新门限等参数*/
_param_ekf2_predict_us(_params->filter_update_interval_us),
_param_ekf2_mag_delay(_params->mag_delay_ms),
_param_ekf2_baro_delay(_params->baro_delay_ms),
_param_ekf2_gps_delay(_params->gps_delay_ms),
_param_ekf2_of_delay(_params->flow_delay_ms),
_param_ekf2_rng_delay(_params->range_delay_ms),
_param_ekf2_asp_delay(_params->airspeed_delay_ms),
_param_ekf2_ev_delay(_params->ev_delay_ms),
_param_ekf2_avel_delay(_params->auxvel_delay_ms),
_param_ekf2_gyr_noise(_params->gyro_noise),
_param_ekf2_acc_noise(_params->accel_noise),
_param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise),
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
_param_ekf2_mag_e_noise(_params->mage_p_noise),
_param_ekf2_mag_b_noise(_params->magb_p_noise),
_param_ekf2_wind_noise(_params->wind_vel_p_noise),
_param_ekf2_terr_noise(_params->terrain_p_noise),
_param_ekf2_terr_grad(_params->terrain_gradient),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),
_param_ekf2_gps_p_noise(_params->gps_pos_noise),
_param_ekf2_noaid_noise(_params->pos_noaid_noise),
_param_ekf2_baro_noise(_params->baro_noise),
_param_ekf2_baro_gate(_params->baro_innov_gate),
_param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone),
_param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt),
_param_ekf2_gps_p_gate(_params->gps_pos_innov_gate),
_param_ekf2_gps_v_gate(_params->gps_vel_innov_gate),
_param_ekf2_tas_gate(_params->tas_innov_gate),
_param_ekf2_head_noise(_params->mag_heading_noise),
_param_ekf2_mag_noise(_params->mag_noise),
_param_ekf2_eas_noise(_params->eas_noise),
_param_ekf2_beta_gate(_params->beta_innov_gate),
_param_ekf2_beta_noise(_params->beta_noise),
_param_ekf2_mag_decl(_params->mag_declination_deg),
_param_ekf2_hdg_gate(_params->heading_innov_gate),
_param_ekf2_mag_gate(_params->mag_innov_gate),
_param_ekf2_decl_type(_params->mag_declination_source),
_param_ekf2_mag_type(_params->DIS_USE_MAG_LELIN), //LELIN修改文件为common.h 新增DIS_USE_MAG_LELIN = 5
_param_ekf2_mag_acclim(_params->mag_acc_gate),
_param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate),
_param_ekf2_gps_check(_params->gps_check_mask),
_param_ekf2_req_eph(_params->req_hacc),
_param_ekf2_req_epv(_params->req_vacc),
_param_ekf2_req_sacc(_params->req_sacc),
_param_ekf2_req_nsats(_params->req_nsats),
_param_ekf2_req_pdop(_params->req_pdop),
_param_ekf2_req_hdrift(_params->req_hdrift),
_param_ekf2_req_vdrift(_params->req_vdrift),
_param_ekf2_aid_mask(_params->fusion_mode),
_param_ekf2_hgt_mode(_params->vdist_sensor_type),
_param_ekf2_terr_mask(_params->terrain_fusion_mode),
_param_ekf2_noaid_tout(_params->valid_timeout_max),
_param_ekf2_rng_noise(_params->range_noise),
_param_ekf2_rng_sfe(_params->range_noise_scaler),
_param_ekf2_rng_gate(_params->range_innov_gate),
_param_ekf2_min_rng(_params->rng_gnd_clearance),
_param_ekf2_rng_pitch(_params->rng_sens_pitch),
_param_ekf2_rng_aid(_params->range_aid),
_param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid),
_param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid),
_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
_param_ekf2_evv_gate(_params->ev_vel_innov_gate),
_param_ekf2_evp_gate(_params->ev_pos_innov_gate),
_param_ekf2_of_n_min(_params->flow_noise),
_param_ekf2_of_n_max(_params->flow_noise_qual_min),
_param_ekf2_of_qmin(_params->flow_qual_min),
_param_ekf2_of_gate(_params->flow_innov_gate),
_param_ekf2_imu_pos_x(_params->imu_pos_body(0)),
_param_ekf2_imu_pos_y(_params->imu_pos_body(1)),
_param_ekf2_imu_pos_z(_params->imu_pos_body(2)),
_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
_param_ekf2_of_pos_x(_params->flow_pos_body(0)),
_param_ekf2_of_pos_y(_params->flow_pos_body(1)),
_param_ekf2_of_pos_z(_params->flow_pos_body(2)),
_param_ekf2_ev_pos_x(_params->ev_pos_body(0)),
_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
_param_ekf2_arsp_thr(_params->arsp_thr),
_param_ekf2_tau_vel(_params->vel_Tau),
_param_ekf2_tau_pos(_params->pos_Tau),
_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
_param_ekf2_abias_init(_params->switch_on_accel_bias),
_param_ekf2_angerr_init(_params->initial_tilt_err),
_param_ekf2_abl_lim(_params->acc_bias_lim),
_param_ekf2_abl_acclim(_params->acc_bias_learn_acc_lim),
_param_ekf2_abl_gyrlim(_params->acc_bias_learn_gyr_lim),
_param_ekf2_abl_tau(_params->acc_bias_learn_tc),
_param_ekf2_drag_noise(_params->drag_noise),
_param_ekf2_bcoef_x(_params->bcoef_x),
_param_ekf2_bcoef_y(_params->bcoef_y),
_param_ekf2_mcoef(_params->mcoef),
_param_ekf2_aspd_max(_params->max_correction_airspeed),
_param_ekf2_pcoef_xp(_params->static_pressure_coef_xp),
_param_ekf2_pcoef_xn(_params->static_pressure_coef_xn),
_param_ekf2_pcoef_yp(_params->static_pressure_coef_yp),
_param_ekf2_pcoef_yn(_params->static_pressure_coef_yn),
_param_ekf2_pcoef_z(_params->static_pressure_coef_z),
_param_ekf2_mag_check(_params->check_mag_strength),
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
// advertise expected minimal topic set immediately to ensure logging
/*立即广告了一些uORB主题，以确保能够记录最小的一组话题*/
_attitude_pub.advertise();
_local_position_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
}
EKF2::~EKF2()
{
perf_free(_ecl_ekf_update_perf);
perf_free(_ecl_ekf_update_full_perf);
perf_free(_msg_missed_imu_perf);
perf_free(_msg_missed_air_data_perf);
perf_free(_msg_missed_airspeed_perf);
perf_free(_msg_missed_distance_sensor_perf);
perf_free(_msg_missed_gps_perf);
perf_free(_msg_missed_landing_target_pose_perf);
perf_free(_msg_missed_magnetometer_perf);
perf_free(_msg_missed_odometry_perf);
perf_free(_msg_missed_optical_flow_perf);
}
/*
EKF2::multi_init 是 EKF2 类的一个成员函数，用于初始化多个 IMU 和磁力计实例，并确保所有相关主题的一致性
EKF2::multi_init 函数通过广告一系列 uORB 主题来确保实例编号的一致性，并尝试更改 IMU 和磁力计实例。
如果所有相关实例编号一致，则初始化成功并调度任务；否则，打印错误信息并返回失败。
*/
bool EKF2::multi_init(int imu, int mag)
{
// advertise all topics to ensure consistent uORB instance numbering
/*函数广告（advertise）了一系列的 uORB 主题，以确保 uORB 实例编号的一致性。
这些主题包括时间戳、估算器的各种偏差、事件标志、GPS 状态、创新测试比率、创新方差、
创新数据、光流速度、传感器偏差、状态、状态标志、视觉里程计、偏航估计等*/
_ekf2_timestamps_pub.advertise();
_estimator_baro_bias_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_gps_status_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
_estimator_optical_flow_vel_pub.advertise();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
_estimator_visual_odometry_aligned_pub.advertise();
_yaw_est_pub.advertise();
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_wind_pub.advertise();
/*该行代码尝试更改车辆 IMU 和磁力计订阅的实例编号。如果成功更改两个实例，则 changed_instance 为 true*/
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
const int status_instance = _estimator_states_pub.get_instance();
/*
检查实例一致性并调度任务
status_instance 是有效的（大于等于 0）。
IMU 和磁力计实例更改成功（changed_instance 为 true）。
_attitude_pub、_local_position_pub 和 _global_position_pub 的实例编号与 status_instance 一致。
*/
if ((status_instance >= 0) && changed_instance
&& (_attitude_pub.get_instance() == status_instance)
&& (_local_position_pub.get_instance() == status_instance)
&& (_global_position_pub.get_instance() == status_instance)) {
_instance = status_instance;
ScheduleNow();
return true;
}
//如果条件不满足，则打印错误
PX4_ERR("publication instance problem: %d att: %d lpos: %d gpos: %d", status_instance,
_attitude_pub.get_instance(), _local_position_pub.get_instance(), _global_position_pub.get_instance());
return false;
}
//打印EKF2的状态信息
int EKF2::print_status()
{
PX4_INFO_RAW("ekf2:%d EKF dt: %.4fs, IMU dt: %.4fs, attitude: %d, local position: %d, global position: %d\n",
_instance, (double)_ekf.get_dt_ekf_avg(), (double)_ekf.get_dt_imu_avg(), _ekf.attitude_valid(),
_ekf.local_position_is_valid(), _ekf.global_position_is_valid());
perf_print_counter(_ecl_ekf_update_perf);
perf_print_counter(_ecl_ekf_update_full_perf);
perf_print_counter(_msg_missed_imu_perf);
perf_print_counter(_msg_missed_air_data_perf);
perf_print_counter(_msg_missed_airspeed_perf);
perf_print_counter(_msg_missed_distance_sensor_perf);
perf_print_counter(_msg_missed_gps_perf);
perf_print_counter(_msg_missed_landing_target_pose_perf);
perf_print_counter(_msg_missed_magnetometer_perf);
perf_print_counter(_msg_missed_odometry_perf);
perf_print_counter(_msg_missed_optical_flow_perf);
//在调试构建模式下（DEBUG_BUILD 定义存在），这段代码会调用 _ekf.print_status() 打印更多的 EKF 状态信息
#if defined(DEBUG_BUILD)
_ekf.print_status();
#endif // DEBUG_BUILD
return 0;
}
void EKF2::Run()
{
/*检查是否有退出信号，如果有，将取消订阅传感器数据，以释放资源并终止循环*/
if (should_exit()) {
_sensor_combined_sub.unregisterCallback();
_vehicle_imu_sub.unregisterCallback();
return;
}
/*check for parameter updates
检查是否有参数更新或回调尚未注册，如果有参数更新，它将更新本地参数缓存，并根据新参数调整EKF的行为*/
if (_parameter_update_sub.updated() || !_callback_registered) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage 从存储中更新参数
updateParams();
_ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s);
// The airspeed scale factor correcton is only available via parameter as used by the airspeed module
// 空速比例因子校正仅可通过空速模块使用的参数获得
param_t param_aspd_scale = param_find("ASPD_SCALE_1");
if (param_aspd_scale != PARAM_INVALID) {
param_get(param_aspd_scale, &_airspeed_scale_factor);
}
// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
//如果使用气压，确保传感器间隔最小值足以容纳系统平均气压输出
if (_params->vdist_sensor_type == 0) {
float sens_baro_rate = 0.f;
if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
if (sens_baro_rate > 0) {
float interval_ms = roundf(1000.f / sens_baro_rate);
if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
_params->sensor_interval_max_ms = interval_ms;
}
}
}
}
// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
//如果使用mag，请确保传感器间隔最小值足以容纳系统平均mag输出
if (0) { //LELIN24.7.19修改 _params->mag_fusion_type != MAG_FUSE_TYPE_NONE
float sens_mag_rate = 0.f;
if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
if (sens_mag_rate > 0) {
float interval_ms = roundf(1000.f / sens_mag_rate);
if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
_params->sensor_interval_max_ms = interval_ms;
}
}
}
}
}
/*这部分确保至少有一个传感器的回调已注册，以便接受传感器数据，这取决于是否处于多实例模式*/
if (!_callback_registered) {
if (_multi_mode) {
_callback_registered = _vehicle_imu_sub.registerCallback();
} else {
_callback_registered = _sensor_combined_sub.registerCallback();
}
if (!_callback_registered) {
ScheduleDelayed(10_ms);
return;
}
}
/*处理来自车辆命令、状态和着陆检测的更新，以反映车辆类型、运动状态和环境条件的变化*/
if (_vehicle_command_sub.updated()) { //处理待定的命令、如设置GPS全局原点
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
if (!_ekf.control_status_flags().in_air) {
uint64_t origin_time {};
double latitude = vehicle_command.param5;
double longitude = vehicle_command.param6;
float altitude = vehicle_command.param7;
_ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
// Validate the ekf origin status.
_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
}
}
}
}
bool imu_updated = false;
imuSample imu_sample_new {};
hrt_abstime imu_dt = 0; // for tracking time slip later 用于稍后跟踪时间偏差
/*这段代码主要处理了在多实例模式下，从_vehicle_imu_sub订阅者中更新IMU（惯性测量单元）数据的过程*/
if (_multi_mode) {
const unsigned last_generation = _vehicle_imu_sub.get_last_generation(); //获取赏赐数据的生成代数
vehicle_imu_s imu;
imu_updated = _vehicle_imu_sub.update(&imu); //更新IMU数据
if (imu_updated && (_vehicle_imu_sub.get_last_generation() != last_generation + 1)) { //检查数据跳变
perf_count(_msg_missed_imu_perf);
}
if (imu_updated) {
//如果IMU数据确实更新了，代码更新imu_sample_new结构体包括时间戳、角增量时间、角增量、线速度增量时间和线速度增量。
//这里还将角增量时间从微秒转换为秒
imu_sample_new.time_us = imu.timestamp_sample;
imu_sample_new.delta_ang_dt = imu.delta_angle_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{imu.delta_angle};
imu_sample_new.delta_vel_dt = imu.delta_velocity_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{imu.delta_velocity};
if (imu.delta_velocity_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = imu.delta_velocity_clipping & vehicle_imu_s::CLIPPING_Z;
}
imu_dt = imu.delta_angle_dt;
if ((_device_id_accel == 0) || (_device_id_gyro == 0)) {
//更新设备ID和校准计数
_device_id_accel = imu.accel_device_id;
_device_id_gyro = imu.gyro_device_id;
_accel_calibration_count = imu.accel_calibration_count;
_gyro_calibration_count = imu.gyro_calibration_count;
} else {
if ((imu.accel_calibration_count != _accel_calibration_count)
|| (imu.accel_device_id != _device_id_accel)) {
PX4_DEBUG("%d - resetting accelerometer bias", _instance);
_device_id_accel = imu.accel_device_id;
_ekf.resetAccelBias();
_accel_calibration_count = imu.accel_calibration_count;
// reset bias learning
_accel_cal = {};
}
if ((imu.gyro_calibration_count != _gyro_calibration_count)
|| (imu.gyro_device_id != _device_id_gyro)) {
PX4_DEBUG("%d - resetting rate gyro bias", _instance);
_device_id_gyro = imu.gyro_device_id;
_ekf.resetGyroBias();
_gyro_calibration_count = imu.gyro_calibration_count;
// reset bias learning
_gyro_cal = {};
}
}
}
} else {
const unsigned last_generation = _sensor_combined_sub.get_last_generation();
//获取sensor_combined主题
sensor_combined_s sensor_combined;
imu_updated = _sensor_combined_sub.update(&sensor_combined);
//尝试从sensor_combined订阅者中更新sensor_combined结构体
if (imu_updated && (_sensor_combined_sub.get_last_generation() != last_generation + 1)) {
//数据流有跳过，某些数据包已丢失
perf_count(_msg_missed_imu_perf);
}
if (imu_updated) { //更新imu_sample_new的时间戳、角速度积分时间、角增量、线加速度积分时间和线速度增量
imu_sample_new.time_us = sensor_combined.timestamp;
imu_sample_new.delta_ang_dt = sensor_combined.gyro_integral_dt * 1.e-6f;
imu_sample_new.delta_ang = Vector3f{sensor_combined.gyro_rad} * imu_sample_new.delta_ang_dt;
imu_sample_new.delta_vel_dt = sensor_combined.accelerometer_integral_dt * 1.e-6f;
imu_sample_new.delta_vel = Vector3f{sensor_combined.accelerometer_m_s2} * imu_sample_new.delta_vel_dt;
if (sensor_combined.accelerometer_clipping > 0) {
imu_sample_new.delta_vel_clipping[0] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_X;
imu_sample_new.delta_vel_clipping[1] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Y;
imu_sample_new.delta_vel_clipping[2] = sensor_combined.accelerometer_clipping & sensor_combined_s::CLIPPING_Z;
}
imu_dt = sensor_combined.gyro_integral_dt;
if (sensor_combined.accel_calibration_count != _accel_calibration_count) {
// 重置加速计偏置和相关学习
PX4_DEBUG("%d - resetting accelerometer bias", _instance);
_ekf.resetAccelBias();
_accel_calibration_count = sensor_combined.accel_calibration_count;
// reset bias learning
_accel_cal = {};
}
if (sensor_combined.gyro_calibration_count != _gyro_calibration_count) {
// 重置陀螺仪偏置和相关学习
PX4_DEBUG("%d - resetting rate gyro bias", _instance);
_ekf.resetGyroBias();
_gyro_calibration_count = sensor_combined.gyro_calibration_count;
// reset bias learning
_gyro_cal = {};
}
}
if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {
// 检查是否需要更新设备ID并重置偏置
sensor_selection_s sensor_selection;
if (_sensor_selection_sub.copy(&sensor_selection)) {
if (_device_id_accel != sensor_selection.accel_device_id) {
_device_id_accel = sensor_selection.accel_device_id;
_ekf.resetAccelBias();
// reset bias learning
_accel_cal = {};
}
if (_device_id_gyro != sensor_selection.gyro_device_id) {
_device_id_gyro = sensor_selection.gyro_device_id;
_ekf.resetGyroBias();
// reset bias learning
_gyro_cal = {};
}
}
}
}
if (imu_updated) {
const hrt_abstime now = imu_sample_new.time_us;
// push imu data into estimator 这里将IMU（惯性测量单元）数据传递给EKF，
_ekf.setIMUData(imu_sample_new);
PublishAttitude(now);
/*publish attitude immediately (uses quaternion from output predictor) 并立即发布当前的姿态信息。
姿态信息通常是通过EKF预测输出的四元数来确定的。
这部分代码监测时间滑移，即实际时间与积分时间之间的差异。
_integrated_time_us变量累积了自启动以来的IMU积分时间，
而_last_time_slip_us则反映了实际时间与积分时间的偏差*/
// integrate time to monitor time slippage
if (_start_time_us > 0) {
_integrated_time_us += imu_dt;
_last_time_slip_us = (imu_sample_new.time_us - _start_time_us) - _integrated_time_us;
} else {
_start_time_us = imu_sample_new.time_us;
_last_time_slip_us = 0;
}
// update all other topics if they have new data
if (_status_sub.updated()) {
//更新EKF关于是否融合侧滑角、是否固定翼以及是否可以飞行中观测航向
vehicle_status_s vehicle_status;
if (_status_sub.copy(&vehicle_status)) {
const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
// only fuse synthetic sideslip measurements if conditions are met
_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
_ekf.set_is_fixed_wing(is_fixed_wing);
_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
}
}
if (_vehicle_land_detected_sub.updated()) {
//更新EKF关于车辆是否静止、是否在地面效应中，以及在起飞和降落时重置空中状态和传感器校准...
vehicle_land_detected_s vehicle_land_detected;
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
_ekf.set_vehicle_at_rest(vehicle_land_detected.at_rest);
if (vehicle_land_detected.in_ground_effect) {
_ekf.set_gnd_effect();
}
const bool was_in_air = _ekf.control_status_flags().in_air;
const bool in_air = !vehicle_land_detected.landed;
if (!was_in_air && in_air) {
// takeoff
_ekf.set_in_air_status(true);
// reset learned sensor calibrations on takeoff 起飞时重置已学习的传感器校准
_accel_cal = {};
_gyro_cal = {};
_mag_cal = {};
} else if (was_in_air && !in_air) {
// landed
_ekf.set_in_air_status(false);
}
}
}
// ekf2_timestamps (using 0.1 ms relative timestamps) 时间戳初始化
ekf2_timestamps_s ekf2_timestamps {// 初始化其他时间戳为无效...
.timestamp = now,
.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.vehicle_magnetometer_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
.visual_odometry_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
};
/*更新传感器样本
这一系列函数调用用于更新各种传感器数据样本，并将这些数据整合进EKF中。
每个函数负责处理一种类型的传感器数据，如空速、辅助速度、气压、光流、GPS、磁力计和测距仪。*/
UpdateAirspeedSample(ekf2_timestamps);
UpdateAuxVelSample(ekf2_timestamps);
UpdateBaroSample(ekf2_timestamps);
UpdateFlowSample(ekf2_timestamps);
UpdateGpsSample(ekf2_timestamps);
// UpdateMagSample(ekf2_timestamps); //LELIN 24-7-19
UpdateRangeSample(ekf2_timestamps);
/*这部分代码处理外部视觉里程计数据，如果收到了新的视觉里程计数据，UpdateExtVisionSample函数会被调用，
返回值new_ev_odom指示是否有新的外部视觉里程计数据可用*/
vehicle_odometry_s ev_odom;
const bool new_ev_odom = UpdateExtVisionSample(ekf2_timestamps, ev_odom);
// run the EKF update and output 执行EKF更新
const hrt_abstime ekf_update_start = hrt_absolute_time();//记录EKF更新开始的时间，用于计算更新所需的时间。
if (_ekf.update()) {
// 调用EKF的update函数进行状态更新。如果更新成功，后续将发布各种估计结果和状态信息，包括位置、速度、风速估计、传感器偏差等。
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
PublishLocalPosition(now);
PublishOdometry(now, imu_sample_new);
PublishGlobalPosition(now);
PublishWindEstimate(now);
// publish status/logging messages
PublishBaroBias(now);
PublishEventFlags(now);
PublishGpsStatus(now);
PublishInnovations(now);
PublishInnovationTestRatios(now);
PublishInnovationVariances(now);
PublishOpticalFlowVel(now);
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishYawEstimatorStatus(now);
UpdateAccelCalibration(now);
UpdateGyroCalibration(now);
UpdateMagCalibration(now);
PublishSensorBias(now);
} else {
// ekf no update
perf_set_elapsed(_ecl_ekf_update_perf, hrt_elapsed_time(&ekf_update_start));
}
// publish external visual odometry after fixed frame alignment if new odometry is received
/*如果new_ev_odom为真，表示有新的外部视觉里程计数据，将调用PublishOdometryAligned函数发布对齐后的里程计数据。*/
if (new_ev_odom) { //发布外部视觉里程计数据
PublishOdometryAligned(now, ev_odom);
//发布ekf2_timestamps结构体，包含了所有传感器数据的相对时间戳，用于同步和调试目的
}
// publish ekf2_timestamps 发布时间戳数据
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
// re-schedule as backup timeout 重新调度次任务以确保即使没有新数据到达时，EKF也能定期运行
ScheduleDelayed(100_ms);
}
/*发布飞行器的当前姿态信息*/
void EKF2::PublishAttitude(const hrt_abstime ×tamp)
{
if (_ekf.attitude_valid()) {
// generate vehicle attitude quaternion data 生成车辆姿态四元数数据
vehicle_attitude_s att;
att.timestamp_sample = timestamp;
const Quatf q{_ekf.calculate_quaternion()};
q.copyTo(att.q);
_ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter);
att.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_attitude_pub.publish(att);
} else if (_replay_mode) {
// in replay mode we have to tell the replay module not to wait for an update
// we do this by publishing an attitude with zero timestamp
//在重播模式下，我们必须告诉重播模块不要等待更新
//我们通过发布一个时间戳为零的状态来实现这一点
vehicle_attitude_s att{};
_attitude_pub.publish(att);
}
}
/*发布气压计偏差估计信息*/
void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
{
if (_device_id_baro != 0) {
const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
estimator_baro_bias_s baro_bias{};
baro_bias.timestamp_sample = _ekf.get_baro_sample_delayed().time_us;
baro_bias.baro_device_id = _device_id_baro;
baro_bias.bias = status.bias;
baro_bias.bias_var = status.bias_var;
baro_bias.innov = status.innov;
baro_bias.innov_var = status.innov_var;
baro_bias.innov_test_ratio = status.innov_test_ratio;
baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_baro_bias_pub.publish(baro_bias);
_last_baro_bias_published = status.bias;
}
}
}
/*发布EKF产生的信息事件和警告事件*/
void EKF2::PublishEventFlags(const hrt_abstime ×tamp) //参数timestamp事件的时间戳
{
// information events
/*从 EKF 获取当前的信息事件标志，并检查是否有新的信息事件。
如果有，更新 information_event_updated 标志，并增加 _filter_information_event_changes 计数器。*/
uint32_t information_events = _ekf.information_event_status().value;
bool information_event_updated = false;
if (information_events != 0) {
information_event_updated = true;
_filter_information_event_changes++;
}
// warning events
/*从 EKF 获取当前的警告事件标志，并检查是否有新的警告事件。
如果有，更新 warning_event_updated 标志，并增加 _filter_warning_event_changes 计数器*/
uint32_t warning_events = _ekf.warning_event_status().value;
bool warning_event_updated = false;
if (warning_events != 0) {
warning_event_updated = true;
_filter_warning_event_changes++;
}
if (information_event_updated || warning_event_updated) {
//如果有新的信息或警告事件，则创建一个新的 estimator_event_flags_s 结构体，并设置其时间戳。
estimator_event_flags_s event_flags{};
event_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
//设置所有的信息事件标志，包括GPS检查是否通过、速度和位置重置等
event_flags.information_event_changes = _filter_information_event_changes;
event_flags.gps_checks_passed = _ekf.information_event_flags().gps_checks_passed;
event_flags.reset_vel_to_gps = _ekf.information_event_flags().reset_vel_to_gps;
event_flags.reset_vel_to_flow = _ekf.information_event_flags().reset_vel_to_flow;
event_flags.reset_vel_to_vision = _ekf.information_event_flags().reset_vel_to_vision;
event_flags.reset_vel_to_zero = _ekf.information_event_flags().reset_vel_to_zero;
event_flags.reset_pos_to_last_known = _ekf.information_event_flags().reset_pos_to_last_known;
event_flags.reset_pos_to_gps = _ekf.information_event_flags().reset_pos_to_gps;
event_flags.reset_pos_to_vision = _ekf.information_event_flags().reset_pos_to_vision;
event_flags.starting_gps_fusion = _ekf.information_event_flags().starting_gps_fusion;
event_flags.starting_vision_pos_fusion = _ekf.information_event_flags().starting_vision_pos_fusion;
event_flags.starting_vision_vel_fusion = _ekf.information_event_flags().starting_vision_vel_fusion;
event_flags.starting_vision_yaw_fusion = _ekf.information_event_flags().starting_vision_yaw_fusion;
event_flags.yaw_aligned_to_imu_gps = _ekf.information_event_flags().yaw_aligned_to_imu_gps;
//设置所有的警告事件标志，包括GPS质量差、GPS融合超时、高度传感器超时等
event_flags.warning_event_changes = _filter_warning_event_changes;
event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor;
event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout;
event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped;
event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate;
event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout;
event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use;
event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset;
event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course;
event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use;
event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped;
event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped;
event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
//更新并发布事件标志
/*设置事件标志的时间戳，并更新和发布事件标志。然后，记录最后一次发布事件标志的时间戳，并清除 EKF 中的信息和警告事件。*/
event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update(event_flags);
_last_event_flags_publish = event_flags.timestamp;
_ekf.clear_information_events();
_ekf.clear_warning_events();
} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
// continue publishing periodically
//如果没有新的信息或警告事件，并且距离上次发布事件标志的时间已经超过 1 秒，则继续定期发布事件标志。
_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update();
_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
}
}
/*函数从 EKF（扩展卡尔曼滤波器）中获取全球位置数据，并将其格式化后发布。-*/
void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
{
if (_ekf.global_position_is_valid()) {//检查全球位置是否有效
const Vector3f position{_ekf.getPosition()}; //获取当前位置 存储在position中
// generate and publish global position data
vehicle_global_position_s global_pos;//创建结构体
global_pos.timestamp_sample = timestamp;//并设置时间戳
// Position of local NED origin in GPS / WGS84 frame
//将局部 NED（北东地）原点的位置重投影到 GPS / WGS84 坐标系中，并设置 global_pos 的经纬度
_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
//从 EKF 获取位置重置信息，并设置 global_pos 的经纬度重置计数器
float delta_xy[2];
_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
//设置 global_pos 的高度信息，alt 表示高于海平面的高度（以米为单位），
//并使用 filter_altitude_ellipsoid 函数滤波椭球高度。
global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);
// global altitude has opposite sign of local down position
//从 EKF 获取高度重置信息，并设置 global_pos 的高度变化量
float delta_z;
uint8_t z_reset_counter;
_ekf.get_posD_reset(&delta_z, &z_reset_counter);
global_pos.delta_alt = -delta_z;
_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);//从 EKF 获取全球位置精度，包括水平精度 eph 和垂直精度 epv
//如果地形估计有效，则设置 global_pos 的地形高度和有效性标志；否则，将地形高度设置为 NAN，并标记为无效。
if (_ekf.isTerrainEstimateValid()) {
// Terrain altitude in m, WGS84
global_pos.terrain_alt = _ekf.getEkfGlobalOriginAltitude() - _ekf.getTerrainVertPos();
global_pos.terrain_alt_valid = true;
} else {
global_pos.terrain_alt = NAN;
global_pos.terrain_alt_valid = false;
}
//根据 EKF 的控制状态标志，设置 global_pos 的死算标志（即使用惯性导航或风死算）
global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
|| _ekf.control_status_flags().wind_dead_reckoning;
//设置 global_pos 的时间戳，并发布全球位置数据
global_pos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_global_position_pub.publish(global_pos);
}
}
/*发布GPS状态信息*/
void EKF2::PublishGpsStatus(const hrt_abstime ×tamp)
{
//从 EKF 获取延迟的GPS样本时间戳，并存储在 timestamp_sample 中
const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
//如果当前样本的时间戳和上次发布的时间戳相同，说明该GPS状态已经发布过，函数直接返回，不再继续执行。
if (timestamp_sample == _last_gps_status_published) {
return;
}
//创建一个 estimator_gps_status_s 结构体 estimator_gps_status，并设置其时间戳样本
estimator_gps_status_s estimator_gps_status{};
estimator_gps_status.timestamp_sample = timestamp_sample;
//从 EKF 获取水平和垂直方向的GPS漂移率以及过滤后的水平速度，并设置 estimator_gps_status 的相应字段
estimator_gps_status.position_drift_rate_horizontal_m_s = _ekf.gps_horizontal_position_drift_rate_m_s();
estimator_gps_status.position_drift_rate_vertical_m_s = _ekf.gps_vertical_position_drift_rate_m_s();
estimator_gps_status.filtered_horizontal_speed_m_s = _ekf.gps_filtered_horizontal_velocity_m_s();
//从 EKF 获取GPS检查是否通过，并设置 estimator_gps_status 的 checks_passed 字段
estimator_gps_status.checks_passed = _ekf.gps_checks_passed();
//从 EKF 获取GPS检查失败的各项状态标志，并设置 estimator_gps_status 的相应字段
estimator_gps_status.check_fail_gps_fix = _ekf.gps_check_fail_status_flags().fix;
estimator_gps_status.check_fail_min_sat_count = _ekf.gps_check_fail_status_flags().nsats;
estimator_gps_status.check_fail_max_pdop = _ekf.gps_check_fail_status_flags().pdop;
estimator_gps_status.check_fail_max_horz_err = _ekf.gps_check_fail_status_flags().hacc;
estimator_gps_status.check_fail_max_vert_err = _ekf.gps_check_fail_status_flags().vacc;
estimator_gps_status.check_fail_max_spd_err = _ekf.gps_check_fail_status_flags().sacc;
estimator_gps_status.check_fail_max_horz_drift = _ekf.gps_check_fail_status_flags().hdrift;
estimator_gps_status.check_fail_max_vert_drift = _ekf.gps_check_fail_status_flags().vdrift;
estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed;
estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed;
//设置 estimator_gps_status 的时间戳，并发布GPS状态数据
estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_gps_status_pub.publish(estimator_gps_status);
//更新 _last_gps_status_published，记录本次发布的GPS状态的时间戳
_last_gps_status_published = timestamp_sample;
}
/*发布EKF的创新（innovation）数据
初始化创新数据结构体
创建并初始化 estimator_innovations_s 结构体 innovations。
设置 innovations 的时间戳样本为延迟的IMU样本时间。
获取创新数据
从EKF获取GPS速度和位置创新数据，并设置 innovations 的相关字段。
从EKF获取EV（视觉估计）速度和位置创新数据，并设置 innovations 的相关字段。
从EKF获取气压高度、激光测距高度、辅助速度、光流、航向、磁场、阻力、空速、侧滑角、地形高度和地形高度速率的创新数据，并设置 innovations 的相关字段。
辅助垂直速度创新数据尚未支持，设置为 NAN。
发布创新数据
设置 innovations 的时间戳，如果处于重播模式，则使用传入的时间戳，否则使用当前时间。
发布创新数据。
飞行前检查
如果飞行器不在空中，更新飞行前检查器的状态并进行更新。
如果飞行器状态从空中过渡到地面，重置飞行前检查器。
*/
void EKF2::PublishInnovations(const hrt_abstime ×tamp)
{
// publish estimator innovation data 发布估计器创新数据
estimator_innovations_s innovations{};
innovations.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_ekf.getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
_ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
_ekf.getBaroHgtInnov(innovations.baro_vpos);
_ekf.getRngHgtInnov(innovations.rng_vpos);
_ekf.getAuxVelInnov(innovations.aux_hvel);
_ekf.getFlowInnov(innovations.flow);
_ekf.getHeadingInnov(innovations.heading);
_ekf.getMagInnov(innovations.mag_field);
_ekf.getDragInnov(innovations.drag);
_ekf.getAirspeedInnov(innovations.airspeed);
_ekf.getBetaInnov(innovations.beta);
_ekf.getHaglInnov(innovations.hagl);
_ekf.getHaglRateInnov(innovations.hagl_rate);
// Not yet supported 尚未支持
innovations.aux_vvel = NAN;
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovations_pub.publish(innovations);
//计算噪声滤波之后的速度创新数据，用于飞行前检查
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (!_ekf.control_status_flags().in_air) {
// TODO: move to run before publications 移动到发布前运行
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {
// reset preflight checks if transitioning back to landed
// 如果从飞行状态过渡到着陆状态，重置飞行前检查
_preflt_checker.reset();
}
}
/*发布EKF的创新检验比率（innovation test ratios）*/
void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp)
{
// publish estimator innovation test ratio data
estimator_innovations_s test_ratios{};
test_ratios.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0],
test_ratios.gps_vpos);
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos);
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
_ekf.getHeadingInnovRatio(test_ratios.heading);
_ekf.getMagInnovRatio(test_ratios.mag_field[0]);
_ekf.getDragInnovRatio(&test_ratios.drag[0]);
_ekf.getAirspeedInnovRatio(test_ratios.airspeed);
_ekf.getBetaInnovRatio(test_ratios.beta);
_ekf.getHaglInnovRatio(test_ratios.hagl);
_ekf.getHaglRateInnovRatio(test_ratios.hagl_rate);
// Not yet supported
test_ratios.aux_vvel = NAN;
test_ratios.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovation_test_ratios_pub.publish(test_ratios);
}
/*
发布EKF的创新方差（innovation variances）
初始化创新检验比率数据结构体
创建并初始化 estimator_innovations_s 结构体 test_ratios。
设置 test_ratios 的时间戳样本为延迟的IMU样本时间。
获取创新检验比率数据
从EKF获取GPS速度和位置创新检验比率数据，并设置 test_ratios 的相关字段。
从EKF获取EV（视觉估计）速度和位置创新检验比率数据，并设置 test_ratios 的相关字段。
从EKF获取气压高度、激光测距高度、辅助速度、光流、航向、磁场、阻力、空速、侧滑角、地形高度和地形高度速率的创新检验比率数据，并设置 test_ratios 的相关字段。
辅助垂直速度创新检验比率数据尚未支持，设置为 NAN。
发布创新检验比率数据
设置 test_ratios 的时间戳，如果处于重播模式，则使用传入的时间戳，否则使用当前时间。
发布创新检验比率数据。
*/
void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp)
{
// publish estimator innovation variance data
estimator_innovations_s variances{};
variances.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_ekf.getGpsVelPosInnovVar(variances.gps_hvel, variances.gps_vvel, variances.gps_hpos, variances.gps_vpos);
_ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos);
_ekf.getBaroHgtInnovVar(variances.baro_vpos);
_ekf.getRngHgtInnovVar(variances.rng_vpos);
_ekf.getAuxVelInnovVar(variances.aux_hvel);
_ekf.getFlowInnovVar(variances.flow);
_ekf.getHeadingInnovVar(variances.heading);
_ekf.getMagInnovVar(variances.mag_field);
_ekf.getDragInnovVar(variances.drag);
_ekf.getAirspeedInnovVar(variances.airspeed);
_ekf.getBetaInnovVar(variances.beta);
_ekf.getHaglInnovVar(variances.hagl);
_ekf.getHaglRateInnovVar(variances.hagl_rate);
// Not yet supported
variances.aux_vvel = NAN;
variances.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovation_variances_pub.publish(variances);
}
/*发布由扩展卡尔曼滤波器（EKF）计算出的局部位置信息*/
void EKF2::PublishLocalPosition(const hrt_abstime ×tamp)
{
vehicle_local_position_s lpos;
// generate vehicle local position data 生成车辆本地位置数据
lpos.timestamp_sample = timestamp;
// Position of body origin in local NED frame 身体原点在局部NED帧中的位置
// 从EKF获取飞行器在局部NED坐标系中的位置和速度，并将这些数据设置到 lpos 中
const Vector3f position{_ekf.getPosition()};
lpos.x = position(0);
lpos.y = position(1);
lpos.z = position(2);
// Velocity of body origin in local NED frame (m/s)
const Vector3f velocity{_ekf.getVelocity()};
lpos.vx = velocity(0);
lpos.vy = velocity(1);
lpos.vz = velocity(2);
// vertical position time derivative (m/s)
// 从EKF获取垂直位置时间导数和加速度信息，并将其设置到 lpos 中
lpos.z_deriv = _ekf.getVerticalPositionDerivative();
// Acceleration of body origin in local frame
const Vector3f vel_deriv{_ekf.getVelocityDerivative()};
lpos.ax = vel_deriv(0);
lpos.ay = vel_deriv(1);
lpos.az = vel_deriv(2);
// TODO: better status reporting 设置位置和速度的有效性标志
lpos.xy_valid = _ekf.local_position_is_valid();
lpos.z_valid = !_preflt_checker.hasVertFailed();
lpos.v_xy_valid = _ekf.local_position_is_valid();
lpos.v_z_valid = !_preflt_checker.hasVertFailed();
// Position of local NED origin in GPS / WGS84 frame
// 如果全局原点有效，则设置参考点信息（包括时间戳、纬度、经度和海拔）；否则，将其设置为无效状态。
if (_ekf.global_origin_valid()) {
lpos.ref_timestamp = _ekf.global_origin().getProjectionReferenceTimestamp();
lpos.ref_lat = _ekf.global_origin().getProjectionReferenceLat(); // Reference point latitude in degrees
lpos.ref_lon = _ekf.global_origin().getProjectionReferenceLon(); // Reference point longitude in degrees
lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude(); // Reference point in MSL altitude meters
lpos.xy_global = true;
lpos.z_global = true;
} else {
lpos.ref_timestamp = 0;
lpos.ref_lat = static_cast<double>(NAN);
lpos.ref_lon = static_cast<double>(NAN);
lpos.ref_alt = NAN;
lpos.xy_global = false;
lpos.z_global = false;
}
//获取并设置航向和航向变化信息
Quatf delta_q_reset;
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
lpos.heading = Eulerf(_ekf.getQuaternion()).psi();
lpos.delta_heading = Eulerf(delta_q_reset).psi();
lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete();
// Distance to bottom surface (ground) in meters
// constrain the distance to ground to _rng_gnd_clearance
// 设置飞行器与地面的距离信息，包括距离、有效性标志和传感器位字段
lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get());
lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield();
//从EKF获取位置和速度的精度信息，并将其设置到 lpos 中
_ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
_ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);
// get state reset information of position and velocity
// 从EKF获取位置和速度的状态重置信息，并将其设置到 lpos 中
_ekf.get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
_ekf.get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
// get control limit information
// 从EKF获取控制限制信息，并将其设置到 lpos 中。如果这些限制值不是有限值，则将其设置为无穷大。
_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
// convert NaN to INFINITY
if (!PX4_ISFINITE(lpos.vxy_max)) {
lpos.vxy_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.vz_max)) {
lpos.vz_max = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_min)) {
lpos.hagl_min = INFINITY;
}
if (!PX4_ISFINITE(lpos.hagl_max)) {
lpos.hagl_max = INFINITY;
}
// publish vehicle local position data 设置时间戳，并发布局部位置信息
lpos.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_local_position_pub.publish(lpos);
}
/*用于发布由EKF估计的里程计（odometry）数据*/
void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu)
{
// generate vehicle odometry data
vehicle_odometry_s odom;
odom.timestamp_sample = timestamp;
odom.local_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Vehicle odometry position
const Vector3f position{_ekf.getPosition()};
odom.x = position(0);
odom.y = position(1);
odom.z = position(2);
// Vehicle odometry linear velocity
odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_FRD;
const Vector3f velocity{_ekf.getVelocity()};
odom.vx = velocity(0);
odom.vy = velocity(1);
odom.vz = velocity(2);
// Vehicle odometry quaternion
_ekf.getQuaternion().copyTo(odom.q);
// Vehicle odometry angular rates
const Vector3f gyro_bias{_ekf.getGyroBias()};
const Vector3f rates{imu.delta_ang / imu.delta_ang_dt};
odom.rollspeed = rates(0) - gyro_bias(0);
odom.pitchspeed = rates(1) - gyro_bias(1);
odom.yawspeed = rates(2) - gyro_bias(2);
// get the covariance matrix size
static constexpr size_t POS_URT_SIZE = sizeof(odom.pose_covariance) / sizeof(odom.pose_covariance[0]);
static constexpr size_t VEL_URT_SIZE = sizeof(odom.velocity_covariance) / sizeof(odom.velocity_covariance[0]);
// Get covariances to vehicle odometry
float covariances[24];
_ekf.covariances_diagonal().copyTo(covariances);
// initially set pose covariances to 0
for (size_t i = 0; i < POS_URT_SIZE; i++) {
odom.pose_covariance[i] = 0.0;
}
// set the position variances
odom.pose_covariance[odom.COVARIANCE_MATRIX_X_VARIANCE] = covariances[7];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Y_VARIANCE] = covariances[8];
odom.pose_covariance[odom.COVARIANCE_MATRIX_Z_VARIANCE] = covariances[9];
// TODO: implement propagation from quaternion covariance to Euler angle covariance
// by employing the covariance law
// initially set velocity covariances to 0
for (size_t i = 0; i < VEL_URT_SIZE; i++) {
odom.velocity_covariance[i] = 0.0;
}
// set the linear velocity variances
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VX_VARIANCE] = covariances[4];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VY_VARIANCE] = covariances[5];
odom.velocity_covariance[odom.COVARIANCE_MATRIX_VZ_VARIANCE] = covariances[6];
odom.reset_counter = _ekf.get_quat_reset_count()
+ _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count()
+ _ekf.get_posNE_reset_count() + _ekf.get_posD_reset_count();
// publish vehicle odometry data
odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_odometry_pub.publish(odom);
}
//将外部视觉（EV）里程计数据转换并发布到EKF的导航坐标系中
void EKF2::PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_odometry_s &ev_odom)
{
const Quatf quat_ev2ekf = _ekf.getVisionAlignmentQuaternion(); // rotates from EV to EKF navigation frame
const Dcmf ev_rot_mat(quat_ev2ekf);
vehicle_odometry_s aligned_ev_odom{ev_odom};
// Rotate external position and velocity into EKF navigation frame
const Vector3f aligned_pos = ev_rot_mat * Vector3f(ev_odom.x, ev_odom.y, ev_odom.z);
aligned_ev_odom.x = aligned_pos(0);
aligned_ev_odom.y = aligned_pos(1);
aligned_ev_odom.z = aligned_pos(2);
switch (ev_odom.velocity_frame) {
case vehicle_odometry_s::BODY_FRAME_FRD: {
const Vector3f aligned_vel = Dcmf(_ekf.getQuaternion()) * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
case vehicle_odometry_s::LOCAL_FRAME_FRD: {
const Vector3f aligned_vel = ev_rot_mat * Vector3f(ev_odom.vx, ev_odom.vy, ev_odom.vz);
aligned_ev_odom.vx = aligned_vel(0);
aligned_ev_odom.vy = aligned_vel(1);
aligned_ev_odom.vz = aligned_vel(2);
break;
}
}
aligned_ev_odom.velocity_frame = vehicle_odometry_s::LOCAL_FRAME_NED;
// Compute orientation in EKF navigation frame
Quatf ev_quat_aligned = quat_ev2ekf * Quatf(ev_odom.q) ;
ev_quat_aligned.normalize();
ev_quat_aligned.copyTo(aligned_ev_odom.q);
quat_ev2ekf.copyTo(aligned_ev_odom.q_offset);
aligned_ev_odom.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_visual_odometry_aligned_pub.publish(aligned_ev_odom);
}
//用于发布传感器偏差信息
void EKF2::PublishSensorBias(const hrt_abstime ×tamp)
{
// estimator_sensor_bias
const Vector3f gyro_bias{_ekf.getGyroBias()};
const Vector3f accel_bias{_ekf.getAccelBias()};
const Vector3f mag_bias{_ekf.getMagBias()};
// publish at ~1 Hz, or sooner if there's a change
if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
|| (accel_bias - _last_accel_bias_published).longerThan(0.001f)
|| (mag_bias - _last_mag_bias_published).longerThan(0.001f)
|| (timestamp >= _last_sensor_bias_published + 1_s)) {
estimator_sensor_bias_s bias{};
bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
// take device ids from sensor_selection_s if not using specific vehicle_imu_s
if (_device_id_gyro != 0) {
bias.gyro_device_id = _device_id_gyro;
gyro_bias.copyTo(bias.gyro_bias);
bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
bias.gyro_bias_valid = true; // TODO
bias.gyro_bias_stable = _gyro_cal.cal_available;
_last_gyro_bias_published = gyro_bias;
}
if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
bias.accel_device_id = _device_id_accel;
accel_bias.copyTo(bias.accel_bias);
bias.accel_bias_limit = _params->acc_bias_lim;
_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
bias.accel_bias_valid = true; // TODO
bias.accel_bias_stable = _accel_cal.cal_available;
_last_accel_bias_published = accel_bias;
}
if (_device_id_mag != 0) {
bias.mag_device_id = _device_id_mag;
mag_bias.copyTo(bias.mag_bias);
bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
_ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance);
bias.mag_bias_valid = true; // TODO
bias.mag_bias_stable = _mag_cal.cal_available;
_last_mag_bias_published = mag_bias;
}
bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_sensor_bias_pub.publish(bias);
_last_sensor_bias_published = bias.timestamp;
}
}
/*发布EKF的状态估计和对应的协方差矩阵对角线元素*/
void EKF2::PublishStates(const hrt_abstime ×tamp)
{
// publish estimator states
estimator_states_s states;
states.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
states.n_states = Ekf::_k_num_states;
_ekf.getStateAtFusionHorizonAsVector().copyTo(states.states);
_ekf.covariances_diagonal().copyTo(states.covariances);
states.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_states_pub.publish(states);
}
/* EKF2::PublishStatus 是一个用于发布估计器状态的成员函数。
该函数从 EKF 实例中获取各种状态信息和测试结果，然后将这些信息填充到 estimator_status_s 结构体中，
最终通过 uORB 发布。以下是对该函数的详细解释*/
void EKF2::PublishStatus(const hrt_abstime ×tamp)
{
estimator_status_s status{};
status.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; //获取IMU延迟样本时间戳
_ekf.getOutputTrackingError().copyTo(status.output_tracking_error); //获取输出跟踪误差
// only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
// the GPS Fix bit, which is always checked)
status.gps_check_fail_flags = _ekf.gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);
status.control_mode_flags = _ekf.control_status().value;
status.filter_fault_flags = _ekf.fault_status().value;
uint16_t innov_check_flags_temp = 0;
_ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
status.vel_test_ratio, status.pos_test_ratio,
status.hgt_test_ratio, status.tas_test_ratio,
status.hagl_test_ratio, status.beta_test_ratio);
// Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
// TODO: legacy use only, those flags are also in estimator_status_flags
status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);
_ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
_ekf.get_ekf_soln_status(&status.solution_status_flags);
// reset counters
status.reset_count_vel_ne = _ekf.state_reset_status().velNE_counter;
status.reset_count_vel_d = _ekf.state_reset_status().velD_counter;
status.reset_count_pos_ne = _ekf.state_reset_status().posNE_counter;
status.reset_count_pod_d = _ekf.state_reset_status().posD_counter;
status.reset_count_quat = _ekf.state_reset_status().quat_counter;
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status.accel_device_id = _device_id_accel;
status.baro_device_id = _device_id_baro;
status.gyro_device_id = _device_id_gyro;
status.mag_device_id = _device_id_mag;
status.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_pub.publish(status);
}
/*
该函数用于发布由扩展卡尔曼滤波器（EKF）计算的状态标志信息。状态标志包括滤波器控制状态、故障状态和创新检验失败状态。发布频率大约为1 Hz，或者当状态发生变化时立即发布。
主要步骤和功能：
检查是否需要更新：
函数通过比较当前时间与上次发布的时间，确定是否需要更新状态标志。如果滤波器控制状态、故障状态或创新检验失败状态发生变化，也会触发更新。
更新状态标志：
如果状态发生变化，函数会更新相应的状态标志变量，并增加状态变化计数器。
构建并发布状态标志消息：
构建 estimator_status_flags_s 结构体，填充各种状态标志信息，包括滤波器控制状态、故障状态、创新检验失败状态及其变化计数器。最后，发布状态标志消息，并更新上次发布的时间戳。
*/
void EKF2::PublishStatusFlags(const hrt_abstime ×tamp)
{
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
bool update = (timestamp >= _last_status_flags_publish + 1_s);
// filter control status
if (_ekf.control_status().value != _filter_control_status) {
update = true;
_filter_control_status = _ekf.control_status().value;
_filter_control_status_changes++;
}
// filter fault status
if (_ekf.fault_status().value != _filter_fault_status) {
update = true;
_filter_fault_status = _ekf.fault_status().value;
_filter_fault_status_changes++;
}
// innovation check fail status
if (_ekf.innov_check_fail_status().value != _innov_check_fail_status) {
update = true;
_innov_check_fail_status = _ekf.innov_check_fail_status().value;
_innov_check_fail_status_changes++;
}
if (update) {
estimator_status_flags_s status_flags{};
status_flags.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
status_flags.control_status_changes = _filter_control_status_changes;
status_flags.cs_tilt_align = _ekf.control_status_flags().tilt_align;
status_flags.cs_yaw_align = _ekf.control_status_flags().yaw_align;
status_flags.cs_gps = _ekf.control_status_flags().gps;
status_flags.cs_opt_flow = _ekf.control_status_flags().opt_flow;
status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg;
status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D;
status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec;
status_flags.cs_in_air = _ekf.control_status_flags().in_air;
status_flags.cs_wind = _ekf.control_status_flags().wind;
status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt;
status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt;
status_flags.cs_gps_hgt = _ekf.control_status_flags().gps_hgt;
status_flags.cs_ev_pos = _ekf.control_status_flags().ev_pos;
status_flags.cs_ev_yaw = _ekf.control_status_flags().ev_yaw;
status_flags.cs_ev_hgt = _ekf.control_status_flags().ev_hgt;
status_flags.cs_fuse_beta = _ekf.control_status_flags().fuse_beta;
status_flags.cs_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status_flags.cs_fixed_wing = _ekf.control_status_flags().fixed_wing;
status_flags.cs_mag_fault = _ekf.control_status_flags().mag_fault;
status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd;
status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect;
status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck;
status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw;
status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel;
status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z;
status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest;
status_flags.cs_gps_yaw_fault = _ekf.control_status_flags().gps_yaw_fault;
status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault;
status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning;
status_flags.cs_rng_kin_consistent = _ekf.control_status_flags().rng_kin_consistent;
status_flags.fault_status_changes = _filter_fault_status_changes;
status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x;
status_flags.fs_bad_mag_y = _ekf.fault_status_flags().bad_mag_y;
status_flags.fs_bad_mag_z = _ekf.fault_status_flags().bad_mag_z;
status_flags.fs_bad_hdg = _ekf.fault_status_flags().bad_hdg;
status_flags.fs_bad_mag_decl = _ekf.fault_status_flags().bad_mag_decl;
status_flags.fs_bad_airspeed = _ekf.fault_status_flags().bad_airspeed;
status_flags.fs_bad_sideslip = _ekf.fault_status_flags().bad_sideslip;
status_flags.fs_bad_optflow_x = _ekf.fault_status_flags().bad_optflow_X;
status_flags.fs_bad_optflow_y = _ekf.fault_status_flags().bad_optflow_Y;
status_flags.fs_bad_vel_n = _ekf.fault_status_flags().bad_vel_N;
status_flags.fs_bad_vel_e = _ekf.fault_status_flags().bad_vel_E;
status_flags.fs_bad_vel_d = _ekf.fault_status_flags().bad_vel_D;
status_flags.fs_bad_pos_n = _ekf.fault_status_flags().bad_pos_N;
status_flags.fs_bad_pos_e = _ekf.fault_status_flags().bad_pos_E;
status_flags.fs_bad_pos_d = _ekf.fault_status_flags().bad_pos_D;
status_flags.fs_bad_acc_bias = _ekf.fault_status_flags().bad_acc_bias;
status_flags.fs_bad_acc_vertical = _ekf.fault_status_flags().bad_acc_vertical;
status_flags.fs_bad_acc_clipping = _ekf.fault_status_flags().bad_acc_clipping;
status_flags.innovation_fault_status_changes = _innov_check_fail_status_changes;
status_flags.reject_hor_vel = _ekf.innov_check_fail_status_flags().reject_hor_vel;
status_flags.reject_ver_vel = _ekf.innov_check_fail_status_flags().reject_ver_vel;
status_flags.reject_hor_pos = _ekf.innov_check_fail_status_flags().reject_hor_pos;
status_flags.reject_ver_pos = _ekf.innov_check_fail_status_flags().reject_ver_pos;
status_flags.reject_mag_x = _ekf.innov_check_fail_status_flags().reject_mag_x;
status_flags.reject_mag_y = _ekf.innov_check_fail_status_flags().reject_mag_y;
status_flags.reject_mag_z = _ekf.innov_check_fail_status_flags().reject_mag_z;
status_flags.reject_yaw = _ekf.innov_check_fail_status_flags().reject_yaw;
status_flags.reject_airspeed = _ekf.innov_check_fail_status_flags().reject_airspeed;
status_flags.reject_sideslip = _ekf.innov_check_fail_status_flags().reject_sideslip;
status_flags.reject_hagl = _ekf.innov_check_fail_status_flags().reject_hagl;
status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X;
status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_flags_pub.publish(status_flags);
_last_status_flags_publish = status_flags.timestamp;
}
}
/*
该函数用于发布由扩展卡尔曼滤波器（EKF）计算的航向估计器状态。航向估计器状态包括复合航向、航向方差、航向创新、北向和东向创新及其权重。
主要步骤和功能：
获取航向估计数据：
函数通过调用 _ekf.getDataEKFGSF 获取航向估计数据。如果数据有效，则继续下一步。
构建并发布航向估计器状态消息：
构建 yaw_estimator_status_s 结构体，填充航向估计器状态数据，包括复合航向、航向方差、航向创新、北向和东向创新及其权重。最后，发布航向估计器状态消息。
*/
void EKF2::PublishYawEstimatorStatus(const hrt_abstime ×tamp)
{
static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
"yaw_estimator_status_s::yaw wrong size");
yaw_estimator_status_s yaw_est_test_data;
if (_ekf.getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance,
yaw_est_test_data.yaw,
yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
yaw_est_test_data.weight)) {
yaw_est_test_data.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
yaw_est_test_data.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_yaw_est_pub.publish(yaw_est_test_data);
}
}
/*
该函数用于发布由扩展卡尔曼滤波器（EKF）计算的风速估计信息。仅当EKF声明风速估计有效时，才会发布风速估计信息。
主要步骤和功能：
检查风速估计有效性：
通过 _ekf.get_wind_status() 检查EKF是否声明风速估计有效。
构建风速估计消息：
如果风速估计有效，则构建 wind_s 结构体。填充风速估计数据，包括北向和东向的风速及其方差，空速创新及其方差，侧滑角创新及其方差。
发布风速估计消息：
填充时间戳后，发布风速估计消息。
*/
void EKF2::PublishWindEstimate(const hrt_abstime ×tamp)
{
if (_ekf.get_wind_status()) {
// Publish wind estimate only if ekf declares them valid
wind_s wind{};
wind.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
const Vector2f wind_vel = _ekf.getWindVelocity();
const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
_ekf.getAirspeedInnov(wind.tas_innov);
_ekf.getAirspeedInnovVar(wind.tas_innov_var);
_ekf.getBetaInnov(wind.beta_innov);
_ekf.getBetaInnovVar(wind.beta_innov_var);
wind.windspeed_north = wind_vel(0);
wind.windspeed_east = wind_vel(1);
wind.variance_north = wind_vel_var(0);
wind.variance_east = wind_vel_var(1);
wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_wind_pub.publish(wind);
}
}
/*
该函数用于发布由扩展卡尔曼滤波器（EKF）计算的光流速度估计信息。
主要步骤和功能：
检查光流补偿时间：
通过 _ekf.getFlowCompensated().longerThan(0.f) 检查光流补偿时间是否大于零。
构建光流速度估计消息：
如果光流补偿时间有效，则构建 estimator_optical_flow_vel_s 结构体。填充光流速度估计数据，包括身体坐标系下的速度、东北坐标系下的速度、未补偿和补偿后的光流积分、陀螺积分。
发布光流速度估计消息：
填充时间戳后，发布光流速度估计消息。
*/
void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp)
{
if (_ekf.getFlowCompensated().longerThan(0.f)) {
estimator_optical_flow_vel_s flow_vel{};
flow_vel.timestamp_sample = _ekf.get_imu_sample_delayed().time_us;
_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
_ekf.getFlowUncompensated().copyTo(flow_vel.flow_uncompensated_integral);
_ekf.getFlowCompensated().copyTo(flow_vel.flow_compensated_integral);
_ekf.getFlowGyro().copyTo(flow_vel.gyro_rate_integral);
flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_optical_flow_vel_pub.publish(flow_vel);
}
}
/*
该函数用于过滤椭球高度数据，计算相对于平均海平面高度的偏移量。
主要步骤和功能：
计算高度差：
计算GPS椭球高度和平均海平面高度之间的高度差。
初始化高度偏移：
如果 _gps_alttitude_ellipsoid_previous_timestamp 为零，则初始化高度偏移 _wgs84_hgt_offset 为高度差，并记录当前时间戳。
应用低通滤波器：
如果时间戳更新，则应用一阶低通滤波器，对高度偏移进行滤波。计算时间差 dt，并根据高度差和当前偏移值计算修正率 offset_rate_correction。更新高度偏移值 _wgs84_hgt_offset。
返回修正后的高度：
返回修正后的平均海平面高度。
*/
float EKF2::filter_altitude_ellipsoid(float amsl_hgt)
{
float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;
if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {
_wgs84_hgt_offset = height_diff;
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
} else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {
// apply a 10 second first order low pass filter to baro offset
float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
_gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
_wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
}
return amsl_hgt + _wgs84_hgt_offset;
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的空速样本。
主要步骤和功能：
优先使用已验证的空速数据：
如果 airspeed_validated 主题有更新数据，则获取最新的已验证空速数据，并检查数据是否为有限值且校准空速大于零。如果条件满足，则构建 airspeedSample 结构体并设置 EKF 的空速数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的已验证空速消息。
使用原始空速数据作为备用：
如果 airspeed_validated 主题在超过3秒内没有更新，且 airspeed 主题有更新数据，则获取最新的原始空速数据，并应用空速比例因子对其进行校正。如果数据有效，则构建 airspeedSample 结构体并设置 EKF 的空速数据。同时更新 ekf2_timestamps 的空速时间戳。
*/
void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF airspeed sample
// prefer ORB_ID(airspeed_validated) if available, otherwise fallback to raw airspeed ORB_ID(airspeed)
if (_airspeed_validated_sub.updated()) {
const unsigned last_generation = _airspeed_validated_sub.get_last_generation();
airspeed_validated_s airspeed_validated;
if (_airspeed_validated_sub.update(&airspeed_validated)) {
if (_msg_missed_airspeed_validated_perf == nullptr) {
_msg_missed_airspeed_validated_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed validated messages missed");
} else if (_airspeed_validated_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_airspeed_validated_perf);
}
if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
&& PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
&& (airspeed_validated.calibrated_airspeed_m_s > 0.f)
) {
airspeedSample airspeed_sample {
.time_us = airspeed_validated.timestamp,
.true_airspeed = airspeed_validated.true_airspeed_m_s,
.eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
}
_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
}
} else if ((hrt_elapsed_time(&_airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
// use ORB_ID(airspeed) if ORB_ID(airspeed_validated) is unavailable
const unsigned last_generation = _airspeed_sub.get_last_generation();
airspeed_s airspeed;
if (_airspeed_sub.update(&airspeed)) {
if (_msg_missed_airspeed_perf == nullptr) {
_msg_missed_airspeed_perf = perf_alloc(PC_COUNT, MODULE_NAME": airspeed messages missed");
} else if (_airspeed_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_airspeed_perf);
}
// The airspeed measurement received via ORB_ID(airspeed) topic has not been corrected
// for scale factor errors and requires the ASPD_SCALE correction to be applied.
const float true_airspeed_m_s = airspeed.true_airspeed_m_s * _airspeed_scale_factor;
if (PX4_ISFINITE(airspeed.true_airspeed_m_s)
&& PX4_ISFINITE(airspeed.indicated_airspeed_m_s)
&& (airspeed.indicated_airspeed_m_s > 0.f)
) {
airspeedSample airspeed_sample {
.time_us = airspeed.timestamp_sample,
.true_airspeed = true_airspeed_m_s,
.eas2tas = airspeed.true_airspeed_m_s / airspeed.indicated_airspeed_m_s,
};
_ekf.setAirspeedData(airspeed_sample);
}
ekf2_timestamps.airspeed_timestamp_rel = (int16_t)((int64_t)airspeed.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的辅助速度样本。
主要步骤和功能：
获取最新的着陆目标姿态数据：
如果 landing_target_pose 主题有更新数据，则获取最新的着陆目标姿态数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的着陆目标姿态消息。
验证目标静止性和速度估计：
如果目标是静止的且速度估计有效，则构建 auxVelSample 结构体，其中速度的符号取反（相对于目标的速度）并设置 EKF 的辅助速度数据。
*/
void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF auxillary velocity sample
// - use the landing target pose estimate as another source of velocity data
const unsigned last_generation = _landing_target_pose_sub.get_last_generation();
landing_target_pose_s landing_target_pose;
if (_landing_target_pose_sub.update(&landing_target_pose)) {
if (_msg_missed_landing_target_pose_perf == nullptr) {
_msg_missed_landing_target_pose_perf = perf_alloc(PC_COUNT, MODULE_NAME": landing_target_pose messages missed");
} else if (_landing_target_pose_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_landing_target_pose_perf);
}
// we can only use the landing target if it has a fixed position and a valid velocity estimate
if (landing_target_pose.is_static && landing_target_pose.rel_vel_valid) {
// velocity of vehicle relative to target has opposite sign to target relative to vehicle
auxVelSample auxvel_sample{
.time_us = landing_target_pose.timestamp,
.vel = Vector3f{-landing_target_pose.vx_rel, -landing_target_pose.vy_rel, 0.0f},
.velVar = Vector3f{landing_target_pose.cov_vx_rel, landing_target_pose.cov_vy_rel, 0.0f},
};
_ekf.setAuxVelData(auxvel_sample);
}
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的气压高度样本。
主要步骤和功能：
获取最新的气压数据：
如果 vehicle_air_data 主题有更新数据，则获取最新的气压数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的气压数据消息。
检查和重置气压计校准：
如果气压计设备ID发生变化或者校准计数增加，则重置气压计设备ID和校准计数。
设置空气密度和气压高度数据：
设置 EKF 的空气密度和气压高度数据，并更新 ekf2_timestamps 的气压数据时间戳。
*/
void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF baro sample
const unsigned last_generation = _airdata_sub.get_last_generation();
vehicle_air_data_s airdata;
if (_airdata_sub.update(&airdata)) {
if (_msg_missed_air_data_perf == nullptr) {
_msg_missed_air_data_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_air_data messages missed");
} else if (_airdata_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_air_data_perf);
}
bool reset = false;
// check if barometer has changed
if (airdata.baro_device_id != _device_id_baro) {
if (_device_id_baro != 0) {
PX4_WARN("%d - baro sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_baro, airdata.baro_device_id);
}
reset = true;
} else if (airdata.calibration_count > _baro_calibration_count) {
// existing calibration has changed, reset saved baro bias
PX4_DEBUG("%d - baro %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_baro);
reset = true;
}
if (reset) {
// TODO: reset baro ref and bias estimate?
_device_id_baro = airdata.baro_device_id;
_baro_calibration_count = airdata.calibration_count;
}
_ekf.set_air_density(airdata.rho);
_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter});
_device_id_baro = airdata.baro_device_id;
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的外部视觉样本。
主要步骤和功能：
获取最新的外部视觉里程计数据：
如果 ev_odom 主题有更新数据，则获取最新的外部视觉里程计数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的视觉里程计消息。
初始化外部视觉样本结构体 extVisionSample。
处理有效的速度数据：
检查 ev_odom 中的速度数据是否为有限值。
如果数据有效，则将其存储到 ev_data.vel 中，并根据速度数据的框架设置 ev_data.vel_frame。
如果误差估计不可用，则使用参数定义的默认值。
处理有效的位置数据：
检查 ev_odom 中的位置数据是否为有限值。
如果数据有效，则将其存储到 ev_data.pos 中。
使用数据中的协方差或参数定义的默认值设置位置误差。
处理有效的方向数据：
检查 ev_odom 中的四元数数据是否为有限值。
如果数据有效，则将其存储到 ev_data.quat 中。
使用数据中的协方差或参数定义的默认值设置方向误差。
使用外部计算机的时间戳：
设置 ev_data.time_us 为外部视觉数据的时间戳。
如果有新数据，则将外部视觉数据设置到 EKF 中。
更新时间戳并返回结果：
更新 ekf2_timestamps 的视觉里程计时间戳。
返回布尔值表示是否有新数据。
*/
bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps, vehicle_odometry_s &ev_odom)
{
// EKF external vision sample
bool new_ev_odom = false;
const unsigned last_generation = _ev_odom_sub.get_last_generation();
if (_ev_odom_sub.update(&ev_odom)) {
if (_msg_missed_odometry_perf == nullptr) {
_msg_missed_odometry_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_visual_odometry messages missed");
} else if (_ev_odom_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_odometry_perf);
}
extVisionSample ev_data{};
// if error estimates are unavailable, use parameter defined defaults
// check for valid velocity data
if (PX4_ISFINITE(ev_odom.vx) && PX4_ISFINITE(ev_odom.vy) && PX4_ISFINITE(ev_odom.vz)) {
ev_data.vel(0) = ev_odom.vx;
ev_data.vel(1) = ev_odom.vy;
ev_data.vel(2) = ev_odom.vz;
if (ev_odom.velocity_frame == vehicle_odometry_s::BODY_FRAME_FRD) {
ev_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD;
} else {
ev_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD;
}
// velocity measurement error from ev_data or parameters
float param_evv_noise_var = sq(_param_ekf2_evv_noise.get());
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE])
&& PX4_ISFINITE(ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])) {
ev_data.velCov(0, 0) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VX_VARIANCE];
ev_data.velCov(0, 1) = ev_data.velCov(1, 0) = ev_odom.velocity_covariance[1];
ev_data.velCov(0, 2) = ev_data.velCov(2, 0) = ev_odom.velocity_covariance[2];
ev_data.velCov(1, 1) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VY_VARIANCE];
ev_data.velCov(1, 2) = ev_data.velCov(2, 1) = ev_odom.velocity_covariance[7];
ev_data.velCov(2, 2) = ev_odom.velocity_covariance[ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE];
} else {
ev_data.velCov = matrix::eye<float, 3>() * param_evv_noise_var;
}
new_ev_odom = true;
}
// check for valid position data
if (PX4_ISFINITE(ev_odom.x) && PX4_ISFINITE(ev_odom.y) && PX4_ISFINITE(ev_odom.z)) {
ev_data.pos(0) = ev_odom.x;
ev_data.pos(1) = ev_odom.y;
ev_data.pos(2) = ev_odom.z;
float param_evp_noise_var = sq(_param_ekf2_evp_noise.get());
// position measurement error from ev_data or parameters
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE])
&& PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])) {
ev_data.posVar(0) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE]);
ev_data.posVar(1) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]);
ev_data.posVar(2) = fmaxf(param_evp_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]);
} else {
ev_data.posVar.setAll(param_evp_noise_var);
}
new_ev_odom = true;
}
// check for valid orientation data
if (PX4_ISFINITE(ev_odom.q[0])) {
ev_data.quat = Quatf(ev_odom.q);
// orientation measurement error from ev_data or parameters
float param_eva_noise_var = sq(_param_ekf2_eva_noise.get());
if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) {
ev_data.angVar = fmaxf(param_eva_noise_var, ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE]);
} else {
ev_data.angVar = param_eva_noise_var;
}
new_ev_odom = true;
}
// use timestamp from external computer, clocks are synchronized when using MAVROS
ev_data.time_us = ev_odom.timestamp_sample;
if (new_ev_odom) {
_ekf.setExtVisionData(ev_data);
}
ekf2_timestamps.visual_odometry_timestamp_rel = (int16_t)((int64_t)ev_odom.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
return new_ev_odom;
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的光流样本。
主要步骤和功能：
获取最新的光流数据：
如果 optical_flow 主题有更新数据，则获取最新的光流数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的光流数据消息。
初始化光流样本结构体 flowSample：
设置时间戳、光流角速度和陀螺仪角速度。
光流传感器的符号约定与 EKF 的约定相反，因此对光流数据进行符号变换。
设置积分时间和质量。
检查光流数据的有效性：
确认光流数据的有限性并且积分时间小于1秒。
保存光流传感器报告的传感器限制：
设置光流传感器的最大流速、最小和最大地面距离。
将光流数据设置到 EKF 中：
如果数据有效，则将光流数据设置到 EKF 中。
更新时间戳并返回结果：
更新 ekf2_timestamps 的光流时间戳。
返回布尔值表示是否有新数据。
*/
bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF flow sample
bool new_optical_flow = false;
const unsigned last_generation = _optical_flow_sub.get_last_generation();
optical_flow_s optical_flow;
if (_optical_flow_sub.update(&optical_flow)) {
if (_msg_missed_optical_flow_perf == nullptr) {
_msg_missed_optical_flow_perf = perf_alloc(PC_COUNT, MODULE_NAME": optical_flow messages missed");
} else if (_optical_flow_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_optical_flow_perf);
}
flowSample flow {
.time_us = optical_flow.timestamp,
// NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
// is produced by a RH rotation of the image about the sensor axis.
.flow_xy_rad = Vector2f{-optical_flow.pixel_flow_x_integral, -optical_flow.pixel_flow_y_integral},
.gyro_xyz = Vector3f{-optical_flow.gyro_x_rate_integral, -optical_flow.gyro_y_rate_integral, -optical_flow.gyro_z_rate_integral},
.dt = 1e-6f * (float)optical_flow.integration_timespan,
.quality = optical_flow.quality,
};
if (PX4_ISFINITE(optical_flow.pixel_flow_y_integral) &&
PX4_ISFINITE(optical_flow.pixel_flow_x_integral) &&
flow.dt < 1) {
// Save sensor limits reported by the optical flow sensor
_ekf.set_optical_flow_limits(optical_flow.max_flow_rate, optical_flow.min_ground_distance,
optical_flow.max_ground_distance);
_ekf.setOpticalFlowData(flow);
new_optical_flow = true;
}
ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
return new_optical_flow;
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的GPS样本。
主要步骤和功能：
获取最新的GPS数据：
如果 vehicle_gps_position_sub 主题有更新数据，则获取最新的GPS位置数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的GPS消息。
初始化GPS消息结构体 gps_message：
从 vehicle_gps_position 中提取并设置时间戳、位置、速度、方位角等数据。
设置GPS数据到EKF：
将提取的GPS数据设置到EKF中。
更新时间戳和海拔信息：
更新GPS时间戳和椭球体海拔信息。
*/
void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
{
// EKF GPS message
const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation();
vehicle_gps_position_s vehicle_gps_position;
if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) {
if (_msg_missed_gps_perf == nullptr) {
_msg_missed_gps_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_gps_position messages missed");
} else if (_vehicle_gps_position_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_gps_perf);
}
gps_message gps_msg{
.time_usec = vehicle_gps_position.timestamp,
.lat = vehicle_gps_position.lat,
.lon = vehicle_gps_position.lon,
.alt = vehicle_gps_position.alt,
.yaw = vehicle_gps_position.heading,
.yaw_offset = vehicle_gps_position.heading_offset,
.fix_type = vehicle_gps_position.fix_type,
.eph = vehicle_gps_position.eph,
.epv = vehicle_gps_position.epv,
.sacc = vehicle_gps_position.s_variance_m_s,
.vel_m_s = vehicle_gps_position.vel_m_s,
.vel_ned = Vector3f{
vehicle_gps_position.vel_n_m_s,
vehicle_gps_position.vel_e_m_s,
vehicle_gps_position.vel_d_m_s
},
.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
.nsats = vehicle_gps_position.satellites_used,
.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
+ vehicle_gps_position.vdop * vehicle_gps_position.vdop),
};
_ekf.setGpsData(gps_msg);
_gps_time_usec = gps_msg.time_usec;
_gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid;
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的磁力计样本。
主要步骤和功能：
获取最新的磁力计数据：
如果 magnetometer_sub 主题有更新数据，则获取最新的磁力计数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的磁力计消息。
检测磁力计是否改变：
如果磁力计设备ID发生变化或校准计数发生变化，则重置磁力计偏差。
设置磁力计数据到EKF：
将提取的磁力计数据设置到EKF中。
更新时间戳：
更新磁力计时间戳
*/
void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
{
const unsigned last_generation = _magnetometer_sub.get_last_generation();
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.update(&magnetometer)) {
if (_msg_missed_magnetometer_perf == nullptr) {
_msg_missed_magnetometer_perf = perf_alloc(PC_COUNT, MODULE_NAME": vehicle_magnetometer messages missed");
} else if (_magnetometer_sub.get_last_generation() != last_generation + 1) {
perf_count(_msg_missed_magnetometer_perf);
}
bool reset = false;
// check if magnetometer has changed
if (magnetometer.device_id != _device_id_mag) {
if (_device_id_mag != 0) {
PX4_WARN("%d - mag sensor ID changed %" PRIu32 " -> %" PRIu32, _instance, _device_id_mag, magnetometer.device_id);
}
reset = true;
} else if (magnetometer.calibration_count > _mag_calibration_count) {
// existing calibration has changed, reset saved mag bias
PX4_DEBUG("%d - mag %" PRIu32 " calibration updated, resetting bias", _instance, _device_id_mag);
reset = true;
}
if (reset) {
_ekf.resetMagBias();
_device_id_mag = magnetometer.device_id;
_mag_calibration_count = magnetometer.calibration_count;
// reset magnetometer bias learning
_mag_cal = {};
}
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}});
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的距离传感器样本。
主要步骤和功能：
选择距离传感器：
如果没有选择距离传感器，则遍历所有广告的距离传感器，选择第一个正确朝向的传感器。
获取最新的距离传感器数据：
如果已经选择了距离传感器且该传感器有更新数据，则获取最新的距离传感器数据。
统计丢失的消息：
通过检查 last_generation 和最新消息的生成代数，统计丢失的距离传感器消息。
设置距离传感器数据到EKF：
将提取的距离传感器数据设置到EKF中。
保存传感器限制：
保存距离传感器报告的传感器最小和最大距离限制。
更新时间戳和选择状态：
更新距离传感器时间戳和选择状态。如果距离传感器数据超时，则重置选择状态
*/
void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
{
distance_sensor_s distance_sensor;
if (_distance_sensor_selected < 0) {
if (_distance_sensor_subs.advertised()) {
for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
if (_distance_sensor_subs[i].update(&distance_sensor)) {
// only use the first instace which has the correct orientation
if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
&& (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
int ndist = orb_group_count(ORB_ID(distance_sensor));
if (ndist > 1) {
PX4_INFO("%d - selected distance_sensor:%d (%d advertised)", _instance, i, ndist);
}
_distance_sensor_selected = i;
_last_range_sensor_update = distance_sensor.timestamp;
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation() - 1;
break;
}
}
}
}
}
if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
// EKF range sample
if (_msg_missed_distance_sensor_perf == nullptr) {
_msg_missed_distance_sensor_perf = perf_alloc(PC_COUNT, MODULE_NAME": distance_sensor messages missed");
} else if (_distance_sensor_subs[_distance_sensor_selected].get_last_generation() != _distance_sensor_last_generation +
1) {
perf_count(_msg_missed_distance_sensor_perf);
}
_distance_sensor_last_generation = _distance_sensor_subs[_distance_sensor_selected].get_last_generation();
if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
rangeSample range_sample {
.time_us = distance_sensor.timestamp,
.rng = distance_sensor.current_distance,
.quality = distance_sensor.signal_quality,
};
_ekf.setRangeData(range_sample);
// Save sensor limits reported by the rangefinder
_ekf.set_rangefinder_limits(distance_sensor.min_distance, distance_sensor.max_distance);
_last_range_sensor_update = distance_sensor.timestamp;
return;
}
ekf2_timestamps.distance_sensor_timestamp_rel = (int16_t)((int64_t)distance_sensor.timestamp / 100 -
(int64_t)ekf2_timestamps.timestamp / 100);
}
if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) {
_distance_sensor_selected = -1;
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的加速度计校准数据。
主要步骤和功能：
检查是否禁止加速度计偏置校准：
如果参数 MASK_INHIBIT_ACC_BIAS 设置为真，则禁用加速度计偏置校准并返回。
定义最大允许的方差和方差比：
max_var_allowed 和 max_var_ratio 分别定义最大允许的方差和最大方差比。
获取加速度计偏置方差：
从EKF获取加速度计偏置方差。
检查条件是否适合学习加速度计偏置：
确保EKF处于正确模式，没有滤波器故障，并且各种状态和标志满足特定条件。
检查加速度计偏置方差是否在允许范围内。
更新校准时间和可用性：
如果满足条件，累积校准时间。当总时间超过30秒时，标记校准数据为可用。
更新上次时间戳。
重置校准数据：
如果条件不满足，则重置校准数据
*/
void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp)
{
if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
_accel_cal.cal_available = false;
return;
}
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
const Vector3f bias_variance{_ekf.getAccelBiasVariance()};
// Check if conditions are OK for learning of accelerometer bias values
// the EKF is operating in the correct mode and there are no filter faults
if ((_ekf.fault_status().value == 0)
&& !_ekf.accel_bias_inhibited()
&& !_preflt_checker.hasHorizFailed() && !_preflt_checker.hasVertFailed()
&& (_ekf.control_status_flags().baro_hgt || _ekf.control_status_flags().rng_hgt
|| _ekf.control_status_flags().gps_hgt || _ekf.control_status_flags().ev_hgt)
&& !_ekf.warning_event_flags().height_sensor_timeout && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset
&& !_ekf.innov_check_fail_status_flags().reject_ver_pos && !_ekf.innov_check_fail_status_flags().reject_ver_vel
&& (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min())
) {
if (_accel_cal.last_us != 0) {
_accel_cal.total_time_us += timestamp - _accel_cal.last_us;
// consider bias estimates stable when we have accumulated sufficient time
if (_accel_cal.total_time_us > 30_s) {
_accel_cal.cal_available = true;
}
}
_accel_cal.last_us = timestamp;
} else {
_accel_cal = {};
}
}
/*
该函数用于更新扩展卡尔曼滤波器（EKF）的陀螺仪校准数据。
主要步骤和功能：
定义最大允许的方差和方差比：
max_var_allowed 和 max_var_ratio 分别定义最大允许的方差和最大方差比。
获取陀螺仪偏置方差：
从EKF获取陀螺仪偏置方差。
检查条件是否适合学习陀螺仪偏置：
确保EKF没有滤波器故障，并且陀螺仪偏置方差在允许范围内。
更新校准时间和可用性：
如果满足条件，累积校准时间。当总时间超过30秒时，标记校准数据为可用。
更新上次时间戳。
重置校准数据：
如果条件不满足，则重置校准数据
*/
void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp)
{
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
const Vector3f bias_variance{_ekf.getGyroBiasVariance()};
// Check if conditions are OK for learning of gyro bias values
// the EKF is operating in the correct mode and there are no filter faults
if ((_ekf.fault_status().value == 0)
&& (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min())
) {
if (_gyro_cal.last_us != 0) {
_gyro_cal.total_time_us += timestamp - _gyro_cal.last_us;
// consider bias estimates stable when we have accumulated sufficient time
if (_gyro_cal.total_time_us > 30_s) {
_gyro_cal.cal_available = true;
}
}
_gyro_cal.last_us = timestamp;
} else {
// conditions are NOT OK for learning bias, reset
_gyro_cal = {};
}
}
/*更新磁力计（magnetometer）的校准状态 LELIN*/
void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp)
{
// Check if conditions are OK for learning of magnetometer bias values
// the EKF is operating in the correct mode and there are no filter faults
static constexpr float max_var_allowed = 1e-3f;
static constexpr float max_var_ratio = 1e2f;
const Vector3f bias_variance{_ekf.getMagBiasVariance()};
bool valid = _ekf.control_status_flags().in_air
&& (_ekf.fault_status().value == 0)
&& (bias_variance.max() < max_var_allowed)
&& (bias_variance.max() < max_var_ratio * bias_variance.min());
if (valid && _ekf.control_status_flags().mag_3D) {
if (_mag_cal.last_us != 0) {
_mag_cal.total_time_us += timestamp - _mag_cal.last_us;
// consider bias estimates stable when we have accumulated sufficient time
if (_mag_cal.total_time_us > 30_s) {
_mag_cal.cal_available = true;
}
}
_mag_cal.last_us = timestamp;
} else {
// conditions are NOT OK for learning magnetometer bias, reset timestamp
// but keep the accumulated calibration time
_mag_cal.last_us = 0;
if (!valid) {
// if a filter fault has occurred, assume previous learning was invalid and do not
// count it towards total learning time.
_mag_cal.total_time_us = 0;
}
}
// update stored declination value
if (!_mag_decl_saved) {
float declination_deg;
if (_ekf.get_mag_decl_deg(&declination_deg)) {
_param_ekf2_mag_decl.update();
if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) {
_param_ekf2_mag_decl.set(declination_deg);
_param_ekf2_mag_decl.commit_no_notification();
}
_mag_decl_saved = true;
}
}
}
int EKF2::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int EKF2::task_spawn(int argc, char *argv[])
{
bool success = false;
bool replay_mode = false;
if (argc > 1 && !strcmp(argv[1], "-r")) {
PX4_INFO("replay mode enabled");
replay_mode = true;
}
#if !defined(CONSTRAINED_FLASH)
bool multi_mode = false;
int32_t imu_instances = 0;
int32_t mag_instances = 0;
int32_t sens_imu_mode = 1;
param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
if (sens_imu_mode == 0) {
// ekf selector requires SENS_IMU_MODE = 0
multi_mode = true;
// IMUs (1 - 4 supported)
param_get(param_find("EKF2_MULTI_IMU"), &imu_instances);
if (imu_instances < 1 || imu_instances > 4) {
const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
PX4_WARN("EKF2_MULTI_IMU limited %" PRId32 " -> %" PRId32, imu_instances, imu_instances_limited);
param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited);
imu_instances = imu_instances_limited;
}
int32_t sens_mag_mode = 1;
param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
if (sens_mag_mode == 0) {
param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
// Mags (1 - 4 supported)
if (mag_instances < 1 || mag_instances > 4) {
const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited);
param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
mag_instances = mag_instances_limited;
}
} else {
mag_instances = 1;
}
}
if (multi_mode) {
// Start EKF2Selector if it's not already running
if (_ekf2_selector.load() == nullptr) {
EKF2Selector *inst = new EKF2Selector();
if (inst) {
_ekf2_selector.store(inst);
} else {
PX4_ERR("Failed to create EKF2 selector");
return PX4_ERROR;
}
}
const hrt_abstime time_started = hrt_absolute_time();
const int multi_instances = math::min(imu_instances * mag_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
int multi_instances_allocated = 0;
// allocate EKF2 instances until all found or arming
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
while ((multi_instances_allocated < multi_instances)
&& (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
&& ((hrt_elapsed_time(&time_started) < 30_s)
|| (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {
vehicle_status_sub.update();
for (uint8_t mag = 0; mag < mag_instances; mag++) {
uORB::SubscriptionData<vehicle_magnetometer_s> vehicle_mag_sub{ORB_ID(vehicle_magnetometer), mag};
for (uint8_t imu = 0; imu < imu_instances; imu++) {
uORB::SubscriptionData<vehicle_imu_s> vehicle_imu_sub{ORB_ID(vehicle_imu), imu};
vehicle_mag_sub.update();
// Mag & IMU data must be valid, first mag can be ignored initially
if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
if (!ekf2_instance_created[imu][mag]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
_objects[actual_instance].store(ekf2_inst);
success = true;
multi_instances_allocated++;
ekf2_instance_created[imu][mag] = true;
PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
_ekf2_selector.load()->ScheduleNow();
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
delete ekf2_inst;
break;
}
} else {
PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
px4_usleep(100000);
break;
}
}
} else {
px4_usleep(1000); // give the sensors extra time to start
break;
}
}
}
if (multi_instances_allocated < multi_instances) {
px4_usleep(10000);
}
}
}
#endif // !CONSTRAINED_FLASH
else {
// otherwise launch regular
EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
if (ekf2_inst) {
_objects[0].store(ekf2_inst);
ekf2_inst->ScheduleNow();
success = true;
}
}
return success ? PX4_OK : PX4_ERROR;
}
int EKF2::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Attitude and position estimator using an Extended Kalman Filter. It is used for Multirotors and Fixed-Wing.
The documentation can be found on the [ECL/EKF Overview & Tuning](https://docs.px4.io/master/en/advanced_config/tuning_the_ecl_ekf.html) page.
ekf2 can be started in replay mode (`-r`): in this mode, it does not access the system time, but only uses the
timestamps from the sensor topics.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ekf2", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAM_FLAG('r', "Enable replay mode", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
#if !defined(CONSTRAINED_FLASH)
PRINT_MODULE_USAGE_COMMAND_DESCR("select_instance", "Request switch to new estimator instance");
PRINT_MODULE_USAGE_ARG("<instance>", "Specify desired estimator instance", false);
#endif // !CONSTRAINED_FLASH
return 0;
}
extern "C" __EXPORT int ekf2_main(int argc, char *argv[])
{
if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
return EKF2::print_usage();
}
if (strcmp(argv[1], "start") == 0) {
int ret = 0;
EKF2::lock_module();
ret = EKF2::task_spawn(argc - 1, argv + 1);
if (ret < 0) {
PX4_ERR("start failed (%i)", ret);
}
EKF2::unlock_module();
return ret;
#if !defined(CONSTRAINED_FLASH)
} else if (strcmp(argv[1], "select_instance") == 0) {
if (EKF2::trylock_module()) {
if (_ekf2_selector.load()) {
if (argc > 2) {
int instance = atoi(argv[2]);
_ekf2_selector.load()->RequestInstance(instance);
} else {
EKF2::unlock_module();
return EKF2::print_usage("instance required");
}
} else {
PX4_ERR("multi-EKF not active, unable to select instance");
}
EKF2::unlock_module();
} else {
PX4_WARN("module locked, try again later");
}
return 0;
#endif // !CONSTRAINED_FLASH
} else if (strcmp(argv[1], "status") == 0) {
if (EKF2::trylock_module()) {
#if !defined(CONSTRAINED_FLASH)
if (_ekf2_selector.load()) {
_ekf2_selector.load()->PrintStatus();
}
#endif // !CONSTRAINED_FLASH
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
if (_objects[i].load()) {
PX4_INFO_RAW("\n");
_objects[i].load()->print_status();
}
}
EKF2::unlock_module();
} else {
PX4_WARN("module locked, try again later");
}
return 0;
} else if (strcmp(argv[1], "stop") == 0) {
EKF2::lock_module();
if (argc > 2) {
int instance = atoi(argv[2]);
if (instance >= 0 && instance < EKF2_MAX_INSTANCES) {
PX4_INFO("stopping instance %d", instance);
EKF2 *inst = _objects[instance].load();
if (inst) {
inst->request_stop();
px4_usleep(20000); // 20 ms
delete inst;
_objects[instance].store(nullptr);
}
} else {
PX4_ERR("invalid instance %d", instance);
}
} else {
// otherwise stop everything
bool was_running = false;
#if !defined(CONSTRAINED_FLASH)
if (_ekf2_selector.load()) {
PX4_INFO("stopping ekf2 selector");
_ekf2_selector.load()->Stop();
delete _ekf2_selector.load();
_ekf2_selector.store(nullptr);
was_running = true;
}
#endif // !CONSTRAINED_FLASH
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
EKF2 *inst = _objects[i].load();
if (inst) {
PX4_INFO("stopping ekf2 instance %d", i);
was_running = true;
inst->request_stop();
px4_usleep(20000); // 20 ms
delete inst;
_objects[i].store(nullptr);
}
}
if (!was_running) {
PX4_WARN("not running");
}
}
EKF2::unlock_module();
return PX4_OK;
}
EKF2::lock_module(); // Lock here, as the method could access _object.
int ret = EKF2::custom_command(argc - 1, argv + 1);
EKF2::unlock_module();
return ret;
}
EKF2Selector.cpp
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "EKF2Selector.hpp"
using namespace time_literals;
using matrix::Quatf;
using matrix::Vector2f;
using math::constrain;
using math::radians;
EKF2Selector::EKF2Selector() :
ModuleParams(nullptr), //初始化基类ModuleParams，用于参数管理
ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers)//初始化基类ScheduledWorkItem，将任务名称设为"ekf2_selector"，工作队列设为nav_and_controllers
{ /*发布一系列消息*/
_estimator_selector_status_pub.advertise();
_sensor_selection_pub.advertise();
_vehicle_attitude_pub.advertise();
_vehicle_global_position_pub.advertise();
_vehicle_local_position_pub.advertise();
_vehicle_odometry_pub.advertise();
_wind_pub.advertise();
}
EKF2Selector::~EKF2Selector()
{
Stop();
}
void EKF2Selector::Stop() //循环遍历所有EKF实例，取消注册姿态和状态的回调函数
{
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
_instance[i].estimator_attitude_sub.unregisterCallback();
_instance[i].estimator_status_sub.unregisterCallback();
}
ScheduleClear(); //清除所有计划
}
/*
旧实例变更原因：
检查旧实例的状态，如果存在过滤器故障、超时、陀螺仪故障、加速度计故障或不健康状态，设置相应的原因。
新实例变更原因：
如果新实例是用户选择的，设置相应的原因。
打印变更信息：
如果旧实例或新实例有变更原因，打印变更信息，使用PX4_WARN记录
*/
void EKF2Selector::PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance)
{
const char *old_reason = nullptr;
if (_instance[old_instance].filter_fault) {
old_reason = " (filter fault)";
} else if (_instance[old_instance].timeout) {
old_reason = " (timeout)";
} else if (_gyro_fault_detected) {
old_reason = " (gyro fault)";
} else if (_accel_fault_detected) {
old_reason = " (accel fault)";
} else if (!_instance[_selected_instance].healthy.get_state() && (_instance[_selected_instance].healthy_count > 0)) {
// skipped if previous instance was never healthy in the first place (eg initialization) 如果前一个实例从一开始就不健康（例如初始化），则跳过
old_reason = " (unhealthy)";
}
const char *new_reason = nullptr;
if (_request_instance.load() == new_instance) {
new_reason = " (user selected)";
}
if (old_reason || new_reason) {
if (old_reason == nullptr) {
old_reason = "";
}
if (new_reason == nullptr) {
new_reason = "";
}
PX4_WARN("primary EKF changed %" PRIu8 "%s -> %" PRIu8 "%s", old_instance, old_reason, new_instance, new_reason);
}
}
bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
{
if ((ekf_instance != _selected_instance) && (ekf_instance < _available_instances)) {//确保ekf_instance不是当前选择的实例，并且在可用实例范围内
// update sensor_selection immediately 立即更新sensor_selection
sensor_selection_s sensor_selection{};
sensor_selection.accel_device_id = _instance[ekf_instance].accel_device_id;
sensor_selection.gyro_device_id = _instance[ekf_instance].gyro_device_id;
sensor_selection.timestamp = hrt_absolute_time();
_sensor_selection_pub.publish(sensor_selection);
/*如果当前有选定的实例（_selected_instance不是无效值），则取消旧实例的回调注册，并打印实例变更信息*/
if (_selected_instance != INVALID_INSTANCE) { //设置选定实例的加速度计和陀螺仪设备ID以及时间戳，并发布消息
// switch callback registration 切换回调注册
//注册新实例的回调函数
_instance[_selected_instance].estimator_attitude_sub.unregisterCallback();
_instance[_selected_instance].estimator_status_sub.unregisterCallback();
PrintInstanceChange(_selected_instance, ekf_instance);
}
_instance[ekf_instance].estimator_attitude_sub.registerCallback();
_instance[ekf_instance].estimator_status_sub.registerCallback();
_selected_instance = ekf_instance;
/*更新选定实例的索引，实例变更计数和时间戳*/
_instance_changed_count++;
_last_instance_change = sensor_selection.timestamp;
_instance[ekf_instance].time_last_selected = _last_instance_change;
// reset all relative test ratios重置所有相对测试比率重置为0
for (uint8_t i = 0; i < _available_instances; i++) {
_instance[i].relative_test_ratio = 0;
}
return true;
}
return false;
}
/*
检查IMU不一致性：
检测陀螺仪和加速度计的故障，通过累积误差和阈值判断是否有故障。
更新错误评分：
遍历所有实例，更新每个实例的状态信息，计算组合测试比率并判断实例是否健康。
如果检测到陀螺仪或加速度计故障，立即标记对应的EKF实例为不健康。
更新相对测试比率：
如果主实例已更新，则更新其他实例的相对测试比率，根据误差差值调整相对测试比率。
返回更新状态：
返回主实例或其他实例是否有更新的状态
*/
bool EKF2Selector::UpdateErrorScores()
{
// first check imu inconsistencies 首先检查IMU不一致性
_gyro_fault_detected = false;
uint32_t faulty_gyro_id = 0;
_accel_fault_detected = false;
uint32_t faulty_accel_id = 0;
if (_sensors_status_imu.updated()) {
sensors_status_imu_s sensors_status_imu;
if (_sensors_status_imu.copy(&sensors_status_imu)) {
const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f);
_last_update_us = sensors_status_imu.timestamp;
{
const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get());
const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get());
uint8_t n_gyros = 0;
uint8_t n_gyro_exceedances = 0;
float largest_accumulated_gyro_error = 0.0f;
uint8_t largest_gyro_error_index = 0;
for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) {
// check for gyros with excessive difference to mean using accumulated error 检查与均值差异过大的陀螺仪，使用累积误差
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++;
}
if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) {
largest_accumulated_gyro_error = _accumulated_gyro_error[i];
largest_gyro_error_index = i;
}
} else {
// no sensor 没有传感器
_accumulated_gyro_error[i] = NAN;
}
}
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 如果有3个或更多传感器，累积误差最大的传感器是故障传感器
_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;
}
}
}
{
const float accel_threshold = _param_ekf2_sel_imu_accel.get();
const float velocity_threshold = _param_ekf2_sel_imu_velocity.get();
uint8_t n_accels = 0;
uint8_t n_accel_exceedances = 0;
float largest_accumulated_accel_error = 0.0f;
uint8_t largest_accel_error_index = 0;
for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) {
// check for accelerometers with excessive difference to mean using accumulated error 检查与均值差异过大的加速度计，使用累积误差
if (sensors_status_imu.accel_device_ids[i] != 0) {
n_accels++;
_accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s;
_accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f);
if (_accumulated_accel_error[i] > velocity_threshold) {
n_accel_exceedances++;
}
if (_accumulated_accel_error[i] > largest_accumulated_accel_error) {
largest_accumulated_accel_error = _accumulated_accel_error[i];
largest_accel_error_index = i;
}
} else {
// no sensor 没有传感器
_accumulated_accel_error[i] = NAN;
}
}
if (n_accel_exceedances > 0) {
if (n_accels >= 3) {
// If there are 3 or more sensors, the one with the largest accumulated error is faulty 如果有3个或更多传感器，累积误差最大的传感器是故障传感器
_accel_fault_detected = true;
faulty_accel_id = sensors_status_imu.accel_device_ids[largest_accel_error_index];
} else if (n_accels == 2) {
// A fault is present, but the faulty sensor identity cannot be determined 存在故障，但无法确定故障传感器的身份
_accel_fault_detected = true;
}
}
}
}
}
bool updated = false;
bool primary_updated = false;
// default estimator timeout 默认估计器超时时间
const hrt_abstime status_timeout = 50_ms;
// calculate individual error scores 计算各个实例的错误评分
for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
const bool prev_healthy = _instance[i].healthy.get_state();
estimator_status_s status;
if (_instance[i].estimator_status_sub.update(&status)) {
_instance[i].timestamp_last = status.timestamp;
_instance[i].accel_device_id = status.accel_device_id;
_instance[i].gyro_device_id = status.gyro_device_id;
_instance[i].baro_device_id = status.baro_device_id;
_instance[i].mag_device_id = status.mag_device_id;
if ((i + 1) > _available_instances) {
_available_instances = i + 1;
updated = true;
}
if (i == _selected_instance) {
primary_updated = true;
}
// test ratios are invalid when 0, >= 1 is a failure 测试比率为0或大于等于1时无效，1或更大表示失败
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;
}
if (!PX4_ISFINITE(status.hgt_test_ratio) || (status.hgt_test_ratio <= 0.f)) {
status.hgt_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);
_instance[i].filter_fault = (status.filter_fault_flags != 0);
_instance[i].timeout = false;
if (!_instance[i].warning) {
_instance[i].time_last_no_warning = status.timestamp;
}
if (!PX4_ISFINITE(_instance[i].relative_test_ratio)) {
_instance[i].relative_test_ratio = 0;
}
} else if (!_instance[i].timeout && (hrt_elapsed_time(&_instance[i].timestamp_last) > status_timeout)) {
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
_instance[i].timeout = true;
}
// if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay 如果EKF使用的陀螺仪故障，立即声明EKF不健康
if (_gyro_fault_detected && (faulty_gyro_id != 0) && (_instance[i].gyro_device_id == faulty_gyro_id)) {
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
}
// if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay 如果EKF使用的加速度计故障，立即声明EKF不健康
if (_accel_fault_detected && (faulty_accel_id != 0) && (_instance[i].accel_device_id == faulty_accel_id)) {
_instance[i].healthy.set_state_and_update(false, hrt_absolute_time());
}
if (prev_healthy != _instance[i].healthy.get_state()) {
updated = true;
_selector_status_publish = true;
if (!prev_healthy) {
_instance[i].healthy_count++;
}
}
}
// update relative test ratios if primary has updated 如果主实例已更新，则更新相对测试比率
if (primary_updated) {
for (uint8_t i = 0; i < _available_instances; i++) {
if (i != _selected_instance) {
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 只有当误差比主实例小至少EKF2_SEL_ERR_RED时才减少误差，以防止不必要的选择变更
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);
if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) {
// increase status publication rate if there's movement towards a potential instance change 如果有向潜在实例变更的移动，增加状态发布频率
_selector_status_publish = true;
}
}
}
}
}
return (primary_updated || updated);
}
/*
更新选定的estimator_attitude：
从选定的EKF实例中订阅estimator_attitude数据。
检查实例是否有变化，并记录变化情况。
处理四元数重置计数：
如果有重置事件（计数增加或实例变化），计算四元数增量重置。
保存总的重置计数和增量重置。
检查时间戳是否有效：
确保时间戳是单调递增的，如果数据是陈旧的则不发布。
发布数据：
更新四元数重置计数和增量重置。
设置当前时间戳并发布vehicle_attitude数据
*/
void EKF2Selector::PublishVehicleAttitude()
{
// selected estimator_attitude -> vehicle_attitude 从选定的estimator_attitude订阅数据并发布为vehicle_attitude
vehicle_attitude_s attitude;
if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) {
bool instance_change = false;
if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) {
_attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance();
instance_change = true;
}
if (_attitude_last.timestamp != 0) {
if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) {
// propogate deltas from estimator data while maintaining the overall reset counts 传播来自估计器数据的增量，同时保持总体重置计数
++_quat_reset_counter;
_delta_q_reset = Quatf{attitude.delta_q_reset};
} else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) {
// on reset compute deltas from last published data 在重置时从上次发布的数据计算增量
++_quat_reset_counter;
_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
}
} else {
_quat_reset_counter = attitude.quat_reset_counter;
_delta_q_reset = Quatf{attitude.delta_q_reset};
}
bool publish = true;
// 确保通过重置的时间戳样本单调递增，如果估计器的数据是陈旧的，则不发布
// ensure monotonically increasing timestamp_sample through reset, don't publish
// estimator's attitude for system (vehicle_attitude) if it's stale
if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample)
|| (hrt_elapsed_time(&attitude.timestamp) > 10_ms)) { //从选定的estimator_attitude订阅数据并发布为vehicle_attitude
publish = false;
}
// save last primary estimator_attitude as published with original resets保存上次的主要估计器姿态作为发布的原始重置数据
_attitude_last = attitude;
if (publish) {
// republish with total reset count and current timestamp使用总重置计数和当前时间戳重新发布
attitude.quat_reset_counter = _quat_reset_counter;
_delta_q_reset.copyTo(attitude.delta_q_reset);
attitude.timestamp = hrt_absolute_time();
_vehicle_attitude_pub.publish(attitude);
}
}
}
/*
更新选定的estimator_local_position：
从选定的EKF实例中订阅estimator_local_position数据。
检查实例是否有变化，并记录变化情况。
处理位置和速度重置计数：
如果有重置事件（计数增加或实例变化），计算位置和速度的增量重置。
保存总的重置计数和增量重置。
检查时间戳是否有效：
确保时间戳是单调递增的，如果数据是陈旧的则不发布。
发布数据：
更新位置和速度重置计数和增量重置。
设置当前时间戳并发布vehicle_local_position数据。
*/
void EKF2Selector::PublishVehicleLocalPosition()
{
// selected estimator_local_position -> vehicle_local_position 从选定的estimator_local_position订阅数据并发布为vehicle_local_position
vehicle_local_position_s local_position;
if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) {
bool instance_change = false;
if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) {
_local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance();
instance_change = true;
}
if (_local_position_last.timestamp != 0) {
// XY reset XY重置
if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) {
++_xy_reset_counter;
_delta_xy_reset = Vector2f{local_position.delta_xy};
} else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) {
++_xy_reset_counter;
_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
}
// Z reset
if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) {
++_z_reset_counter;
_delta_z_reset = local_position.delta_z;
} else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) {
++_z_reset_counter;
_delta_z_reset = local_position.z - _local_position_last.z;
}
// VXY reset
if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) {
++_vxy_reset_counter;
_delta_vxy_reset = Vector2f{local_position.delta_vxy};
} else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) {
++_vxy_reset_counter;
_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
}
// VZ reset
if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) {
++_vz_reset_counter;
_delta_vz_reset = local_position.delta_vz;
} else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) {
++_vz_reset_counter;
_delta_vz_reset = local_position.vz - _local_position_last.vz;
}
// heading reset 航向重置
if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) {
++_heading_reset_counter;
_delta_heading_reset = local_position.delta_heading;
} else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) {
++_heading_reset_counter;
_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
}
} else {
_xy_reset_counter = local_position.xy_reset_counter;
_z_reset_counter = local_position.z_reset_counter;
_vxy_reset_counter = local_position.vxy_reset_counter;
_vz_reset_counter = local_position.vz_reset_counter;
_heading_reset_counter = local_position.heading_reset_counter;
_delta_xy_reset = Vector2f{local_position.delta_xy};
_delta_z_reset = local_position.delta_z;
_delta_vxy_reset = Vector2f{local_position.delta_vxy};
_delta_vz_reset = local_position.delta_vz;
_delta_heading_reset = local_position.delta_heading;
}
bool publish = true;
// 确保通过重置的时间戳样本单调递增，如果估计器的数据是陈旧的，则不发布
// ensure monotonically increasing timestamp_sample through reset, don't publish
// estimator's local position for system (vehicle_local_position) if it's stale
if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample)
|| (hrt_elapsed_time(&local_position.timestamp) > 20_ms)) {
publish = false;
}
// 保存上次的主要估计器本地位置作为发布的原始重置数据
// save last primary estimator_local_position as published with original resets
_local_position_last = local_position;
if (publish) {
// 使用总重置计数和当前时间戳重新发布
// republish with total reset count and current timestamp
local_position.xy_reset_counter = _xy_reset_counter;
local_position.z_reset_counter = _z_reset_counter;
local_position.vxy_reset_counter = _vxy_reset_counter;
local_position.vz_reset_counter = _vz_reset_counter;
local_position.heading_reset_counter = _heading_reset_counter;
_delta_xy_reset.copyTo(local_position.delta_xy);
local_position.delta_z = _delta_z_reset;
_delta_vxy_reset.copyTo(local_position.delta_vxy);
local_position.delta_vz = _delta_vz_reset;
local_position.delta_heading = _delta_heading_reset;
local_position.timestamp = hrt_absolute_time();
_vehicle_local_position_pub.publish(local_position);
}
}
}
/*
更新选定的 estimator_odometry：
从选定的 EKF 实例中订阅 estimator_odometry 数据。
检查实例是否有变化，并记录变化情况。
处理里程计重置计数：
如果有重置事件（计数增加或实例变化），增加重置计数。
保存总的重置计数。
检查时间戳是否有效：
确保时间戳是单调递增的，如果数据是陈旧的则不发布。
发布数据：
更新里程计重置计数。
设置当前时间戳并发布 vehicle_odometry 数据
*/
void EKF2Selector::PublishVehicleOdometry()
{
// selected estimator_odometry -> vehicle_odometry 从选定的estimator_odometry订阅数据并发布为vehicle_odometry
vehicle_odometry_s odometry;
if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) {
bool instance_change = false;
if (_instance[_selected_instance].estimator_odometry_sub.get_instance() != _odometry_instance_prev) {
_odometry_instance_prev = _instance[_selected_instance].estimator_odometry_sub.get_instance();
instance_change = true;
}
if (_odometry_last.timestamp != 0) {
// reset 如果有重置事件（计数增加或实例变化），增加重置计数
if (instance_change || (odometry.reset_counter != _odometry_last.reset_counter)) {
++_odometry_reset_counter;
}
} else {
_odometry_reset_counter = odometry.reset_counter;
}
bool publish = true;
// 确保时间戳样本单调递增，如果估计器的数据是陈旧的，则不发布
// ensure monotonically increasing timestamp_sample through reset, don't publish
// estimator's odometry for system (vehicle_odometry) if it's stale
if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample)
|| (hrt_elapsed_time(&odometry.timestamp) > 20_ms)) {
publish = false;
}
// 保存上次的主要估计器里程计作为发布的原始重置数据
// save last primary estimator_odometry as published with original resets
_odometry_last = odometry;
if (publish) {
// republish with total reset count and current timestamp 使用总重置计数和当前时间戳重新发布
odometry.reset_counter = _odometry_reset_counter;
odometry.timestamp = hrt_absolute_time();
_vehicle_odometry_pub.publish(odometry);
}
}
}
/*
更新选定的 estimator_global_position：
从选定的 EKF 实例中订阅 estimator_global_position 数据。
检查实例是否有变化，并记录变化情况。
处理全局位置重置计数：
如果有重置事件（计数增加或实例变化），计算纬度/经度和高度的增量重置。
保存总的重置计数和增量重置。
检查时间戳是否有效：
确保时间戳是单调递增的，如果数据是陈旧的则不发布。
发布数据：
更新全局位置重置计数和增量重置。
设置当前时间戳并发布 vehicle_global_position 数据。
*/
void EKF2Selector::PublishVehicleGlobalPosition()
{
// selected estimator_global_position -> vehicle_global_position
// 从选定的estimator_global_position订阅数据并发布为vehicle_global_position
vehicle_global_position_s global_position;
if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) {
bool instance_change = false;
if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) {
_global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance();
instance_change = true;
}
if (_global_position_last.timestamp != 0) {
// lat/lon reset 处理纬度/经度重置计数
if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) {
++_lat_lon_reset_counter;
// TODO: delta latitude/longitude
_delta_lat_reset = global_position.lat - _global_position_last.lat;
_delta_lon_reset = global_position.lon - _global_position_last.lon;
} else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) {
++_lat_lon_reset_counter;
_delta_lat_reset = global_position.lat - _global_position_last.lat;
_delta_lon_reset = global_position.lon - _global_position_last.lon;
}
// alt reset 处理高度重置计数
if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) {
++_alt_reset_counter;
_delta_alt_reset = global_position.delta_alt;
} else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) {
++_alt_reset_counter;
_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
}
} else {
_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
_alt_reset_counter = global_position.alt_reset_counter;
_delta_alt_reset = global_position.delta_alt;
}
bool publish = true;
// 确保时间戳样本单调递增，如果估计器的数据是陈旧的则不发布
// ensure monotonically increasing timestamp_sample through reset, don't publish
// estimator's global position for system (vehicle_global_position) if it's stale
if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample)
|| (hrt_elapsed_time(&global_position.timestamp) > 20_ms)) {
publish = false;
}
// 保存上次的主要估计器全局位置作为发布的原始重置数据
// save last primary estimator_global_position as published with original resets
_global_position_last = global_position;
if (publish) {
// 使用总重置计数和当前时间戳重新发布
// republish with total reset count and current timestamp
global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
global_position.alt_reset_counter = _alt_reset_counter;
global_position.delta_alt = _delta_alt_reset;
global_position.timestamp = hrt_absolute_time();
_vehicle_global_position_pub.publish(global_position);
}
}
}
/*
更新选定的 estimator_wind：
从选定的 EKF 实例中订阅 estimator_wind 数据。
检查时间戳是否有效：
确保时间戳是单调递增的，如果数据是陈旧的则不发布。
保存上次的风估计数据：
保存当前的 estimator_wind 数据作为上次的风估计数据。
发布数据：
更新时间戳并发布 wind 数据
*/
void EKF2Selector::PublishWindEstimate()
{
// selected estimator_wind -> wind // 选定的 estimator_wind 数据 -> wind
wind_s wind;
if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) {
bool publish = true;
// 确保时间戳样本单调递增，如果数据是陈旧的则不发布
// ensure monotonically increasing timestamp_sample through reset, don't publish
// estimator's wind for system (wind) if it's stale
if ((wind.timestamp_sample <= _wind_last.timestamp_sample)
|| (hrt_elapsed_time(&wind.timestamp) > 100_ms)) {
publish = false;
}
// save last primary wind 保存上一次的主要风估计
_wind_last = wind;
// publish estimator's wind for system unless it's stale 发布估计器的风数据，除非它是陈旧的
if (publish) {
// republish with current timestamp 使用当前时间戳重新发布
wind.timestamp = hrt_absolute_time();
_wind_pub.publish(wind);
}
}
}
/*
检查参数更新：
如果参数有更新，加载新的参数。
更新错误评分：
更新所有 EKF 实例的组合测试比例。
选择有效实例：
如果没有有效实例，选择第一个具有有效 IMU 的实例。
检查并选择最佳实例：
遍历所有可用实例，找到错误率最低且健康的实例。
如果当前实例不健康，切换到最佳健康实例。
如果发现错误显著较低的实例且时间条件满足，切换到该实例。
如果用户请求切换实例，尝试切换到用户选择的实例。
发布选择器状态：
如果有任何状态更改或定时器到期，发布选择器状态。
发布选定估计器数据：
调用各个发布函数，发布车辆姿态、本地位置、全局位置、里程计和风估计数据。
重新安排定时器：
重新安排备份超时，以确保定期运行
*/
void EKF2Selector::Run()
{
// check for parameter updates 检查参数更新
if (_parameter_update_sub.updated()) {
// clear update 清除更新
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage 从存储中更新参数
updateParams();
}
// update combined test ratio for all estimators 更新所有估计器的组合测试比例
const bool updated = UpdateErrorScores();
// if no valid instance then force select first instance with valid IMU 如果没有有效实例，则强制选择第一个具有有效 IMU 的实例
if (_selected_instance == INVALID_INSTANCE) {
for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) {
if ((_instance[i].accel_device_id != 0)
&& (_instance[i].gyro_device_id != 0)) {
if (SelectInstance(i)) {
break;
}
}
}
// if still invalid return early and check again on next scheduled run 如果仍然无效，则提前返回并在下次计划运行时再次检查
if (_selected_instance == INVALID_INSTANCE) {
ScheduleDelayed(100_ms);
return;
}
}
if (updated) {
const uint8_t available_instances_prev = _available_instances;
const uint8_t selected_instance_prev = _selected_instance;
const uint32_t instance_changed_count_prev = _instance_changed_count;
const hrt_abstime last_instance_change_prev = _last_instance_change;
bool lower_error_available = false;
float alternative_error = 0.f; // looking for instances that have error lower than the current primary 查找错误率低于当前主实例的实例
float best_test_ratio = FLT_MAX;
uint8_t best_ekf = _selected_instance;
uint8_t best_ekf_alternate = INVALID_INSTANCE;
uint8_t best_ekf_different_imu = INVALID_INSTANCE;
// loop through all available instances to find if an alternative is available 遍历所有可用实例，查找是否有可用替代实例
for (int i = 0; i < _available_instances; i++) {
// Use an alternative instance if - 使用一个替代实例 如果-
// (healthy and has updated recently) （健康并且最近更新）
// AND 并且
// has relative error less than selected instance and has not been the selected instance for at least 10 seconds 相对错误小于选定实例并且至少10秒内未选定
// OR 或
// selected instance has stopped updating 选定实例已停止更新
if (_instance[i].healthy.get_state() && (i != _selected_instance)) {
const float test_ratio = _instance[i].combined_test_ratio;
const float relative_error = _instance[i].relative_test_ratio;
if (relative_error < alternative_error) {
best_ekf_alternate = i;
alternative_error = relative_error;
// relative error less than selected instance and has not been the selected instance for at least 10 seconds 相对错误小于选定实例并且至少10秒内未选定
if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) {
lower_error_available = true;
}
}
if ((test_ratio > 0) && (test_ratio < best_test_ratio)) {
best_ekf = i;
best_test_ratio = test_ratio;
// also check next best available ekf using a different IMU 还检查使用不同 IMU 的下一个最佳可用 EKF
if (_instance[i].accel_device_id != _instance[_selected_instance].accel_device_id) {
best_ekf_different_imu = i;
}
}
}
}
if (!_instance[_selected_instance].healthy.get_state()) {
// prefer the best healthy instance using a different IMU 更喜欢使用不同 IMU 的最佳健康实例
if (!SelectInstance(best_ekf_different_imu)) {
// otherwise switch to the healthy instance with best overall test ratio 否则切换到健康实例，其整体测试比例最佳
SelectInstance(best_ekf);
}
} else if (lower_error_available
&& ((hrt_elapsed_time(&_last_instance_change) > 10_s)
|| (_instance[_selected_instance].warning
&& (hrt_elapsed_time(&_instance[_selected_instance].time_last_no_warning) > 1_s)))) {
//如果此实例的相对错误显著低于当前主实例，我们认为它是更好的实例，并希望切换到它，即使当前主实例是健康的
// if this instance has a significantly lower relative error to the active primary, we consider it as a
// better instance and would like to switch to it even if the current primary is healthy
SelectInstance(best_ekf_alternate);
} else if (_request_instance.load() != INVALID_INSTANCE) {
const uint8_t new_instance = _request_instance.load();
// attempt to switch to user manually selected instance 尝试切换到用户手动选择的实例
if (!SelectInstance(new_instance)) {
PX4_ERR("unable to switch to user selected instance %d", new_instance);
}
// reset
_request_instance.store(INVALID_INSTANCE);
}
// publish selector status at ~1 Hz or immediately on any change 以~1 Hz 或任何更改立即发布选择器状态
if (_selector_status_publish || (hrt_elapsed_time(&_last_status_publish) > 1_s)
|| (available_instances_prev != _available_instances)
|| (selected_instance_prev != _selected_instance)
|| (instance_changed_count_prev != _instance_changed_count)
|| (last_instance_change_prev != _last_instance_change)
|| _accel_fault_detected || _gyro_fault_detected) {
PublishEstimatorSelectorStatus();
_selector_status_publish = false;
}
}
// republish selected estimator data for system 重新发布系统的选定估计器数据
PublishVehicleAttitude();
PublishVehicleLocalPosition();
PublishVehicleGlobalPosition();
PublishVehicleOdometry();
PublishWindEstimate();
// re-schedule as backup timeout 重新安排备份超时
ScheduleDelayed(FILTER_UPDATE_PERIOD);
}
/*
初始化状态消息：
创建一个 estimator_selector_status_s 类型的 selector_status 对象。
填充状态消息：
设置主实例、可用实例数、实例更改计数、最后一次实例更改时间。
设置当前选定实例的传感器设备 ID（加速度计、气压计、陀螺仪、磁力计）。
设置陀螺仪和加速度计故障检测标志。
填充每个实例的测试比例和健康状态：
遍历所有 EKF 实例，填充它们的组合测试比例、相对测试比例和健康状态。
填充 IMU 累计错误：
填充累计的陀螺仪和加速度计错误。
发布状态消息：
设置时间戳并发布 selector_status。
更新 _last_status_publish 为当前时间戳。
*/
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;
}
/*
解释
打印可用实例数：
使用 PX4_INFO 打印可用实例的数量。
检查并打印选定实例：
如果没有选定实例（_selected_instance == INVALID_INSTANCE），使用 PX4_WARN 打印警告信息。
遍历所有可用实例并打印其状态：
使用 PX4_INFO 打印每个实例的设备 ID（加速度计、陀螺仪、磁力计），健康状态，组合测试比例，相对测试比例。
如果实例是当前选定的实例，在输出信息后附加一个星号 *。
总结
PublishEstimatorSelectorStatus 函数用于收集并发布当前选定的 EKF 实例及其相关状态信息，
包括传感器故障信息和测试比例。PrintStatus 函数则是用于打印当前所有可用实例的状态信息，以便进行调试和监控。
*/
void EKF2Selector::PrintStatus()
{
PX4_INFO("available instances: %" PRIu8, _available_instances);
if (_selected_instance == INVALID_INSTANCE) {
PX4_WARN("selected instance: None");
}
for (int i = 0; i < _available_instances; i++) {
const EstimatorInstance &inst = _instance[i];
PX4_INFO("%" PRIu8 ": ACC: %" PRIu32 ", GYRO: %" PRIu32 ", MAG: %" PRIu32 ", %s, test ratio: %.7f (%.5f) %s",
inst.instance, inst.accel_device_id, inst.gyro_device_id, inst.mag_device_id,
inst.healthy.get_state() ? "healthy" : "unhealthy",
(double)inst.combined_test_ratio, (double)inst.relative_test_ratio,
(_selected_instance == i) ? "*" : "");
}
}
ekf2_params.c
/****************************************************************************
*
* Copyright (c) 2015-2016 Estimation and Control Library (ECL). All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name ECL nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ekf2_params.c
* Parameter definition for ekf2.
*
* @author Roman Bast <bapstroman@gmail.com>
*
*/
/**
* EKF预测周期
*
* EKF预测周期，以微秒为单位。理想情况下，这应该是IMU时间增量的整数倍。
* 实际的滤波更新将是IMU更新的整数倍。
*
* @group EKF2
* @min 1000
* @max 20000
* @unit us
*/
PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000);
/**
* 磁力计测量相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0);
/**
* 气压计测量相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0);
/**
* GPS测量相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110);
/**
* 光流测量相对于IMU测量的延迟
*
* 假设测量是在积分周期的末尾标记时间戳的
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 20);
/**
* 测距仪测量相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5);
/**
* 空速测量相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 100);
/**
* 视觉位置估计相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175);
/**
* 辅助速度估计（例如从着陆目标）相对于IMU测量的延迟
*
* @group EKF2
* @min 0
* @max 300
* @unit ms
* @reboot_required true
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5);
/**
* 控制GPS检查的整数位掩码。
*
* 设置位为1以启用检查。以下位位置启用的检查
* 0 : 最小所需卫星数由EKF2_REQ_NSATS设置
* 1 : 最大允许PDOP由EKF2_REQ_PDOP设置
* 2 : 最大允许水平位置误差由EKF2_REQ_EPH设置
* 3 : 最大允许垂直位置误差由EKF2_REQ_EPV设置
* 4 : 最大允许速度误差由EKF2_REQ_SACC设置
* 5 : 最大允许水平位置速率由EKF2_REQ_HDRIFT设置。此检查仅在车辆在地面且静止时运行。
* 6 : 最大允许垂直位置速率由EKF2_REQ_VDRIFT设置。此检查仅在车辆在地面且静止时运行。
* 7 : 最大允许水平速度由EKF2_REQ_HDRIFT设置。此检查仅在车辆在地面且静止时运行。
* 8 : 最大允许垂直速度差异由EKF2_REQ_VDRIFT设置
*
* @group EKF2
* @min 0
* @max 511
* @bit 0 最小卫星数（EKF2_REQ_NSATS）
* @bit 1 最大PDOP（EKF2_REQ_PDOP）
* @bit 2 最大水平位置误差（EKF2_REQ_EPH）
* @bit 3 最大垂直位置误差（EKF2_REQ_EPV）
* @bit 4 最大速度误差（EKF2_REQ_SACC）
* @bit 5 最大水平位置速率（EKF2_REQ_HDRIFT）
* @bit 6 最大垂直位置速率（EKF2_REQ_VDRIFT）
* @bit 7 最大水平速度（EKF2_REQ_HDRIFT）
* @bit 8 最大垂直速度差异（EKF2_REQ_VDRIFT）
*/
PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245);
/**
* 使用GPS所需的EPH。
*
* @group EKF2
* @min 2
* @max 100
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 3.0f);
/**
* 使用GPS所需的EPV。
*
* @group EKF2
* @min 2
* @max 100
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 5.0f);
/**
* 使用GPS所需的速度精度。
*
* @group EKF2
* @min 0.5
* @max 5.0
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 0.5f);
/**
* 使用GPS所需的卫星数。
*
* @group EKF2
* @min 4
* @max 12
*/
PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6);
/**
* 使用GPS的最大PDOP。
*
* @group EKF2
* @min 1.5
* @max 5.0
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_PDOP, 2.5f);
/**
* 使用GPS的最大水平漂移速度。
*
* @group EKF2
* @min 0.1
* @max 1.0
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.1f);
/**
* 使用GPS的最大垂直漂移速度。
*
* @group EKF2
* @min 0.1
* @max 1.5
* @decimal 2
* @unit m/s
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.2f);
/**
* 用于协方差预测的陀螺仪噪声。
*
* @group EKF2
* @min 0.0001
* @max 0.1
* @unit rad/s
* @decimal 4
*/
PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f);
/**
* 用于协方差预测的加速度计噪声。
*
* @group EKF2
* @min 0.01
* @max 1.0
* @unit m/s^2
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f);
/**
* 用于IMU陀螺仪偏置预测的过程噪声。
*
* @group EKF2
* @min 0.0
* @max 0.01
* @unit rad/s^2
* @decimal 6
*/
PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f);
/**
* 用于IMU加速度计偏置预测的过程噪声。
*
* @group EKF2
* @min 0.0
* @max 0.01
* @unit m/s^3
* @decimal 6
*/
PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-3f);
/**
* 用于机体磁场预测的过程噪声。
*
* @group EKF2
* @min 0.0
* @max 0.1
* @unit gauss/s
* @decimal 6
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f);
/**
* 用于地磁场预测的过程噪声。
*
* @group EKF2
* @min 0.0
* @max 0.1
* @unit gauss/s
* @decimal 6
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f);
/**
* 用于风速预测的过程噪声。
*
* @group EKF2
* @min 0.0
* @max 1.0
* @unit m/s^2
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f);
/**
* GPS水平速度测量噪声。
*
* @group EKF2
* @min 0.01
* @max 5.0
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.3f);
/**
* GPS位置测量噪声。
*
* @group EKF2
* @min 0.01
* @max 10.0
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.5f);
/**
* 非辅助位置保持的测量噪声。
*
* @group EKF2
* @min 0.5
* @max 50.0
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f);
/**
* 气压高度测量噪声。
*
* @group EKF2
* @min 0.01
* @max 15.0
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.5f);
/**
* 磁航向融合的测量噪声。
*
* @group EKF2
* @min 0.01
* @max 1.0
* @unit rad
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.3f);
/**
* 磁力计三轴融合的测量噪声。
*
* @group EKF2
* @min 0.001
* @max 1.0
* @unit gauss
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f);
/**
* 空速融合的测量噪声。
*
* @group EKF2
* @min 0.5
* @max 5.0
* @unit m/s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f);
/**
* 合成侧滑融合的门限大小
*
* 设置创新一致性测试中使用的标准偏差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f);
/**
* 合成侧滑融合的噪声。
*
* @group EKF2
* @min 0.1
* @max 1.0
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f);
/**
* 磁航向融合的门限大小
*
* 设置创新一致性测试中使用的标准偏差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 2.6f);
/**
* 磁力计XYZ组件融合的门限大小
*
* 设置创新一致性测试中使用的标准偏差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f);
/**
* 控制磁偏角处理的整数位掩码。
*
* 设置以下位置的位以启用功能。
* 0 : 设置为true以在GPS位置可用时使用geo_lookup库中的磁偏角，设置为false以始终使用EKF2_MAG_DECL值。
* 1 : 设置为true以在车辆解除武装时将EKF2_MAG_DECL参数保存为EKF返回的值。
* 2 : 设置为true以在使用三轴磁力计融合时始终将磁偏角作为观测值使用。
*
* @group EKF2
* @min 0
* @max 7
* @bit 0 使用geo_lookup磁偏角
* @bit 1 解除武装时保存EKF2_MAG_DECL
* @bit 2 将磁偏角作为观测值使用
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7);
/**
* 磁力计融合类型
*
* 整数控制使用的磁力计融合类型 - 磁航向或三分量矢量。将磁力计数据作为三分量矢量融合可学习固定在车辆机身上的硬铁误差，但需要稳定的地磁场。
* 如果设置为“自动”，则在地面时使用磁航向融合，飞行时使用三轴磁场融合，如果没有足够的运动使偏航或磁场状态可观察，则回退到磁航向融合。
* 如果设置为“磁航向”，则始终使用磁航向融合。
* 如果设置为“三轴”，则始终使用三轴磁场融合。
* 如果设置为“VTOL自定义”，行为与“自动”相同，但如果融合空速，则磁力计融合只允许修改磁场状态。这可以用于具有较大磁场干扰的VTOL平台，以防止在前向飞行操作期间学习到不正确的偏差状态，这可能会在转换为悬停飞行后不利地影响估算精度。
* 如果设置为“MC自定义”，行为与“自动”相同，但如果没有使用地球框架位置或速度观测，则不会使用磁力计。这允许车辆在没有GPS的情况下在不能使用磁场提供航向参考的环境中运行。在飞行前，如果运动测试表明车辆是静止的，则假设偏航角是恒定的。这允许车辆在飞行前放置在地面上学习偏航陀螺仪偏差。
* 如果设置为“无”，则在任何情况下都不使用磁力计。如果没有外部的偏航源，则可以使用起飞后结合GPS速度测量的水平运动来对齐偏航角，所需时间取决于运动量和GPS数据质量。如果通过EKF2_AID_MASK参数选择了其他外部偏航源，也可以使用这些源。
* @group EKF2
* @value 0 自动
* @value 1 磁航向
* @value 2 三轴
* @value 3 VTOL自定义
* @value 4 MC自定义
* @value 5 无
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 5); //LELIN20240723 原始数据为0
/**
* 自动选择磁力计融合方法时使用的水平加速度阈值。
*
* 当磁力计融合方法设置为自动（EKF2_MAG_TYPE = 0）时使用此参数。如果过滤后的水平加速度大于此参数值，则EKF将使用三轴磁力计融合。
*
* @group EKF2
* @min 0.0
* @max 5.0
* @unit m/s^2
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f);
/**
* 自动选择磁力计融合方法时使用的偏航速率阈值。
*
* 当磁力计融合方法设置为自动（EKF2_MAG_TYPE = 0）时使用此参数。如果过滤后的偏航速率大于此参数值，则EKF将使用三轴磁力计融合。
*
* @group EKF2
* @min 0.0
* @max 1.0
* @unit rad/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_YAWLIM, 0.25f);
/**
* 用于气压和GPS高度融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f);
/**
* 用于高度融合的气压死区范围
*
* 设置应用于负气压创新的死区值。启用死区时，EKF2_GND_EFF_DZ > 0。
*
* @group EKF2
* @min 0.0
* @max 10.0
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 4.0f);
/**
* 地效区的地面高度
*
* 设置到地面高度的最大距离，在此范围内预期会出现负气压创新。
*
* @group EKF2
* @min 0.0
* @max 5.0
* @unit m
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GND_MAX_HGT, 0.5f);
/**
* 用于GPS水平位置融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f);
/**
* 用于GPS速度融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f);
/**
* 用于TAS融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
/**
* 控制数据融合和辅助方法的整数位掩码。
*
* 设置以下位置的位以启用：
* 0：设置为true以使用可用的GPS数据
* 1：设置为true以使用可用的光流数据
* 2：设置为true以抑制IMU delta速度偏差估计
* 3：设置为true以启用视觉位置融合
* 4：设置为true以启用视觉偏航融合。如果第7位为true，则不能使用。
* 5：设置为true以启用多旋翼阻力特定力融合
* 6：如果EV观测值在非NED参考框架中并且需要在使用前旋转，则设置为true
* 7：设置为true以启用GPS偏航融合。如果第4位为true，则不能使用。
*
* @group EKF2
* @min 0
* @max 511
* @bit 0 使用GPS
* @bit 1 使用光流
* @bit 2 抑制IMU偏差估计
* @bit 3 视觉位置融合
* @bit 4 视觉偏航融合
* @bit 5 多旋翼阻力融合
* @bit 6 旋转外部视觉
* @bit 7 GPS偏航融合
* @bit 8 视觉速度融合
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_AID_MASK, 0); //LELIN
/**
* 决定EKF使用的主要高度数据来源。
*
* 只有在平坦表面操作时才应使用测距传感器选项，因为局部NED原点会随着地面高度的变化而上下移动。
*
* @group EKF2
* @value 0 气压
* @value 1 GPS
* @value 2 测距传感器
* @value 3 视觉
* @reboot_required true
*/
PARAM_DEFINE_INT32(EKF2_HGT_MODE, 0); //LELIN
/**
* 控制地形估计器融合来源的整数位掩码
*
* 设置以下位置的位以启用：
* 0：设置为true以使用测距仪数据
* 1：设置为true以使用光流数据
*
* @group EKF2
* @min 0
* @max 3
* @bit 0 使用测距仪
* @bit 1 使用光流
*/
PARAM_DEFINE_INT32(EKF2_TERR_MASK, 3);
/**
* 在EKF报告水平导航解决方案无效之前，最后一次融合约束速度漂移的测量的最大时间。
*
* @group EKF2
* @min 500000
* @max 10000000
* @unit us
*/
PARAM_DEFINE_INT32(EKF2_NOAID_TOUT, 5000000);
/**
* 测距仪融合的测量噪声
*
* @group EKF2
* @min 0.01
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f);
/**
* 测距仪范围依赖噪声缩放因子
*
* 指定测距仪噪声随测量范围增加的程度。
*
* @group EKF2
* @min 0.0
* @max 0.2
* @unit m/m
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_SFE, 0.05f);
/**
* 测距仪融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
/**
* 车辆在地面时预期的测距仪读数
*
* 如果车辆在地面上，运动测试表明车辆没有移动，并且测距仪返回无效或无数据，则地形估计器将使用EKF2_MIN_RNG的假定范围值，以便在起飞开始时有地形高度估计，这种情况适用于测距仪可能在地面时处于其最小测量距离内的情况。
*
* @group EKF2
* @min 0.01
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
/**
* 是否从参数或视觉消息中设置外部视觉观测噪声
*
* 如果设置为true，则观测噪声直接从参数中设置；如果设置为false，则测量噪声取自视觉消息，参数用作下限。
*
* @boolean
* @group EKF2
*/
PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0);
/**
* 视觉位置观测的测量噪声，用于设置消息中包含的不确定性的下限或替代
*
* @group EKF2
* @min 0.01
* @unit m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f);
/**
* 视觉速度观测的测量噪声，用于设置消息中包含的不确定性的下限或替代
*
* @group EKF2
* @min 0.01
* @unit m/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f);
/**
* 视觉角度观测的测量噪声，用于设置消息中包含的不确定性的下限或替代
*
* @group EKF2
* @min 0.05
* @unit rad
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f);
/**
* 当光流传感器报告的质量指标达到最大值时的测量噪声
*
* @group EKF2
* @min 0.05
* @unit rad/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f);
/**
* 光流传感器的测量噪声
*
* （当其报告的质量指标达到 EKF2_OF_QMIN 设置的最小值时）。
* 必须满足以下条件：EKF2_OF_N_MAX >= EKF2_OF_N_MIN
*
* @group EKF2
* @min 0.05
* @unit rad/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f);
/**
* 仅当光流传感器报告的质量指标 >= EKF2_OF_QMIN 时，才会使用光流数据。
*
* @group EKF2
* @min 0
* @max 255
*/
PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1);
/**
* 光流融合的门限大小
*
* 设置创新一致性测试中使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f);
/**
* 地形高度过程噪声 - 解释车辆高度估计的不稳定性
*
* @group EKF2
* @min 0.5
* @unit m/s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f);
/**
* 地形梯度的大小
*
* @group EKF2
* @min 0.0
* @unit m/m
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f);
/**
* IMU在机身框架中的X位置（相对于重心的前轴原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_IMU_POS_X, 0.0f);
/**
* IMU在机身框架中的Y位置（相对于重心的右轴原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f);
/**
* IMU在机身框架中的Z位置（相对于重心的下轴原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f);
/**
* GPS天线在机身框架中的X位置（相对于重心的前轴原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f);
/**
* GPS天线在机身框架中的Y位置（相对于重心的右轴原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f);
/**
* Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity)
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f);
/**
* 测距仪原点在机体坐标系中的X位置（前轴，以车辆重心为原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_POS_X, 0.0f);
/**
* 测距仪原点在机体坐标系中的Y位置（右轴，以车辆重心为原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Y, 0.0f);
/**
* 测距仪原点在机体坐标系中的Z位置（下轴，以车辆重心为原点）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Z, 0.0f);
/**
* 光流传感器焦点在机体坐标系中的X轴位置（前轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_OF_POS_X, 0.0f);
/**
* 光流传感器焦点在机体坐标系中的Y轴位置（右轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
/**
* 光流传感器焦点在机体坐标系中的Z轴位置（下轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
/**
* VI传感器焦点在机体坐标系中的X轴位置（前轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f);
/**
* VI传感器焦点在机体坐标系中的Y轴位置（右轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
/**
* VI传感器焦点在机体坐标系中的Z轴位置（下轴，原点相对于飞机重心）
*
* @group EKF2
* @unit m
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
/**
* 空速融合阈值。
*
* 设为零将禁用空速融合。任何其他正值将确定仍将融合的最小空速。设为飞机失速速度的约90%。
* 要在GPS丢失后继续导航，必须同时激活空速融合和侧滑融合。
* 使用EKF2_FUSE_BETA激活侧滑融合。
*
* @group EKF2
* @min 0.0
* @unit m/s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f);
/**
* 确定是否应融合合成侧滑测量值的布尔值。
*
* 值为1表示融合处于活动状态
* 要在GPS丢失后继续导航，必须同时激活侧滑融合和空速融合。
* 使用EKF2_ARSP_THR激活空速融合。
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_FUSE_BETA, 0);
/**
* 速度输出预测和平滑滤波器的时间常数
*
* @group EKF2
* @max 1.0
* @unit s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.25f);
/**
* 位置输出预测和平滑滤波器的时间常数。控制输出跟踪EKF状态的紧密程度。
*
* @group EKF2
* @min 0.1
* @max 1.0
* @unit s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f);
/**
* 1-σ IMU陀螺仪开机偏置
*
* @group EKF2
* @min 0.0
* @max 0.2
* @unit rad/s
* @reboot_required true
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f);
/**
* 1-σ IMU加速度计开机偏置
*
* @group EKF2
* @min 0.0
* @max 0.5
* @unit m/s^2
* @reboot_required true
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f);
/**
* 重力向量对齐后倾斜角不确定性（1-σ）
*
* @group EKF2
* @min 0.0
* @max 0.5
* @unit rad
* @reboot_required true
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f);
/**
* 超声波高度计俯仰偏移。
*
* @group EKF2
* @min -0.75
* @max 0.75
* @unit rad
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
/**
* 超声波高度计辅助。
*
* 如果启用此参数，则估计器将使用超声波高度计测量值来估计其高度，即使超声波高度计不是主要的高度来源。
* 它只会在满足超声波测量融合条件时才这样做。这允许在低速和低高度操作（例如起飞和着陆）期间使用超声波高度计，
* 其中旋翼气流会干扰气压计，可能会破坏EKF状态估计。该功能旨在用于垂直起降操作，且水平飞行不会发生在EKF2_RNG_A_HMAX以上。
* 如果飞行器运动导致主要高度传感器和超声波高度计之间频繁切换，本地位置原点可能会积累偏移。此外，
* 超声波高度计测量值的可靠性较低，可能会出现意外错误。因此，如果需要准确控制相对于地面的高度，
* 建议使用MPC_ALT_MODE参数，除非气压计误差严重到影响着陆和起飞。
*
* @group EKF2
* @value 0 超声波辅助禁用
* @value 1 超声波辅助启用
*/
PARAM_DEFINE_INT32(EKF2_RNG_AID, 1);
/**
* 超声波辅助模式下允许的最大水平速度。
*
* 如果飞行器的水平速度超过此值，则估计器将不会融合超声波测量值来估计其高度。仅在启用超声波辅助模式（EKF2_RNG_AID = 启用）时适用。
*
* @group EKF2
* @min 0.1
* @max 2
* @unit m/s
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f);
/**
* 超声波辅助模式下允许的最大绝对高度（相对于地面的高度）。
*
* 如果飞行器的绝对高度超过此值，则估计器将不会融合超声波测量值来估计其高度。仅在启用超声波辅助模式（EKF2_RNG_AID = 启用）时适用。
*
* @group EKF2
* @min 1.0
* @max 10.0
* @unit m
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f);
/**
* 用于超声波辅助融合的创新一致性检查的门限大小。
*
* 较低的值意味着为了使用超声波高度计进行高度估计，高度变化必须更加稳定。
*
* @group EKF2
* @unit SD
* @min 0.1
* @max 5.0
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f);
/**
* 报告的超声波高度计信号质量需为非零以被声明为有效的最短持续时间（秒）。
*
* @group EKF2
* @unit s
* @min 0.1
* @max 5
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f);
/**
* 用于超声波高度计运动一致性检查的门限大小。
*
* 要使用，距离传感器测量值在垂直轴上的时间导数需与无人机的估计垂直速度在统计上保持一致。
*
* 降低此值可使滤波器在处理超声波高度计错误数据（卡住、反射等）时更为稳健。
*
* 注意：在调整此门限之前，请先调整超声波高度计噪声参数（EKF2_RNG_NOISE 和 EKF2_RNG_SFE）。
*
* @group EKF2
* @unit SD
* @min 0.1
* @max 5.0
*/
PARAM_DEFINE_FLOAT(EKF2_RNG_K_GATE, 1.0f);
/**
* 视觉速度估计融合的门限大小。
*
* 设置创新一致性测试使用的标准差数量。
*
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f);
/**
* 视觉位置融合的门限大小
*
* 设置创新一致性测试使用的标准差数量。
* @group EKF2
* @min 1.0
* @unit SD
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f);
/**
* 用于多旋翼特定阻力模型的特定阻力观测噪声方差
*
* 增加此值会使多旋翼风估计调整速度变慢。
*
* @group EKF2
* @min 0.5
* @max 10.0
* @unit (m/s^2)^2
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f);
/**
* 用于多旋翼风估计的X轴弹道系数
*
* 该参数控制多旋翼飞行时沿前/后轴产生的钝体阻力预测，
* 使在启用EKF2_AID_MASK参数时能估计风漂移。EKF2_BCOEF_X参数应初始设置为质量/投影正面面积的比值，
* 并与EKF2_MCOEF一起调整以最小化X轴阻力特定力创新序列的方差。
* 由此效应产生的阻力随速度平方变化。将此参数设置为零可关闭此轴的钝体阻力模型。
* 由此效应产生的阻力由EKF2_MCOEF参数单独指定。
*
* @group EKF2
* @min 0.0
* @max 200.0
* @unit kg/m^2
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 100.0f);
/**
* 用于多旋翼风估计的Y轴弹道系数
*
* 该参数控制多旋翼飞行时沿右/左轴产生的钝体阻力预测，
* 使在启用EKF2_AID_MASK参数时能估计风漂移。EKF2_BCOEF_Y参数应初始设置为质量/投影侧面面积的比值，
* 并与EKF2_MCOEF一起调整以最小化Y轴阻力特定力创新序列的方差。
* 由此效应产生的阻力随速度平方变化。将此参数设置为零可关闭此轴的钝体阻力模型。
* 由此效应产生的阻力由EKF2_MCOEF参数单独指定。
*
* @group EKF2
* @min 0.0
* @max 200.0
* @unit kg/m^2
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 100.0f);
/**
* 用于多旋翼风估计的螺旋桨动量阻力系数
*
* 该参数控制多旋翼飞行时由螺旋桨产生的阻力预测，
* 使在启用EKF2_AID_MASK参数时能估计风漂移。由此效应产生的阻力随速度线性变化，
* 这是因为一部分垂直于螺旋桨旋转轴的空气速度在通过旋翼盘时损失了。这改变了流动的动量，产生了阻力反作用力。
* 在比较相同直径的无导流罩螺旋桨时，该效应大致与侧面观测到的螺旋桨叶片面积成正比，并随着螺旋桨选择而变化。
* 动量阻力对有导流罩的旋翼来说显著更高。例如，在海平面条件下以10 m/s飞行时，如果水平保持零滚转/俯仰状态，
* 会产生1.5 m/s²的旋翼诱导阻力减速，则EKF2_MCOEF应设置为0.15 = (1.5/10.0)。将EKF2_MCOEF设置为正值以启用此阻力效应的风估计。
* 要考虑由速度平方变化的阻力产生的阻力，请参阅EKF2_BCOEF_X和EKF2_BCOEF_Y参数的文档。
* EKF2_MCOEF参数应与EKF2_BCOEF_X和EKF2_BCOEF_Y一起调整，以最小化X轴和Y轴阻力特定力创新序列的方差。
*
* @group EKF2
* @min 0
* @max 1.0
* @unit 1/s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_MCOEF, 0.15f);
/**
* 用于校正气压高度误差效应的单轴最大空速上限
*
* @group EKF2
* @min 5.0
* @max 50.0
* @unit m/s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_ASPD_MAX, 20.0f);
/**
* 正X轴的静压位置误差系数
*
* 这是由沿X轴正方向的风相对速度产生的静压误差与动态压的比值。
* 如果在前向飞行过程中气压高度估计上升，则这将是一个负数。
*
* @group EKF2
* @min -0.5
* @max 0.5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_PCOEF_XP, 0.0f);
/**
* 负X轴的静压位置误差系数
*
* 这是由沿X轴负方向的风相对速度产生的静压误差与动态压的比值。
* 如果在后向飞行过程中气压高度估计上升，则这将是一个负数。
*
* @group EKF2
* @min -0.5
* @max 0.5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_PCOEF_XN, 0.0f);
/**
* 正Y轴的静压位置误差系数
*
* 这是由沿Y轴正方向的风相对速度产生的静压误差与动态压的比值。
* 如果在右向飞行过程中气压高度估计上升，则这将是一个负数。
*
* @group EKF2
* @min -0.5
* @max 0.5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_PCOEF_YP, 0.0f);
/**
* 负Y轴的静压位置误差系数
*
* 这是由沿Y轴负方向的风相对速度产生的静压误差与动态压的比值。
* 如果在左向飞行过程中气压高度估计上升，则这将是一个负数。
*
* @group EKF2
* @min -0.5
* @max 0.5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_PCOEF_YN, 0.0f);
/**
* Z轴的静压位置误差系数
*
* 这是由沿Z轴风相对速度产生的静压误差与动态压的比值。
*
* @group EKF2
* @min -0.5
* @max 0.5
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_PCOEF_Z, 0.0f);
/**
* 加速度计偏置学习极限
*
* EKF增量速度偏置状态将限制在相当于此值的+-范围内。
*
* @group EKF2
* @min 0.0
* @max 0.8
* @unit m/s^2
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_ABL_LIM, 0.4f);
/**
* 允许IMU偏置学习的IMU加速度最大值
*
* 如果IMU加速度矢量的大小超过此值，EKF增量速度状态估计将被抑制。
* 这减少了高机动加速度和IMU非线性及尺度因子误差对增量速度偏置估计的不利影响。
*
* @group EKF2
* @min 20.0
* @max 200.0
* @unit m/s^2
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_ABL_ACCLIM, 25.0f);
/**
* 允许IMU偏置学习的IMU陀螺仪角速率最大值
*
* 如果IMU角速率矢量的大小超过此值，EKF增量速度状态估计将被抑制。
* 这减少了快速旋转速率及相关误差对增量速度偏置估计的不利影响。
*
* @group EKF2
* @min 2.0
* @max 20.0
* @unit rad/s
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
/**
* 用于抑制增量速度偏置学习的加速度和角速率大小检查的时间常数
*
* 用于检查是否应抑制学习的角速率和加速度的矢量大小会通过一个指数衰减的峰值保持滤波器。
* 该参数控制衰减的时间常数。
*
* @group EKF2
* @min 0.1
* @max 1.0
* @unit s
* @decimal 2
*/
PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f);
/**
* 启动时的GPS健康时间要求
*
* 标记为GPS健康状态所需的最低连续无GPS故障时间段。
* 它可以减少以加快初始化速度，但建议对飞行器保持此不变。
*
* @group EKF2
* @min 0.1
* @decimal 1
* @unit s
* @reboot_required true
*/
PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f);
/**
* 磁场强度测试选择
*
* 设置时，EKF会检查磁场强度以决定磁力计数据是否有效。
* 如果接收到GPS数据，则磁场与世界磁场模型（WMM）进行比较，否则使用平均值。
* 该检查有助于拒绝偶尔的硬铁干扰。
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1);
/**
* 启用合成磁力计Z分量测量
*
* 适用于受强磁干扰的车辆。
* 对于磁航向融合，磁力计Z测量值将被一个使用无人机位置的三维磁场矢量知识计算的合成值取代。
* 因此，如果无人机的位置是已知的，这个参数才会有影响。
* 对于三维磁融合，磁力计Z测量值将被忽略，而不是融合合成值。
*
* @group EKF2
* @boolean
*/
PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0);
/**
* EKF-GSF AHRS计算中使用的真实空速默认值
*
* 如果没有空速测量，EKF-GSF AHRS计算在补偿转弯时的离心加速度时将假设此真实空速值。
* 在固定翼飞行模式下禁用离心加速度补偿，请将此参数设置为零。
*
* @group EKF2
* @min 0.0
* @unit m/s
* @max 100.0
* @decimal 1
*/
PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f);
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
