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

1.controlFilterModes()

controlFilterModes() 是 NavEKF2_core 类中的一个关键函数，用于控制和管理扩展卡尔曼滤波器（EKF）的不同工作模式。该函数在 UpdateFilter 方法中被调用，确保滤波器根据系统状态（如飞行状态、电机状态等）动态调整其行为，以优化状态估计的准确性和可靠性。

//函数检查点击解锁和必要的检查和模式改变;
void NavEKF2_core::controlFilterModes()
{// Determine motor arm status确定电机状态//状态跟踪：保存之前的电机状态 (prevMotorsArmed) 并获取当前的电机状态 (motorsArmed)。prevMotorsArmed = motorsArmed;//检测电机启动：如果当前电机已启动且之前未启动，记录启动时间 (timeAtArming_ms)。motorsArmed = dal.get_armed();if (motorsArmed && !prevMotorsArmed) {// set the time at which we arm to assist with checkstimeAtArming_ms =  imuSampleTime_ms;}// Detect if we are in flight on or ground//飞行检测：通过分析传感器数据（如加速度、速度、GPS 数据等）确定系统当前是处于飞行状态还是地面状态。detectFlight();// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to// avoid unnecessary operations//学习模式：决定是否启用对风速和磁场的学习setWindMagStateLearningMode();// Check the alignmnent status of the tilt and yaw attitude// Used during initial bootstrap alignment of the filter//姿态对齐：检查滤波器的姿态（俯仰、滚转、偏航）对齐是否完成，尤其是在初始启动或重启后。checkAttitudeAlignmentStatus();// Set the type of inertial navigation aiding used//辅助模式：根据当前的系统状态和传感器数据，设置滤波器使用的惯性导航辅助类型（如GPS、视觉、光流等）。setAidingMode();}

2.readIMUData() 

readIMUData() 函数在 NavEKF2_core 类中负责读取惯性测量单元（IMU）的增量角度和增量速度数据，并将其下采样到目标频率（例如 100Hz），以便存储在 EKF 使用的数据缓冲区中。该函数确保滤波器接收到的 IMU 数据既准确又适时，避免由于高频数据带来的计算负担，同时确保数据的质量不受下采样方法引入的锥形（coning）或滑动（sculling）误差的影响。

void NavEKF2_core::readIMUData()
{//获取 IMU 相关对象const auto &ins = dal.ins();// average IMU sampling rate//计算平均 IMU 采样时间dtIMUavg = ins.get_loop_delta_t();// use the nominated imu or primary if not available//选择活跃的加速度计和陀螺仪uint8_t accel_active, gyro_active;if (ins.use_accel(imu_index)) {accel_active = imu_index;} else {accel_active = ins.get_primary_accel();}if (ins.use_gyro(imu_index)) {gyro_active = imu_index;} else {gyro_active = ins.get_primary_gyro();}//处理陀螺仪的切换if (gyro_active != gyro_index_active) {// we are switching active gyro at runtime. Copy over the// biases we have learned from the previously inactive// gyro. We don't re-init the bias uncertainty as it should// have the same uncertainty as the previously active gyro切换活跃陀螺仪时，复制之前不活跃陀螺仪的偏置stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;gyro_index_active = gyro_active;// use the gyro scale factor we have previously used on this// IMU (if any). We don't reset the variances as we don't want// errors after switching to be mis-assigned to the gyro scale// factor使用之前为该 IMU 使用的陀螺仪比例因子stateStruct.gyro_scale = inactiveBias[gyro_active].gyro_scale;}//处理加速度计的切换if (accel_active != accel_index_active) {// switch to the learned accel bias for this IMUstateStruct.accel_zbias = inactiveBias[accel_active].accel_zbias;accel_index_active = accel_active;}// update the inactive bias states// 更新不活跃的偏置状态learnInactiveBiases();//读取增量速度和加速度数据readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();imuDataNew.accel_index = accel_index_active;// Get delta angle data from primary gyro or primary if not availablereadDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);imuDataNew.gyro_index = gyro_index_active;// Get current time stamp//获取当前时间戳imuDataNew.time_ms = imuSampleTime_ms;// use the most recent IMU index for the downsampled IMU// data. This isn't strictly correct if we switch IMUs between// samplesimuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;imuDataDownSampledNew.accel_index = imuDataNew.accel_index;// Accumulate the measurement time interval for the delta velocity and angle dataimuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;// Rotate quaternon atitude from previous to new and normalise.// Accumulation using quaternions prevents introduction of coning errors due to downsampling//处理四元数旋转imuQuatDownSampleNew.rotate(imuDataNew.delAng);imuQuatDownSampleNew.normalize();// Rotate the latest delta velocity into body frame at the start of accumulation//将增量速度旋转到机体坐标系Matrix3F deltaRotMat;imuQuatDownSampleNew.rotation_matrix(deltaRotMat);// Apply the delta velocity to the delta velocity accumulatorimuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;// Keep track of the number of IMU frames since the last state predictionframesSincePredict++;/** If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,* then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more* than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the* IMU data.*/if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) &&startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {// convert the accumulated quaternion to an equivalent delta angleimuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);// Time stamp the dataimuDataDownSampledNew.time_ms = imuSampleTime_ms;// Write data to the FIFO IMU bufferstoredIMU.push_youngest_element(imuDataDownSampledNew);// calculate the achieved average time step rate for the EKFftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT);dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;// zero the accumulated IMU data and quaternionimuDataDownSampledNew.delAng.zero();imuDataDownSampledNew.delVel.zero();imuDataDownSampledNew.delAngDT = 0.0f;imuDataDownSampledNew.delVelDT = 0.0f;imuDataDownSampledNew.gyro_index = gyro_index_active;imuDataDownSampledNew.accel_index = accel_index_active;imuQuatDownSampleNew[0] = 1.0f;imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycleframesSincePredict = 0;// set the flag to let the filter know it has new IMU data and needs to runrunUpdates = true;// extract the oldest available data from the FIFO bufferimuDataDelayed = storedIMU.get_oldest_element();// protect against delta time going to zero// TODO - check if calculations can tolerate 0ftype minDT = 0.1f*dtEkfAvg;imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);updateTimingStatistics();// correct the extracted IMU data for sensor errorsdelAngCorrected = imuDataDelayed.delAng;delVelCorrected = imuDataDelayed.delVel;correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);} else {// we don't have new IMU data in the buffer so don't run filter updates on this time steprunUpdates = false;}
}

步骤解析：

  1. 四元数转轴角：将累积的四元数转换为等效的增量角度，以便存储和后续使用。
  2. 时间戳：为下采样数据记录当前时间戳。
  3. 存储到 FIFO 缓冲区：将下采样的 IMU 数据存储到 FIFO 缓冲区，供 EKF 使用。
  4. 计算 EKF 平均时间步：根据当前时间步更新 EKF 的平均时间步，以适应实际的采样频率。
  5. 重置累积数据和四元数：清零累积的增量角度和增量速度，以及重置四元数为单位四元数。
  6. 重置帧计数器：将自上次预测以来的 IMU 帧数重置为零，准备开始新的预测周期。
  7. 设置更新标志：设置 runUpdates 标志，指示 EKF 有新的 IMU 数据需要处理。
  8. 提取 FIFO 缓冲区中的最旧数据：获取最旧的 IMU 数据，确保数据按顺序处理。
  9. 保护最小时间步：确保增量时间步不会小于设定的最小值，避免时间步为零导致的计算问题。
  10. 更新时间统计：更新滤波器的时间统计信息，以监控滤波器的性能。
  11. 校正传感器误差：对提取的 IMU 数据进行传感器误差校正，包括角度和速度的校正。

3.UpdateStrapdownEquationsNED() 

UpdateStrapdownEquationsNED() 函数在 NavEKF2_core 类中扮演着关键角色，负责使用惯性测量单元（IMU）提供的增量角度和增量速度数据，更新滤波器的姿态、速度和位置状态。这一过程涉及从机体坐标系到导航坐标系（NED，北、东、地）的转换，并考虑地球旋转的影响 

void NavEKF2_core::UpdateStrapdownEquationsNED()
{// update the quaternion states by rotating from the previous attitude through// the delta angle rotation quaternion and normalise// apply correction for earth's rotation rate// % * - and + operators have been overloaded//通过以前的姿态旋转更新四元数，并归一化应用于修正地球旋转速度矢量，后面运算符进行了重载stateStruct.quat.rotate(delAngCorrected - prevTnb * earthRateNED*imuDataDelayed.delAngDT);得到四元数矩阵,考虑地球旋转stateStruct.quat.normalize();//进行归一化处理// transform body delta velocities to delta velocities in the nav frame--速度矢量到导航坐标系// use the nav frame from previous time step as the delta velocities// have been rotated into that frame// * and + operators have been overloadedVector3F delVelNav;  // delta velocity vector in earth axesdelVelNav  = prevTnb.mul_transpose(delVelCorrected);delVelNav.z += GRAVITY_MSS*imuDataDelayed.delVelDT;//去掉重力影响// 计算机体到导航余弦矩阵calculate the nav to body cosine matrixstateStruct.quat.inverse().rotation_matrix(prevTnb);// 算速度变化率（用于发射探测和其他功能）calculate the rate of change of velocity (used for launch detect and other functions)velDotNED = delVelNav / imuDataDelayed.delVelDT;// 进行低通滤波处理-apply a first order lowpass filtervelDotNEDfilt = velDotNED * 0.05f + velDotNEDfilt * 0.95f;// calculate a magnitude of the filtered nav acceleration (required for GPS// variance estimation)//计算滤波后的导航加速度的幅度（GPS方差估计会用到这个值）accNavMag = velDotNEDfilt.length();accNavMagHoriz = norm(velDotNEDfilt.x , velDotNEDfilt.y);// if we are not aiding, then limit the horizontal magnitude of acceleration//如果我们不辅助，那么限制加速度的水平大小。防止大机动瞬变干扰姿态// to prevent large manoeuvre transients disturbing the attitudeif ((PV_AidingMode == AID_NONE) && (accNavMagHoriz > 5.0f)) {ftype gain = 5.0f/accNavMagHoriz;delVelNav.x *= gain;delVelNav.y *= gain;}// 保存速度用于梯形下次积分的位置计算save velocity for use in trapezoidal integration for position calcuationVector3F lastVelocity = stateStruct.velocity;// 计算速度sum delta velocities to get velocitystateStruct.velocity += delVelNav;// 计算位置信息apply a trapezoidal integration to velocities to calculate positionstateStruct.position += (stateStruct.velocity + lastVelocity) * (imuDataDelayed.delVelDT*0.5f);// 测量到达到达上次复位后的偏置δ角和时间accumulate the bias delta angle and time since last reset by an OF measurement arrivaldelAngBodyOF += delAngCorrected;delTimeOF += imuDataDelayed.delAngDT;// 限制状态变量，防止发散limit states to protect against divergenceConstrainStates();
}

UpdateStrapdownEquationsNED() 函数在 EKF 的状态预测过程中起到了至关重要的作用，具体体现在以下几个方面：

  • 姿态更新：通过增量角度数据更新姿态四元数，并考虑地球自转的影响，确保姿态状态的准确性。
  • 速度与位置更新：将机体坐标系下的增量速度转换到导航坐标系，累加到速度状态，并通过梯形积分法更新位置状态，确保速度和位置的准确预测。
  • 噪声与偏置处理：通过低通滤波和偏置累积，减少噪声和系统误差对状态预测的影响，提升滤波器的精度和鲁棒性。
  • 状态约束：限制加速度幅度和约束状态变量，防止滤波器状态因异常输入或数值误差而发散，确保滤波器的稳定性。

通过这些步骤，UpdateStrapdownEquationsNED() 函数有效地实现了 EKF 的状态预测部分，为后续的状态更新和卡尔曼增益计算奠定了坚实的基础。

1. 姿态（Attitude）

  • 四元数（Quaternion）：
    • stateStruct.quat：表示姿态的四元数，包含4个分量（q0, q1, q2, q3）。

2. 位置（Position）

  • 三维位置向量：
    • stateStruct.position：表示导航坐标系（NED，北、东、地）中的位置，包含3个分量（北、东、地）。

3. 速度（Velocity）

  • 三维速度向量：
    • stateStruct.velocity：表示导航坐标系中的速度，包含3个分量（VN, VE, VD）。

4. 传感器偏置（Sensor Biases）

  • 陀螺仪偏置（Gyro Bias）：
    • stateStruct.gyro_bias：陀螺仪在X、Y、Z轴的偏置，包含3个分量。
  • 加速度计偏置（Accel Bias）：
    • stateStruct.accel_zbias：加速度计在Z轴的偏置，代码中仅显示Z轴，但通常加速度计偏置在X、Y、Z轴均有，包含3个分量。
  • 陀螺仪比例因子（Gyro Scale Factor）：
    • stateStruct.gyro_scale：陀螺仪在X、Y、Z轴的比例因子，包含3个分量。

5. 地球自转率的影响（Earth Rate Correction）

  • 地球自转率矢量：
    • earthRateNED：表示地球自转率在导航坐标系中的分量，通常包含3个分量，但作为常量存在，不一定作为状态变量。

6. 其他状态变量

  • 速度变化率（Rate of Change of Velocity）：
    • velDotNED 和 velDotNEDfilt：虽然这些是滤波过程中的中间变量，但它们可以视为对系统状态的估计。
  • 导航加速度幅度（Navigation Acceleration Magnitude）：
    • accNavMag 和 accNavMagHoriz：用于辅助判断和滤波器调整，不直接作为状态变量，但影响滤波器的动态调整。
  • 偏置增量角度和时间（Bias Delta Angle and Time）：
    • delAngBodyOF 和 delTimeOF：用于累积偏置和时间，辅助偏置估计。

4.CovariancePrediction() 

void NavEKF2_core::CovariancePrediction()
{ftype windVelSigma; // wind velocity 1-sigma process noise - m/sftype dAngBiasSigma;// delta angle bias 1-sigma process noise - rad/sftype dVelBiasSigma;// delta velocity bias 1-sigma process noise - m/sftype dAngScaleSigma;// delta angle scale factor 1-Sigma process noiseftype magEarthSigma;// earth magnetic field 1-sigma process noiseftype magBodySigma; // body magnetic field 1-sigma process noiseftype daxNoise;     // X axis delta angle noise variance rad^2ftype dayNoise;     // Y axis delta angle noise variance rad^2ftype dazNoise;     // Z axis delta angle noise variance rad^2ftype dvxNoise;     // X axis delta velocity variance noise (m/s)^2ftype dvyNoise;     // Y axis delta velocity variance noise (m/s)^2ftype dvzNoise;     // Z axis delta velocity variance noise (m/s)^2ftype dvx;          // X axis delta velocity (m/s)ftype dvy;          // Y axis delta velocity (m/s)ftype dvz;          // Z axis delta velocity (m/s)ftype dax;          // X axis delta angle (rad)ftype day;          // Y axis delta angle (rad)ftype daz;          // Z axis delta angle (rad)ftype q0;           // attitude quaternionftype q1;           // attitude quaternionftype q2;           // attitude quaternionftype q3;           // attitude quaternionftype dax_b;        // X axis delta angle measurement bias (rad)ftype day_b;        // Y axis delta angle measurement bias (rad)ftype daz_b;        // Z axis delta angle measurement bias (rad)ftype dax_s;        // X axis delta angle measurement scale factorftype day_s;        // Y axis delta angle measurement scale factorftype daz_s;        // Z axis delta angle measurement scale factorftype dvz_b;        // Z axis delta velocity measurement bias (rad)Vector25 SF;Vector5 SG;Vector8 SQ;Vector24 processNoise;// calculate covariance prediction process noise// use filtered height rate to increase wind process noise when climbing or descending// this allows for wind gradient effects.// filter height rate using a 10 second time constant filter//计算高度速率的滤波值，使用了10秒的时间常数滤波器。dt = imuDataDelayed.delAngDT;ftype alpha = 0.1f * dt;hgtRate = hgtRate * (1.0f - alpha) - stateStruct.velocity.z * alpha;// use filtered height rate to increase wind process noise when climbing or descending// this allows for wind gradient effects.//计算不同类型的过程噪声，包括风速噪声、角度偏置噪声、速度偏置噪声、角度比例因子噪声和磁场噪声。windVelSigma  = dt * constrain_ftype(frontend->_windVelProcessNoise, 0.0f, 1.0f) * (1.0f + constrain_ftype(frontend->_wndVarHgtRateScale, 0.0f, 1.0f) * fabsF(hgtRate));dAngBiasSigma = sq(dt) * constrain_ftype(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);dVelBiasSigma = sq(dt) * constrain_ftype(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);dAngScaleSigma = dt * constrain_ftype(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f);magEarthSigma = dt * constrain_ftype(frontend->_magEarthProcessNoise, 0.0f, 1.0f);magBodySigma  = dt * constrain_ftype(frontend->_magBodyProcessNoise, 0.0f, 1.0f);//将计算出的过程噪声分配到 processNoise 数组的不同索引中。for (uint8_t i= 0; i<=8;  i++) processNoise[i] = 0.0f;for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;if (dal.get_takeoff_expected()) {processNoise[15] = 0.0f;} else {processNoise[15] = dVelBiasSigma;}for (uint8_t i=16; i<=18; i++) processNoise[i] = magEarthSigma;for (uint8_t i=19; i<=21; i++) processNoise[i] = magBodySigma;for (uint8_t i=22; i<=23; i++) processNoise[i] = windVelSigma;//将过程噪声进行平方化处理，以便在后续协方差计算中使用。for (uint8_t i= 0; i<=stateIndexLim; i++) processNoise[i] = sq(processNoise[i]);// set variables used to calculate covariance growth//更新与速度、角度、四元数、偏置和比例因子相关的状态变量。这些变量会在协方差预测中使用。dvx = imuDataDelayed.delVel.x;dvy = imuDataDelayed.delVel.y;dvz = imuDataDelayed.delVel.z;dax = imuDataDelayed.delAng.x;day = imuDataDelayed.delAng.y;daz = imuDataDelayed.delAng.z;q0 = stateStruct.quat[0];q1 = stateStruct.quat[1];q2 = stateStruct.quat[2];q3 = stateStruct.quat[3];dax_b = stateStruct.gyro_bias.x;day_b = stateStruct.gyro_bias.y;daz_b = stateStruct.gyro_bias.z;dax_s = stateStruct.gyro_scale.x;day_s = stateStruct.gyro_scale.y;daz_s = stateStruct.gyro_scale.z;dvz_b = stateStruct.accel_zbias;//计算角速度噪声和加速度噪声，确保噪声值在合理范围内并进行平方处理。ftype _gyrNoise = constrain_ftype(frontend->_gyrNoise, 0.0f, 1.0f);daxNoise = dayNoise = dazNoise = sq(dt*_gyrNoise);ftype _accNoise = constrain_ftype(frontend->_accNoise, 0.0f, 10.0f);dvxNoise = dvyNoise = dvzNoise = sq(dt*_accNoise);// calculate the predicted covariance due to inertial sensor error propagation// we calculate the upper diagonal and copy to take advantage of symmetry//计算由于惯性传感器误差传播引起的预测协方差。//计算SF（协方差传播状态向量）计算一个长度为 24 的数组 SF，其中存储了状态变量（如角度偏置、四元数）对协方差传播的贡献。利用了四元数的特性来帮助计算。SF[0] = daz_b/2 - (daz*daz_s)/2;SF[1] = day_b/2 - (day*day_s)/2;SF[2] = dax_b/2 - (dax*dax_s)/2;SF[3] = q3/2 - (q0*SF[0])/2 + (q1*SF[1])/2 - (q2*SF[2])/2;SF[4] = q0/2 - (q1*SF[2])/2 - (q2*SF[1])/2 + (q3*SF[0])/2;SF[5] = q1/2 + (q0*SF[2])/2 - (q2*SF[0])/2 - (q3*SF[1])/2;SF[6] = q3/2 + (q0*SF[0])/2 - (q1*SF[1])/2 - (q2*SF[2])/2;SF[7] = q0/2 - (q1*SF[2])/2 + (q2*SF[1])/2 - (q3*SF[0])/2;SF[8] = q0/2 + (q1*SF[2])/2 - (q2*SF[1])/2 - (q3*SF[0])/2;SF[9] = q2/2 + (q0*SF[1])/2 + (q1*SF[0])/2 + (q3*SF[2])/2;SF[10] = q2/2 - (q0*SF[1])/2 - (q1*SF[0])/2 + (q3*SF[2])/2;SF[11] = q2/2 + (q0*SF[1])/2 - (q1*SF[0])/2 - (q3*SF[2])/2;SF[12] = q1/2 + (q0*SF[2])/2 + (q2*SF[0])/2 + (q3*SF[1])/2;SF[13] = q1/2 - (q0*SF[2])/2 + (q2*SF[0])/2 - (q3*SF[1])/2;SF[14] = q3/2 + (q0*SF[0])/2 + (q1*SF[1])/2 + (q2*SF[2])/2;SF[15] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);SF[16] = dvz_b - dvz;SF[17] = dvx;SF[18] = dvy;SF[19] = sq(q2);SF[20] = SF[19] - sq(q0) + sq(q1) - sq(q3);SF[21] = SF[19] + sq(q0) - sq(q1) - sq(q3);SF[22] = 2*q0*q1 - 2*q2*q3;SF[23] = SF[19] - sq(q0) - sq(q1) + sq(q3);SF[24] = 2*q1*q2;//计算SG（状态误差）计算一个长度为 5 的数组 SG，用于存储与四元数相关的平方值，这些值将用于后续计算协方差。SG[0] = - sq(q0) - sq(q1) - sq(q2) - sq(q3);SG[1] = sq(q3);SG[2] = sq(q2);SG[3] = sq(q1);SG[4] = sq(q0);//计算SQ（状态噪声）计算一个长度为 8 的数组 SQ，用于存储不同噪声项（如速度噪声）对协方差传播的贡献。SQ[0] = - dvyNoise*(2*q0*q1 + 2*q2*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);SQ[1] = dvxNoise*(2*q0*q2 - 2*q1*q3)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvzNoise*(2*q0*q2 + 2*q1*q3)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);SQ[2] = dvyNoise*(2*q0*q3 - 2*q1*q2)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxNoise*(2*q0*q3 + 2*q1*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);SQ[3] = sq(SG[0]);SQ[4] = 2*q2*q3;SQ[5] = 2*q1*q3;SQ[6] = 2*q1*q2;SQ[7] = SG[4];//计算SPP（预测协方差）计算一个长度为 23 的向量 SPP，用于存储最终的预测协方差。这些计算结合了 SF、SQ 和四元数的属性，以便于更新状态估计。Vector23 SPP;SPP[0] = SF[17]*(2*q0*q1 + 2*q2*q3) + SF[18]*(2*q0*q2 - 2*q1*q3);SPP[1] = SF[18]*(2*q0*q2 + 2*q1*q3) + SF[16]*(SF[24] - 2*q0*q3);SPP[2] = 2*q3*SF[8] + 2*q1*SF[11] - 2*q0*SF[14] - 2*q2*SF[13];SPP[3] = 2*q1*SF[7] + 2*q2*SF[6] - 2*q0*SF[12] - 2*q3*SF[10];SPP[4] = 2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12];SPP[5] = 2*q0*SF[8] + 2*q2*SF[11] + 2*q1*SF[13] + 2*q3*SF[14];SPP[6] = 2*q0*SF[7] + 2*q3*SF[6] + 2*q2*SF[10] + 2*q1*SF[12];SPP[7] = SF[18]*SF[20] - SF[16]*(2*q0*q1 + 2*q2*q3);SPP[8] = 2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9];SPP[9] = 2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9];SPP[10] = SF[17]*SF[20] + SF[16]*(2*q0*q2 - 2*q1*q3);SPP[11] = SF[17]*SF[21] - SF[18]*(SF[24] + 2*q0*q3);SPP[12] = SF[17]*SF[22] - SF[16]*(SF[24] + 2*q0*q3);SPP[13] = 2*q0*SF[4] + 2*q1*SF[5] + 2*q3*SF[3] + 2*q2*SF[9];SPP[14] = 2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13];SPP[15] = SF[18]*SF[23] + SF[17]*(SF[24] - 2*q0*q3);SPP[16] = daz*SF[19] + daz*sq(q0) + daz*sq(q1) + daz*sq(q3);SPP[17] = day*SF[19] + day*sq(q0) + day*sq(q1) + day*sq(q3);SPP[18] = dax*SF[19] + dax*sq(q0) + dax*sq(q1) + dax*sq(q3);SPP[19] = SF[16]*SF[23] - SF[17]*(2*q0*q2 + 2*q1*q3);SPP[20] = SF[16]*SF[21] - SF[18]*SF[22];SPP[21] = 2*q0*q2 + 2*q1*q3;SPP[22] = SF[15];//状态抑制逻辑//当 inhibitMagStates 为真时，清零 P 矩阵中的第 16 到 21 行和列，表示抑制磁状态。if (inhibitMagStates) {zeroRows(P,16,21);zeroCols(P,16,21);} else if (inhibitWindStates) {//如果 inhibitWindStates 为真，则清零第 22 和 23 行列，表示抑制风状态。zeroRows(P,22,23);zeroCols(P,22,23);}//如果磁状态未被抑制且上一次的磁状态被抑制，则需要重置磁体方差。if (!inhibitMagStates && lastInhibitMagStates) {// when starting 3D fusion we want to reset body mag variancesneedMagBodyVarReset = true;}//如果需要重置方差，将第 19 到 21 行和列清零，然后将第 19 行和列的值设置为磁噪声的平方。if (needMagBodyVarReset) {// reset body mag variancesneedMagBodyVarReset = false;zeroCols(P,19,21);zeroRows(P,19,21);P[19][19] = sq(frontend->_magNoise);P[20][20] = P[19][19];P[21][21] = P[19][19];}//保存当前的磁状态抑制标志，以便在下一次迭代中使用。lastInhibitMagStates = inhibitMagStates;nextP[0][0] = daxNoise*SQ[3] + SPP[5]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[9][0]*SPP[22] + P[12][0]*SPP[18] + P[2][0]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) - SPP[4]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[9][1]*SPP[22] + P[12][1]*SPP[18] + P[2][1]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[8]*(P[0][2]*SPP[5] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18] - P[1][2]*(2*q0*SF[6] - 2*q3*SF[7] - 2*q1*SF[10] + 2*q2*SF[12])) + SPP[22]*(P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[9][9]*SPP[22] + P[12][9]*SPP[18] + P[2][9]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9])) + SPP[18]*(P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[9][12]*SPP[22] + P[12][12]*SPP[18] + P[2][12]*(2*q1*SF[3] - 2*q2*SF[4] - 2*q3*SF[5] + 2*q0*SF[9]));nextP[0][1] = SPP[6]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) - SPP[2]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[22]*(P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18]) + SPP[17]*(P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18]) - (2*q0*SF[5] - 2*q1*SF[4] - 2*q2*SF[3] + 2*q3*SF[9])*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);nextP[1][1] = dayNoise*SQ[3] - SPP[2]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[6]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) - SPP[9]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[22]*(P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17]) + SPP[17]*(P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17]);nextP[0][2] = SPP[13]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[3]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[22]*(P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18]) + SPP[16]*(P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]);nextP[1][2] = SPP[13]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[3]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[22]*(P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17]) + SPP[16]*(P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17]) + (2*q2*SF[8] - 2*q0*SF[11] - 2*q1*SF[14] + 2*q3*SF[13])*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]);nextP[2][2] = dazNoise*SQ[3] - SPP[3]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[14]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[13]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[22]*(P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16]) + SPP[16]*(P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16]);nextP[0][3] = P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18] + SPP[1]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[15]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) - SPP[21]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);nextP[1][3] = P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17] + SPP[1]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[15]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) - SPP[21]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);nextP[2][3] = P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16] + SPP[1]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[15]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) - SPP[21]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + (SF[16]*SF[23] - SF[17]*SPP[21])*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);nextP[3][3] = P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21] + dvyNoise*sq(SQ[6] - 2*q0*q3) + dvzNoise*sq(SQ[5] + 2*q0*q2) + SPP[1]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[19]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[15]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) - SPP[21]*(P[3][15] + P[0][15]*SPP[1] + P[2][15]*SPP[15] - P[15][15]*SPP[21] + P[1][15]*(SF[16]*SF[23] - SF[17]*SPP[21])) + dvxNoise*sq(SG[1] + SG[2] - SG[3] - SQ[7]);nextP[0][4] = P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18] + SF[22]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) + SPP[12]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]) + SPP[20]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[11]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]);nextP[1][4] = P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17] + SF[22]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) + SPP[12]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]) + SPP[20]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[11]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]);nextP[2][4] = P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16] + SF[22]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) + SPP[12]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]) + SPP[20]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[11]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]);nextP[3][4] = P[3][4] + SQ[2] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21] + SF[22]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) + SPP[12]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]) + SPP[20]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[11]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]);nextP[4][4] = P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11] + dvxNoise*sq(SQ[6] + 2*q0*q3) + dvzNoise*sq(SQ[4] - 2*q0*q1) + SF[22]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) + SPP[12]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]) + SPP[20]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[11]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + dvyNoise*sq(SG[1] - SG[2] + SG[3] - SQ[7]);nextP[0][5] = P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18] + SF[20]*(P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18]) - SPP[7]*(P[0][0]*SPP[5] - P[1][0]*SPP[4] + P[2][0]*SPP[8] + P[9][0]*SPP[22] + P[12][0]*SPP[18]) + SPP[0]*(P[0][2]*SPP[5] - P[1][2]*SPP[4] + P[2][2]*SPP[8] + P[9][2]*SPP[22] + P[12][2]*SPP[18]) + SPP[10]*(P[0][1]*SPP[5] - P[1][1]*SPP[4] + P[2][1]*SPP[8] + P[9][1]*SPP[22] + P[12][1]*SPP[18]);nextP[1][5] = P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17] + SF[20]*(P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17]) - SPP[7]*(P[1][0]*SPP[6] - P[0][0]*SPP[2] - P[2][0]*SPP[9] + P[10][0]*SPP[22] + P[13][0]*SPP[17]) + SPP[0]*(P[1][2]*SPP[6] - P[0][2]*SPP[2] - P[2][2]*SPP[9] + P[10][2]*SPP[22] + P[13][2]*SPP[17]) + SPP[10]*(P[1][1]*SPP[6] - P[0][1]*SPP[2] - P[2][1]*SPP[9] + P[10][1]*SPP[22] + P[13][1]*SPP[17]);nextP[2][5] = P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16] + SF[20]*(P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16]) - SPP[7]*(P[0][0]*SPP[14] - P[1][0]*SPP[3] + P[2][0]*SPP[13] + P[11][0]*SPP[22] + P[14][0]*SPP[16]) + SPP[0]*(P[0][2]*SPP[14] - P[1][2]*SPP[3] + P[2][2]*SPP[13] + P[11][2]*SPP[22] + P[14][2]*SPP[16]) + SPP[10]*(P[0][1]*SPP[14] - P[1][1]*SPP[3] + P[2][1]*SPP[13] + P[11][1]*SPP[22] + P[14][1]*SPP[16]);nextP[3][5] = P[3][5] + SQ[1] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21] + SF[20]*(P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21]) - SPP[7]*(P[3][0] + P[0][0]*SPP[1] + P[1][0]*SPP[19] + P[2][0]*SPP[15] - P[15][0]*SPP[21]) + SPP[0]*(P[3][2] + P[0][2]*SPP[1] + P[1][2]*SPP[19] + P[2][2]*SPP[15] - P[15][2]*SPP[21]) + SPP[10]*(P[3][1] + P[0][1]*SPP[1] + P[1][1]*SPP[19] + P[2][1]*SPP[15] - P[15][1]*SPP[21]);nextP[4][5] = P[4][5] + SQ[0] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11] + SF[20]*(P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11]) - SPP[7]*(P[4][0] + P[15][0]*SF[22] + P[0][0]*SPP[20] + P[1][0]*SPP[12] + P[2][0]*SPP[11]) + SPP[0]*(P[4][2] + P[15][2]*SF[22] + P[0][2]*SPP[20] + P[1][2]*SPP[12] + P[2][2]*SPP[11]) + SPP[10]*(P[4][1] + P[15][1]*SF[22] + P[0][1]*SPP[20] + P[1][1]*SPP[12] + P[2][1]*SPP[11]);nextP[5][5] = P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0] + dvxNoise*sq(SQ[5] - 2*q0*q2) + dvyNoise*sq(SQ[4] + 2*q0*q1) + SF[20]*(P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0]) - SPP[7]*(P[5][0] + P[15][0]*SF[20] - P[0][0]*SPP[7] + P[1][0]*SPP[10] + P[2][0]*SPP[0]) + SPP[0]*(P[5][2] + P[15][2]*SF[20] - P[0][2]*SPP[7] + P[1][2]*SPP[10] + P[2][2]*SPP[0]) + SPP[10]*(P[5][1] + P[15][1]*SF[20] - P[0][1]*SPP[7] + P[1][1]*SPP[10] + P[2][1]*SPP[0]) + dvzNoise*sq(SG[1] - SG[2] - SG[3] + SQ[7]);nextP[0][6] = P[0][6]*SPP[5] - P[1][6]*SPP[4] + P[2][6]*SPP[8] + P[9][6]*SPP[22] + P[12][6]*SPP[18] + dt*(P[0][3]*SPP[5] - P[1][3]*SPP[4] + P[2][3]*SPP[8] + P[9][3]*SPP[22] + P[12][3]*SPP[18]);nextP[1][6] = P[1][6]*SPP[6] - P[0][6]*SPP[2] - P[2][6]*SPP[9] + P[10][6]*SPP[22] + P[13][6]*SPP[17] + dt*(P[1][3]*SPP[6] - P[0][3]*SPP[2] - P[2][3]*SPP[9] + P[10][3]*SPP[22] + P[13][3]*SPP[17]);nextP[2][6] = P[0][6]*SPP[14] - P[1][6]*SPP[3] + P[2][6]*SPP[13] + P[11][6]*SPP[22] + P[14][6]*SPP[16] + dt*(P[0][3]*SPP[14] - P[1][3]*SPP[3] + P[2][3]*SPP[13] + P[11][3]*SPP[22] + P[14][3]*SPP[16]);nextP[3][6] = P[3][6] + P[0][6]*SPP[1] + P[1][6]*SPP[19] + P[2][6]*SPP[15] - P[15][6]*SPP[21] + dt*(P[3][3] + P[0][3]*SPP[1] + P[1][3]*SPP[19] + P[2][3]*SPP[15] - P[15][3]*SPP[21]);nextP[4][6] = P[4][6] + P[15][6]*SF[22] + P[0][6]*SPP[20] + P[1][6]*SPP[12] + P[2][6]*SPP[11] + dt*(P[4][3] + P[15][3]*SF[22] + P[0][3]*SPP[20] + P[1][3]*SPP[12] + P[2][3]*SPP[11]);nextP[5][6] = P[5][6] + P[15][6]*SF[20] - P[0][6]*SPP[7] + P[1][6]*SPP[10] + P[2][6]*SPP[0] + dt*(P[5][3] + P[15][3]*SF[20] - P[0][3]*SPP[7] + P[1][3]*SPP[10] + P[2][3]*SPP[0]);nextP[6][6] = P[6][6] + P[3][6]*dt + dt*(P[6][3] + P[3][3]*dt);nextP[0][7] = P[0][7]*SPP[5] - P[1][7]*SPP[4] + P[2][7]*SPP[8] + P[9][7]*SPP[22] + P[12][7]*SPP[18] + dt*(P[0][4]*SPP[5] - P[1][4]*SPP[4] + P[2][4]*SPP[8] + P[9][4]*SPP[22] + P[12][4]*SPP[18]);nextP[1][7] = P[1][7]*SPP[6] - P[0][7]*SPP[2] - P[2][7]*SPP[9] + P[10][7]*SPP[22] + P[13][7]*SPP[17] + dt*(P[1][4]*SPP[6] - P[0][4]*SPP[2] - P[2][4]*SPP[9] + P[10][4]*SPP[22] + P[13][4]*SPP[17]);nextP[2][7] = P[0][7]*SPP[14] - P[1][7]*SPP[3] + P[2][7]*SPP[13] + P[11][7]*SPP[22] + P[14][7]*SPP[16] + dt*(P[0][4]*SPP[14] - P[1][4]*SPP[3] + P[2][4]*SPP[13] + P[11][4]*SPP[22] + P[14][4]*SPP[16]);nextP[3][7] = P[3][7] + P[0][7]*SPP[1] + P[1][7]*SPP[19] + P[2][7]*SPP[15] - P[15][7]*SPP[21] + dt*(P[3][4] + P[0][4]*SPP[1] + P[1][4]*SPP[19] + P[2][4]*SPP[15] - P[15][4]*SPP[21]);nextP[4][7] = P[4][7] + P[15][7]*SF[22] + P[0][7]*SPP[20] + P[1][7]*SPP[12] + P[2][7]*SPP[11] + dt*(P[4][4] + P[15][4]*SF[22] + P[0][4]*SPP[20] + P[1][4]*SPP[12] + P[2][4]*SPP[11]);nextP[5][7] = P[5][7] + P[15][7]*SF[20] - P[0][7]*SPP[7] + P[1][7]*SPP[10] + P[2][7]*SPP[0] + dt*(P[5][4] + P[15][4]*SF[20] - P[0][4]*SPP[7] + P[1][4]*SPP[10] + P[2][4]*SPP[0]);nextP[6][7] = P[6][7] + P[3][7]*dt + dt*(P[6][4] + P[3][4]*dt);nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);nextP[0][8] = P[0][8]*SPP[5] - P[1][8]*SPP[4] + P[2][8]*SPP[8] + P[9][8]*SPP[22] + P[12][8]*SPP[18] + dt*(P[0][5]*SPP[5] - P[1][5]*SPP[4] + P[2][5]*SPP[8] + P[9][5]*SPP[22] + P[12][5]*SPP[18]);nextP[1][8] = P[1][8]*SPP[6] - P[0][8]*SPP[2] - P[2][8]*SPP[9] + P[10][8]*SPP[22] + P[13][8]*SPP[17] + dt*(P[1][5]*SPP[6] - P[0][5]*SPP[2] - P[2][5]*SPP[9] + P[10][5]*SPP[22] + P[13][5]*SPP[17]);nextP[2][8] = P[0][8]*SPP[14] - P[1][8]*SPP[3] + P[2][8]*SPP[13] + P[11][8]*SPP[22] + P[14][8]*SPP[16] + dt*(P[0][5]*SPP[14] - P[1][5]*SPP[3] + P[2][5]*SPP[13] + P[11][5]*SPP[22] + P[14][5]*SPP[16]);nextP[3][8] = P[3][8] + P[0][8]*SPP[1] + P[1][8]*SPP[19] + P[2][8]*SPP[15] - P[15][8]*SPP[21] + dt*(P[3][5] + P[0][5]*SPP[1] + P[1][5]*SPP[19] + P[2][5]*SPP[15] - P[15][5]*SPP[21]);nextP[4][8] = P[4][8] + P[15][8]*SF[22] + P[0][8]*SPP[20] + P[1][8]*SPP[12] + P[2][8]*SPP[11] + dt*(P[4][5] + P[15][5]*SF[22] + P[0][5]*SPP[20] + P[1][5]*SPP[12] + P[2][5]*SPP[11]);nextP[5][8] = P[5][8] + P[15][8]*SF[20] - P[0][8]*SPP[7] + P[1][8]*SPP[10] + P[2][8]*SPP[0] + dt*(P[5][5] + P[15][5]*SF[20] - P[0][5]*SPP[7] + P[1][5]*SPP[10] + P[2][5]*SPP[0]);nextP[6][8] = P[6][8] + P[3][8]*dt + dt*(P[6][5] + P[3][5]*dt);nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);nextP[0][9] = P[0][9]*SPP[5] - P[1][9]*SPP[4] + P[2][9]*SPP[8] + P[9][9]*SPP[22] + P[12][9]*SPP[18];nextP[1][9] = P[1][9]*SPP[6] - P[0][9]*SPP[2] - P[2][9]*SPP[9] + P[10][9]*SPP[22] + P[13][9]*SPP[17];nextP[2][9] = P[0][9]*SPP[14] - P[1][9]*SPP[3] + P[2][9]*SPP[13] + P[11][9]*SPP[22] + P[14][9]*SPP[16];nextP[3][9] = P[3][9] + P[0][9]*SPP[1] + P[1][9]*SPP[19] + P[2][9]*SPP[15] - P[15][9]*SPP[21];nextP[4][9] = P[4][9] + P[15][9]*SF[22] + P[0][9]*SPP[20] + P[1][9]*SPP[12] + P[2][9]*SPP[11];nextP[5][9] = P[5][9] + P[15][9]*SF[20] - P[0][9]*SPP[7] + P[1][9]*SPP[10] + P[2][9]*SPP[0];nextP[6][9] = P[6][9] + P[3][9]*dt;nextP[7][9] = P[7][9] + P[4][9]*dt;nextP[8][9] = P[8][9] + P[5][9]*dt;nextP[9][9] = P[9][9];nextP[0][10] = P[0][10]*SPP[5] - P[1][10]*SPP[4] + P[2][10]*SPP[8] + P[9][10]*SPP[22] + P[12][10]*SPP[18];nextP[1][10] = P[1][10]*SPP[6] - P[0][10]*SPP[2] - P[2][10]*SPP[9] + P[10][10]*SPP[22] + P[13][10]*SPP[17];nextP[2][10] = P[0][10]*SPP[14] - P[1][10]*SPP[3] + P[2][10]*SPP[13] + P[11][10]*SPP[22] + P[14][10]*SPP[16];nextP[3][10] = P[3][10] + P[0][10]*SPP[1] + P[1][10]*SPP[19] + P[2][10]*SPP[15] - P[15][10]*SPP[21];nextP[4][10] = P[4][10] + P[15][10]*SF[22] + P[0][10]*SPP[20] + P[1][10]*SPP[12] + P[2][10]*SPP[11];nextP[5][10] = P[5][10] + P[15][10]*SF[20] - P[0][10]*SPP[7] + P[1][10]*SPP[10] + P[2][10]*SPP[0];nextP[6][10] = P[6][10] + P[3][10]*dt;nextP[7][10] = P[7][10] + P[4][10]*dt;nextP[8][10] = P[8][10] + P[5][10]*dt;nextP[9][10] = P[9][10];nextP[10][10] = P[10][10];nextP[0][11] = P[0][11]*SPP[5] - P[1][11]*SPP[4] + P[2][11]*SPP[8] + P[9][11]*SPP[22] + P[12][11]*SPP[18];nextP[1][11] = P[1][11]*SPP[6] - P[0][11]*SPP[2] - P[2][11]*SPP[9] + P[10][11]*SPP[22] + P[13][11]*SPP[17];nextP[2][11] = P[0][11]*SPP[14] - P[1][11]*SPP[3] + P[2][11]*SPP[13] + P[11][11]*SPP[22] + P[14][11]*SPP[16];nextP[3][11] = P[3][11] + P[0][11]*SPP[1] + P[1][11]*SPP[19] + P[2][11]*SPP[15] - P[15][11]*SPP[21];nextP[4][11] = P[4][11] + P[15][11]*SF[22] + P[0][11]*SPP[20] + P[1][11]*SPP[12] + P[2][11]*SPP[11];nextP[5][11] = P[5][11] + P[15][11]*SF[20] - P[0][11]*SPP[7] + P[1][11]*SPP[10] + P[2][11]*SPP[0];nextP[6][11] = P[6][11] + P[3][11]*dt;nextP[7][11] = P[7][11] + P[4][11]*dt;nextP[8][11] = P[8][11] + P[5][11]*dt;nextP[9][11] = P[9][11];nextP[10][11] = P[10][11];nextP[11][11] = P[11][11];nextP[0][12] = P[0][12]*SPP[5] - P[1][12]*SPP[4] + P[2][12]*SPP[8] + P[9][12]*SPP[22] + P[12][12]*SPP[18];nextP[1][12] = P[1][12]*SPP[6] - P[0][12]*SPP[2] - P[2][12]*SPP[9] + P[10][12]*SPP[22] + P[13][12]*SPP[17];nextP[2][12] = P[0][12]*SPP[14] - P[1][12]*SPP[3] + P[2][12]*SPP[13] + P[11][12]*SPP[22] + P[14][12]*SPP[16];nextP[3][12] = P[3][12] + P[0][12]*SPP[1] + P[1][12]*SPP[19] + P[2][12]*SPP[15] - P[15][12]*SPP[21];nextP[4][12] = P[4][12] + P[15][12]*SF[22] + P[0][12]*SPP[20] + P[1][12]*SPP[12] + P[2][12]*SPP[11];nextP[5][12] = P[5][12] + P[15][12]*SF[20] - P[0][12]*SPP[7] + P[1][12]*SPP[10] + P[2][12]*SPP[0];nextP[6][12] = P[6][12] + P[3][12]*dt;nextP[7][12] = P[7][12] + P[4][12]*dt;nextP[8][12] = P[8][12] + P[5][12]*dt;nextP[9][12] = P[9][12];nextP[10][12] = P[10][12];nextP[11][12] = P[11][12];nextP[12][12] = P[12][12];nextP[0][13] = P[0][13]*SPP[5] - P[1][13]*SPP[4] + P[2][13]*SPP[8] + P[9][13]*SPP[22] + P[12][13]*SPP[18];nextP[1][13] = P[1][13]*SPP[6] - P[0][13]*SPP[2] - P[2][13]*SPP[9] + P[10][13]*SPP[22] + P[13][13]*SPP[17];nextP[2][13] = P[0][13]*SPP[14] - P[1][13]*SPP[3] + P[2][13]*SPP[13] + P[11][13]*SPP[22] + P[14][13]*SPP[16];nextP[3][13] = P[3][13] + P[0][13]*SPP[1] + P[1][13]*SPP[19] + P[2][13]*SPP[15] - P[15][13]*SPP[21];nextP[4][13] = P[4][13] + P[15][13]*SF[22] + P[0][13]*SPP[20] + P[1][13]*SPP[12] + P[2][13]*SPP[11];nextP[5][13] = P[5][13] + P[15][13]*SF[20] - P[0][13]*SPP[7] + P[1][13]*SPP[10] + P[2][13]*SPP[0];nextP[6][13] = P[6][13] + P[3][13]*dt;nextP[7][13] = P[7][13] + P[4][13]*dt;nextP[8][13] = P[8][13] + P[5][13]*dt;nextP[9][13] = P[9][13];nextP[10][13] = P[10][13];nextP[11][13] = P[11][13];nextP[12][13] = P[12][13];nextP[13][13] = P[13][13];nextP[0][14] = P[0][14]*SPP[5] - P[1][14]*SPP[4] + P[2][14]*SPP[8] + P[9][14]*SPP[22] + P[12][14]*SPP[18];nextP[1][14] = P[1][14]*SPP[6] - P[0][14]*SPP[2] - P[2][14]*SPP[9] + P[10][14]*SPP[22] + P[13][14]*SPP[17];nextP[2][14] = P[0][14]*SPP[14] - P[1][14]*SPP[3] + P[2][14]*SPP[13] + P[11][14]*SPP[22] + P[14][14]*SPP[16];nextP[3][14] = P[3][14] + P[0][14]*SPP[1] + P[1][14]*SPP[19] + P[2][14]*SPP[15] - P[15][14]*SPP[21];nextP[4][14] = P[4][14] + P[15][14]*SF[22] + P[0][14]*SPP[20] + P[1][14]*SPP[12] + P[2][14]*SPP[11];nextP[5][14] = P[5][14] + P[15][14]*SF[20] - P[0][14]*SPP[7] + P[1][14]*SPP[10] + P[2][14]*SPP[0];nextP[6][14] = P[6][14] + P[3][14]*dt;nextP[7][14] = P[7][14] + P[4][14]*dt;nextP[8][14] = P[8][14] + P[5][14]*dt;nextP[9][14] = P[9][14];nextP[10][14] = P[10][14];nextP[11][14] = P[11][14];nextP[12][14] = P[12][14];nextP[13][14] = P[13][14];nextP[14][14] = P[14][14];nextP[0][15] = P[0][15]*SPP[5] - P[1][15]*SPP[4] + P[2][15]*SPP[8] + P[9][15]*SPP[22] + P[12][15]*SPP[18];nextP[1][15] = P[1][15]*SPP[6] - P[0][15]*SPP[2] - P[2][15]*SPP[9] + P[10][15]*SPP[22] + P[13][15]*SPP[17];nextP[2][15] = P[0][15]*SPP[14] - P[1][15]*SPP[3] + P[2][15]*SPP[13] + P[11][15]*SPP[22] + P[14][15]*SPP[16];nextP[3][15] = P[3][15] + P[0][15]*SPP[1] + P[1][15]*SPP[19] + P[2][15]*SPP[15] - P[15][15]*SPP[21];nextP[4][15] = P[4][15] + P[15][15]*SF[22] + P[0][15]*SPP[20] + P[1][15]*SPP[12] + P[2][15]*SPP[11];nextP[5][15] = P[5][15] + P[15][15]*SF[20] - P[0][15]*SPP[7] + P[1][15]*SPP[10] + P[2][15]*SPP[0];nextP[6][15] = P[6][15] + P[3][15]*dt;nextP[7][15] = P[7][15] + P[4][15]*dt;nextP[8][15] = P[8][15] + P[5][15]*dt;nextP[9][15] = P[9][15];nextP[10][15] = P[10][15];nextP[11][15] = P[11][15];nextP[12][15] = P[12][15];nextP[13][15] = P[13][15];nextP[14][15] = P[14][15];nextP[15][15] = P[15][15];if (stateIndexLim > 15) {nextP[0][16] = P[0][16]*SPP[5] - P[1][16]*SPP[4] + P[2][16]*SPP[8] + P[9][16]*SPP[22] + P[12][16]*SPP[18];nextP[1][16] = P[1][16]*SPP[6] - P[0][16]*SPP[2] - P[2][16]*SPP[9] + P[10][16]*SPP[22] + P[13][16]*SPP[17];nextP[2][16] = P[0][16]*SPP[14] - P[1][16]*SPP[3] + P[2][16]*SPP[13] + P[11][16]*SPP[22] + P[14][16]*SPP[16];nextP[3][16] = P[3][16] + P[0][16]*SPP[1] + P[1][16]*SPP[19] + P[2][16]*SPP[15] - P[15][16]*SPP[21];nextP[4][16] = P[4][16] + P[15][16]*SF[22] + P[0][16]*SPP[20] + P[1][16]*SPP[12] + P[2][16]*SPP[11];nextP[5][16] = P[5][16] + P[15][16]*SF[20] - P[0][16]*SPP[7] + P[1][16]*SPP[10] + P[2][16]*SPP[0];nextP[6][16] = P[6][16] + P[3][16]*dt;nextP[7][16] = P[7][16] + P[4][16]*dt;nextP[8][16] = P[8][16] + P[5][16]*dt;nextP[9][16] = P[9][16];nextP[10][16] = P[10][16];nextP[11][16] = P[11][16];nextP[12][16] = P[12][16];nextP[13][16] = P[13][16];nextP[14][16] = P[14][16];nextP[15][16] = P[15][16];nextP[16][16] = P[16][16];nextP[0][17] = P[0][17]*SPP[5] - P[1][17]*SPP[4] + P[2][17]*SPP[8] + P[9][17]*SPP[22] + P[12][17]*SPP[18];nextP[1][17] = P[1][17]*SPP[6] - P[0][17]*SPP[2] - P[2][17]*SPP[9] + P[10][17]*SPP[22] + P[13][17]*SPP[17];nextP[2][17] = P[0][17]*SPP[14] - P[1][17]*SPP[3] + P[2][17]*SPP[13] + P[11][17]*SPP[22] + P[14][17]*SPP[16];nextP[3][17] = P[3][17] + P[0][17]*SPP[1] + P[1][17]*SPP[19] + P[2][17]*SPP[15] - P[15][17]*SPP[21];nextP[4][17] = P[4][17] + P[15][17]*SF[22] + P[0][17]*SPP[20] + P[1][17]*SPP[12] + P[2][17]*SPP[11];nextP[5][17] = P[5][17] + P[15][17]*SF[20] - P[0][17]*SPP[7] + P[1][17]*SPP[10] + P[2][17]*SPP[0];nextP[6][17] = P[6][17] + P[3][17]*dt;nextP[7][17] = P[7][17] + P[4][17]*dt;nextP[8][17] = P[8][17] + P[5][17]*dt;nextP[9][17] = P[9][17];nextP[10][17] = P[10][17];nextP[11][17] = P[11][17];nextP[12][17] = P[12][17];nextP[13][17] = P[13][17];nextP[14][17] = P[14][17];nextP[15][17] = P[15][17];nextP[16][17] = P[16][17];nextP[17][17] = P[17][17];nextP[0][18] = P[0][18]*SPP[5] - P[1][18]*SPP[4] + P[2][18]*SPP[8] + P[9][18]*SPP[22] + P[12][18]*SPP[18];nextP[1][18] = P[1][18]*SPP[6] - P[0][18]*SPP[2] - P[2][18]*SPP[9] + P[10][18]*SPP[22] + P[13][18]*SPP[17];nextP[2][18] = P[0][18]*SPP[14] - P[1][18]*SPP[3] + P[2][18]*SPP[13] + P[11][18]*SPP[22] + P[14][18]*SPP[16];nextP[3][18] = P[3][18] + P[0][18]*SPP[1] + P[1][18]*SPP[19] + P[2][18]*SPP[15] - P[15][18]*SPP[21];nextP[4][18] = P[4][18] + P[15][18]*SF[22] + P[0][18]*SPP[20] + P[1][18]*SPP[12] + P[2][18]*SPP[11];nextP[5][18] = P[5][18] + P[15][18]*SF[20] - P[0][18]*SPP[7] + P[1][18]*SPP[10] + P[2][18]*SPP[0];nextP[6][18] = P[6][18] + P[3][18]*dt;nextP[7][18] = P[7][18] + P[4][18]*dt;nextP[8][18] = P[8][18] + P[5][18]*dt;nextP[9][18] = P[9][18];nextP[10][18] = P[10][18];nextP[11][18] = P[11][18];nextP[12][18] = P[12][18];nextP[13][18] = P[13][18];nextP[14][18] = P[14][18];nextP[15][18] = P[15][18];nextP[16][18] = P[16][18];nextP[17][18] = P[17][18];nextP[18][18] = P[18][18];nextP[0][19] = P[0][19]*SPP[5] - P[1][19]*SPP[4] + P[2][19]*SPP[8] + P[9][19]*SPP[22] + P[12][19]*SPP[18];nextP[1][19] = P[1][19]*SPP[6] - P[0][19]*SPP[2] - P[2][19]*SPP[9] + P[10][19]*SPP[22] + P[13][19]*SPP[17];nextP[2][19] = P[0][19]*SPP[14] - P[1][19]*SPP[3] + P[2][19]*SPP[13] + P[11][19]*SPP[22] + P[14][19]*SPP[16];nextP[3][19] = P[3][19] + P[0][19]*SPP[1] + P[1][19]*SPP[19] + P[2][19]*SPP[15] - P[15][19]*SPP[21];nextP[4][19] = P[4][19] + P[15][19]*SF[22] + P[0][19]*SPP[20] + P[1][19]*SPP[12] + P[2][19]*SPP[11];nextP[5][19] = P[5][19] + P[15][19]*SF[20] - P[0][19]*SPP[7] + P[1][19]*SPP[10] + P[2][19]*SPP[0];nextP[6][19] = P[6][19] + P[3][19]*dt;nextP[7][19] = P[7][19] + P[4][19]*dt;nextP[8][19] = P[8][19] + P[5][19]*dt;nextP[9][19] = P[9][19];nextP[10][19] = P[10][19];nextP[11][19] = P[11][19];nextP[12][19] = P[12][19];nextP[13][19] = P[13][19];nextP[14][19] = P[14][19];nextP[15][19] = P[15][19];nextP[16][19] = P[16][19];nextP[17][19] = P[17][19];nextP[18][19] = P[18][19];nextP[19][19] = P[19][19];nextP[0][20] = P[0][20]*SPP[5] - P[1][20]*SPP[4] + P[2][20]*SPP[8] + P[9][20]*SPP[22] + P[12][20]*SPP[18];nextP[1][20] = P[1][20]*SPP[6] - P[0][20]*SPP[2] - P[2][20]*SPP[9] + P[10][20]*SPP[22] + P[13][20]*SPP[17];nextP[2][20] = P[0][20]*SPP[14] - P[1][20]*SPP[3] + P[2][20]*SPP[13] + P[11][20]*SPP[22] + P[14][20]*SPP[16];nextP[3][20] = P[3][20] + P[0][20]*SPP[1] + P[1][20]*SPP[19] + P[2][20]*SPP[15] - P[15][20]*SPP[21];nextP[4][20] = P[4][20] + P[15][20]*SF[22] + P[0][20]*SPP[20] + P[1][20]*SPP[12] + P[2][20]*SPP[11];nextP[5][20] = P[5][20] + P[15][20]*SF[20] - P[0][20]*SPP[7] + P[1][20]*SPP[10] + P[2][20]*SPP[0];nextP[6][20] = P[6][20] + P[3][20]*dt;nextP[7][20] = P[7][20] + P[4][20]*dt;nextP[8][20] = P[8][20] + P[5][20]*dt;nextP[9][20] = P[9][20];nextP[10][20] = P[10][20];nextP[11][20] = P[11][20];nextP[12][20] = P[12][20];nextP[13][20] = P[13][20];nextP[14][20] = P[14][20];nextP[15][20] = P[15][20];nextP[16][20] = P[16][20];nextP[17][20] = P[17][20];nextP[18][20] = P[18][20];nextP[19][20] = P[19][20];nextP[20][20] = P[20][20];nextP[0][21] = P[0][21]*SPP[5] - P[1][21]*SPP[4] + P[2][21]*SPP[8] + P[9][21]*SPP[22] + P[12][21]*SPP[18];nextP[1][21] = P[1][21]*SPP[6] - P[0][21]*SPP[2] - P[2][21]*SPP[9] + P[10][21]*SPP[22] + P[13][21]*SPP[17];nextP[2][21] = P[0][21]*SPP[14] - P[1][21]*SPP[3] + P[2][21]*SPP[13] + P[11][21]*SPP[22] + P[14][21]*SPP[16];nextP[3][21] = P[3][21] + P[0][21]*SPP[1] + P[1][21]*SPP[19] + P[2][21]*SPP[15] - P[15][21]*SPP[21];nextP[4][21] = P[4][21] + P[15][21]*SF[22] + P[0][21]*SPP[20] + P[1][21]*SPP[12] + P[2][21]*SPP[11];nextP[5][21] = P[5][21] + P[15][21]*SF[20] - P[0][21]*SPP[7] + P[1][21]*SPP[10] + P[2][21]*SPP[0];nextP[6][21] = P[6][21] + P[3][21]*dt;nextP[7][21] = P[7][21] + P[4][21]*dt;nextP[8][21] = P[8][21] + P[5][21]*dt;nextP[9][21] = P[9][21];nextP[10][21] = P[10][21];nextP[11][21] = P[11][21];nextP[12][21] = P[12][21];nextP[13][21] = P[13][21];nextP[14][21] = P[14][21];nextP[15][21] = P[15][21];nextP[16][21] = P[16][21];nextP[17][21] = P[17][21];nextP[18][21] = P[18][21];nextP[19][21] = P[19][21];nextP[20][21] = P[20][21];nextP[21][21] = P[21][21];if (stateIndexLim > 21) {nextP[0][22] = P[0][22]*SPP[5] - P[1][22]*SPP[4] + P[2][22]*SPP[8] + P[9][22]*SPP[22] + P[12][22]*SPP[18];nextP[1][22] = P[1][22]*SPP[6] - P[0][22]*SPP[2] - P[2][22]*SPP[9] + P[10][22]*SPP[22] + P[13][22]*SPP[17];nextP[2][22] = P[0][22]*SPP[14] - P[1][22]*SPP[3] + P[2][22]*SPP[13] + P[11][22]*SPP[22] + P[14][22]*SPP[16];nextP[3][22] = P[3][22] + P[0][22]*SPP[1] + P[1][22]*SPP[19] + P[2][22]*SPP[15] - P[15][22]*SPP[21];nextP[4][22] = P[4][22] + P[15][22]*SF[22] + P[0][22]*SPP[20] + P[1][22]*SPP[12] + P[2][22]*SPP[11];nextP[5][22] = P[5][22] + P[15][22]*SF[20] - P[0][22]*SPP[7] + P[1][22]*SPP[10] + P[2][22]*SPP[0];nextP[6][22] = P[6][22] + P[3][22]*dt;nextP[7][22] = P[7][22] + P[4][22]*dt;nextP[8][22] = P[8][22] + P[5][22]*dt;nextP[9][22] = P[9][22];nextP[10][22] = P[10][22];nextP[11][22] = P[11][22];nextP[12][22] = P[12][22];nextP[13][22] = P[13][22];nextP[14][22] = P[14][22];nextP[15][22] = P[15][22];nextP[16][22] = P[16][22];nextP[17][22] = P[17][22];nextP[18][22] = P[18][22];nextP[19][22] = P[19][22];nextP[20][22] = P[20][22];nextP[21][22] = P[21][22];nextP[22][22] = P[22][22];nextP[0][23] = P[0][23]*SPP[5] - P[1][23]*SPP[4] + P[2][23]*SPP[8] + P[9][23]*SPP[22] + P[12][23]*SPP[18];nextP[1][23] = P[1][23]*SPP[6] - P[0][23]*SPP[2] - P[2][23]*SPP[9] + P[10][23]*SPP[22] + P[13][23]*SPP[17];nextP[2][23] = P[0][23]*SPP[14] - P[1][23]*SPP[3] + P[2][23]*SPP[13] + P[11][23]*SPP[22] + P[14][23]*SPP[16];nextP[3][23] = P[3][23] + P[0][23]*SPP[1] + P[1][23]*SPP[19] + P[2][23]*SPP[15] - P[15][23]*SPP[21];nextP[4][23] = P[4][23] + P[15][23]*SF[22] + P[0][23]*SPP[20] + P[1][23]*SPP[12] + P[2][23]*SPP[11];nextP[5][23] = P[5][23] + P[15][23]*SF[20] - P[0][23]*SPP[7] + P[1][23]*SPP[10] + P[2][23]*SPP[0];nextP[6][23] = P[6][23] + P[3][23]*dt;nextP[7][23] = P[7][23] + P[4][23]*dt;nextP[8][23] = P[8][23] + P[5][23]*dt;nextP[9][23] = P[9][23];nextP[10][23] = P[10][23];nextP[11][23] = P[11][23];nextP[12][23] = P[12][23];nextP[13][23] = P[13][23];nextP[14][23] = P[14][23];nextP[15][23] = P[15][23];nextP[16][23] = P[16][23];nextP[17][23] = P[17][23];nextP[18][23] = P[18][23];nextP[19][23] = P[19][23];nextP[20][23] = P[20][23];nextP[21][23] = P[21][23];nextP[22][23] = P[22][23];nextP[23][23] = P[23][23];}}// Copy upper diagonal to lower diagonal taking advantage of symmetryfor (uint8_t colIndex=0; colIndex<=stateIndexLim; colIndex++){for (uint8_t rowIndex=0; rowIndex<colIndex; rowIndex++){nextP[colIndex][rowIndex] = nextP[rowIndex][colIndex];}}// add the general state process noise variancesfor (uint8_t i=0; i<=stateIndexLim; i++){nextP[i][i] = nextP[i][i] + processNoise[i];}// if the total position variance exceeds 1e4 (100m), then stop covariance// growth by setting the predicted to the previous values// This prevent an ill conditioned matrix from occurring for long periods// without GPSif ((P[6][6] + P[7][7]) > 1e4f){for (uint8_t i=6; i<=7; i++){for (uint8_t j=0; j<=stateIndexLim; j++){nextP[i][j] = P[i][j];nextP[j][i] = P[j][i];}}}// copy covariances to outputCopyCovariances();// constrain diagonals to prevent ill-conditioningConstrainVariances();
}

5.runYawEstimatorPrediction() 

void NavEKF2_core::runYawEstimatorPrediction()
{// 检查航向估计器是否不为空，并且 GPS 融合模式小于等于 1if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {ftype trueAirspeed;//定义真实空速变量// 如果默认空速为正并且假设侧滑角为零if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {// 检查 IMU 数据延迟时间与真实空速数据延迟时间的差值是否小于 5000 毫秒if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {trueAirspeed = tasDataDelayed.tas;//  使用延迟的真实空速} else {trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();// 计算真实空速}} else {trueAirspeed = 0.0f;// 如果不满足条件，则将真实空速设为 0}// 更新航向估计器yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);}
}
  • 函数 runYawEstimatorPrediction 主要用于更新航向估计器。
  • 检查航向估计器是否存在且 GPS 融合模式符合条件。
  • 根据 IMU 数据和真实空速数据的延迟，决定使用哪个空速进行更新。
  • 最后，调用航向估计器的 update 方法来更新相关数据。

未完待续~~

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.xdnf.cn/news/1551145.html

如若内容造成侵权/违法违规/事实不符，请联系一条长河网进行投诉反馈，一经查实，立即删除！

相关文章

如何快速建立自己的异地互联的远程视频监控系统，通过web浏览器可以直接查看公网上的监控视频（上）

如何快速建立自己的异地互联的远程视频监控系统，通过web浏览器可以直接查看公网上的监控视频（上）

目录 一、需求 二、方案 2.1、计划方案 2.2、实施准备 2.2.1所需配置的产品和服务 2.2.1.1云主机 &#xff08;1&#xff09;选择云平台 &#xff08;2&#xff09;配置云服务器 2.2.2.2视频监控平台软件 &#xff08;1&#xff09;视频监控平台软件 &#xff08;2&am…
阅读更多...
Gromacs位置限制问题

Gromacs位置限制问题

Atom index n in position_restraints out of bounds A common problem is placing position restraint files for multiple molecules out of order.(一个常见的问题是无序放置多个分子的位置约束文件。)Recall that a position restraint itp (page 449) file containing a …
阅读更多...
08_OpenCV文字图片绘制

08_OpenCV文字图片绘制

import cv2 import numpy as npimg cv2.imread(image0.jpg,1) font cv2.FONT_HERSHEY_SIMPLEXcv2.rectangle(img,(500,400),(200,100),(0,255,0),20) # 1 dst 2 文字内容 3 坐标 4 5 字体大小 6 color 7 粗细 8 line type cv2.putText(img,flower,(200,50),font,1,(0,0,250)…
阅读更多...
今日早报 每日精选15条新闻简报 每天一分钟 知晓天下事 9月30日，星期一

今日早报 每日精选15条新闻简报 每天一分钟 知晓天下事 9月30日，星期一

每天一分钟&#xff0c;知晓天下事&#xff01; 2024年9月30日 星期一 农历八月廿八 1、 央行&#xff1a;首套、二套房存量房贷利率批量下调&#xff0c;平均降幅0.5%左右&#xff1b;取消个人房贷重定价周期最短1年限制。 2、 住建部&#xff1a;对商品房建设严控增量、优化…
阅读更多...
FPGA实现PCIE视频采集转SDI输出，基于GTX+XDMA中断架构，提供2套工程源码和技术支持

FPGA实现PCIE视频采集转SDI输出，基于GTX+XDMA中断架构，提供2套工程源码和技术支持

目录 1、前言工程概述免责声明 2、相关方案推荐我已有的PCIE方案本博已有的 SDI 编解码方案 3、PCIE基础知识扫描4、工程详细设计方案工程设计原理框图电脑端视频QT上位机XDMA配置及使用XDMA中断模块FDMA图像缓存SDI视频编码之-->RGB转BT1120SDI视频编码之-->SMPTE SD/HD…
阅读更多...
【C语言指南】数据类型详解（上）——内置类型

【C语言指南】数据类型详解（上）——内置类型

&#x1f493; 博客主页&#xff1a;倔强的石头的CSDN主页 &#x1f4dd;Gitee主页&#xff1a;倔强的石头的gitee主页 ⏩ 文章专栏&#xff1a;《C语言指南》 期待您的关注 目录 引言 1. 整型&#xff08;Integer Types&#xff09; 2. 浮点型&#xff08;Floating-Point …
阅读更多...
atop系统监控工具

atop系统监控工具

atop命令可以看作是top命令的增强版&#xff0c;它可以显示更详细的进程信息&#xff0c;如进程的CPU使用率、进程的内存使用率、进程的I/O使用率、网络使用率等&#xff1b;提供更丰富的统计信息及更灵活的配置&#xff0c;可以通过参数来控制显示内容和行为。 1、top和atop对…
阅读更多...
x-cmd pkg | tokei - 代码统计利器，助你快速了解项目进度

x-cmd pkg | tokei - 代码统计利器，助你快速了解项目进度

目录 简介首次用户技术特点竞品和相关项目进一步阅读 简介 tokei 是一个使用 Rust 编写的显示有关代码统计信息的命令行工具&#xff0c;可以分门别类的统计目录内的代码行数。 tokei 具有良好的跨平台性&#xff0c;可以在 Linux、macOS、Windows 等多种平台上安装运行。 首…
阅读更多...
国产长芯微LDC8411数模转换芯片DAC完全P2P替代DAC8411

国产长芯微LDC8411数模转换芯片DAC完全P2P替代DAC8411

LDC8411&#xff08;16位&#xff09;器件是低功耗、单通道、电压输出数模转换器&#xff08;DAC&#xff09;。它们提供了出色的线性度&#xff0c;并最大限度地减少了不希望的码间瞬态电压&#xff0c;同时在引脚兼容系列中提供了一条简单的升级路径。所有设备都使用一个多功…
阅读更多...
ubuntu切换源方式记录（清华源、中科大源、阿里源）

ubuntu切换源方式记录（清华源、中科大源、阿里源）

文章目录 前言一、中科大源二、清华源三、阿里源 前言 记录ubunut切换各个源的方式。 备注&#xff1a;更换源之后使用sudo apt-get update更新索引。 提示&#xff1a;以下是本篇文章正文内容&#xff0c;下面案例可供参考 一、中科大源 地址&#xff1a;https://mirrors.u…
阅读更多...
Shell入门基础学习笔记

Shell入门基础学习笔记

目录 第1章 Shell概述 第2章 Shell解析器 第3章 Shell脚本入门 第4章 Shell中的变量 4.1 系统变量 4.2 自定义变量 4.3 特殊变量&#xff1a;$n 4.4 特殊变量&#xff1a;$# 4.5 特殊变量&#xff1a;$*、$ 4.6 特殊变量&#xff1a;$&#xff1f; 第5章 运算符 …
阅读更多...
4.模拟电子技术笔记——半导体三极管

4.模拟电子技术笔记——半导体三极管

写在前面 这个是第四个模电笔记&#xff0c;我们讲半导体三极管 这一章的很多概念都很重要&#xff0c;并且有一些需要记忆的内容&#xff0c;要认真对待 笔记部分 1.半导体三极管的基本原理简述 1.1结构&#xff1a; 1.这个箭头是PN结正向导通方向 2.有两个类型&#xf…
阅读更多...
PCL 移动立方体重建（HOPPE）

PCL 移动立方体重建（HOPPE）

目录 一、概述二、代码三、结果 一、概述 PCL中的 pcl::MarchingCubes<pcl::PointXYZRGBNormal>:函数实现移动立方体重建的代码示例。 二、代码 #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/io/ply_io.h> #include <pcl/point…
阅读更多...
生成-理解大一统：一文浅谈多模态大模型最新研究进展

生成-理解大一统：一文浅谈多模态大模型最新研究进展

在过去几年中&#xff0c;多模态智能的两个关键支柱——理解和生成&#xff0c;取得了显著进展。多模态大型语言模型&#xff08;MLLMs&#xff09;&#xff0c;如 LLaVA&#xff0c;在视觉语言任务&#xff08;例如视觉问答&#xff09;中表现出色。同时&#xff0c;去噪扩散概…
阅读更多...
转行AI产品经理前真后悔没看到这篇…

转行AI产品经理前真后悔没看到这篇…

最近AI行业发展势头正盛&#xff0c;很多人私信我说都来问我AI产品经理转行的事&#xff0c;希望我能给一些意见 文科生能不能做产品经理&#xff0c;大家对这些是一头雾水&#xff0c;也不知道AI产品经理具体都做些什么&#xff0c;又要具备那些能力 因为在不同的业务发展不一…
阅读更多...
领夹麦克风性价比最高？一文看懂领夹麦克风什么牌子的好

领夹麦克风性价比最高？一文看懂领夹麦克风什么牌子的好

近几年随着网络直播、短视频等新兴行业的发展&#xff0c;筑就了一个全民视频创作的时代。而领夹麦克风也是凭借轻便、便携的特性&#xff0c;获得了广大短视频创作者的青睐&#xff0c;领夹麦克风的需求量也是不断增加。也正是因为如此&#xff0c;如今市面上的领夹麦克风品牌…
阅读更多...
一文上手SpringSecurity【八】

一文上手SpringSecurity【八】

RBAC&#xff08;Role-Based Access Control&#xff09;&#xff0c;基于角色的访问控制。通过用户关联角色&#xff0c;角色关联权限&#xff0c;来间接的为用户赋予权限。 一、RBAC介绍 RBAC&#xff08;Role-Based Access Control&#xff09;&#xff0c;即基于角色的访…
阅读更多...
企业微信：客户联系自带群发工具和聊天工具

企业微信：客户联系自带群发工具和聊天工具

前言 上篇博客介绍了一些客户联系的开启和配置&#xff0c;接下来我们来使用客户联系自带群发工具和聊天工具。 突然发现官方的文档已经很详细了&#xff0c;我这里给出一些简单的描述&#xff1a; 企业微信如何使用群发助手&#xff1f;-帮助中心-企业微信 群发工具 群发消息给…
阅读更多...
Python 中的lambda函数表达式

Python 中的lambda函数表达式

lambda x:xn 这是一个 Python 中的lambda函数表达式。它定义了一个匿名函数&#xff0c;该函数接受一个参数x&#xff0c;并返回xn的值。再定义常数n。 n 5 my_function lambda x: x n print(my_function(3)) 运行结果&#xff1a; 在上述代码中&#xff0c;首先定义了…
阅读更多...
win10专业版永久关闭自动更新功能

win10专业版永久关闭自动更新功能

如何关闭 Windows 10 自动更新 Windows 10 默认情况下会自动安装更新&#xff0c;这可能会导致系统不稳定或问题。如果您想关闭自动更新&#xff0c;可以使用以下方法&#xff1a; 方法 1&#xff1a;使用设置应用程序 打开“设置”应用程序。转到“更新和安全”。在“Windo…
阅读更多...
最新文章

Copyright @ 2022~2023