九天揽月带你玩转EKF纸老虎（3）

摘要

本节主要分析ardupilot多旋翼部分的ekf代码，欢迎批评指正！！！

来自雷迅官网的注释：大家可以浏览下
直升机和固定翼可以使用扩展卡尔曼滤波器（EKF）算法，基于陀螺仪，加速度计，罗盘，GPS，空速和大气压力测量来估计飞机位置，速度和角度方向。

EKF与简单的互补滤波器算法（即“惯性导航”）相比优点在于：通过融合所有可用的测量数据，能够更好的过滤掉有明显误差的测量数据，这使飞机不会受单个传感器故障的影响。 EKF还可以使用诸如光流和激光测距仪等可选传感器进行测量，以辅助导航。

目前稳定版本的ArduPilot使用EKF2作为主要态度和位置估算来源，DCM在后台静止运行。如果飞行控制器有两个（或多个）IMU可用，则两个EKF“内核”（即EKF的两个实例）将并行运行，每个使用不同的IMU。飞控会选取一个传感器数据一致性最好，性能最佳的EKF核心作为单个EKF输出使用，其它核心不起输出作用。

大多数用户不需要修改任何EKF参数，但下面的信息提供了一些最常见的参数变化信息。更详细的信息可以在开发者EKF wiki页面找到。

第一：ekf初始化

1.EKF数据初始化

（1）void Copter::setup()

void Copter::setup() 
{
    cliSerial = hal.console;

    //加载初始化参数表到 in var_info[]s
    AP_Param::setup_sketch_defaults();

    // 初始化无人机的结构布局
    StorageManager::set_layout_copter();

    //注册传感器
    init_ardupilot();//重点看这个函数

    // 初始化 main loop 任务
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));

    // 初始化 main loop 任务
    perf_info_reset();
    //获取fast_loop时间
    fast_loopTimer = AP_HAL::micros();
}

（2）void Copter::init_ardupilot()

void Copter::init_ardupilot()
{

    reset_control_switch();
    init_aux_switches();   //初始化外部开关，具有一定的功能

    //IMU初始化
    printf("startup_INS_ground\r\n");
    startup_INS_ground();//重点看这个函数
    printf("startup_INS_ground_end\r\n");
    // set landed flags
    set_land_complete(true);
}

（3）void Copter::startup_INS_ground()

void Copter::startup_INS_ground()
{
    // initialise ahrs (may push imu calibration into the mpu6000 if using that device).
    //初始化AHRS（如果使用该设备，可以将IMU校准推到MPU6000）。
    ahrs.init(); //初始化板层的方向
    ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);

    //预热偏差和校准陀螺仪偏移------------Warm up and calibrate gyro offsets

    ins.init(scheduler.get_loop_rate_hz());

    //复位航姿参考系统，保护陀螺仪便宜----reset ahrs including gyro bias
    ahrs.reset(); //重点函数
}

　　　　　　　　　　　　　　下面对上面初始化函数进行各个函数分析

１＞ahrs.init(); //初始化板层的方向

  virtual void init() 
    {
        set_orientation();// 设置板层方向
    };

    void set_orientation()//允许运行时更改定向，这使得初始配置更容易。
    {
        _ins.set_board_orientation((enum Rotation)_board_orientation.get());//设置板层方向
        if (_compass != nullptr) //采用罗盘作为方向
        {
            _compass->set_board_orientation((enum Rotation)_board_orientation.get());
        }
    }

2＞ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER)

  void set_vehicle_class(AHRS_VehicleClass vclass) 
    {
        _vehicle_class = vclass;
    }

3＞ins.init(scheduler.get_loop_rate_hz())

//这个函数不再往下分析，之前在读取MPU6000已经分析过，关键就是获取ekf需要的加速度，陀螺仪数据，需要理解前后端的概念！！！

void AP_InertialSensor::init(uint16_t sample_rate)
{
    // remember the sample rate
    _sample_rate = sample_rate;
    _loop_delta_t = 1.0f / sample_rate;  //采样频率转换成多少ms

    if (_gyro_count == 0 && _accel_count == 0)
    {
        _start_backends(); //后端读取数据
    }

    // initialise accel scale if need be. This is needed as we can't
    // give non-zero default values for vectors in AP_Param
    for (uint8_t i=0; iif (_accel_scale[i].get().is_zero())
        {
            _accel_scale[i].set(Vector3f(1,1,1));
        }
    }

    // calibrate gyros unless gyro calibration has been disabled
    if (gyro_calibration_timing() != GYRO_CAL_NEVER)
    {
        init_gyro();
    }

    _sample_period_usec = 1000*1000UL / _sample_rate;

    // establish the baseline time between samples
    _delta_time = 0;
    _next_sample_usec = 0;
    _last_sample_usec = 0;
    _have_sample = false;
}

