ArduSub程序学习（11）--EKF实现逻辑②

1.InitialiseFilter(void)

扩展卡尔曼滤波器2 (EKF2) 的初始化流程。这个函数的核心功能是设置并启动 EKF2 滤波器，包括内存分配、滤波器核心设置以及与惯性测量单元 (IMU) 的关联。

//EKF2初始化
bool NavEKF2::InitialiseFilter(void)
{
    //通过 start_frame 函数，开启一个与 EKF2 初始化相关的日志框架。
    AP::dal().start_frame(AP_DAL::FrameType::InitialiseFilterEKF2);

    // Return immediately if there is insufficient memory
    if (core_malloc_failed) {
        return false;
    }

    initFailure = InitFailures::UNKNOWN;
    if (_enable == 0) {
        if (AP::dal().get_ekf_type() == 2) {
            initFailure = InitFailures::NO_ENABLE;
        }
        return false;
    }
    //获取传感器数据，gyro,acc数据
    const auto &ins = AP::dal().ins();

    //获取 IMU 的采样时间和系统的循环速率。
    imuSampleTime_us = AP::dal().micros64();

    // remember expected frame time
    //根据循环时间，记录预期的时间
    const float loop_rate = ins.get_loop_rate_hz();
    if (!is_positive(loop_rate)) {
        return false;
    }
    _frameTimeUsec = 1e6 / loop_rate;

    // expected number of IMU frames per prediction
    //算有多少个预测imu方程，因为这里有好几组imu，都可以进行ekf
    _framesPerPrediction = uint8_t((EKF_TARGET_DT / (_frameTimeUsec * 1.0e-6) + 0.5));

    if (core == nullptr) {

        // don't allow more filters than IMUs
        //不要对一个IMU进行多次滤波
        uint8_t mask = (1U<<ins.get_accel_count())-1;
        _imuMask.set(_imuMask.get() & mask);
        
        // count IMUs from mask
        num_cores = __builtin_popcount(_imuMask);

        // abort if num_cores is zero
        if (num_cores == 0) {
            if (ins.get_accel_count() == 0) {
                initFailure = InitFailures::NO_IMUS;
            } else {
                initFailure = InitFailures::NO_MASK;
            }
            return false;
        }

        // check if there is enough memory to create the EKF cores
        if (AP::dal().available_memory() < sizeof(NavEKF2_core)*num_cores + 4096) {
            initFailure = InitFailures::NO_MEM;
            core_malloc_failed = true;
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: not enough memory available");
            return false;
        }

        // try to allocate from CCM RAM, fallback to Normal RAM if not available or full
        core = (NavEKF2_core*)AP::dal().malloc_type(sizeof(NavEKF2_core)*num_cores, AP_DAL::MEM_FAST);
        if (core == nullptr) {
            initFailure = InitFailures::NO_MEM;
            core_malloc_failed = true;
            GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "NavEKF2: memory allocation failed");
            return false;
        }

        //Call Constructors on all cores
        for (uint8_t i = 0; i < num_cores; i++) {
            new (&core[i]) NavEKF2_core(this);
        }

        // set the IMU index for the cores
        //遍历所有 IMU，调用 setup_core() 函数为每个 EKF2 核心进行设置
        num_cores = 0;
        for (uint8_t i=0; i<7; i++) {
            if (_imuMask & (1U<<i)) {
                if(!core[num_cores].setup_core(i, num_cores)) {
                    // if any core setup fails, free memory, zero the core pointer and abort
                    hal.util->free_type(core, sizeof(NavEKF2_core)*num_cores, AP_HAL::Util::MEM_FAST);
                    core = nullptr;
                    initFailure = InitFailures::NO_SETUP;
                    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "NavEKF2: core %d setup failed", num_cores);
                    return false;
                }
                num_cores++;
            }
        }

        // Set the primary initially to be the lowest index
        primary = 0;
    }

    // invalidate shared origin
    common_origin_valid = false;
    
    // initialise the cores. We return success only if all cores
    // initialise successfully
    bool ret = true;
    for (uint8_t i=0; i<num_cores; i++) {
        ret &= core[i].InitialiseFilterBootstrap();//初始化每个 EKF2 核心。
    }

    // zero the structs used capture reset events
    memset(&yaw_reset_data, 0, sizeof(yaw_reset_data));
    memset((void *)&pos_reset_data, 0, sizeof(pos_reset_data));
    memset(&pos_down_reset_data, 0, sizeof(pos_down_reset_data));

    return ret;
}

• 检查内存是否不足：首先检查系统是否有足够的内存。如果内存不足，立即返回 false，并设置 initFailure 为内存分配失败。

