Imu_PreIntegrate_01 预积分

/** 对一笔IMU数据进行积分*/
void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt)
{mvMeasurements.push_back(integrable(acceleration,angVel,dt));//记录原始数据// Position is updated firstly, as it depends on previously computed velocity and rotation.// Velocity is updated secondly, as it depends on previously computed rotation.// Rotation is the last to be updated.// 先更新Position再velocity再Rotation//Matrices to compute covarianceEigen::Matrix<float,9,9> A;//噪声的递推矩阵AA.setIdentity();Eigen::Matrix<float,9,6> B;//噪声的递推矩阵BB.setZero();Eigen::Vector3f acc, accW;acc << acceleration(0)-b.bax, acceleration(1)-b.bay, acceleration(2)-b.baz;// 这里的acc = 公式里的 w-bias。 后面的 Wacc = exp((w-b)*delta_t）accW << angVel(0)-b.bwx, angVel(1)-b.bwy, angVel(2)-b.bwz;//更新平均值 初始化的时候，如果需要有一定的速度才能初始化mFastInit，就要参考avgA = (dT*avgA + dR*acc*dt)/(dT+dt);avgW = (dT*avgW + accW*dt)/(dT+dt);// Update delta position dP and velocity dV (rely on no-updated delta rotation)dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;//公式 15-43 这里的acc已经扣除biasdV = dV + dR*acc*dt; //公式 15-40 这里的acc已经扣除bias// Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation)Eigen::Matrix<float,3,3> Wacc = Sophus::SO3f::hat(acc);//Wacc = exp((w-b)*delta_t）// 开始更新噪声A.block<3,3>(3,0) = -dR*dt*Wacc;//公式 15-48 A row2 col1A.block<3,3>(6,0) = -0.5f*dR*dt*dt*Wacc;//公式 15-48 A row3 col1A.block<3,3>(6,3) = Eigen::DiagonalMatrix<float,3>(dt, dt, dt);//公式 15-48 A row3 col2B.block<3,3>(3,3) = dR*dt;//公式 15-48 B row2 col2B.block<3,3>(6,3) = 0.5f*dR*dt*dt;//公式 15-48 B row3 col2// Update position and velocity jacobians wrt bias correctionJPa = JPa + JVa*dt -0.5f*dR*dt*dt;//零偏更新 公式 15-67JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;//零偏更新 公式 15-66JVa = JVa - dR*dt;//零偏更新 公式 15-62JVg = JVg - dR*dt*Wacc*JRg;//零偏更新 公式 15-63// Update delta rotationIntegratedRotation dRi(angVel,b,dt);//公式 15-29dR = NormalizeRotation(dR*dRi.deltaR);//归一化// Compute rotation parts of matrices A and BA.block<3,3>(0,0) = dRi.deltaR.transpose();//公式 15-48 A row1 col1B.block<3,3>(0,0) = dRi.rightJ*dt;//公式 15-48 B row1 col1// Update covariance// 更新协方差矩阵，矩阵位位9*6矩阵 Nga为6*6矩阵 todoC.block<9,9>(0,0) = A * C.block<9,9>(0,0) * A.transpose() + B*Nga*B.transpose();C.block<6,6>(9,9) += NgaWalk;// Update rotation jacobian wrt bias correctionJRg = dRi.deltaR.transpose()*JRg - dRi.rightJ*dt;//零偏更新 公式 15-60// Total integrated timedT += dt;
}