4＞ins.init(scheduler.get_loop_rate_hz())

void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
    AP_AHRS_DCM::reset(recover_eulers); //复位DCM
    _dcm_attitude(roll, pitch, yaw); //初始化姿态
    if (_ekf2_started) 
    {
        _ekf2_started = EKF2.InitialiseFilter();//初始化ekf2
    }
    if (_ekf3_started) 
    {
        _ekf3_started = EKF3.InitialiseFilter();//初始化ekf3
    }
}

1>>ekf2初始化

bool NavEKF2::InitialiseFilter(void)
{
    if (_enable == 0) //如果ekf2没有使能，这里就禁止运行ekf下面的流程
    {
        return false;
    }
    const AP_InertialSensor &ins = _ahrs->get_ins();  //获取传感器数据

    imuSampleTime_us = AP_HAL::micros64(); //获取IMU采样时间

    //记住预期的帧时间---- remember expected frame time
    _frameTimeUsec = 1e6 / ins.get_sample_rate();

    //每个预测的IMU帧的预期数目--- expected number of IMU frames per prediction
    _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));

    //看看我们是否会做日志记录--- see if we will be doing logging
    DataFlash_Class *dataflash = DataFlash_Class::instance();
    if (dataflash != nullptr)
    {
        logging.enabled = dataflash->log_replay();
    }

    if (core == nullptr)
    {

        //不要为1 IMU运行多个过滤器---- don't run multiple filters for 1 IMU
        uint8_t mask = (1U<1;
        _imuMask.set(_imuMask.get() & mask);

        //开始计数IMU------count IMUs from mask
        num_cores = 0;
        for (uint8_t i=0; i<7; i++)
        {
            if (_imuMask & (1U<if (hal.util->available_memory() < sizeof(NavEKF2_core)*num_cores + 4096)
        {
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory");
            _enable.set(0);
            return false;
        }

        core = new NavEKF2_core[num_cores]; //构造了两个EKF对象
        if (core == nullptr)
        {
            _enable.set(0);
            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF2: allocation failed");
            return false;
        }

        //为内核设置IMU索引------ set the IMU index for the cores
        num_cores = 0;
        for (uint8_t i=0; i<7; i++)
        {
            if (_imuMask & (1U<if(!core[num_cores].setup_core(this, i, num_cores))  //初始化传感器
                {
                    return false;
                }
                num_cores++;
            }
        }

        //将初值设为最低索引---- Set the primary initially to be the lowest index
        primary = 0;
    }

    // initialise the cores. We return success only if all cores
    // initialise successfully
    //只有当所有核心成功初始化时，我们才返回成功。
    bool ret = true;
    for (uint8_t i=0; i//初始化传感器引导
    }

    //使用捕获重置事件的结构------------ zero the structs used capture reset events
    memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
    memset(&pos_reset_data, 0, sizeof(pos_reset_data));
    memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));

    check_log_write();
    return ret;
}

重点函数：core[i].InitialiseFilterBootstrap()


bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
    //如果我们是一架飞机，没有GPS锁定，那就不要初始化。-------------If we are a plane and don't have GPS lock then don't initialise
    if (assume_zero_sideslip() && _ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D)
    {
        statesInitialised = false;
        return false;
    }

    //将重新使用的变量设置为零--- set re-used variables to zero
    InitialiseVariables();

    // Initialise IMU data
    dtIMUavg = _ahrs->get_ins().get_loop_delta_t(); //计算获取IMU数据需要的时间
    readIMUData();
    storedIMU.reset_history(imuDataNew);
    imuDataDelayed = imuDataNew;

    //加速度矢量在XYZ机体坐标轴---------acceleration vector in XYZ body axes measured by the IMU (m/s^2)
    Vector3f initAccVec;

    // 我们应该在几个周期内进行平均读数。===TODO we should average accel readings over several cycles
    initAccVec = _ahrs->get_ins().get_accel(imu_index);

    //获取地磁数据-----------------------read the magnetometer data
    readMagData();

    //归一化加速度数据--------------------normalise the acceleration vector
    float pitch=0, roll=0;
    if (initAccVec.length() > 0.001f)
    {
        initAccVec.normalize();

        //使用加速度数据，计算初始俯仰角------------------calculate initial pitch angle
        pitch = asinf(initAccVec.x);

        //使用加速度数据，计算初始化横滚角-----------------calculate initial roll angle
        roll = atan2f(-initAccVec.y , -initAccVec.z);
    }

    //欧拉角到四元素------------------------------------calculate initial roll and pitch orientation
    stateStruct.quat.from_euler(roll, pitch, 0.0f);

    //初始化运动状态------------------------------------initialise dynamic states
    stateStruct.velocity.zero();//速度
    stateStruct.position.zero();//位置
    stateStruct.angErr.zero();  //角度误差

    //初始化静态模型状态变量------------------------------initialise static process model states
    stateStruct.gyro_bias.zero();
    stateStruct.gyro_scale.x = 1.0f;
    stateStruct.gyro_scale.y = 1.0f;
    stateStruct.gyro_scale.z = 1.0f;
    stateStruct.accel_zbias = 0.0f;
    stateStruct.wind_vel.zero();
    stateStruct.earth_magfield.zero();
    stateStruct.body_magfield.zero();

    //读取GPS数据，并设置位置和速度状态信息---------------- read the GPS and set the position and velocity states
    readGpsData();
    ResetVelocity(); //复位速度
    ResetPosition(); //复位位置信息

    //读取气压计数据------------------------------------read the barometer and set the height state
    readBaroData();
    ResetHeight();  //复位高度数据

    // 在NED坐标系下定义地球旋转矢量----------------------define Earth rotation vector in the NED navigation frame
    calcEarthRateNED(earthRateNED, _ahrs->get_home().lat);

    //初始化协方差矩阵----------------------------------initialise the covariance matrix
    CovarianceInit();

    //复位输出状态变量----------------------------------reset output states
    StoreOutputReset();

    //现在状态已经初始化为真-----------------------------set to true now that states have be initialised
    statesInitialised = true;

    return true;
}

2.EKF参数变量

NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
    _ahrs(ahrs),
    _baro(baro),
    _rng(rng),
    gpsNEVelVarAccScale(0.05f),     // Scale factor applied to horizontal velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
    gpsDVelVarAccScale(0.07f),      // Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration - used when GPS doesn't report speed error
    gpsPosVarAccScale(0.05f),       // Scale factor applied to horizontal position measurement variance due to manoeuvre acceleration
    magDelay_ms(60),                // Magnetometer measurement delay (msec)
    tasDelay_ms(240),               // Airspeed measurement delay (msec)
    tiltDriftTimeMax_ms(15000),      // Maximum number of ms allowed without any form of tilt aiding (GPS, flow, TAS, etc)
    posRetryTimeUseVel_ms(10000),   // Position aiding retry time with velocity measurements (msec)
    posRetryTimeNoVel_ms(7000),     // Position aiding retry time without velocity measurements (msec)
    hgtRetryTimeMode0_ms(10000),    // Height retry time with vertical velocity measurement (msec)
    hgtRetryTimeMode12_ms(5000),    // Height retry time without vertical velocity measurement (msec)
    tasRetryTime_ms(5000),          // True airspeed timeout and retry interval (msec)
    magFailTimeLimit_ms(10000),     // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec)
    magVarRateScale(0.005f),        // scale factor applied to magnetometer variance due to angular rate and measurement timing jitter. Assume timing jitter of 10msec
    gyroBiasNoiseScaler(2.0f),      // scale factor applied to imu gyro bias learning before the vehicle is armed
    hgtAvg_ms(100),                 // average number of msec between height measurements
    betaAvg_ms(100),                // average number of msec between synthetic sideslip measurements
    covTimeStepMax(0.1f),           // maximum time (sec) between covariance prediction updates
    covDelAngMax(0.05f),            // maximum delta angle between covariance prediction updates
    DCM33FlowMin(0.71f),            // If Tbn(3,3) is less than this number, optical flow measurements will not be fused as tilt is too high.
    fScaleFactorPnoise(1e-10f),     // Process noise added to focal length scale factor state variance at each time step
    flowTimeDeltaAvg_ms(100),       // average interval between optical flow measurements (msec)
    flowIntervalMax_ms(100),        // maximum allowable time between flow fusion events
    gndEffectTimeout_ms(1000),      // time in msec that baro ground effect compensation will timeout after initiation
    turbulenceEffectTimeout_ms(300), // add by weihli
    gndEffectBaroScaler(4.0f),      // scaler applied to the barometer observation variance when operating in ground effect
    gndGradientSigma(50),           // RMS terrain gradient percentage assumed by the terrain height estimation
    fusionTimeStep_ms(10),          // The minimum number of msec between covariance prediction and fusion operations
    runCoreSelection(false)         // true when the default primary core has stabilised after startup and core selection can run
{
    AP_Param::setup_object_defaults(this, var_info);//构造函数中初始化参数，所以在创建对象将会调用
}

const AP_Param::GroupInfo NavEKF2::var_info[] = 
{
  //中间参数不写出来了，太多，针对每个进行分析
}

（1）EKF2使能参数

 // @Param: ENABLE（使能）
 // @DisplayName: Enable EKF2--使能EKF2
 // @Description: 这使是使能EKF2的参数。启用EKF2仅用在数学运算，但并不意味着它将被用于飞行控制。要想将其用于飞行控制，需要设置：AHRS_EKF_TYPE＝2。重新启动或重启将需要在更改EK2_ENABLE启用的值后才能执行。
值：0：禁用，1：启用
AP_GROUPINFO_FLAGS("ENABLE", 0, NavEKF2, _enable, 1, AP_PARAM_FLAG_ENABLE),

（2）GPS类型选择参数

    // @Param: GPS_TYPE
    // @DisplayName: GPS控制模式-----GPS mode control
    // @Description: 这控制着GPS测量的使用：设置0：使用3D速度和2D位置；设置1：使用2D速度和2D位置；设置2：使用2D位置；设置3：禁止使用GPS——当使用光学流量传感器飞行时，在GPS质量很差并且受到大的多径误差影响的环境中，这很有用。
   // @Values: 0:GPS 3D Vel and 2D Pos, 1:GPS 2D vel and 2D pos, 2:GPS 2D pos, 3:No GPS

   AP_GROUPINFO("GPS_TYPE", 1, NavEKF2, _fusionModeGPS, 0),

（3）GPS水平速度噪声（m/s）参数

 // @Param: VELNE_M_NSE
    // @DisplayName: GPS水平测速噪声（m/s）参数
    // @Description: 这设定了GPS接收机报告的用于:设置水平速度观测噪声的速度精度的下限。如果所使用的接收机模型不提供速度精度估计，则将使用参数值。增加它可以减少GPS水平速度测量的加权。
    // @Range: 0.05 5.0
    // @Increment: 0.05
    // @User: Advanced
    // @Units: m/s
    AP_GROUPINFO("VELNE_M_NSE", 2, NavEKF2, _gpsHorizVelNoise, VELNE_M_NSE_DEFAULT),

（4）GPS垂直速度噪声（m/s）参数

   // @Param: VELD_M_NSE
    // @DisplayName: GPS垂直速度噪声（m/s）参数
    // @Description: 这就对GPS接收机报告的用于：设置垂直速度观测噪声的速度精度，设置了下限。如果所使用的接收机模型不提供速度精度估计，则将使用参数值。增加它可以减少GPS垂直速度测量的加权。
    // @Range: 0.05 5.0
    // @Increment: 0.05
    // @User: Advanced
    // @Units: m/s
    AP_GROUPINFO("VELD_M_NSE", 3, NavEKF2, _gpsVertVelNoise, VELD_M_NSE_DEFAULT),

（5）GPS水平速度噪声（m/s）参数

   // @Param: VEL_I_GATE
    // @DisplayName: GPS velocity innovation gate size
    // @Description: This sets the percentage number of standard deviations applied to the GPS velocity measurement innovation consistency check. Decreasing it makes it more likely that good measurements willbe rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("VEL_I_GATE", 4, NavEKF2, _gpsVelInnovGate, VEL_I_GATE_DEFAULT),

（6）GPS水平速度噪声（m/s）参数

   // @Param: POSNE_M_NSE
    // @DisplayName: GPS horizontal position measurement noise (m)
    // @Description: This sets the GPS horizontal position observation noise. Increasing it reduces the weighting of GPS horizontal position measurements.
    // @Range: 0.1 10.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("POSNE_M_NSE", 5, NavEKF2, _gpsHorizPosNoise, POSNE_M_NSE_DEFAULT),

（7）GPS水平速度噪声（m/s）参数


    // @Param: POS_I_GATE
    // @DisplayName: GPS position measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the GPS position measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("POS_I_GATE", 6, NavEKF2, _gpsPosInnovGate, POS_I_GATE_DEFAULT),

（8）GPS水平速度噪声（m/s）参数

    // @Param: GLITCH_RAD
    // @DisplayName: GPS glitch radius gate size (m)
    // @Description: This controls the maximum radial uncertainty in position between the value predicted by the filter and the value measured by the GPS before the filter position and velocity states are reset to the GPS. Making this value larger allows the filter to ignore larger GPS glitches but also means that non-GPS errors such as IMU and compass can create a larger error in position before the filter is forced back to the GPS position.
    // @Range: 10 100
    // @Increment: 5
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("GLITCH_RAD", 7, NavEKF2, _gpsGlitchRadiusMax, GLITCH_RADIUS_DEFAULT),

（9）GPS水平速度噪声（m/s）参数

  // @Param: GPS_DELAY
    // @DisplayName: GPS measurement delay (msec)
    // @Description: This is the number of msec that the GPS measurements lag behind the inertial measurements.
    // @Range: 0 250
    // @Increment: 10
    // @User: Advanced
    // @Units: milliseconds
    AP_GROUPINFO("GPS_DELAY", 8, NavEKF2, _gpsDelay_ms, 220),

（10）GPS水平速度噪声（m/s）参数

 // Height measurement parameters

    // @Param: ALT_SOURCE
    // @DisplayName: Primary altitude sensor source
    // @Description: This parameter controls the primary height sensor used by the EKF. If the selected option cannot be used, it will default to Baro as the primary height source. Setting 0 will use the baro altitude at all times. Setting 1 uses the range finder and is only available in combination with optical flow navigation (EK2_GPS_TYPE = 3). Setting 2 uses GPS. Setting 3 uses the range beacon data. NOTE - the EK2_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
    // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
    // @User: Advanced
    AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 0),

