redoPreintegration 和 propagation 定义类似

// Propagates pose, speeds and biases with given IMU measurements. int ImuError::redoPreintegration(const okvis::kinematics::Transformation& /*T_WS*/, const okvis::SpeedAndBias & speedAndBiases) const int ImuError::propagation(const okvis::ImuMeasurementDeque & imuMeasurements, const okvis::ImuParameters & imuParams, okvis::kinematics::Transformation& T_WS, okvis::SpeedAndBias & speedAndBiases, const okvis::Time & t_start, const okvis::Time & t_end, covariance_t* covariance, jacobian_t* jacobian){ // now the propagation okvis::Time time = t_start; okvis::Time end = t_end; // sanity check: assert(imuMeasurements.front().timeStamp<=time); if (!(imuMeasurements.back().timeStamp >= end)) return -1; // nothing to do... // initial condition Eigen::Vector3d r_0 = T_WS.r(); Eigen::Quaterniond q_WS_0 = T_WS.q(); Eigen::Matrix3d C_WS_0 = T_WS.C();

propagation 初值赋值： 位姿 translation 部分： r0=t{TWS} 位姿转换成四元数： qWS0=q{TWS} 位姿旋转部分： CWS0=C{TWS}

// increments (initialise with identity) Eigen::Quaterniond Delta_q(1,0,0,0); Eigen::Matrix3d C_integral = Eigen::Matrix3d::Zero(); Eigen::Matrix3d C_doubleintegral = Eigen::Matrix3d::Zero(); Eigen::Vector3d acc_integral = Eigen::Vector3d::Zero(); Eigen::Vector3d acc_doubleintegral = Eigen::Vector3d::Zero();

积分初值: 四元数积分： Δq=(1,0,0,0) 旋转矩阵积分： ∫C=0(3,3) 旋转矩阵双重积分： ∬C=0(3,3) 加速度积分： ∫a=(0,0,0) 加速度双重积分： ∬a=(0,0,0)

// cross matrix accumulatrion Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();

Mcross=0(3,3)

// sub-Jacobians Eigen::Matrix3d dalpha_db_g = Eigen::Matrix3d::Zero(); Eigen::Matrix3d dv_db_g = Eigen::Matrix3d::Zero(); Eigen::Matrix3d dp_db_g = Eigen::Matrix3d::Zero();

子雅各比矩阵初始化 角速度对角速度偏置偏导： dαdbg=0(3,3) 速度对角速度偏置偏导： dvdbg=0(3,3) 位移对角速度偏置偏导： dpdbg=0(3,3)

// the Jacobian of the increment (w/o biases) Eigen::Matrix<double,15,15> P_delta = Eigen::Matrix<double,15,15>::Zero();

increament 变量 δx 偏导矩阵初始化： Pδ=I(15,15)

double Delta_t = 0;

从最开始到当前次积分的时间间隔： Δt=0

for

// hasStarted 第一次执行标识 bool hasStarted = false; int i = 0; for (okvis::ImuMeasurementDeque::const_iterator it = imuMeasurements.begin(); it != imuMeasurements.end(); ++it) { Eigen::Vector3d omega_S_0 = it->measurement.gyroscopes; Eigen::Vector3d acc_S_0 = it->measurement.accelerometers; Eigen::Vector3d omega_S_1 = (it + 1)->measurement.gyroscopes; Eigen::Vector3d acc_S_1 = (it + 1)->measurement.accelerometers;

it 个角速度测量： Sω0 it 个加速度测量： Sa0 it+1 个IMU 测量： Sω1 it+1 个加速度测量： Sa1

