OKVIS 中的 propagation 代码公式版

来源：互联网发布：华康少女字体mac 编辑：程序博客网时间：2024/05/20 10:12

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 conditionEigen::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}
12

// 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 accumulatrionEigen::Matrix3d cross = Eigen::Matrix3d::Zero();

Mcross=0(3,3)

// sub-JacobiansEigen::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 - [\int a * d t + 0.25 * (C + C 1) * S a * d t * d t] \times 0 - [0.5 * (C + C 1) * S a * d t] \times 00 I (3, 3) * d t 0000 d t * d v d b g + 0.25 * d t * d t * (C * [S a] \times * M c r o s s + C 1 * [S a] \times * M c r o s s 1) - d t * C 1 0.5 * d t * (C * [a S] \times * M c r o s s + C 1 * [a S] \times * M c r o s s 1) 00 00 - 0.5 * (C + C 1) * d t 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

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 δ + ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ σ 2 p * I (3, 3) 0000 0 σ 2 d α * I (3, 3) 000 00 σ 2 v * I (3, 3) 00 000 σ 2 b g * I (3, 3) 0 0000 σ 2 b a * 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 - [C W S 0 * \iint a] \times 0 - [C W S 0 * \int a] \times 00 I (3, 3) * Δ t 0000 C W S 0 * d p d b g - C W S 0 * d α d b g C W S 0 * d v d b g 00 - C W S 0 * \iint C 0 - C W S 0 * \int C 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

  // 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;}

输出方差矩阵

方差 P 是从通过参数传进来，这里并没有清掉，说明是不断传播的
T=I(15,15)

T = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ C W S 0 0000 0 C W S 0 000 00 C W S 0 00 0000000000 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

P=T∗Pδ∗TT

4 0