（11）GPS水平速度噪声（m/s）参数

 // @Param: ALT_M_NSE
    // @DisplayName: Altitude measurement noise (m)
    // @Description: This is the RMS value of noise in the altitude measurement. Increasing it reduces the weighting of the baro measurement and will make the filter respond more slowly to baro measurement errors, but will make it more sensitive to GPS and accelerometer errors.
    // @Range: 0.1 10.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("ALT_M_NSE", 10, NavEKF2, _baroAltNoise, ALT_M_NSE_DEFAULT),  //默认是3.0

（12）GPS水平速度噪声（m/s）参数

    // @Param: HGT_I_GATE
    // @DisplayName: Height measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the height measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("HGT_I_GATE", 11, NavEKF2, _hgtInnovGate, HGT_I_GATE_DEFAULT),

（13）GPS水平速度噪声（m/s）参数

   // @Param: HGT_DELAY
    // @DisplayName: Height measurement delay (msec)
    // @Description: This is the number of msec that the Height measurements lag behind the inertial measurements.
    // @Range: 0 250
    // @Increment: 10
    // @User: Advanced
    // @Units: milliseconds
    AP_GROUPINFO("HGT_DELAY", 12, NavEKF2, _hgtDelay_ms, 60),

（14）GPS水平速度噪声（m/s）参数

    // Magnetometer measurement parameters

    // @Param: MAG_M_NSE
    // @DisplayName: Magnetometer measurement noise (Gauss)
    // @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
    // @Range: 0.01 0.5
    // @Increment: 0.01
    // @User: Advanced
    // @Units: gauss
    AP_GROUPINFO("MAG_M_NSE", 13, NavEKF2, _magNoise, MAG_M_NSE_DEFAULT),