// time delta okvis::Time nexttime; if ((it + 1) == imuMeasurements.end()) { nexttime = t_end; } else nexttime = (it + 1)->timeStamp; double dt = (nexttime - time).toSec(); // 当 end 小于 nexttime 时，end 处 IMU 的测量值通过插值得到 if (end < nexttime) { double interval = (nexttime - it->timeStamp).toSec(); nexttime = t_end; dt = (nexttime - time).toSec(); const double r = dt / interval; omega_S_1 = ((1.0 - r) * omega_S_0 + r * omega_S_1).eval(); acc_S_1 = ((1.0 - r) * acc_S_0 + r * acc_S_1).eval(); } if (dt <= 0.0) { continue; } Delta_t += dt; // 同样对于输入初始时刻 IMU 的测量值通过插值得到 if (!hasStarted) { hasStarted = true; const double r = dt / (nexttime - it->timeStamp).toSec(); omega_S_0 = (r * omega_S_0 + (1.0 - r) * omega_S_1).eval(); acc_S_0 = (r * acc_S_0 + (1.0 - r) * acc_S_1).eval(); } // ensure integrity double sigma_g_c = imuParams.sigma_g_c; double sigma_a_c = imuParams.sigma_a_c;

t0 到 t1 时间间隔： dt Δt=Δt+dt 从配置文件中读取的 gyro noise density [rad/s/sqrt(Hz)]： σgc 从配置文件中读取的 accelerometer noise density [m/s^2/sqrt(Hz)]： σac

// 读入的数据超过设定的最大值，不确定度乘 100 if (fabs(omega_S_0[0]) > imuParams.g_max || fabs(omega_S_0[1]) > imuParams.g_max || fabs(omega_S_0[2]) > imuParams.g_max || fabs(omega_S_1[0]) > imuParams.g_max || fabs(omega_S_1[1]) > imuParams.g_max || fabs(omega_S_1[2]) > imuParams.g_max) { sigma_g_c *= 100; LOG(WARNING) << "gyr saturation"; } if (fabs(acc_S_0[0]) > imuParams.a_max || fabs(acc_S_0[1]) > imuParams.a_max || fabs(acc_S_0[2]) > imuParams.a_max || fabs(acc_S_1[0]) > imuParams.a_max || fabs(acc_S_1[1]) > imuParams.a_max || fabs(acc_S_1[2]) > imuParams.a_max) { sigma_a_c *= 100; LOG(WARNING) << "acc saturation"; } //由角速度测量值和时间间隔积分得到四元数 // actual propagation // orientation: Eigen::Quaterniond dq; const Eigen::Vector3d omega_S_true = (0.5*(omega_S_0+omega_S_1) - speedAndBiases.segment<3>(3)); const double theta_half = omega_S_true.norm() * 0.5 * dt; const double sinc_theta_half = ode::sinc(theta_half); const double cos_theta_half = cos(theta_half); dq.vec() = sinc_theta_half * omega_S_true * 0.5 * dt; dq.w() = cos_theta_half; Eigen::Quaterniond Delta_q_1 = Delta_q * dq;

四元数积分： dq 角速度设为时间 t0 和 t1 平均值： Sω=0.5∗(Sω0+Sω1)−bg dqv=sin(||12Sω dt||)∗(12Sω dt) dqw=cos(||12Sω dt||) dq=(dqv,dqw) 当前次四元数积分： Δq1=Δq∗dq

// rotation matrix integral: const Eigen::Matrix3d C = Delta_q.toRotationMatrix(); const Eigen::Matrix3d C_1 = Delta_q_1.toRotationMatrix(); const Eigen::Vector3d acc_S_true = (0.5*(acc_S_0+acc_S_1) - speedAndBiases.segment<3>(6)); const Eigen::Matrix3d C_integral_1 = C_integral + 0.5*(C + C_1)*dt; const Eigen::Vector3d acc_integral_1 = acc_integral + 0.5*(C + C_1)*acc_S_true*dt; // rotation matrix double integral: C_doubleintegral += C_integral*dt + 0.25*(C + C_1)*dt*dt; acc_doubleintegral += acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt;

四元数转化成旋转矩阵： C=M{Δq} 四元数转化成旋转矩阵： C1=M{Δq1} 加速度设为时间 t0 和 t1 平均值： Sa=0.5∗(Sa0+Sa1)−ba ∫′C=∫C+0.5∗(C+C1)∗dt ∫′a=∫a+0.5∗(C+C1)∗Sa∗dt ∬C=∬C+∫C∗dt+0.25∗(C+C1)∗dt∗dt ∬a=∬a+∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt

