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

目录

EKF的五大主要步骤

1.状态预测（State Prediction）

2.协方差预测（Covariance Prediction）

3.卡尔曼增益计算（Kalman Gain Calculation）

4.状态更新（State Update）

5.协方差更新（Covariance Update） 

补充说明：滤波器重置机制

下面开始寻找EKF五大公式

EKF的五大主要步骤

1. 状态预测（State Prediction）
2. 协方差预测（Covariance Prediction）
3. 卡尔曼增益计算（Kalman Gain Calculation）
4. 状态更新（State Update）
5. 协方差更新（Covariance Update）

/********************************************************
*                 UPDATE FUNCTIONS                      *
********************************************************/
// Update Filter States - this should be called whenever new IMU data is available
//更新扩展卡尔曼滤波器 (EKF) 的核心方法
void NavEKF2_core::UpdateFilter(bool predict)
{
    // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started

    //设置标志用来向ekf2滤波器,指示前端已允许启动新的状态预测周期。
    startPredictEnabled = predict;

    // don't run filter updates if states have not been initialised
    //如果未初始化状态完成，请不要运行过滤器更新
    if (!statesInitialised) {
        return;
    }

    // start the timer used for load measurement
    //启动用于负载测量的定时器
#if ENABLE_EKF_TIMING
    void *istate = hal.scheduler->disable_interrupts_save();
    static uint32_t timing_start_us;
    timing_start_us = dal.micros();
#endif

    fill_scratch_variables();

    // TODO - in-flight restart method

    //get starting time for update step获取更新步骤的开始时间
    imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

    // Check arm status and perform required checks and mode changes
    //检查遥控器是否解锁电机和执行必要的检查和模式
    controlFilterModes();

    // read IMU data as delta angles and velocities
    //读取IMU数据作为角度和速度
    readIMUData();

    // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer

    //如果在缓冲区中存在新的IMU数据，在满足的融合时间内，运行EKF方程
    if (runUpdates) {
        // Predict states using IMU data from the delayed time horizon
        //预测方程：使用IMU的数据，从延迟尺度时间-
        //EKF第一个方程（状态预测方程）
        //负责根据IMU数据（如角速度和加速度）预测系统的下一个状态。这涉及到使用系统的运动学模型（例如，导航方程）来预测飞机或飞行器在下一个时间步的状态。
        UpdateStrapdownEquationsNED();

        // 预测协方差增长Predict the covariance growth
        //EKF第二个方程（协方差预测方程）
        //用于预测状态协方差矩阵的增长。它通过应用系统的状态转移矩阵和过程噪声协方差矩阵，计算预测后的协方差。这一步骤反映了在预测过程中由于过程噪声引入的不确定性增加。
        CovariancePrediction();

        // Run the IMU prediction step for the GSF yaw estimator algorithm
        // using IMU and optionally true airspeed data.
        // Must be run before SelectMagFusion() to provide an up to date yaw estimate
        runYawEstimatorPrediction();

        // 使用地磁更新滤波器状态Update states using  magnetometer data
        //后面都EFK第三四个方程，（更新状态）（卡尔曼增益的计算）
        //卡尔曼增益的计算通常在各个传感器融合函数内部完成，如 SelectMagFusion(), SelectVelPosFusion() 等。
        SelectMagFusion();

        // 使用GPS和气压计进行状态更新Update states using GPS and altimeter data
        SelectVelPosFusion();

        // Run the GPS velocity correction step for the GSF yaw estimator algorithm
        // and use the yaw estimate to reset the main EKF yaw if requested
        // Muat be run after SelectVelPosFusion() so that fresh GPS data is available
        runYawEstimatorCorrection();

        // 使用测距仪器更新滤波器状态Update states using range beacon data
        SelectRngBcnFusion();

        // 使用光流传感器进行更新数据Update states using optical flow data
        SelectFlowFusion();

        // 使用空速计数据进行更新数据Update states using airspeed data
        SelectTasFusion();

        // 更新应用侧滑约束假设的飞越飞行器状态Update states using sideslip constraint assumption for fly-forward vehicles
        SelectBetaFusion();

        // 更新滤波器的状态Update the filter status
        //EKF第5个方程，用于更新滤波器的状态信息，如健康状态、滤波器的稀疏程度等。这有助于监控滤波器的性能，并在必要时触发滤波器的重置。
        updateFilterStatus();
    }

    // 从融合数据到数据输出Wind output forward from the fusion to output time horizon
    //EKF第5个方程，负责计算最终的输出状态，并更新协方差矩阵。这包括应用卡尔曼增益对状态和协方差进行最终调整，以反映最新的测量数据。
    calcOutputStates();

    // stop the timer used for load measurement
#if ENABLE_EKF_TIMING
    static uint32_t total_us;
    static uint32_t timing_counter;
    total_us += dal.micros() - timing_start_us;
    if (timing_counter++ == 4000) {
        hal.console->printf("ekf2 avg %.2f us\n", total_us / float(timing_counter));
        total_us = 0;
        timing_counter = 0;
    }
    hal.scheduler->restore_interrupts(istate);
#endif

    /*
      this is a check to cope with a vehicle sitting idle on the
      ground and getting over-confident of the state. The symptoms
      would be "gyros still settling" when the user tries to arm. In
      that state the EKF can't recover, so we do a hard reset and let
      it try again.
     */
    if (filterStatus.value != 0) {
        last_filter_ok_ms = dal.millis();
    }
    if (filterStatus.value == 0 &&
        last_filter_ok_ms != 0 &&
        dal.millis() - last_filter_ok_ms > 5000 &&
        !dal.get_armed()) {
        // we've been unhealthy for 5 seconds after being healthy, reset the filter
        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
        last_filter_ok_ms = 0;
        statesInitialised = false;
        InitialiseFilterBootstrap();
    }
    
}