（15）GPS水平速度噪声（m/s）参数

    // @Param: MAG_CAL
    // @DisplayName: Magnetometer default fusion mode
    // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK2_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK2_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK2_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK2_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK2_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter.
    // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always
    // @User: Advanced
    AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT),

（16）GPS水平速度噪声（m/s）参数


    // @Param: MAG_I_GATE
    // @DisplayName: Magnetometer measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the magnetometer measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("MAG_I_GATE", 15, NavEKF2, _magInnovGate, MAG_I_GATE_DEFAULT),

（17）GPS水平速度噪声（m/s）参数

 // Airspeed measurement parameters

    // @Param: EAS_M_NSE
    // @DisplayName: Equivalent airspeed measurement noise (m/s)
    // @Description: This is the RMS value of noise in equivalent airspeed measurements used by planes. Increasing it reduces the weighting of airspeed measurements and will make wind speed estimates less noisy and slower to converge. Increasing also increases navigation errors when dead-reckoning without GPS measurements.
    // @Range: 0.5 5.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m/s
    AP_GROUPINFO("EAS_M_NSE", 16, NavEKF2, _easNoise, 1.4f),

（18）GPS水平速度噪声（m/s）参数

  // @Param: EAS_I_GATE
    // @DisplayName: Airspeed measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the airspeed measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("EAS_I_GATE", 17, NavEKF2, _tasInnovGate, 400),