// Jacobian parts dalpha_db_g += dt*C_1; const Eigen::Matrix3d cross_1 = dq.inverse().toRotationMatrix()*cross + okvis::kinematics::rightJacobian(omega_S_true*dt)*dt; const Eigen::Matrix3d acc_S_x = okvis::kinematics::crossMx(acc_S_true); Eigen::Matrix3d dv_db_g_1 = dv_db_g + 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1); dp_db_g += dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1);

dαdbg=dαdbg+C1∗dt Mcross1=C{dq−1}∗Mcross+Jr{Sω∗dt}∗dt dvdbg′=dvdbg+0.5∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1) dpdbg=dpdbg+dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)

covariance propagation

// covariance propagation if (covariance) { Eigen::Matrix<double,15,15> F_delta = Eigen::Matrix<double,15,15>::Identity(); // transform F_delta.block<3,3>(0,3) = -okvis::kinematics::crossMx(acc_integral*dt + 0.25*(C + C_1)*acc_S_true*dt*dt); F_delta.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*dt; F_delta.block<3,3>(0,9) = dt*dv_db_g + 0.25*dt*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1); F_delta.block<3,3>(0,12) = -C_integral*dt + 0.25*(C + C_1)*dt*dt; F_delta.block<3,3>(3,9) = -dt*C_1; F_delta.block<3,3>(6,3) = -okvis::kinematics::crossMx(0.5*(C + C_1)*acc_S_true*dt); F_delta.block<3,3>(6,9) = 0.5*dt*(C*acc_S_x*cross + C_1*acc_S_x*cross_1); F_delta.block<3,3>(6,12) = -0.5*(C + C_1)*dt; P_delta = F_delta*P_delta*F_delta.transpose();

Fδ=I(15,15) Fδ(0:2,3:5)=−[∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt]× Fδ(0:2,6:8)=I(3,3)∗dt Fδ(0:2,9:11)=dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1) Fδ(3:5,9:11)=−dt∗C1 Fδ(6:8,3:5)=−[0.5∗(C+C1)∗Sa∗dt]× Fδ(6:8,9:11)=0.5∗dt∗(C∗[aS]×∗Mcross+C1∗[aS]×∗Mcross1) Fδ(6:8,12:15)=−0.5∗(C+C1)∗dt

Fδ=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪00000−[∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt]×0−[0.5∗(C+C1)∗Sa∗dt]×00I(3,3)∗dt0000dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)−dt∗C10.5∗dt∗(C∗[aS]×∗Mcross+C1∗[aS]×∗Mcross1)0000−0.5∗(C+C1)∗dt00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪ Pδ=Fδ∗Pδ∗FTδ

// add noise. Note that transformations with rotation matrices can be ignored, since the noise is isotropic. //F_tot = F_delta*F_tot; const double sigma2_dalpha = dt * sigma_g_c * sigma_g_c; P_delta(3,3) += sigma2_dalpha; P_delta(4,4) += sigma2_dalpha; P_delta(5,5) += sigma2_dalpha; const double sigma2_v = dt * sigma_a_c * imuParams.sigma_a_c; P_delta(6,6) += sigma2_v; P_delta(7,7) += sigma2_v; P_delta(8,8) += sigma2_v; const double sigma2_p = 0.5*dt*dt*sigma2_v; P_delta(0,0) += sigma2_p; P_delta(1,1) += sigma2_p; P_delta(2,2) += sigma2_p; const double sigma2_b_g = dt * imuParams.sigma_gw_c * imuParams.sigma_gw_c; P_delta(9,9) += sigma2_b_g; P_delta(10,10) += sigma2_b_g; P_delta(11,11) += sigma2_b_g; const double sigma2_b_a = dt * imuParams.sigma_aw_c * imuParams.sigma_aw_c; P_delta(12,12) += sigma2_b_a; P_delta(13,13) += sigma2_b_a; P_delta(14,14) += sigma2_b_a; }

