ardupilot EKF2速度位置融合算法

目录

摘要

本节主要记录自己看EKF2的速度位置融合算法。

1.更新滤波器

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
    //设置标志以向筛选器指示前端已授予启动新状态预测周期的权限
	startPredictEnabled = predict;

    // don't run filter updates if states have not been initialised
	//如果状态尚未初始化，则不运行筛选器更新
    if (!statesInitialised) 
    {
        return;
    }

    // start the timer used for load measurement
    //启动用于负载测量的计时器
#if EK2_DISABLE_INTERRUPTS
    irqstate_t istate = irqsave();
#endif
    hal.util->perf_begin(_perf_UpdateFilter);

    // TODO - in-flight restart method

    //get starting time for update step
    //TODO-飞行中重启方法
    //获取更新步骤的开始时间
    imuSampleTime_ms = frontend->imuSampleTime_us / 1000;

    // Check arm status and perform required checks and mode changes
    //检查arm状态并执行所需的检查和模式更改
    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数据预测状态
        UpdateStrapdownEquationsNED();

        // Predict the covariance growth
        //预测协方差增长
        CovariancePrediction();

        // Update states using  magnetometer data
        //利用地磁数据更新状态变量
        SelectMagFusion();

        // Update states using GPS and altimeter data
        //使用GPS和高度计数据更新状态
        SelectVelPosFusion();

        // 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
        //更新滤波器状态
        updateFilterStatus();
    }

    // Wind output forward from the fusion to output time horizon
    //风输出前从融合到输出时间范围的
    calcOutputStates();

    // stop the timer used for load measurement
    //停止用于负载测量的计时器
    hal.util->perf_end(_perf_UpdateFilter);
#if EK2_DISABLE_INTERRUPTS
    irqrestore(istate);
#endif
}

2.使用GPS和测距仪更新EKF2的速度，位置信息

   SelectVelPosFusion();