（19）GPS水平速度噪声（m/s）参数

 // Rangefinder measurement parameters

    // @Param: RNG_M_NSE
    // @DisplayName: Range finder measurement noise (m)
    // @Description: This is the RMS value of noise in the range finder measurement. Increasing it reduces the weighting on this measurement.
    // @Range: 0.1 10.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("RNG_M_NSE", 18, NavEKF2, _rngNoise, 0.5f),

（20）GPS水平速度噪声（m/s）参数

  // @Param: RNG_I_GATE
    // @DisplayName: Range finder measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the range finder innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("RNG_I_GATE", 19, NavEKF2, _rngInnovGate, 500),

（21）GPS水平速度噪声（m/s）参数

  // Optical flow measurement parameters

    // @Param: MAX_FLOW
    // @DisplayName: Maximum valid optical flow rate
    // @Description: This sets the magnitude maximum optical flow rate in rad/sec that will be accepted by the filter
    // @Range: 1.0 4.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: rad/s
    AP_GROUPINFO("MAX_FLOW", 20, NavEKF2, _maxFlowRate, 2.5f),

（22）GPS水平速度噪声（m/s）参数

 // @Param: FLOW_M_NSE
    // @DisplayName: Optical flow measurement noise (rad/s)
    // @Description: This is the RMS value of noise and errors in optical flow measurements. Increasing it reduces the weighting on these measurements.
    // @Range: 0.05 1.0
    // @Increment: 0.05
    // @User: Advanced
    // @Units: rad/s
    AP_GROUPINFO("FLOW_M_NSE", 21, NavEKF2, _flowNoise, FLOW_M_NSE_DEFAULT),

