状态更新和卡尔曼增益的计算我选择一个进行举例
1.SelectMagFusion
SelectMagFusion 函数主要负责选择和处理磁力计（磁传感器）数据的融合过程。这在导航系统中尤为重要，因为磁力计用于提供航向（偏航角）的信息，结合其他传感器数据（如IMU和GPS）来提高导航精度。
void NavEKF2_core::SelectMagFusion()
{
// clear the flag that lets other processes know that the expensive magnetometer fusion operation has been performed on that time step
// used for load levelling
//表示当前时间步尚未执行磁力计的融合操作。
magFusePerformed = false;
// Handle case where we are not using a yaw sensor of any type and and attempt to reset the yaw in
// flight using the output from the GSF yaw estimator.
//处理不使用任何类型的航向传感器，并尝试在飞行中使用GSF航向估计器的输出重置航向的情况
if (!use_compass() && tiltAlignComplete) {
if ((onGround || !assume_zero_sideslip()) && (imuSampleTime_ms - lastYawTime_ms > 140)) {
fuseEulerYaw();
}
if (yawAlignComplete) {
return;
}
yawAlignComplete = EKFGSF_resetMainFilterYaw();
return;
}
// If we are using the compass and the magnetometer has been unhealthy for too long we declare a timeout
// 如果我们正在使用指南针，并且磁力计长时间不健康，则声明超时
if (magHealth) {
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - lastHealthyMagTime_ms) > frontend->magFailTimeLimit_ms && use_compass()) {
magTimeout = true;
}
// check for and read new magnetometer measurements
// 检查并读取新的磁力计测量值
readMagData();
// check for availability of magnetometer data to fuse
// 检查是否有可用于融合的磁力计数据
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
// Control reset of yaw and magnetic field states if we are using compass data
//如果有可用于融合的磁力计数据并且正在使用指南针，控制航向和磁场状态的重置
if (magDataToFuse && use_compass()) {
controlMagYawReset();
}
// determine if conditions are right to start a new fusion cycle
// wait until the EKF time horizon catches up with the measurement
// 判断条件是否适合开始新的融合周期
// 等待EKF时间范围与测量对齐
bool dataReady = (magDataToFuse && statesInitialised && use_compass() && yawAlignComplete);
if (dataReady) {
// use the simple method of declination to maintain heading if we cannot use the magnetic field states
// 如果不能使用磁场状态，使用简单的偏角方法维持航向
if(inhibitMagStates || magStateResetRequest || !magStateInitComplete) {
fuseEulerYaw();
// zero the test ratio output from the inactive 3-axis magnetometer fusion
// 清零来自非激活三轴磁力计融合的测试比率输出
magTestRatio.zero();
} else {
// if we are not doing aiding with earth relative observations (eg GPS) then the declination is
// maintained by fusing declination as a synthesised observation
// We also fuse declination if we are using the WMM tables
// 如果不使用地球相对观测的辅助（例如GPS），则通过融合偏角作为合成观测来维持偏角
// 如果使用WMM表，也融合偏角
if (PV_AidingMode != AID_ABSOLUTE ||
(frontend->_mag_ef_limit > 0 && have_table_earth_field)) {
FuseDeclination(0.34f);
}
// fuse the three magnetometer componenents using sequential fusion of each axis
// 使用顺序融合每个轴的三轴磁力计组件
FuseMagnetometer();
// zero the test ratio output from the inactive simple magnetometer yaw fusion
// 清零来自非激活简单磁力计航向融合的测试比率输出
yawTestRatio = 0.0f;
}
}
// If the final yaw reset has been performed and the state variances are sufficiently low
// record that the earth field has been learned.
// 如果最终的航向重置已经执行，并且状态方差足够低，记录地磁场已经被学习
if (!magFieldLearned && finalInflightMagInit) {
magFieldLearned = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f));
}
// record the last learned field variances
// 记录最后学习到的地磁场方差
if (magFieldLearned && !inhibitMagStates) {
earthMagFieldVar.x = P[16][16];
earthMagFieldVar.y = P[17][17];
earthMagFieldVar.z = P[18][18];
bodyMagFieldVar.x = P[19][19];
bodyMagFieldVar.y = P[20][20];
bodyMagFieldVar.z = P[21][21];
}
}
2.FuseMagnetometer
这段代码是一个名为 FuseMagnetometer 的函数，属于 NavEKF2_core 类。该函数的主要功能是将磁力计（磁力传感器）的数据融合到扩展卡尔曼滤波器（EKF）中，以提高姿态估计的准确性。
void NavEKF2_core::FuseMagnetometer()
{
// declarations
//声明和引用mag_state中的变量
ftype &q0 = mag_state.q0;
ftype &q1 = mag_state.q1;
ftype &q2 = mag_state.q2;
ftype &q3 = mag_state.q3;
ftype &magN = mag_state.magN;
ftype &magE = mag_state.magE;
ftype &magD = mag_state.magD;
ftype &magXbias = mag_state.magXbias;
ftype &magYbias = mag_state.magYbias;
ftype &magZbias = mag_state.magZbias;
Matrix3F &DCM = mag_state.DCM;
Vector3F &MagPred = mag_state.MagPred;
ftype &R_MAG = mag_state.R_MAG;
ftype *SH_MAG = &mag_state.SH_MAG[0];
Vector24 H_MAG;
Vector6 SK_MX;
Vector6 SK_MY;
Vector6 SK_MZ;
// copy required states to local variable names
// 将需要的状态变量从stateStruct复制到本地变量
q0 = stateStruct.quat[0];
q1 = stateStruct.quat[1];//stateStruct.quat 表示姿态的四元数，用于描述旋转。
q2 = stateStruct.quat[2];
q3 = stateStruct.quat[3];
magN = stateStruct.earth_magfield[0];//earth_magfield 表示地球磁场在地理坐标系下的分量（北、东、下）。
magE = stateStruct.earth_magfield[1];
magD = stateStruct.earth_magfield[2];
magXbias = stateStruct.body_magfield[0];
magYbias = stateStruct.body_magfield[1];//body_magfield 表示磁力计在机体坐标系下的偏置。
magZbias = stateStruct.body_magfield[2];
// rotate predicted earth components into body axes and calculate
// predicted measurements
// 根据四元数计算方向余弦矩阵，从地理坐标系转换到机体坐标系。
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
DCM[0][1] = 2.0f*(q1*q2 + q0*q3);
DCM[0][2] = 2.0f*(q1*q3-q0*q2);
DCM[1][0] = 2.0f*(q1*q2 - q0*q3);
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
DCM[1][2] = 2.0f*(q2*q3 + q0*q1);
DCM[2][0] = 2.0f*(q1*q3 + q0*q2);
DCM[2][1] = 2.0f*(q2*q3 - q0*q1);
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 计算预测的磁力计测量值，根据DCM将地磁场分量旋转到机体坐标系
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
// calculate the measurement innovation for each axis
// 计算每个轴的测量创新，预测测量值与实际测量值之间的差异，用于更新卡尔曼滤波器的状态。
for (uint8_t i = 0; i<=2; i++) {
innovMag[i] = MagPred[i] - magDataDelayed.mag[i];
}
// scale magnetometer observation error with total angular rate to allow for timing errors
//磁力计的测量噪声协方差
R_MAG = sq(constrain_ftype(frontend->_magNoise, 0.01f, 0.5f)) + sq(frontend->magVarRateScale*delAngCorrected.length() / imuDataDelayed.delAngDT);
// calculate common expressions used to calculate observation jacobians an innovation variance for each component
//预计算的一些与四元数和磁场相关的中间变量，用于后续计算观测雅可比矩阵和创新方差，提高计算效率。
SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
SH_MAG[3] = 2.0f*q0*q1 + 2.0f*q2*q3;
SH_MAG[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
SH_MAG[5] = 2.0f*q0*q2 + 2.0f*q1*q3;
SH_MAG[6] = magE*(2.0f*q0*q1 - 2.0f*q2*q3);
SH_MAG[7] = 2.0f*q1*q3 - 2.0f*q0*q2;
SH_MAG[8] = 2.0f*q0*q3;
// Calculate the innovation variance for each axis
// X axis
// X轴的创新方差计算
varInnovMag[0] = (P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))));
if (varInnovMag[0] >= R_MAG) {//如果创新方差大于等于测量噪声协方差，表示计算条件良好，继续进行融合。
faultStatus.bad_xmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
//否则，认为计算条件不佳（可能存在故障或异常），重置协方差矩阵，
CovarianceInit();
faultStatus.bad_xmag = true;
return;
}
// Y axis
varInnovMag[1] = (P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2.0f*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2.0f*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2.0f*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2.0f*q1*q2)) - P[16][20]*(SH_MAG[8] - 2.0f*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2.0f*q1*q2)));
if (varInnovMag[1] >= R_MAG) {
faultStatus.bad_ymag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
faultStatus.bad_ymag = true;
return;
}
// Z axis
varInnovMag[2] = (P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2.0f*q0*q1 - 2.0f*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2.0f*q0*q1 - 2.0f*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2.0f*q0*q1 - 2.0f*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2.0f*q0*q1 - 2.0f*q2*q3)) - P[17][21]*(2.0f*q0*q1 - 2.0f*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2.0f*q0*q1 - 2.0f*q2*q3)));
if (varInnovMag[2] >= R_MAG) {
faultStatus.bad_zmag = false;
} else {
// the calculation is badly conditioned, so we cannot perform fusion on this step
// we reset the covariance matrix and try again next measurement
CovarianceInit();
faultStatus.bad_zmag = true;
return;
}
// calculate the innovation test ratios
// 计算创新测试比率，用于判断测量是否异常的比率
for (uint8_t i = 0; i<=2; i++) {
magTestRatio[i] = sq(innovMag[i]) / (sq(MAX(0.01f * (ftype)frontend->_magInnovGate, 1.0f)) * varInnovMag[i]);
}
// check the last values from all components and set magnetometer health accordingly
// 检查所有轴的测试比率，设置磁力计健康状态
magHealth = (magTestRatio[0] < 1.0f && magTestRatio[1] < 1.0f && magTestRatio[2] < 1.0f);
// if the magnetometer is unhealthy, do not proceed further
// 如果磁力计不健康，终止融合过程
if (!magHealth) {
return;
}
// perform sequential fusion of magnetometer measurements.
// this assumes that the errors in the different components are
// uncorrelated which is not true, however in the absence of covariance
// data fit is the only assumption we can make
// so we might as well take advantage of the computational efficiencies
// associated with sequential fusion
// calculate observation jacobians and Kalman gains
//计算观测雅可比矩阵和卡尔曼增益
for (uint8_t obsIndex = 0; obsIndex <= 2; obsIndex++) {
if (obsIndex == 0)
{
// calculate observation jacobians
//H_MAG[] 是观测雅可比矩阵（Observation Jacobian），
ZERO_FARRAY(H_MAG);
H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5];
H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
H_MAG[16] = SH_MAG[1];
H_MAG[17] = SH_MAG[4];
H_MAG[18] = SH_MAG[7];
H_MAG[19] = 1.0f;
// calculate Kalman gain
//SK_MX[] 和 SK_MY[] 分别对应融合 X 和 Y 轴观测时的中间变量，用于计算卡尔曼增益。
SK_MX[0] = 1.0f / varInnovMag[0];
SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
SK_MX[3] = SH_MAG[7];
//Kfusion[] 是卡尔曼增益向量
//卡尔曼增益用于平衡预测和实际观测的权重，增益越大，观测的权重越大，反之则更依赖预测模型。Kfusion[]通过状态协方差矩阵 P[][] 和观测矩阵的组合计算。
Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]);
Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]);
Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]);
Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]);
Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]);
Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]);
Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]);
Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]);
Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]);
Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]);
Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]);
Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]);
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]);
Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]);
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]);
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]);
// zero Kalman gains to inhibit wind state estimation
//根据变量 inhibitWindStates 和 inhibitMagStates，代码可能会抑制风状态和磁场状态的估计
if (!inhibitWindStates) {
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]);
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]);
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]);
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]);
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]);
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// set flags to indicate to other processes that fusion has been performed
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
//magFusePerformed = true 表示完成了这次磁力计观测数据的融合。
magFusePerformed = true;
}
else if (obsIndex == 1) // we are now fusing the Y measurement
{
// calculate observation jacobians
ZERO_FARRAY(H_MAG);
H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1];
H_MAG[16] = 2.0f*q1*q2 - SH_MAG[8];
H_MAG[17] = SH_MAG[0];
H_MAG[18] = SH_MAG[3];
H_MAG[20] = 1.0f;
// calculate Kalman gain
SK_MY[0] = 1.0f / varInnovMag[1];
SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5];
SK_MY[3] = SH_MAG[8] - 2.0f*q1*q2;
Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]);
Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]);
Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]);
Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]);
Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]);
Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]);
Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]);
Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]);
Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]);
Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]);
Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]);
Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]);
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]);
Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]);
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]);
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]);
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]);
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]);
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]);
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]);
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]);
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// set flags to indicate to other processes that fusion has been performed
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
}
else if (obsIndex == 2) // we are now fusing the Z measurement
{
// calculate observation jacobians
ZERO_FARRAY(H_MAG);
H_MAG[0] = magN*(SH_MAG[8] - 2.0f*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0];
H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
H_MAG[16] = SH_MAG[5];
H_MAG[17] = 2.0f*q2*q3 - 2.0f*q0*q1;
H_MAG[18] = SH_MAG[2];
H_MAG[21] = 1.0f;
// calculate Kalman gain
SK_MZ[0] = 1.0f / varInnovMag[2];
SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2.0f*q1*q2);
SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1];
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]);
Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]);
Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]);
Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]);
Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]);
Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]);
Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]);
Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]);
Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]);
Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]);
Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]);
Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]);
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]);
Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]);
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]);
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]);
// zero Kalman gains to inhibit wind state estimation
if (!inhibitWindStates) {
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]);
} else {
Kfusion[22] = 0.0f;
Kfusion[23] = 0.0f;
}
// zero Kalman gains to inhibit magnetic field state estimation
if (!inhibitMagStates) {
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]);
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]);
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]);
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]);
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]);
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]);
} else {
for (uint8_t i=16; i<=21; i++) {
Kfusion[i] = 0.0f;
}
}
// set flags to indicate to other processes that fusion has been performed
// this can be used by other fusion processes to avoid fusing on the same frame as this expensive step
magFusePerformed = true;
}
//协方差矩阵和状态向量的更新
// correct the covariance P = (I - K*H)*P
// 修正协方差 P = (I - K*H)*P
// take advantage of the empty columns in KH to reduce the
// number of operations
for (unsigned i = 0; i<=stateIndexLim; i++) {
for (unsigned j = 0; j<=2; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
for (unsigned j = 3; j<=15; j++) {
KH[i][j] = 0.0f;
}
for (unsigned j = 16; j<=21; j++) {
KH[i][j] = Kfusion[i] * H_MAG[j];
}
for (unsigned j = 22; j<=23; j++) {
KH[i][j] = 0.0f;
}
}
for (unsigned j = 0; j<=stateIndexLim; j++) {
for (unsigned i = 0; i<=stateIndexLim; i++) {
ftype res = 0;
res += KH[i][0] * P[0][j];
res += KH[i][1] * P[1][j];
res += KH[i][2] * P[2][j];
res += KH[i][16] * P[16][j];
res += KH[i][17] * P[17][j];
res += KH[i][18] * P[18][j];
res += KH[i][19] * P[19][j];
res += KH[i][20] * P[20][j];
res += KH[i][21] * P[21][j];
KHP[i][j] = res;
}
}
// 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-conditioning.
ForceSymmetry();
ConstrainVariances();
// update the states
// zero the attitude error state - by definition it is assumed to be zero before each observation fusion
//更新状态向量
// 将姿态误差状态置零
stateStruct.angErr.zero();
// correct the state vector
// 修正状态向量
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * innovMag[obsIndex];
}
// add table constraint here for faster convergence
if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
MagTableConstrain();
}
// the first 3 states represent the angular misalignment vector.
// This is used to correct the estimated quaternion on the current time step
stateStruct.quat.rotate(stateStruct.angErr);
} else {
// record bad axis
if (obsIndex == 0) {
faultStatus.bad_xmag = true;
} else if (obsIndex == 1) {
faultStatus.bad_ymag = true;
} else if (obsIndex == 2) {
faultStatus.bad_zmag = true;
}
CovarianceInit();
return;
}
}
}
3. calcOutputStates();
1. IMU数据校正
- 函数首先通过IMU传感器数据来获取角增量 (
delAng) 和速度增量 (delVel)，并应用一些校正函数（correctDeltaAngle和correctDeltaVelocity）来修正这些值。这一步是为了去除IMU中的漂移或噪声对测量的影响。
2. 姿态估计和四元数更新
- 利用修正后的角增量，计算相应的四元数
deltaQuat，这是用来表示从上次姿态更新到当前时间段的旋转变化。 - 更新四元数状态，将之前的姿态通过新的四元数来进行旋转更新，并对四元数进行归一化处理，保持其单位长度（四元数的规范性）。
3. 坐标系转换与速度更新
- 计算体坐标系到导航坐标系的旋转矩阵
Tbn_temp，通过四元数生成该矩阵。 - 将体坐标系下的速度增量转换到导航坐标系，并考虑到重力的影响。
- 通过对速度增量的累加，更新导航坐标系下的速度。
4. 高度滤波与更新
- 实现了一个三阶互补滤波器，用于融合气压计与惯性传感器的高度数据。这一部分引用了一篇关于高度估计滤波器的文献，并使用反向欧拉积分法对高度和高度速率进行估计。
5. 位置更新
- 使用梯形积分法对速度进行积分，进而估计出导航坐标系下的位置。这是一种数值积分方法，可以有效减小误差。
6. IMU与车体中心之间的偏移校正
- 如果IMU传感器不在车辆的重心上，还会计算IMU相对于车辆中心的速度和位置的校正，这涉及到对角速度的交叉积（旋转效应）进行修正。
7. INS与EKF的跟踪校正
-
存储和回放INS状态，确保与EKF估计的状态保持一致性。
-
通过对四元数进行归一化比较，计算INS和EKF之间的误差，接着用一个基于时间延迟的增益来调整角增量（
delAngCorrection），使得INS的状态能准确跟踪EKF的估计值。 -
还会计算速度和位置的跟踪误差，并应用一个PI反馈控制器来修正INS的状态历史。这样做的好处是不引入时间延迟，可以更快速地响应误差，确保估计的精度。
8. 输出状态历史的修正
- 最后，遍历输出状态历史，对其中的速度和位置进行相应的修正，使得历史记录的数据也能够保持与最新的估计结果一致。
void NavEKF2_core::calcOutputStates()
{
// apply corrections to the IMU data
// 应用对IMU数据的校正
Vector3F delAngNewCorrected = imuDataNew.delAng;
Vector3F delVelNewCorrected = imuDataNew.delVel;
correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index);
correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index);
// apply corections to track EKF solution
// 应用校正以跟踪EKF解
Vector3F delAng = delAngNewCorrected + delAngCorrection;
// convert the rotation vector to its equivalent quaternion
// 将旋转向量转换为等效的四元数
QuaternionF deltaQuat;
deltaQuat.from_axis_angle(delAng);
// update the quaternion states by rotating from the previous attitude through
// the delta angle rotation quaternion and normalise
// 通过旋转四元数更新姿态状态并归一化
outputDataNew.quat *= deltaQuat;
outputDataNew.quat.normalize();
// calculate the body to nav cosine matrix
// 计算体坐标系到导航坐标系的旋转矩阵
Matrix3F Tbn_temp;
outputDataNew.quat.rotation_matrix(Tbn_temp);
// transform body delta velocities to delta velocities in the nav frame
// 将体坐标系下的速度增量转换到导航坐标系
Vector3F delVelNav = Tbn_temp*delVelNewCorrected;
delVelNav.z += GRAVITY_MSS*imuDataNew.delVelDT;
// save velocity for use in trapezoidal integration for position calcuation
// 保存上一时刻的速度，用于梯形积分法
Vector3F lastVelocity = outputDataNew.velocity;
// sum delta velocities to get velocity
// 累加速度增量以更新速度
outputDataNew.velocity += delVelNav;
// Implement third order complementary filter for height and height rate
// Reference Paper :
// Optimizing the Gains of the Baro-Inertial Vertical Channel
// Widnall W.S, Sinha P.K,
// AIAA Journal of Guidance and Control, 78-1307R
// Perform filter calculation using backwards Euler integration
// Coefficients selected to place all three filter poles at omega
// 使用向后欧拉积分法进行滤波计算
// 系数选择用于将所有三个滤波器极点置于omega
const ftype CompFiltOmega = M_2PI * constrain_ftype(frontend->_hrt_filt_freq, 0.1f, 30.0f);
ftype omega2 = CompFiltOmega * CompFiltOmega;
ftype pos_err = constrain_ftype(outputDataNew.position.z - vertCompFiltState.pos, -1e5, 1e5);
ftype integ1_input = pos_err * omega2 * CompFiltOmega * imuDataNew.delVelDT;
vertCompFiltState.acc += integ1_input;
ftype integ2_input = delVelNav.z + (vertCompFiltState.acc + pos_err * omega2 * 3.0f) * imuDataNew.delVelDT;
vertCompFiltState.vel += integ2_input;
ftype integ3_input = (vertCompFiltState.vel + pos_err * CompFiltOmega * 3.0f) * imuDataNew.delVelDT;
vertCompFiltState.pos += integ3_input;
// apply a trapezoidal integration to velocities to calculate position
// 使用梯形积分法对速度进行积分以计算位置
outputDataNew.position += (outputDataNew.velocity + lastVelocity) * (imuDataNew.delVelDT*0.5f);
// If the IMU accelerometer is offset from the body frame origin, then calculate corrections
// that can be added to the EKF velocity and position outputs so that they represent the velocity
// and position of the body frame origin.
// Note the * operator has been overloaded to operate as a dot product
// 如果IMU加速度计偏移了车体坐标系原点，则计算相应的校正
if (!accelPosOffset.is_zero()) {
// calculate the average angular rate across the last IMU update
// note delAngDT is prevented from being zero in readIMUData()
// 计算最近一次IMU更新的平均角速度
// 注意在readIMUData()中delAngDT被防止为零
Vector3F angRate = imuDataNew.delAng * (1.0f/imuDataNew.delAngDT);
// Calculate the velocity of the body frame origin relative to the IMU in body frame
// and rotate into earth frame. Note % operator has been overloaded to perform a cross product
// 计算车体坐标系原点相对于IMU的速度（体坐标系下）并旋转到地理坐标系
// 注意 % 操作符已被重载为叉积
Vector3F velBodyRelIMU = angRate % (- accelPosOffset);
velOffsetNED = Tbn_temp * velBodyRelIMU;
// calculate the earth frame position of the body frame origin relative to the IMU
// 计算车体坐标系原点相对于IMU在地理坐标系下的位置
posOffsetNED = Tbn_temp * (- accelPosOffset);
} else {
velOffsetNED.zero();
posOffsetNED.zero();
}
// store INS states in a ring buffer that with the same length and time coordinates as the IMU data buffer
// 在与IMU数据缓冲区相同长度和时间坐标的环形缓冲区中存储INS状态
if (runUpdates) {
// store the states at the output time horizon
// 存储输出时间范围内的状态
storedOutput[storedIMU.get_youngest_index()] = outputDataNew;
// recall the states from the fusion time horizon
// 从融合时间范围内回忆状态
outputDataDelayed = storedOutput[storedIMU.get_oldest_index()];
// compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
// 比较四元数数据与EKF在融合时间范围内的四元数，并计算校正
// divide the demanded quaternion by the estimated to get the error
// 将需求四元数除以估计四元数以获取误差
QuaternionF quatErr = stateStruct.quat / outputDataDelayed.quat;
// Convert to a delta rotation using a small angle approximation
// 使用小角度近似将误差转换为增量旋转
quatErr.normalize();
Vector3F deltaAngErr;
ftype scaler;
if (quatErr[0] >= 0.0f) {
scaler = 2.0f;
} else {
scaler = -2.0f;
}
deltaAngErr.x = scaler * quatErr[1];
deltaAngErr.y = scaler * quatErr[2];
deltaAngErr.z = scaler * quatErr[3];
// calculate a gain that provides tight tracking of the estimator states and
// adjust for changes in time delay to maintain consistent damping ratio of ~0.7
// 计算一个增益，以提供对估计器状态的紧密跟踪，并调整时间延迟以保持约0.7的阻尼比
ftype timeDelay = 1e-3f * (float)(imuDataNew.time_ms - imuDataDelayed.time_ms);
timeDelay = fmaxF(timeDelay, dtIMUavg);
ftype errorGain = 0.5f / timeDelay;
// calculate a corrrection to the delta angle
// that will cause the INS to track the EKF quaternions
// 计算对角度增量的校正，以使INS跟踪EKF的四元数
delAngCorrection = deltaAngErr * errorGain * dtIMUavg;
// calculate velocity and position tracking errors
// 计算速度和位置的跟踪误差
Vector3F velErr = (stateStruct.velocity - outputDataDelayed.velocity);
Vector3F posErr = (stateStruct.position - outputDataDelayed.position);
// collect magnitude tracking error for diagnostics
// 收集用于诊断的跟踪误差
outputTrackError.x = deltaAngErr.length();
outputTrackError.y = velErr.length();
outputTrackError.z = posErr.length();
// convert user specified time constant from centi-seconds to seconds
ftype tauPosVel = constrain_ftype(0.01f*(ftype)frontend->_tauVelPosOutput, 0.1f, 0.5f);
// calculate a gain to track the EKF position states with the specified time constant
ftype velPosGain = dtEkfAvg / constrain_ftype(tauPosVel, dtEkfAvg, 10.0f);
// use a PI feedback to calculate a correction that will be applied to the output state history
posErrintegral += posErr;
velErrintegral += velErr;
Vector3F velCorrection = velErr * velPosGain + velErrintegral * sq(velPosGain) * 0.1f;
Vector3F posCorrection = posErr * velPosGain + posErrintegral * sq(velPosGain) * 0.1f;
// loop through the output filter state history and apply the corrections to the velocity and position states
// this method is too expensive to use for the attitude states due to the quaternion operations required
// but does not introduce a time delay in the 'correction loop' and allows smaller tracking time constants
// to be used
output_elements outputStates;
for (unsigned index=0; index < imu_buffer_length; index++) {
outputStates = storedOutput[index];
// a constant velocity correction is applied
outputStates.velocity += velCorrection;
// a constant position correction is applied
outputStates.position += posCorrection;
// push the updated data to the buffer
storedOutput[index] = outputStates;
}
// update output state to corrected values
outputDataNew = storedOutput[storedIMU.get_youngest_index()];
}
}