• 启用检查：如果 EKF 未启用，则直接返回 false，并将失败原因设置为未启用。

• 获取 IMU 数据：从传感器（IMU）中读取采样时间和循环速率。如果没有有效的循环速率，返回 false。

• IMU 掩码和核芯数量：通过 IMU 掩码计算当前可用的 IMU 核心数量，并根据掩码为每个核分配内存。

• 内存检查和分配：判断是否有足够内存为 EKF 核心分配。如果内存不足，记录失败原因并返回 false。

• 初始化 EKF 核心：对所有可用的 IMU 核心进行初始化，检查是否每个核心都成功初始化。如果任意一个核心初始化失败，则返回 false 并释放内存。

• 设置初始核心：如果核心初始化成功，将第一个核心设置为初始核心。

• 滤波器共享原点无效化：共享的滤波器原点被无效化，确保滤波器重新开始计算。

• 返回结果：最后，只有所有核心都成功初始化时，才返回 true，否则返回 false 并停止初始化过程。

2.InitialiseFilterBootstrap(void)

NavEKF2_core::InitialiseFilterBootstrap() 是一个用于初始化 EKF (扩展卡尔曼滤波器) 核心的函数。它的主要作用是在启动时设置过滤器的初始状态，包括姿态、速度、位置、以及相关的状态变量和传感器数据。

//EKF2核心滤波器初始化
bool NavEKF2_core::InitialiseFilterBootstrap(void)
{
    // If we are a plane and don't have GPS lock then don't initialise
    //如果我们是一架飞机，没有GPS锁定，那么不要初始化
    // GPS 模块成功获取到足够多的卫星信号来计算出准确的三维位置（经度、纬度和高度）
    if (assume_zero_sideslip() && dal.gps().status(dal.gps().primary_sensor()) < AP_DAL_GPS::GPS_OK_FIX_3D) {
        dal.snprintf(prearm_fail_string,
                           sizeof(prearm_fail_string),
                           "EKF2 init failure: No GPS lock");
        statesInitialised = false;
        return false;
    }

    //如果过滤器状态已经初始化 (statesInitialised == true)，函数将会读取IMU、磁力计、GPS 和气压计数据，并在 IMU 缓冲区填满之前返回 false
    if (statesInitialised) {
        // we are initialised, but we don't return true until the IMU
        // buffer has been filled. This prevents a timing
        // vulnerability with a pause in IMU data during filter startup
        readIMUData();
        readMagData();
        readGpsData();
        readBaroData();
        return storedIMU.is_filled();
    }

    // set re-used variables to zero
    //初始化并清零所有将重复使用的变量。
    InitialiseVariables();

    const auto &ins = dal.ins();

    // Initialise IMU data
    dtIMUavg = ins.get_loop_delta_t();
    //初始化IMU，读取 IMU 数据，并将当前的新 IMU 数据存储在历史数据缓冲区中。
    readIMUData();
    storedIMU.reset_history(imuDataNew);
    imuDataDelayed = imuDataNew;

    // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
    // IMU测量的XYZ体轴加速度矢量(m/s^2)
    Vector3F initAccVec;

    // TODO we should average accel readings over several cycles
    我们应该平均几个周期的加速度读数
    initAccVec = ins.get_accel(accel_index_active).toftype();

    // read the magnetometer data
    readMagData();

    // normalise the acceleration vector
    ftype pitch=0, roll=0;
    if (initAccVec.length() > 0.001f) {
        initAccVec.normalize();

        // calculate initial pitch angle
        //根据加速度计的数据，计算初始的俯仰角 (pitch) 和横滚角 (roll)
        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();

    // read the GPS and set the position and velocity states
    //从 GPS 读取数据，并使用这些数据来重置速度和位置状态。确保初始时的位置和速度与 GPS 数据一致。
    readGpsData();
    ResetVelocity();
    ResetPosition();

    // read the barometer and set the height state
    //从气压计读取数据，并将其用于重置高度状态，确保初始高度准确。
    readBaroData();
    ResetHeight();

    // define Earth rotation vector in the NED navigation frame
    //初始化地球自转向量
    calcEarthRateNED(earthRateNED, dal.get_home().lat);

    // initialise the covariance matrix
    //初始化协方差矩阵，用于后续状态估计过程中跟踪状态变量的不确定性。
    CovarianceInit();

    // reset output states
    //重置滤波器的输出状态，确保所有输出变量处于初始状态。
    StoreOutputReset();

    // set to true now that states have be initialised
    //完成初始化
    statesInitialised = true;

    // reset inactive biases
    //重置非活动偏差
    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
        inactiveBias[i].gyro_bias.zero();
        inactiveBias[i].accel_zbias = 0;
        inactiveBias[i].gyro_scale.x = 1;
        inactiveBias[i].gyro_scale.y = 1;
        inactiveBias[i].gyro_scale.z = 1;
    }

    // we initially return false to wait for the IMU buffer to fill
    //尽管初始化完成，函数仍然返回 false，等待 IMU 数据缓冲区填满，防止在启动时使用不完整的 IMU 数据。
    return false;
}