void NavEKF2_core::SelectVelPosFusion()
{
    // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
    // If so, don't fuse measurements on this time step to reduce frame over-runs
    // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
	//检查磁强计是否已在该时间步长上融合，并且滤波器是否以高于200赫兹的速度运行
	//如果是，不要在这个时间步上融合测量值以减少帧溢出
	//只允许一次滑动以防止高速磁强计数据阻止其他测量的融合
	if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed)
    {
        posVelFusionDelayed = true;
        return;
    } else
    {
        posVelFusionDelayed = false;
    }

    // Check for data at the fusion time horizon
	//在融合时间范围内检查数据
    extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);

    // read GPS data from the sensor and check for new data in the buffer
    //从传感器读取GPS数据并检查缓冲器中是否有新数据
    readGpsData();
    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
    // Determine if we need to fuse position and velocity data on this time step
    //确定是否需要融合此时间步上的位置和速度数据
    if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE)
    {
        // set fusion request flags
    	//设置融合请求标志
        if (frontend->_fusionModeGPS <= 1)
        {
            fuseVelData = true;
        } else
        {
            fuseVelData = false;
        }
        fusePosData = true;
        extNavUsedForPos = false;

        // correct GPS data for position offset of antenna phase centre relative to the IMU
        //天线相位中心相对于IMU位置偏移的GPS校正数据
        Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
        if (!posOffsetBody.is_zero())
        {
            // Don't fuse velocity data if GPS doesn't support it
        	//如果GPS不支持，不要融合速度数据
            if (fuseVelData)
            {
                // TODO use a filtered angular rate with a group delay that matches the GPS delay
                Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
                Vector3f velOffsetBody = angRate % posOffsetBody;
                Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
                gpsDataDelayed.vel.x -= velOffsetEarth.x;
                gpsDataDelayed.vel.y -= velOffsetEarth.y;
                gpsDataDelayed.vel.z -= velOffsetEarth.z;
            }

            Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
            gpsDataDelayed.pos.x -= posOffsetEarth.x;
            gpsDataDelayed.pos.y -= posOffsetEarth.y;
            gpsDataDelayed.hgt += posOffsetEarth.z;
        }

        // copy corrected GPS data to observation vector
        //将校正后的GPS数据复制到观测矢量
        if (fuseVelData)
        {
            velPosObs[0] = gpsDataDelayed.vel.x;
            velPosObs[1] = gpsDataDelayed.vel.y;
            velPosObs[2] = gpsDataDelayed.vel.z;
        }
        velPosObs[3] = gpsDataDelayed.pos.x;
        velPosObs[4] = gpsDataDelayed.pos.y;

    } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) //外部惯导模式
    {
        // This is a special case that uses and external nav system for position
        extNavUsedForPos = true;
        activeHgtSource = HGT_SOURCE_EV;
        fuseVelData = false;
        fuseHgtData = true;
        fusePosData = true;
        velPosObs[3] = extNavDataDelayed.pos.x;
        velPosObs[4] = extNavDataDelayed.pos.y;
        velPosObs[5] = extNavDataDelayed.pos.z;

        // if compass is disabled, also use it for yaw
        //如果罗盘被禁用，也可用于偏航
        if (!use_compass())
        {
            extNavUsedForYaw = true;
            if (!yawAlignComplete)
            {
                extNavYawResetRequest = true;
                magYawResetRequest = false;
                gpsYawResetRequest = false;
                controlMagYawReset();
                finalInflightYawInit = true;
            } else
            {
                fuseEulerYaw();
            }
        } else
        {
            extNavUsedForYaw = false;
        }

    } else //否则不进行位置速度的融合
    {
        fuseVelData = false;
        fusePosData = false;
    }

    // we have GPS data to fuse and a request to align the yaw using the GPS course
    //我们有GPS数据要融合，并要求使用GPS航向校准偏航
    if (gpsYawResetRequest)
    {
        realignYawGPS();
    }

    // Select height data to be fused from the available baro, range finder and GPS sources
    //从可用的气压计、测距仪和GPS源中选择要融合的高度数据
    selectHeightForFusion();

    // if we are using GPS, check for a change in receiver and reset position and height
    //如果我们使用GPS，检查接收器是否有变化，并重置位置和高度
    if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx)
    {
        // record the ID of the GPS that we are using for the reset
    	//记录我们用于重置的GPS的ID
        last_gps_idx = gpsDataDelayed.sensor_idx;

        // Store the position before the reset so that we can record the reset delta
        //存储重置前的位置，以便我们可以记录重置增量
        posResetNE.x = stateStruct.position.x;
        posResetNE.y = stateStruct.position.y;

        // Set the position states to the position from the new GPS
        //将位置状态设置为新GPS的位置
        stateStruct.position.x = gpsDataNew.pos.x;
        stateStruct.position.y = gpsDataNew.pos.y;

        // Calculate the position offset due to the reset
        //计算复位引起的位置偏移
        posResetNE.x = stateStruct.position.x - posResetNE.x;
        posResetNE.y = stateStruct.position.y - posResetNE.y;

        // Add the offset to the output observer states
        //将偏移量添加到输出观察者状态
        for (uint8_t i=0; i<imu_buffer_length; i++)
        {
            storedOutput[i].position.x += posResetNE.x;
            storedOutput[i].position.y += posResetNE.y;
        }
        outputDataNew.position.x += posResetNE.x;
        outputDataNew.position.y += posResetNE.y;
        outputDataDelayed.position.x += posResetNE.x;
        outputDataDelayed.position.y += posResetNE.y;

        // store the time of the reset
        //存储重置时间
        lastPosReset_ms = imuSampleTime_ms;

        // If we are alseo using GPS as the height reference, reset the height
        //如果我们也使用GPS作为高度基准，重置高度
        if (activeHgtSource == HGT_SOURCE_GPS)
        {
            // Store the position before the reset so that we can record the reset delta
        	//存储重置前的位置，以便我们可以记录重置增量
        	posResetD = stateStruct.position.z;

            // write to the state vector
        	//写入状态向量
            stateStruct.position.z = -hgtMea;

            // Calculate the position jump due to the reset
            //计算复位引起的位置跳跃
            posResetD = stateStruct.position.z - posResetD;

            // Add the offset to the output observer states
            //将偏移量添加到输出观察者状态
            outputDataNew.position.z += posResetD;
            outputDataDelayed.position.z += posResetD;
            for (uint8_t i=0; i<imu_buffer_length; i++)
            {
                storedOutput[i].position.z += posResetD;
            }

            // store the time of the reset
            //存储重置时间
            lastPosResetD_ms = imuSampleTime_ms;
        }
    }

    // If we are operating without any aiding, fuse in the last known position
    // to constrain tilt drift. This assumes a non-manoeuvring vehicle
    // Do this to coincide with the height fusion
    //如果我们在没有任何帮助的情况下操作，请在最后一个已知位置熔断
    //以限制倾斜漂移。这假设是非机动车辆
    //这样做是为了配合高度融合
    if (fuseHgtData && PV_AidingMode == AID_NONE)
    {
        velPosObs[3] = lastKnownPositionNE.x;
        velPosObs[4] = lastKnownPositionNE.y;
        fusePosData = true;
        fuseVelData = false;
    }

    //执行融合数据---- perform fusion
    if (fuseVelData || fusePosData || fuseHgtData)
    {
        FuseVelPosNED();
        // clear the flags to prevent repeated fusion of the same data
        //清除标记以防止重复融合相同的数据
        fuseVelData = false;
        fuseHgtData = false;
        fusePosData = false;
    }
}

从这里我们可以看出GPS和测距仪是被用在EKF2的观测方程中，预测方程来自加速度和陀螺仪，这里我们分成两部分进行整理，高度的算法和水平位置算法

1.高度融合算法

   //从可用的气压计、测距仪和GPS源中选择要融合的高度数据
    selectHeightForFusion();