1.状态预测（State Prediction）

UpdateStrapdownEquationsNED();

UpdateStrapdownEquationsNED() 函数负责根据IMU数据（如角速度和加速度）预测系统的下一个状态。这涉及到使用系统的运动学模型（例如，导航方程）来预测飞机或飞行器在下一个时间步的状态

2.协方差预测（Covariance Prediction）

CovariancePrediction();

CovariancePrediction() 函数用于预测状态协方差矩阵的增长。它通过应用系统的状态转移矩阵和过程噪声协方差矩阵，计算预测后的协方差。这一步骤反映了在预测过程中由于过程噪声引入的不确定性增加。

3.卡尔曼增益计算（Kalman Gain Calculation）

对应函数：

• 卡尔曼增益的计算通常在各个传感器融合函数内部完成，如 SelectMagFusion(), SelectVelPosFusion() 等。这些函数在融合传感器数据时，会计算相应的卡尔曼增益。

解释：

• 在EKF中，卡尔曼增益用于权衡预测状态与测量数据的信任度。每个传感器的数据融合步骤会根据当前协方差和测量噪声计算出相应的卡尔曼增益，以更新状态估计。

4.状态更新（State Update）

SelectMagFusion();
SelectVelPosFusion();
SelectRngBcnFusion();
SelectFlowFusion();
SelectTasFusion();
SelectBetaFusion();
runYawEstimatorCorrection();

• 这些函数负责使用不同传感器（如磁力计、GPS、测距仪、光流传感器、空速计等）的测量数据来更新滤波器的状态估计。
• 每个 Select*Fusion() 函数对应于特定传感器的数据融合，通过将测量数据与预测状态结合，修正状态估计。
• runYawEstimatorCorrection() 函数特别用于利用GPS速度数据来校正偏航角的估计。

5.协方差更新（Covariance Update） 

calcOutputStates();
updateFilterStatus();

• calcOutputStates() 函数负责计算最终的输出状态，并更新协方差矩阵。这包括应用卡尔曼增益对状态和协方差进行最终调整，以反映最新的测量数据。
• updateFilterStatus() 函数用于更新滤波器的状态信息，如健康状态、滤波器的稀疏程度等。这有助于监控滤波器的性能，并在必要时触发滤波器的重置。

补充说明：滤波器重置机制

if (filterStatus.value != 0) {
    last_filter_ok_ms = dal.millis();
}
if (filterStatus.value == 0 &&
    last_filter_ok_ms != 0 &&
    dal.millis() - last_filter_ok_ms > 5000 &&
    !dal.get_armed()) {
    // 强制重置滤波器并重新初始化
    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u forced reset",(unsigned)imu_index);
    last_filter_ok_ms = 0;
    statesInitialised = false;
    InitialiseFilterBootstrap();
}

该部分代码监控滤波器的健康状态 (filterStatus)。如果滤波器在一段时间内（例如，5000毫秒）处于不健康状态，并且飞行器未解锁 (!dal.get_armed())，则会强制重置滤波器并重新初始化。这确保了滤波器在异常情况下能够恢复，从而维持系统的稳定性和可靠性。 

 未完待续~~

下面逐个分析各个函数