（23）GPS水平速度噪声（m/s）参数

    // @Param: FLOW_I_GATE
    // @DisplayName: Optical Flow measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the optical flow innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("FLOW_I_GATE", 22, NavEKF2, _flowInnovGate, FLOW_I_GATE_DEFAULT),

（24）GPS水平速度噪声（m/s）参数

 // @Param: FLOW_DELAY
    // @DisplayName: Optical Flow measurement delay (msec)
    // @Description: This is the number of msec that the optical flow measurements lag behind the inertial measurements. It is the time from the end of the optical flow averaging period and does not include the time delay due to the 100msec of averaging within the flow sensor.
    // @Range: 0 127
    // @Increment: 10
    // @User: Advanced
    // @Units: milliseconds
    AP_GROUPINFO("FLOW_DELAY", 23, NavEKF2, _flowDelay_ms, FLOW_MEAS_DELAY),

（25）GPS水平速度噪声（m/s）参数

 // State and Covariance Predition Parameters

    // @Param: GYRO_P_NSE
    // @DisplayName: Rate gyro noise (rad/s)
    // @Description: This control disturbance noise controls the growth of estimated error due to gyro measurement errors excluding bias. Increasing it makes the flter trust the gyro measurements less and other measurements more.
    // @Range: 0.0001 0.1
    // @Increment: 0.0001
    // @User: Advanced
    // @Units: rad/s
    AP_GROUPINFO("GYRO_P_NSE", 24, NavEKF2, _gyrNoise, GYRO_P_NSE_DEFAULT),

（26）GPS水平速度噪声（m/s）参数

    // @Param: ACC_P_NSE
    // @DisplayName: Accelerometer noise (m/s^2)
    // @Description: This control disturbance noise controls the growth of estimated error due to accelerometer measurement errors excluding bias. Increasing it makes the flter trust the accelerometer measurements less and other measurements more.
    // @Range: 0.01 1.0
    // @Increment: 0.01
    // @User: Advanced
    // @Units: m/s/s
    AP_GROUPINFO("ACC_P_NSE", 25, NavEKF2, _accNoise, ACC_P_NSE_DEFAULT),

（27）GPS水平速度噪声（m/s）参数


    // @Param: GBIAS_P_NSE
    // @DisplayName: Rate gyro bias stability (rad/s/s)
    // @Description: This state  process noise controls growth of the gyro delta angle bias state error estimate. Increasing it makes rate gyro bias estimation faster and noisier.
    // @Range: 0.00001 0.001
    // @User: Advanced
    // @Units: rad/s/s
    AP_GROUPINFO("GBIAS_P_NSE", 26, NavEKF2, _gyroBiasProcessNoise, GBIAS_P_NSE_DEFAULT),

（28）GPS水平速度噪声（m/s）参数

    // @Param: GSCL_P_NSE
    // @DisplayName: Rate gyro scale factor stability (1/s)
    // @Description: This noise controls the rate of gyro scale factor learning. Increasing it makes rate gyro scale factor estimation faster and noisier.
    // @Range: 0.000001 0.001
    // @User: Advanced
    // @Units: 1/s
    AP_GROUPINFO("GSCL_P_NSE", 27, NavEKF2, _gyroScaleProcessNoise, GSCALE_P_NSE_DEFAULT),