void NavEKF2_core::selectHeightForFusion()
{
    // Read range finder data and check for new data in the buffer
    // This data is used by both height and optical flow fusion processing
	//读取测距仪数据并检查缓冲区中是否有新数据
	//此数据用于高度和光流融合处理
    readRangeFinder();
    rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);

    // correct range data for the body frame position offset relative to the IMU
    // the corrected reading is the reading that would have been taken if the sensor was
    // co-located with the IMU
    //相对于IMU的坐标系位置偏移的正确范围数据
    //修正后的读数是如果传感器与IMU合用
    if (rangeDataToFuse)
    {
        AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx);
        if (sensor != nullptr)
        {
        	//修正传感器正中心
            Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
            if (!posOffsetBody.is_zero())
            {
                Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
                rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
            }
        }
    }

    // read baro height data from the sensor and check for new data in the buffer
    //从传感器读取大气压力高度数据，并检查缓冲器中是否有新数据
    readBaroData();
    baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);

    // select height source
    //选择高度资源
    if (extNavUsedForPos) //外部视觉---4
    {
        // always use external vision as the hight source if using for position.
    	//如果用于位置，始终使用外部视觉作为高光源。
        activeHgtSource = HGT_SOURCE_EV;
    } else if (((frontend->_useRngSwHgt > 0) || (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500))
    {
        if (frontend->_altSource == 1) //仿地传感器---1
        {
            // always use range finder
        	//总是使用测距仪高度
            activeHgtSource = HGT_SOURCE_RNG;
        } else
        {
            // determine if we are above or below the height switch region
        	//确定我们是否高于或低于高度开关区域5000×0.0001*
            float rangeMaxUse = 1e-4f * (float)frontend->_rng.max_distance_cm_orient(ROTATION_PITCH_270) * (float)frontend->_useRngSwHgt;
            bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
            bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;

            // If the terrain height is consistent and we are moving slowly, then it can be
            // used as a height reference in combination with a range finder
            // apply a hysteresis to the speed check to prevent rapid switching
            //如果地形高度一致并且我们移动缓慢，那么
            //与测距仪一起用作高度基准
            //对速度检查应用滞后以防止快速切换
            float horizSpeed = norm(stateStruct.velocity.x, stateStruct.velocity.y);
            bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
            float trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
            bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;

            /*
             * Switch between range finder and primary height source using height above ground and speed thresholds with
             * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
             * which cannot be assumed if the vehicle is moving horizontally.
             * *使用离地高度和速度阈值在测距仪和主高度源之间切换
             *避免快速切换的滞后。使用测距仪测量高度需要一致的地形高度
             *如果车辆在水平方向移动，则无法假设。
            */
            if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG))
            {
                // cannot trust terrain or range finder so stop using range finder height
                if (frontend->_altSource == 0)
                {
                    activeHgtSource = HGT_SOURCE_BARO;
                } else if (frontend->_altSource == 2)
                {
                    activeHgtSource = HGT_SOURCE_GPS;
                }
            } else if (belowLowerSwHgt && trustTerrain && (activeHgtSource != HGT_SOURCE_RNG))
            {
                // reliable terrain and range finder so start using range finder height
                activeHgtSource = HGT_SOURCE_RNG;
            }
        }
    } //使用GPS----2
    else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood)
    {
        activeHgtSource = HGT_SOURCE_GPS;
    } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign)
    {
        activeHgtSource = HGT_SOURCE_BCN;
    } else //默认气压计
    {
        activeHgtSource = HGT_SOURCE_BARO;
    }

    // Use Baro alt as a fallback if we lose range finder, GPS or external nav
    //如果我们失去了测距仪、全球定位系统或外部导航系统，使用气压高度作为后备
    bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && ((imuSampleTime_ms - rngValidMeaTime_ms) > 500));
    bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
    bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
    //如果数据丢失了，直接切换成气压计
    if (lostRngHgt || lostGpsHgt || lostExtNavHgt)
    {
        activeHgtSource = HGT_SOURCE_BARO;
    }

    // if there is new baro data to fuse, calculate filtered baro data required by other processes
    //如果有新的大气压力数据需要融合，则计算其他过程所需的过滤大气压力数据
    if (baroDataToFuse)
    {
        // calculate offset to baro data that enables us to switch to Baro height use during operation
    	//计算气压数据的偏移量，使我们能够在操作期间切换到气压高度使用
    	if  (activeHgtSource != HGT_SOURCE_BARO)
        {
            calcFiltBaroOffset();
        }
        // filtered baro data used to provide a reference for takeoff
        // it is is reset to last height measurement on disarming in performArmingChecks()
    	//用于提供起飞参考的过滤气压数据
    	//在执行解除防护检查时，它被重置为最后一次高度测量（）
    	if (!getTakeoffExpected())
        {
            const float gndHgtFiltTC = 0.5f;
            const float dtBaro = frontend->hgtAvg_ms*1.0e-3f;
            float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
            meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
        }
    }

    // If we are not using GPS as the primary height sensor, correct EKF origin height so that
    // combined local NED position height and origin height remains consistent with the GPS altitude
    // This also enables the GPS height to be used as a backup height source
    //如果我们不使用GPS作为主要高度传感器，请校正EKF原点高度，以便
    //本地NED位置高度和原点高度的组合与GPS高度保持一致
    //这也使得GPS高度可用作备用高度源
    if (gpsDataToFuse &&
            (((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
            ((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
            )
    {
        correctEkfOriginHeight();
    }

    // Select the height measurement source
    //选择高度测量源
    if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EV))
    {
        hgtMea = -extNavDataDelayed.pos.z;
        posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
    } //选择成仿地
    else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG))
    {
        // using range finder data
        // correct for tilt using a flat earth model
    	//使用测距仪数据
    	//使用平地模型校正倾斜
        if (prevTnb.c.z >= 0.7)
        {
            // calculate height above ground
        	//计算离地高度
            hgtMea  = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
            // correct for terrain position relative to datum
            //相对于基准的地形位置校正
            hgtMea -= terrainState;
            //使能融合---- enable fusion
            fuseHgtData = true;
            velPosObs[5] = -hgtMea;
            // set the observation noise
            //设置观测噪声
            posDownObsNoise = sq(constrain_float(frontend->_rngNoise, 0.1f, 10.0f));
            // add uncertainty created by terrain gradient and vehicle tilt
            //增加地形坡度和车辆倾斜造成的不确定性，指定使用测距仪作为高度参考时车辆下方地形的最大坡度
            posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
        } else
        {
            // disable fusion if tilted too far
        	//如果倾斜过大，禁用聚变
            fuseHgtData = false;
          //  gcs().send_text(MAV_SEVERITY_INFO, "HGT BAD");
        }
    } else if  (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS))
    {
        //使用GPS数据---- using GPS data
        hgtMea = gpsDataDelayed.hgt;
        //使能融合---- enable fusion
        velPosObs[5] = -hgtMea;
        fuseHgtData = true;
        // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
        //使用接收器报告的精度设置观测噪声，或根据典型的VDOP/HDOP比率缩放水平噪声
        if (gpsHgtAccuracy > 0.0f)
        {
            posDownObsNoise = sq(constrain_float(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
        } else
        {
            posDownObsNoise = sq(constrain_float(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
        }
    } //使用气压计
    else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO))
    {
        //使用气压计---- using Baro data
        hgtMea = baroDataDelayed.hgt - baroHgtOffset;
        //使能融合---- enable fusion
        velPosObs[5] = -hgtMea;
        fuseHgtData = true;
        //设置观察噪声---- set the observation noise
        posDownObsNoise = sq(constrain_float(frontend->_baroAltNoise, 0.1f, 10.0f));
        // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
        //如果我们可能在地面效应中，减少气压计的权重（增加观测噪声）
        if (getTakeoffExpected() || getTouchdownExpected())
        {
            posDownObsNoise *= frontend->gndEffectBaroScaler;
        }
        // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
        // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
        //如果我们处于起飞模式，高度测量限制在不小于起飞开始时的测量值
        //这可以防止在初始上升过程中，由于直升机下洗损坏了EKF高度而导致的负气压扰动
        if (motorsArmed && getTakeoffExpected())
        {
            hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
        }
    } else //否则融合高度数据失败
    {
    	//gcs().send_text(MAV_SEVERITY_INFO, "BAD FUSE");
        fuseHgtData = false;
    }

    // If we haven't fused height data for a while, then declare the height data as being timed out
    // set timeout period based on whether we have vertical GPS velocity available to constrain drift
    //如果我们有一段时间没有融合高度数据，那么将高度数据声明为超时
    //根据是否有可用的垂直GPS速度来限制漂移，设置超时时间
    hgtRetryTime_ms = (useGpsVertVel && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
    if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms)
    {
        hgtTimeout = true;
    } else
    {
        hgtTimeout = false;
    }
}

这部分重点需要注意的是：

      1.选择主要的高度观察传感器源。
      2.获取观察数据

1.主高度观察传感器

 frontend->_altSource

    // @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
    // @RebootRequired: True
    AP_GROUPINFO("ALT_SOURCE", 9, NavEKF2, _altSource, 0),

这里通过这个参数选择主要的高度传感器源，
_altSource=0：气压计
_altSource=1：测距仪
_altSource=2：GPS
_altSource=3：无线电

2.获取观察数据

velPosObs[5] = -hgtMea; //这里观察数据都取了相反的符合，为方便进行状态估计方便

其中Vector6 velPosObs; // 速度和位置组合测量组的观测（3x1 m，3x1 m/s）

2.进行高度估计

如果选择RTK高度，这里需要注意的是下面这段代码

    // if we are using GPS, check for a change in receiver and reset position and height
    //如果我们使用GPS，检查接收器是否有变化，并重置位置和高度
    if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx)
    {
        // record the ID of the GPS that we are using for the reset
    	//记录我们用于重置的GPS的ID
        last_gps_idx = gpsDataDelayed.sensor_idx;

        // Store the position before the reset so that we can record the reset delta
        //存储重置前的位置，以便我们可以记录重置增量
        posResetNE.x = stateStruct.position.x;
        posResetNE.y = stateStruct.position.y;

        // Set the position states to the position from the new GPS
        //将位置状态设置为新GPS的位置
        stateStruct.position.x = gpsDataNew.pos.x;
        stateStruct.position.y = gpsDataNew.pos.y;

        // Calculate the position offset due to the reset
        //计算复位引起的位置偏移
        posResetNE.x = stateStruct.position.x - posResetNE.x;
        posResetNE.y = stateStruct.position.y - posResetNE.y;

        // Add the offset to the output observer states
        //将偏移量添加到输出观察者状态
        for (uint8_t i=0; i<imu_buffer_length; i++)
        {
            storedOutput[i].position.x += posResetNE.x;
            storedOutput[i].position.y += posResetNE.y;
        }
        outputDataNew.position.x += posResetNE.x;
        outputDataNew.position.y += posResetNE.y;
        outputDataDelayed.position.x += posResetNE.x;
        outputDataDelayed.position.y += posResetNE.y;

        // store the time of the reset
        //存储重置时间
        lastPosReset_ms = imuSampleTime_ms;

        // If we are alseo using GPS as the height reference, reset the height
        //如果我们也使用GPS作为高度基准，重置高度
        if (activeHgtSource == HGT_SOURCE_GPS)
        {
            // Store the position before the reset so that we can record the reset delta
        	//存储重置前的位置，以便我们可以记录重置增量
        	posResetD = stateStruct.position.z;

            // write to the state vector
        	//写入状态向量
            stateStruct.position.z = -hgtMea;

            // Calculate the position jump due to the reset
            //计算复位引起的位置跳跃
            posResetD = stateStruct.position.z - posResetD;

            // Add the offset to the output observer states
            //将偏移量添加到输出观察者状态
            outputDataNew.position.z += posResetD;
            outputDataDelayed.position.z += posResetD;
            for (uint8_t i=0; i<imu_buffer_length; i++)
            {
                storedOutput[i].position.z += posResetD;
            }

            // store the time of the reset
            //存储重置时间
            lastPosResetD_ms = imuSampleTime_ms;
        }
    }