3.UpdateFilter(void)

 NavEKF2::UpdateFilter() 函数的主要功能是更新扩展卡尔曼滤波器（EKF）的状态，确保飞行器在姿态、速度和位置估计方面具有准确性和鲁棒性。该函数不仅处理 EKF 的预测和状态更新，还管理多个 EKF 核心的选择和切换，以应对不健康或误差较大的核心。

void NavEKF2::UpdateFilter(void)
{
    //调用数据抽象层 (DAL) 的 start_frame() 方法，表示开始一帧新的 EKF 2 滤波更新。
    AP::dal().start_frame(AP_DAL::FrameType::UpdateFilterEKF2);

    //核心有效性检查
    if (!core) {
        return;
    }

    imuSampleTime_us = AP::dal().micros64();//更新 IMU 采样时间
    
    bool statePredictEnabled[num_cores];
    //预测步骤启用检查
    for (uint8_t i=0; i<num_cores; i++) {
        // if we have not overrun by more than 3 IMU frames, and we
        // have already used more than 1/3 of the CPU budget for this
        // loop then suppress the prediction step. This allows
        // multiple EKF instances to cooperate on scheduling
        if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
            AP::dal().ekf_low_time_remaining(AP_DAL::EKFType::EKF2, i)) {
            statePredictEnabled[i] = false;
        } else {
            statePredictEnabled[i] = true;
        }
        //每个 EKF 核心都有一个单独的 UpdateFilter() 调用。该循环遍历所有核心，并决定是否启用每个核心的状态预测步骤。
        core[i].UpdateFilter(statePredictEnabled[i]);
    }

    // If the current core selected has a bad error score or is unhealthy, switch to a healthy core with the lowest fault score
    // Don't start running the check until the primary core has started returned healthy for at least 10 seconds to avoid switching
    // due to initial alignment fluctuations and race conditions
    //核心选择机制
    if (!runCoreSelection) {
        static uint64_t lastUnhealthyTime_us = 0;
        if (!core[primary].healthy() || lastUnhealthyTime_us == 0) {
            lastUnhealthyTime_us = imuSampleTime_us;
        }
        runCoreSelection = (imuSampleTime_us - lastUnhealthyTime_us) > 1E7;
    }
    float primaryErrorScore = core[primary].errorScore();
    //核心选择逻辑
    if ((primaryErrorScore > 1.0f || !core[primary].healthy()) && runCoreSelection) {
        float lowestErrorScore = 0.67f * primaryErrorScore;
        uint8_t newPrimaryIndex = primary; // index for new primary
        for (uint8_t coreIndex=0; coreIndex<num_cores; coreIndex++) {

            if (coreIndex != primary) {
                // an alternative core is available for selection only if healthy and if states have been updated on this time step
                bool altCoreAvailable = statePredictEnabled[coreIndex] && coreBetterScore(coreIndex, newPrimaryIndex);

                // If the primary core is unhealthy and another core is available, then switch now
                // If the primary core is still healthy,then switching is optional and will only be done if
                // a core with a significantly lower error score can be found
                float altErrorScore = core[coreIndex].errorScore();
                if (altCoreAvailable && (!core[newPrimaryIndex].healthy() || altErrorScore < lowestErrorScore)) {
                    newPrimaryIndex = coreIndex;
                    lowestErrorScore = altErrorScore;
                }
            }
        }
        // update the yaw and position reset data to capture changes due to the lane switch
        if (newPrimaryIndex != primary) {
            updateLaneSwitchYawResetData(newPrimaryIndex, primary);
            updateLaneSwitchPosResetData(newPrimaryIndex, primary);
            updateLaneSwitchPosDownResetData(newPrimaryIndex, primary);
            primary = newPrimaryIndex;
            lastLaneSwitch_ms = AP::dal().millis();
        }
    }

    //如果飞行器当前在地面且未解锁，系统会强制切换到第一个核心。
    if (primary != 0 && core[0].healthy() && !AP::dal().get_armed()) {
        // when on the ground and disarmed force the first lane. This
        // avoids us ending with with a lottery for which IMU is used
        // in each flight. Otherwise the alignment of the timing of
        // the lane updates with the timing of GPS updates can lead to
        // a lane other than the first one being used as primary for
        // some flights. As different IMUs may have quite different
        // noise characteristics this leads to inconsistent
        // performance
        primary = 0;
    }
}