（29）GPS水平速度噪声（m/s）参数

   // @Param: ABIAS_P_NSE
    // @DisplayName: Accelerometer bias stability (m/s^3)
    // @Description: This noise controls the growth of the vertical accelerometer delta velocity bias state error estimate. Increasing it makes accelerometer bias estimation faster and noisier.
    // @Range: 0.00001 0.001
    // @User: Advanced
    // @Units: m/s/s/s
    AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),

（30）GPS水平速度噪声（m/s）参数

  // 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE

    // @Param: WIND_P_NSE
    // @DisplayName: Wind velocity process noise (m/s^2)
    // @Description: This state process noise controls the growth of wind state error estimates. Increasing it makes wind estimation faster and noisier.
    // @Range: 0.01 1.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m/s/s
    AP_GROUPINFO("WIND_P_NSE", 30, NavEKF2, _windVelProcessNoise, 0.1f),

（31）GPS水平速度噪声（m/s）参数

   // @Param: WIND_PSCALE
    // @DisplayName: Height rate to wind process noise scaler
    // @Description: This controls how much the process noise on the wind states is increased when gaining or losing altitude to take into account changes in wind speed and direction with altitude. Increasing this parameter increases how rapidly the wind states adapt when changing altitude, but does make wind velocity estimation noiser.
    // @Range: 0.0 1.0
    // @Increment: 0.1
    // @User: Advanced
    AP_GROUPINFO("WIND_PSCALE", 31, NavEKF2, _wndVarHgtRateScale, 0.5f),

（32）GPS水平速度噪声（m/s）参数

    // @Param: GPS_CHECK
    // @DisplayName: GPS preflight check
    // @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat.
    // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
    // @User: Advanced
    AP_GROUPINFO("GPS_CHECK",    32, NavEKF2, _gpsCheck, 31),

（33）GPS水平速度噪声（m/s）参数

（34）GPS水平速度噪声（m/s）参数

// @Param: IMU_MASK
    // @DisplayName: Bitmask of active IMUs
    // @Description: 1 byte bitmap of IMUs to use in EKF2. A separate instance of EKF2 will be started for each IMU selected. Set to 1 to use the first IMU only (default), set to 2 to use the second IMU only, set to 3 to use the first and second IMU. Additional IMU's can be used up to a maximum of 6 if memory and processing resources permit. There may be insufficient memory and processing resources to run multiple instances. If this occurs EKF2 will fail to start.
    // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
    // @User: Advanced
    AP_GROUPINFO("IMU_MASK",     33, NavEKF2, _imuMask, 3),

（35）GPS水平速度噪声（m/s）参数

 // @Param: CHECK_SCALE
    // @DisplayName: GPS accuracy check scaler (%)
    // @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error.
    // @Range: 50 200
    // @User: Advanced
    // @Units: %
    AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT),

（36）GPS水平速度噪声（m/s）参数

  // @Param: NOAID_M_NSE
    // @DisplayName: Non-GPS operation position uncertainty (m)
    // @Description: This sets the amount of position variation that the EKF allows for when operating without external measurements (eg GPS or optical flow). Increasing this parameter makes the EKF attitude estimate less sensitive to vehicle manoeuvres but more sensitive to IMU errors.
    // @Range: 0.5 50.0
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("NOAID_M_NSE", 35, NavEKF2, _noaidHorizNoise, 10.0f),

（37）GPS水平速度噪声（m/s）参数

// @Param: LOG_MASK
    // @DisplayName: EKF sensor logging IMU mask
    // @Description: This sets the IMU mask of sensors to do full logging for
    // @Bitmask: 0:FirstIMU,1:SecondIMU,2:ThirdIMU,3:FourthIMU,4:FifthIMU,5:SixthIMU
    // @User: Advanced
    AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),

（38）GPS水平速度噪声（m/s）参数

 // control of magentic yaw angle fusion

    // @Param: YAW_M_NSE
    // @DisplayName: Yaw measurement noise (rad)
    // @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
    // @Range: 0.05 1.0
    // @Increment: 0.05
    // @User: Advanced
    // @Units: rad
    AP_GROUPINFO("YAW_M_NSE", 37, NavEKF2, _yawNoise, 0.5f),

（39）GPS水平速度噪声（m/s）参数

 // @Param: YAW_I_GATE
    // @DisplayName: Yaw measurement gate size
    // @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300),

（40）GPS水平速度噪声（m/s）参数

// @Param: TAU_OUTPUT
    // @DisplayName: Output complementary filter time constant (centi-sec)
    // @Description: Sets the time constant of the output complementary filter/predictor in centi-seconds.
    // @Range: 10 50
    // @Increment: 5
    // @User: Advanced
    // @Units: cs
    AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, 25),