执行数据融合接口

    //执行融合数据---- perform fusion
    if (fuseVelData || fusePosData || fuseHgtData)
    {
        FuseVelPosNED();
        // clear the flags to prevent repeated fusion of the same data
        //清除标记以防止重复融合相同的数据
        fuseVelData = false;
        fuseHgtData = false;
        fusePosData = false;
    }

void NavEKF2_core::FuseVelPosNED()
{
    //开始执行时间--- start performance timer
    hal.util->perf_begin(_perf_FuseVelPosNED);

    // health is set bad until test passed
    //在测试通过之前，运行状况设置为不好
    velHealth = false;
    posHealth = false;
    hgtHealth = false;

    // declare variables used to check measurement errors
    //声明用于检查测量错误的变量
    Vector3f velInnov;

    // declare variables used to control access to arrays
    //声明用于控制对数组的访问的变量
    bool fuseData[6] = {false,false,false,false,false,false};
    uint8_t stateIndex;
    uint8_t obsIndex;

    // declare variables used by state and covariance update calculations
    //声明状态和协方差更新计算使用的变量
    Vector6 R_OBS; //测量协方差，用于融合--- Measurement variances used for fusion
    //仅用于数据检查的测量方差
    Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
    float SK;

    // perform sequential fusion of GPS measurements. This assumes that the
    // errors in the different velocity and position components are
    // uncorrelated which is not true, however in the absence of covariance
    // data from the GPS receiver it is the only assumption we can make
    // so we might as well take advantage of the computational efficiencies
    // associated with sequential fusion
    //执行GPS测量的顺序融合。这假设
    //不同速度和位置分量的误差为
    //不相关，但在没有协方差的情况下是不正确的
    //GPS接收器的数据这是我们唯一能做的假设
    //所以我们不妨利用计算效率
    //与序列融合相关
    if (fuseVelData || fusePosData || fuseHgtData)
    {

        // calculate additional error in GPS position caused by manoeuvring
    	//计算操纵引起的GPS位置附加误差
        float posErr = frontend->gpsPosVarAccScale * accNavMag;

        // estimate the GPS Velocity, GPS horiz position and height measurement variances.
        // Use different errors if operating without external aiding using an assumed position or velocity of zero
        //估计GPS速度、GPS水平位置和高程测量方差。
        //如果在没有外部辅助的情况下使用假定的零位置或零速度进行操作，则使用不同的错误
        if (PV_AidingMode == AID_NONE)
        {
            if (tiltAlignComplete && motorsArmed)
            {
                // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
            	//这是对陀螺仪误差的修正和减少操纵加速度对倾斜估计的影响之间的折衷
            	R_OBS[0] = sq(constrain_float(frontend->_noaidHorizNoise, 0.5f, 50.0f));
            } else
            {
                // Use a smaller value to give faster initial alignment
            	//使用较小的值可以更快地进行初始对齐
                R_OBS[0] = sq(0.5f);
            }
            R_OBS[1] = R_OBS[0];
            R_OBS[2] = R_OBS[0];
            R_OBS[3] = R_OBS[0];
            R_OBS[4] = R_OBS[0];
            for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
        } else
        {
            if (gpsSpdAccuracy > 0.0f)
            {
                // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
            	//使用GPS接收器报告的速度精度（如果可用）并以GPS速度噪声参数设置的值着陆
            	R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
                R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
            } else 
            {
                // calculate additional error in GPS velocity caused by manoeuvring
            	//使用GPS接收器报告的速度精度（如果可用）并以GPS速度噪声参数设置的值着陆
                R_OBS[0] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
                R_OBS[2] = sq(constrain_float(frontend->_gpsVertVelNoise,  0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale  * accNavMag);
            }
            R_OBS[1] = R_OBS[0];
            // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
            //如果可用，使用GPS报告的位置精度，并以GPS位置噪声参数设置的值为下限
            if (gpsPosAccuracy > 0.0f) 
            {
                R_OBS[3] = sq(constrain_float(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
            } else 
            {
                R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
            }
            R_OBS[4] = R_OBS[3];
            // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
            // For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
            // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
            for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
        }
        R_OBS[5] = posDownObsNoise;
        for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];

        // if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
        // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
        // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
        //如果正在使用垂直GPS速度数据和独立的高度源，请检查GPS垂直速度和高度表
        //创新有着相同的标志，是不受限制的。如果是这样，那么它很可能会影响
        //加速计和我们应该禁用GPS和气压计创新一致性检查。
        if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2))
        {
            // calculate innovations for height and vertical GPS vel measurements
        	//计算高程和垂直GPS高程测量的创新
            float hgtErr  = stateStruct.position.z - velPosObs[5];
            float velDErr = stateStruct.velocity.z - velPosObs[2];
            // check if they are the same sign and both more than 3-sigma out of bounds
            //检查它们是否是同一个符号并且都超过3西格玛
            if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2])))
            {
                badIMUdata = true;
            } else
            {
                badIMUdata = false;
            }
        }

        // calculate innovations and check GPS data validity using an innovation consistency check
        // test position measurements
        //使用创新一致性检查计算创新并检查GPS数据的有效性
        //测试位置测量
        if (fusePosData)
        {
            // test horizontal position measurements
        	//测试水平位置测量,这里得到预测值与测量值的差值，也就是新息值
            innovVelPos[3] = stateStruct.position.x - velPosObs[3];
            innovVelPos[4] = stateStruct.position.y - velPosObs[4];
            //获取新息协方差
            varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
            varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
            // apply an innovation consistency threshold test, but don't fail if bad IMU data
            //应用创新一致性阈值测试，但如果IMU数据不好，则不要失败
            float maxPosInnov2 = sq(MAX(0.01f * (float)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
            posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
            posHealth = ((posTestRatio < 1.0f) || badIMUdata);
            // use position data if healthy or timed out
            //如果正常或超时，则使用位置数据
            if (PV_AidingMode == AID_NONE) 
            {
                posHealth = true;
                lastPosPassTime_ms = imuSampleTime_ms;
            } else if (posHealth || posTimeout) 
            {
                posHealth = true;
                lastPosPassTime_ms = imuSampleTime_ms;
                // if timed out or outside the specified uncertainty radius, reset to the GPS
                //如果超时或超出指定的不确定半径，则重置为GPS
                if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) 
                {
                    // reset the position to the current GPS position
                	//复位位置到当前位置
                    ResetPosition();
                    // reset the velocity to the GPS velocity
                    //复位速度到当前速度
                    ResetVelocity();
                    // don't fuse GPS data on this time step
                    //不要在这个时间步上融合GPS数据
                    fusePosData = false;
                    fuseVelData = false;
                    // Reset the position variances and corresponding covariances to a value that will pass the checks
                    //将位置方差和相应的方差重置为将通过检查的值
                    zeroRows(P,6,7);
                    zeroCols(P,6,7);
                    P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
                    P[7][7] = P[6][6];
                    // Reset the normalised innovation to avoid failing the bad fusion tests
                    //重新设置标准化的创新以避免失败的融合测试
                    posTestRatio = 0.0f;
                    velTestRatio = 0.0f;
                }
            } else 
            {
                posHealth = false;
            }
        }

        //测试融合速度---- test velocity measurements
        if (fuseVelData)
        {
            //测试速度测量--- test velocity measurements
            uint8_t imax = 2;
            // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
            //如果用户禁止或我们使用合成数据，不要融合垂直速度观测值
            if (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE || frontend->inhibitGpsVertVelUse) 
            {
                imax = 1;
            }
            // sum of squares of velocity innovations
            //速度创新平方和
            float innovVelSumSq = 0; 
            // sum of velocity innovation variances
            //速度创新方差之和
            float varVelSum = 0; 
            for (uint8_t i = 0; i<=imax; i++) 
            {
                // velocity states start at index 3
            	//速度状态从索引3开始
                stateIndex   = i + 3;
                // calculate innovations using blended and single IMU predicted states
                //使用混合和单一IMU预测状态计算创新
                velInnov[i]  = stateStruct.velocity[i] - velPosObs[i]; // blended
                // calculate innovation variance
                //计算更新方差
                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
                // sum the innovation and innovation variances
                //总的新息和新息协方差
                innovVelSumSq += sq(velInnov[i]);
                varVelSum += varInnovVelPos[i];
            }
            // apply an innovation consistency threshold test, but don't fail if bad IMU data
            // calculate the test ratio
            //应用创新一致性阈值测试，但如果IMU数据不好，则不要失败
            //计算测试比率
            velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (float)frontend->_gpsVelInnovGate, 1.0f)));
            // fail if the ratio is greater than 1
            //如果比率大于1，则失败
            velHealth = ((velTestRatio < 1.0f)  || badIMUdata);
            // use velocity data if healthy, timed out, or in constant position mode
            //如果正常、超时或处于恒定位置模式，则使用速度数据
            if (velHealth || velTimeout) 
            {
                velHealth = true;
                //重新开始超时计数--- restart the timeout count
                lastVelPassTime_ms = imuSampleTime_ms;
                // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
                //如果我们做全辅助和速度融合超时，重置为GPS速度
                if (PV_AidingMode == AID_ABSOLUTE && velTimeout) 
                {
                    //复位速度好GPS速度--- reset the velocity to the GPS velocity
                    ResetVelocity();
                    // don't fuse GPS velocity data on this time step
                    //不使用GPS速度数据在这个时间步调上
                    fuseVelData = false;
                    // Reset the normalised innovation to avoid failing the bad fusion tests
                    //重新设置标准化的创新以避免失败的融合测试
                    velTestRatio = 0.0f;
                }
            } else 
            {
                velHealth = false;
            }
        }

        //测试融合高度---- test height measurements
        if (fuseHgtData)
        {
            //计算高度更新----- calculate height innovations
            innovVelPos[5] = stateStruct.position.z - velPosObs[5];
            varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
            // calculate the innovation consistency test ratio
            //计算创新一致性测试比率
            hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (float)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
            // fail if the ratio is > 1, but don't fail if bad IMU data
            //如果比率大于1，则失败，但如果IMU数据不正确，则不失败
            hgtHealth = ((hgtTestRatio < 1.0f) || badIMUdata);
            // Fuse height data if healthy or timed out or in constant position mode
            //保险丝高度数据（如果正常、超时或处于恒定位置模式）
            if (hgtHealth || hgtTimeout || (PV_AidingMode == AID_NONE && onGround))
            {
                // Calculate a filtered value to be used by pre-flight health checks
                // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
            	//计算飞行前健康检查使用的过滤值
            	//我们需要滤波，因为阵风会产生显著的气压噪声，我们希望能够在惯性解中检测到偏差误差
            	if (onGround)
                {
                    float dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
                    const float hgtInnovFiltTC = 2.0f;
                    float alpha = constrain_float(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
                    //用于飞行前检查的高度创新装置安装的状态
                    hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
                } else
                {
                    hgtInnovFiltState = 0.0f;
                }

                //如果超时，复位高度--- if timed out, reset the height
                if (hgtTimeout)
                {
                    ResetHeight();
                }

                // If we have got this far then declare the height data as healthy and reset the timeout counter
                hgtHealth = true;
                lastHgtPassTime_ms = imuSampleTime_ms;
            }
        }

        // set range for sequential fusion of velocity and position measurements depending on which data is available and its health
        //根据可用数据及其健康状况，设置速度和位置测量的顺序融合范围
        if (fuseVelData && velHealth)
        {
            fuseData[0] = true;
            fuseData[1] = true;
            if (useGpsVertVel) {
                fuseData[2] = true;
            }
            tiltErrVec.zero();
        }
        if (fusePosData && posHealth) {
            fuseData[3] = true;
            fuseData[4] = true;
            tiltErrVec.zero();
        }
        if (fuseHgtData && hgtHealth) {
            fuseData[5] = true;
        }

        // fuse measurements sequentially
        //依次进行保险丝测量
        for (obsIndex=0; obsIndex<=5; obsIndex++)
        {
            if (fuseData[obsIndex])
            {
                stateIndex = 3 + obsIndex;
                // calculate the measurement innovation, using states from a different time coordinate if fusing height data
                // adjust scaling on GPS measurement noise variances if not enough satellites
                //如果融合高度数据，使用来自不同时间坐标的状态计算测量创新
                //如果卫星不足，调整GPS测量噪声方差的标度
                if (obsIndex <= 2)
                {
                    innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
                    R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                }
                else if (obsIndex == 3 || obsIndex == 4)
                {
                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
                    R_OBS[obsIndex] *= sq(gpsNoiseScaler);
                } else if (obsIndex == 5)
                {
                	//进行更新
                    innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
                    const float gndMaxBaroErr = 4.0f;
                    const float gndBaroInnovFloor = -0.5f;

                    if(getTouchdownExpected() && activeHgtSource == HGT_SOURCE_BARO)
                    {
                    	//当预计着陆时，在gndBaroInnovFloor启动气压计创新
                    	//将校正限制在0和gndBaroInnovFloor+gndMaxBaroErr之间
                    	//此函数如下所示：
                        // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
                        // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
                        // this function looks like this:
                        //         |/
                        //---------|---------
                        //    ____/|
                        //   /     |
                        //  /      |
                        innovVelPos[5] += constrain_float(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
                    }
                }

                // calculate the Kalman gain and calculate innovation variances
                //计算Kalman增益并计算创新方差
                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
                SK = 1.0f/varInnovVelPos[obsIndex];
                //计算kg
                for (uint8_t i= 0; i<=15; i++)
                {
                    Kfusion[i] = P[i][stateIndex]*SK;
                }

                // inhibit magnetic field state estimation by setting Kalman gains to zero
                //卡尔曼增益为零抑制磁场状态估计
                if (!inhibitMagStates)
                {
                    for (uint8_t i = 16; i<=21; i++)
                    {
                        Kfusion[i] = P[i][stateIndex]*SK;
                    }
                } else
                {
                    for (uint8_t i = 16; i<=21; i++)
                    {
                        Kfusion[i] = 0.0f;
                    }
                }

                // inhibit wind state estimation by setting Kalman gains to zero
                //将Kalman增益设为零抑制风况估计
                if (!inhibitWindStates)
                {
                    Kfusion[22] = P[22][stateIndex]*SK;
                    Kfusion[23] = P[23][stateIndex]*SK;
                } else
                {
                    Kfusion[22] = 0.0f;
                    Kfusion[23] = 0.0f;
                }

                // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
                // this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
                //更新协方差-利用index=stateIndex处单个状态的直接观测来减少计算
                //这是标准方程P=（I-K*H）*P的数值优化实现；
                for (uint8_t i= 0; i<=stateIndexLim; i++)
                {
                    for (uint8_t j= 0; j<=stateIndexLim; j++)
                    {
                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
                    }
                }
                // Check that we are not going to drive any variances negative and skip the update if so
                //检查我们是否不会将任何差异变为负值，如果是，则跳过更新
                bool healthyFusion = true;
                for (uint8_t i= 0; i<=stateIndexLim; i++)
                {
                    if (KHP[i][i] > P[i][i])
                    {
                        healthyFusion = false;
                    }
                }
                if (healthyFusion)
                {
                    //更新协方差---- update the covariance matrix
                    for (uint8_t i= 0; i<=stateIndexLim; i++)
                    {
                        for (uint8_t j= 0; j<=stateIndexLim; j++)
                        {
                            P[i][j] = P[i][j] - KHP[i][j];
                        }
                    }

                    // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
                    //强制协方差矩阵是对称的，并限制方差以防止病态。
                    ForceSymmetry();
                    ConstrainVariances();

                    // update the states
                    // zero the attitude error state - by definition it is assumed to be zero before each observaton fusion
                    //更新状态
                    //将姿态误差状态归零-根据定义，在每次观测融合之前假设为零
                    stateStruct.angErr.zero();

                    // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
                    //计算状态修正，并对使用混合IMU数据预测的状态的四元数重新标准化
                    for (uint8_t i = 0; i<=stateIndexLim; i++)
                    {
                    	//获取评估数据
                        statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
                    }

                    // the first 3 states represent the angular misalignment vector. This is
                    // is used to correct the estimated quaternion
                    //前3个状态表示角度偏差矢量。这是
                    //用于更正估计的四元数
                    stateStruct.quat.rotate(stateStruct.angErr);

                    // sum the attitude error from velocity and position fusion only
                    // used as a metric for convergence monitoring
                    //仅从速度和位置融合中求和姿态误差
                    //用作收敛性监视的度量
                    if (obsIndex != 5)
                    {
                        tiltErrVec += stateStruct.angErr;
                    }
                    //记录好的融合状态--- record good fusion status
                    if (obsIndex == 0)
                    {
                        faultStatus.bad_nvel = false;
                    } else if (obsIndex == 1) {
                        faultStatus.bad_evel = false;
                    } else if (obsIndex == 2) {
                        faultStatus.bad_dvel = false;
                    } else if (obsIndex == 3) {
                        faultStatus.bad_npos = false;
                    } else if (obsIndex == 4) {
                        faultStatus.bad_epos = false;
                    } else if (obsIndex == 5) {
                        faultStatus.bad_dpos = false;
                    }
                } else 
                {
                    //记录坏的融合状态---- record bad fusion status
                    if (obsIndex == 0) {
                        faultStatus.bad_nvel = true;
                    } else if (obsIndex == 1) {
                        faultStatus.bad_evel = true;
                    } else if (obsIndex == 2) {
                        faultStatus.bad_dvel = true;
                    } else if (obsIndex == 3) {
                        faultStatus.bad_npos = true;
                    } else if (obsIndex == 4) {
                        faultStatus.bad_epos = true;
                    } else if (obsIndex == 5) {
                        faultStatus.bad_dpos = true;
                    }
                }
            }
        }
    }

    //结束时间---- stop performance timer
    hal.util->perf_end(_perf_FuseVelPosNED);
}

这里有个地方需要注意，垂直方向上的观察速度来源

        // copy corrected GPS data to observation vector
        //将校正后的GPS数据复制到观测矢量
        if (fuseVelData)
        {
            velPosObs[0] = gpsDataDelayed.vel.x;
            velPosObs[1] = gpsDataDelayed.vel.y;
            velPosObs[2] = gpsDataDelayed.vel.z;
        }
        velPosObs[3] = gpsDataDelayed.pos.x;
        velPosObs[4] = gpsDataDelayed.pos.y;

水平方向的融合跟垂直方向基本相似，这里暂不讲述