gyro noise density： σgc accelerometer noise density： σac gyro drift noise density： σgwc accelerometer drift noise density： σawc σ2dα=dt∗σgc∗σgc σ2v=dt∗σac∗σac σ2p=0.5∗dt∗dt∗σ2v σ2bg=dt∗σgwc∗σgwc σ2ba=dt∗σawc∗σawc

Pδ=Pδ+⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪σ2p∗I(3,3)00000σ2dα∗I(3,3)00000σ2v∗I(3,3)00000σ2bg∗I(3,3)00000σ2ba∗I(3,3)⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

end covariance propagation

// memory shift Delta_q = Delta_q_1; C_integral = C_integral_1; acc_integral = acc_integral_1; cross = cross_1; dv_db_g = dv_db_g_1; time = nexttime; ++i; if (nexttime == t_end) break; }

Δq=Δq1 ∫C=∫′C ∫a=∫′a Mcross=Mcorss1 dvdbg=dvdbg′

end for

// actual propagation output: const Eigen::Vector3d g_W = imuParams.g * Eigen::Vector3d(0, 0, 6371009).normalized(); T_WS.set(r_0+speedAndBiases.head<3>()*Delta_t + C_WS_0*(acc_doubleintegral/*-C_doubleintegral*speedAndBiases.segment<3>(6)*/) - 0.5*g_W*Delta_t*Delta_t, q_WS_0*Delta_q); speedAndBiases.head<3>() += C_WS_0*(acc_integral/*-C_integral*speedAndBiases.segment<3>(6)*/)-g_W*Delta_t;

输出系统状态量更新

输入重力加速度参数： g gW=g∗(0,0,1)T t{TWS}=r0+v∗Δt+CWS0∗∬a−0.5∗gW∗Δt∗Δt q{TWS}=qWS0∗Δq v=v+CWS0∗∫a−gW∗Δt

// assign Jacobian, if requested if (jacobian) { Eigen::Matrix<double,15,15> & F = *jacobian; F.setIdentity(); // holds for all states, including d/dalpha, d/db_g, d/db_a F.block<3,3>(0,3) = -okvis::kinematics::crossMx(C_WS_0*acc_doubleintegral); F.block<3,3>(0,6) = Eigen::Matrix3d::Identity()*Delta_t; F.block<3,3>(0,9) = C_WS_0*dp_db_g; F.block<3,3>(0,12) = -C_WS_0*C_doubleintegral; F.block<3,3>(3,9) = -C_WS_0*dalpha_db_g; F.block<3,3>(6,3) = -okvis::kinematics::crossMx(C_WS_0*acc_integral); F.block<3,3>(6,9) = C_WS_0*dv_db_g; F.block<3,3>(6,12) = -C_WS_0*C_integral; }

输出雅各比矩阵

J=I(15,15) J(0:2,3:5)=−[CWS0∗∬a]× J(0:2,6:8)=I(3,3)∗Δt J(0:2,9:11)=CWS0∗dpdbg J(0:2,12:14)=−CWS0∗∬C J(3:5,9:11)=−CWS0∗dαdbg J(6:8,3:5)=−[CWS0∗∫a]× J(6:8,9:11)=CWS0∗dvdbg J(6:8,12:14)=−CWS0∗∫C

J=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪00000−[CWS0∗∬a]×0−[CWS0∗∫a]×00I(3,3)∗Δt0000CWS0∗dpdbg−CWS0∗dαdbgCWS0∗dvdbg00−CWS0∗∬C0−CWS0∗∫C00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

// overall covariance, if requested if (covariance) { Eigen::Matrix<double,15,15> & P = *covariance; // transform from local increments to actual states Eigen::Matrix<double,15,15> T = Eigen::Matrix<double,15,15>::Identity(); T.topLeftCorner<3,3>() = C_WS_0; T.block<3,3>(3,3) = C_WS_0; T.block<3,3>(6,6) = C_WS_0; P = T * P_delta * T.transpose(); } return i; }

输出方差矩阵

T=I(15,15)

T=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪CWS000000CWS000000CWS0000000000000⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

P=T∗Pδ∗TT

版权声明：本文为博主原创文章，未经博主允许不得转载。