• 多个 EKF 核心：该系统使用多个 EKF 核心并行计算，并根据每个核心的健康状态和错误得分动态选择最优的核心来提供传感器数据。
• 核心切换机制：通过监控每个核心的健康状况和误差，系统可以在必要时切换到更好的核心，以确保飞行控制的稳定性和可靠性。
• 强制使用第一个核心：在地面状态下，系统强制使用第一个核心来确保一致的 IMU 选择，避免不同核心之间的噪声差异对飞行表现的影响。

 卡尔曼公式

EKF2状态输入变量和状态输出变量

 struct state_elements {//状态输入变量28维
        Vector3F    angErr;         // 0..2角度误差
        Vector3F    velocity;       // 3..5速度矢量
        Vector3F    position;       // 6..8位置矢量
        Vector3F    gyro_bias;      // 9..11陀螺仪数据偏差
        Vector3F    gyro_scale;     // 12..14陀螺仪系数
        ftype       accel_zbias;    // 15加速度垂直偏差
        Vector3F    earth_magfield; // 16..18地理地磁
        Vector3F    body_magfield;  // 19..21机体地磁
        Vector2F    wind_vel;       // 22..23风速
        QuaternionF quat;           // 24..27四元数
    };

    union {
        Vector28 statesArray;
        struct state_elements stateStruct;
    };

    struct output_elements {//状态输出变量10维
        QuaternionF quat;           // 0..3四元数
        Vector3F    velocity;       // 4..6速度
        Vector3F    position;       // 7..9位置
    };

对照上面的状态变量我们看一下初始化协方差矩阵P的函数。

// initialise the covariance matrix初始化协方差矩阵
void NavEKF2_core::CovarianceInit()
{
    // zero the matrix
    //清空矩阵
    memset(&P[0][0], 0, sizeof(P));

    // attitude error
    //姿态误差主要包括滚转角 (roll)、俯仰角 (pitch)、偏航角 (yaw) 相关的不确定性。
    P[0][0]   = 0.1f;
    P[1][1]   = 0.1f;
    P[2][2]   = 0.1f;
    // velocities
    //速度误差，水平和垂直速度误差的协方差矩阵初始化。
    P[3][3]   = sq(frontend->_gpsHorizVelNoise);
    P[4][4]   = P[3][3];
    P[5][5]   = sq(frontend->_gpsVertVelNoise);
    // positions
    //位置误差，水平和垂直位置误差的协方差矩阵初始化。
    P[6][6]   = sq(frontend->_gpsHorizPosNoise);
    P[7][7]   = P[6][6];
    P[8][8]   = sq(frontend->_baroAltNoise);
    // gyro delta angle biases
    //陀螺仪角速度偏差
    P[9][9] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
    P[10][10] = P[9][9];
    P[11][11] = P[9][9];
    // gyro scale factor biases
    //陀螺仪比例因子误差
    P[12][12] = sq(1e-3);
    P[13][13] = P[12][12];
    P[14][14] = P[12][12];
    // Z delta velocity bias
    //加速度计偏差
    P[15][15] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
    // earth magnetic field
    //地磁场误差
    P[16][16] = 0.0f;
    P[17][17] = P[16][16];
    P[18][18] = P[16][16];
    // body magnetic field
    //机身磁场误差
    P[19][19] = 0.0f;
    P[20][20] = P[19][19];
    P[21][21] = P[19][19];
    // wind velocities
    //风速误差
    P[22][22] = 0.0f;
    P[23][23]  = P[22][22];

    // optical flow ground height covariance
    //光流高度误差
    Popt = 0.25f;
}

// reset the output data to the current EKF state
//将当前的状态数据存储到一个缓存中，并重置相关状态以进行后续的计算。
void NavEKF2_core::StoreOutputReset()
{
    outputDataNew.quat = stateStruct.quat;
    outputDataNew.velocity = stateStruct.velocity;
    outputDataNew.position = stateStruct.position;
    // write current measurement to entire table
    for (uint8_t i=0; i<imu_buffer_length; i++) {
        storedOutput[i] = outputDataNew;
    }
    outputDataDelayed = outputDataNew;
    // reset the states for the complementary filter used to provide a vertical position dervative output
    vertCompFiltState.pos = stateStruct.position.z;
    vertCompFiltState.vel = stateStruct.velocity.z;
}

截止目前是卡尔曼的初始化，我们总结下:开始进行计算我们的飞控有几组IMU,根据IMU数，就创建多少用于进行EKF2处理的对象，然后根据创建的不同的IMU对象进行传感器初始化。

未完待续~~