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 memoryif (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 masknum_cores = __builtin_popcount(_imuMask);// abort if num_cores is zeroif (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 coresif (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 fullcore = (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 coresfor (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 aborthal.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 indexprimary = 0;}// invalidate shared origincommon_origin_valid = false;// initialise the cores. We return success only if all cores// initialise successfullybool ret = true;for (uint8_t i=0; i<num_cores; i++) {ret &= core[i].InitialiseFilterBootstrap();//初始化每个 EKF2 核心。}// zero the structs used capture reset eventsmemset(&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 缓冲区填满之前返回 falseif (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 startupreadIMUData();readMagData();readGpsData();readBaroData();return storedIMU.is_filled();}// set re-used variables to zero//初始化并清零所有将重复使用的变量。InitialiseVariables();const auto &ins = dal.ins();// Initialise IMU datadtIMUavg = 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 datareadMagData();// normalise the acceleration vectorftype pitch=0, roll=0;if (initAccVec.length() > 0.001f) {initAccVec.normalize();// calculate initial pitch angle//根据加速度计的数据，计算初始的俯仰角 (pitch) 和横滚角 (roll)pitch = asinF(initAccVec.x);// calculate initial roll angleroll = atan2F(-initAccVec.y , -initAccVec.z);}// calculate initial roll and pitch orientationstateStruct.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 schedulingif (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 primaryfor (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 stepbool 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 foundfloat 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 switchif (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// performanceprimary = 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 tablefor (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 outputvertCompFiltState.pos = stateStruct.position.z;vertCompFiltState.vel = stateStruct.velocity.z;
}

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

未完待续~~