（41）GPS水平速度噪声（m/s）参数

// @Param: MAGE_P_NSE
    // @DisplayName: Earth magnetic field process noise (gauss/s)
    // @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
    // @Range: 0.00001 0.01
    // @User: Advanced
    // @Units: gauss/s
    AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF2, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),

（42）GPS水平速度噪声（m/s）参数

 // @Param: MAGB_P_NSE
    // @DisplayName: Body magnetic field process noise (gauss/s)
    // @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
    // @Range: 0.00001 0.01
    // @User: Advanced
    // @Units: gauss/s
    AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),

（43）GPS水平速度噪声（m/s）参数

  // @Param: RNG_USE_HGT
    // @DisplayName: Range finder switch height percentage
    // @Description: The range finder will be used as the primary height source when below a specified percentage of the sensor maximum as set by the RNGFND_MAX_CM parameter. Set to -1 to prevent range finder use.
    // @Range: -1 70
    // @Increment: 1
    // @User: Advanced
    // @Units: %
    AP_GROUPINFO("RNG_USE_HGT", 42, NavEKF2, _useRngSwHgt, -1),

（44）GPS水平速度噪声（m/s）参数

 // @Param: TERR_GRAD
    // @DisplayName: Maximum terrain gradient
    // @Description: Specifies the maximum gradient of the terrain below the vehicle when it is using range finder as a height reference
    // @Range: 0 0.2
    // @Increment: 0.01
    // @User: Advanced
    AP_GROUPINFO("TERR_GRAD", 43, NavEKF2, _terrGradMax, 0.1f),

（45）GPS水平速度噪声（m/s）参数

 // @Param: BCN_M_NSE
    // @DisplayName: Range beacon measurement noise (m)
    // @Description: This is the RMS value of noise in the range beacon measurement. Increasing it reduces the weighting on this measurement.
    // @Range: 0.1 10.0
    // @Increment: 0.1
    // @User: Advanced
    // @Units: m
    AP_GROUPINFO("BCN_M_NSE", 44, NavEKF2, _rngBcnNoise, 1.0f),

（46）距离警戒测量延迟（ms）

// @Param: BCN_I_GTE
    // @DisplayName: 距离警戒测量栅尺寸
    // @Description: 这设置了应用于距离距离测量创新一致性检查的标准偏差的百分比数。减少它会使很好的测量结果被拒绝。增加它会使不良测量更容易被接受。
    // @Range: 100 1000
    // @Increment: 25
    // @User: Advanced
    AP_GROUPINFO("BCN_I_GTE", 45, NavEKF2, _rngBcnInnovGate, 500),

（47）距离警戒测量延迟参数

  // @Param: BCN_DELAY
    // @DisplayName: 距离警戒测量延迟（MSEC）
    // @Description: 这是ms的数量，距离境界测量落后于惯性测量。它是从光流平均周期结束起的时间，不包括由于在流量传感器内平均100毫秒而造成的时间延迟。
    // @Range: 0 127
    // @Increment: 10
    // @User: Advanced
    // @Units: milliseconds
    AP_GROUPINFO("BCN_DELAY", 46, NavEKF2, _rngBcnDelay_ms, 50),

（48）测距仪最大地面速度参数

 // @Param: RNG_USE_SPD
    // @DisplayName: 测距仪最大地面速度
    // @Description: 当水平地速度大于这个值时，测距仪将不被用作主要的高度源。
    // @Range: 2.0 6.0
    // @Increment: 0.5
    // @User: Advanced
    // @Units: m/s
    AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF2, _useRngSwSpd, 2.0f),

（49）地磁激活参数

// @Param: MAG_MASK
    // @DisplayName: 使能位标志码，主要用于EKF核心的是否使用航向融合
    // @Description:EKF核心的1位位图，它将禁用磁场状态，并在任何时候使用简单的磁航向融合。该参数使得特定的地磁能够被用作飞入具有高水平外部磁干扰的环境的备用，当使用三轴磁力计融合时可能降低EKF姿态估计。注意：在不同的磁芯上使用不同的磁强计融合算法使得由于磁强计误差而导致的不需要的EKF磁芯开关更加可能。
    // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF
    // @User: Advanced
    AP_GROUPINFO("MAG_MASK", 48, NavEKF2, _magMask, 0),

（50）掉高参数

AP_GROUPINFO("DROP_ALT", 49, NavEKF2, _dropAlt, 0), //这个不知道干嘛的

九天揽月带你玩转EKF纸老虎（3）

目录

摘要

第一：ekf初始化

1.EKF数据初始化

2.EKF参数变量

你可能感兴趣的:(ardupilot学习)