lio-mapping 及 VINS-Mono代码及理论推导（2）—— pre_integration
最近在看港科大刚开源的LIO及VINS-Mono论文和代码,他们一部分代码都是相同的，闲暇时间整理一下。持续更新中（如果有时间）。。。
LIO
代码链接： https://github.com/hyye/lio-mapping
相关介绍：https://sites.google.com/view/lio-mapping
论文地址：https://arxiv.org/abs/1904.06993
VINS
代码链接： https://github.com/castiel520/VINS-Mono
论文地址：https://arxiv.org/abs/1708.03852v1
预积分部分核心函数为
MidPointIntegration 预积分函数
VINS_Mono
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}
LIO-Mapping
对比代码发现 LIO-Mapping 与 VINS部分细节不同，注释中有提及。
void MidPointIntegration(double dt,
const Vector3d &acc0, const Vector3d &gyr0,
const Vector3d &acc1, const Vector3d &gyr1,
const Vector3d &delta_p, const Quaterniond &delta_q,
const Vector3d &delta_v, const Vector3d &linearized_ba,
const Vector3d &linearized_bg, Vector3d &result_delta_p,
Quaterniond &result_delta_q, Vector3d &result_delta_v,
Vector3d &result_linearized_ba, Vector3d &result_linearized_bg,
bool update_jacobian) {
//ROS_DEBUG("midpoint integration");
// NOTE: the un_acc here is different from the un_acc in the Estimator
Vector3d un_acc_0 = delta_q * (acc0 - linearized_ba);
Vector3d un_gyr = 0.5 * (gyr0 + gyr1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * dt / 2, un_gyr(1) * dt / 2, un_gyr(2) * dt / 2);
Vector3d un_acc_1 = result_delta_q * (acc1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * dt + 0.5 * un_acc * dt * dt;
result_delta_v = delta_v + un_acc * dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if (update_jacobian) {
Vector3d w_x = 0.5 * (gyr0 + gyr1) - linearized_bg;
Vector3d a_0_x = acc0 - linearized_ba;
Vector3d a_1_x = acc1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x << 0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x << 0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x << 0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
// NOTE: F = Fd = \Phi = I + dF*dt
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * dt * dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * dt) * dt * dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3, 3) * dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * dt * dt;
// F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * -dt;
F.block<3, 3>(0, 12) = -0.1667 * result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * -dt;//0.1667与VINS不同
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3, 3) * dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * dt) * dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * dt * -dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
// NOTE: V = Fd * G_c
// FIXME: verify if it is right, the 0.25 part
MatrixXd V = MatrixXd::Zero(15, 18);
// V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * dt * dt;
V.block<3, 3>(0, 0) = 0.5 * delta_q.toRotationMatrix() * dt * dt;//0.5与VINS不同
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * dt * dt * 0.5 * dt;
// V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * dt * dt;
V.block<3, 3>(0, 6) = 0.5 * result_delta_q.toRotationMatrix() * dt * dt;//0.5与VINS不同
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * dt * 0.5 * dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3, 3) * dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3, 3) * dt;
//step_jacobian = F;
//step_V = V;
jacobian_ = F * jacobian_;
covariance_ = F * covariance_ * F.transpose() + V * noise_ * V.transpose();
}
}
公式推导
下面是两帧雷达数据之间所以IMU数据的位置、速度、旋转量的积分公式
(1) p j = p i + ∑ k = i j − 1 [ v k Δ t + 0.25 ( R k i ( a k − b a k ) + R k + 1 i ( a k − b a k ) ) Δ t 2 ] p_{j}=p_i+\sum_{k=i}^{j-1}[v_k\Delta{t}+0.25(R_{k}^i(a_k-b_{a_k})+R_{k+1}^i(a_k-b_{a_k}))\Delta{t}^2] \tag{1} pj=pi+k=i∑j−1[vkΔt+0.25(Rki(ak−bak)+Rk+1i(ak−bak))Δt2](1)
(2) v j = v i + ∑ k = i j − 1 0.5 [ R k i ( a k − b a k ) Δ t + R k + 1 i ( a k − b a k ) Δ t ] v_{j}=v_{i}+\sum_{k=i}^{j-1}0.5[R_k^i(a_k-b_{a_k})\Delta{t}+R_{k+1}^i(a_k-b_{a_k})\Delta{t}] \tag{2} vj=vi+k=i∑j−10.5[Rki(ak−bak)Δt+Rk+1i(ak−bak)Δt](2)
(3) q j = q i ⊗ ∏ k = i j − 1 δ q k = q i ⊗ ∏ k = i j − 1 [ 1 2 Δ t ( ω k + ω k + 1 2 − b g k ) 1 ] q_j=q_i\otimes\prod_{k=i}^{j-1}\delta{q_k}=q_i\otimes\prod_{k=i}^{j-1}\begin{bmatrix}\frac{1}{2}\Delta{t}(\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k}) \\ 1 \\ \end{bmatrix} \tag{3} qj=qi⊗k=i∏j−1δqk=qi⊗k=i∏j−1[21Δt(2ωk+ωk+1−bgk)1](3)
不考虑噪声的情况下，每来一帧imu数据，其状态转移如下
(4) q k + 1 = q k ⊗ [ 1 2 Δ t ( ω k + ω k + 1 2 − b g k ) 1 ] {q_{k+1}}={q_k}\otimes\begin{bmatrix}\frac{1}{2}\Delta{t}(\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k}) \\ 1 \end{bmatrix} \tag{4} qk+1=qk⊗[21Δt(2ωk+ωk+1−bgk)1](4)
(5) p k + 1 = p k + v k Δ t + 0.25 ( R k ( a k − b a k ) + R k + 1 ( a k + 1 − b a k + 1 ) ) Δ t 2 {p}_{k+1}={p}_{k}+{v}_k\Delta{t}+0.25(R_k(a_k-b_{a_k})+R_{k+1}(a_{k+1}-b_{a_{k+1}}))\Delta{t}^2 \tag{5} pk+1=pk+vkΔt+0.25(Rk(ak−bak)+Rk+1(ak+1−bak+1))Δt2(5)
(6) v k + 1 = v k + 0.5 ( R k ( a k − b a k ) + R k + 1 ( a k + 1 − b a k + 1 ) ) Δ t v_{k+1}=v_{k}+0.5(R_k(a_k-b_{a_k})+R_{k+1}(a_{k+1}-b_{a_{k+1}}))\Delta{t} \tag{6} vk+1=vk+0.5(Rk(ak−bak)+Rk+1(ak+1−bak+1))Δt(6)
(7) b a k + 1 = b a k b_{a_{k+1}}=b_{a_{k}} \tag{7} bak+1=bak(7)
(8) b g k + 1 = b g k b_{g_{k+1}}=b_{g_{k}} \tag{8} bgk+1=bgk(8)
误差状态 δ X = ( δ p , δ θ , δ v , δ b a , δ b g ) \delta{X}=(\delta{p},\delta{\theta},\delta{v},\delta{b_a},\delta{b_g}) δX=(δp,δθ,δv,δba,δbg)
加入噪声影响，加速度和陀螺仪的测量噪声为 n a ∼ N ( 0 , σ a 2 ) n_a\sim\mathcal{N}(0,\sigma_a^2) na∼N(0,σa2) ， n w ∼ N ( 0 , σ w 2 ) n_w\sim\mathcal{N}(0,\sigma_w^2) nw∼N(0,σw2)。 加速度和陀螺仪偏置为随机游走噪声， n b a ∼ N ( 0 , σ b a 2 ) n_{b_a}\sim\mathcal{N}(0,\sigma_{b_a}^2) nba∼N(0,σba2) ， n b w ∼ N ( 0 , σ b w 2 ) n_{b_w}\sim\mathcal{N}(0,\sigma_{b_w}^2) nbw∼N(0,σbw2) ，针对MidPointIntegration，其还需要增加 k + 1 k+1 k+1时刻的测量噪声，也即其噪声矩阵为。
Q = [ n a 0 0 0 0 0 0 n w 0 0 0 0 0 0 n a 0 0 0 0 0 0 n w 0 0 0 0 0 0 n b a 0 0 0 0 0 0 n b w ] Q=\begin{bmatrix} n_{a} & 0 & 0 & 0 & 0 & 0\\ 0 & n_{w} & 0 & 0 & 0 & 0\\ 0 & 0 & n_{a} & 0 & 0 & 0\\ 0 & 0 & 0 & n_{w} & 0 & 0\\ 0 & 0 & 0 & 0 & n_{b_a} & 0\\ 0 & 0 & 0 & 0 & 0 & n_{b_w}\end{bmatrix} Q=⎣⎢⎢⎢⎢⎢⎢⎡na000000nw000000na000000nw000000nba000000nbw⎦⎥⎥⎥⎥⎥⎥⎤
.
根据式(4)，首先求 δ θ ˙ \dot{\delta{\theta}} δθ˙，定义 δ q = [ 1 2 δ θ 1 ] \delta{q}=\begin{bmatrix}\frac{1}{2}\delta{\theta} \\ 1\end{bmatrix} δq=[21δθ1]：
(9) δ θ ˙ = − [ ω k + ω k + 1 2 − b g k ] × δ θ − δ b g + 0.5 ( n w k + n w k + 1 ) \dot{\delta{\theta}}=-[\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k}]_\times \delta{\theta}-\delta{b_{g}}+0.5(n_{w_k}+n_{w_{k+1}}) \tag{9} δθ˙=−[2ωk+ωk+1−bgk]×δθ−δbg+0.5(nwk+nwk+1) (9)
δ θ \delta{\theta} δθ更新为：
(10) δ θ n e w ← ( I − [ ( ω k + ω k + 1 2 − b g k ) ] × Δ t ) δ θ − Δ t δ b g + 0.5 Δ t ( n w k + n w k + 1 ) \delta{\theta^{new}}\leftarrow (I-[(\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k})]_{\times}\Delta{t})\delta{\theta}-\Delta{t}\delta{b_{g}}+0.5\Delta{t}(n_{w_k}+n_{w_{k+1}}) \tag{10} δθnew←(I−[(2ωk+ωk+1−bgk)]×Δt)δθ−Δtδbg+0.5Δt(nwk+nwk+1)(10)
假设加加速度恒定，再求 δ v ˙ \dot{\delta{v}} δv˙ :
(11) δ v ˙ = − 0.5 ( R k [ a k − b a k ] × δ θ + R k + 1 [ a k + 1 − b a k + 1 ] × δ θ n e w ) + 0.5 ( R k + R k + 1 ) δ b a + 0.5 ( R k n a k + R k + 1 n a k + 1 ) = − 0.5 ( R k [ a k − b a k ] × δ θ + R k + 1 [ a k + 1 − b a k + 1 ] × ( ( I − [ ( ω k + ω k + 1 2 − b g k ) ] × Δ t ) δ θ − Δ t δ b g + 0.5 Δ t ( n w k + n w k + 1 ) ) + 0.5 ( R k + R k + 1 ) δ b a + 0.5 ( R k n a k + R k + 1 n a k + 1 ) = − 0.5 ( R k [ a k − b a k ] × + R k + 1 [ a k + 1 − b a k + 1 ] × ( ( I − [ ( ω k + ω k + 1 2 − b g k ) ] × Δ t ) ) δ θ − 0.5 ( R k + R k + 1 ) δ b a + 0.5 ( R k n a k + R k + 1 n a k + 1 ) + 0.5 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t δ b g − 0.25 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t ( n w k + n w k + 1 ) \begin{aligned} \dot{\delta{v}} =&-0.5(R_{k}[a_k-b_{a_k}]_\times\delta{\theta}+R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\delta{\theta^{new}})\\ &+0.5(R_k+R_{k+1})\delta{b_a}+0.5(R_k{n_{a_k}}+R_{k+1}n_{a_{k+1}}) \\ =& -0.5(R_{k}[a_k-b_{a_k}]_\times\delta{\theta}+R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times((I-[(\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k})]_{\times}\Delta{t})\delta{\theta}-\Delta{t}\delta{b_{g}}+0.5\Delta{t}(n_{w_k}+n_{w_{k+1}}))\\ &+0.5(R_k+R_{k+1})\delta{b_a} +0.5(R_k{n_{a_k}}+R_{k+1}n_{a_{k+1}})\\ =& -0.5(R_{k}[a_k-b_{a_k}]_\times+R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times((I-[(\frac{\omega_k+\omega_{k+1}}{2}-b_{g_k})]_{\times}\Delta{t}))\delta{\theta}\\ & -0.5(R_k+R_{k+1})\delta{b_a}+0.5(R_k{n_{a_k}}+R_{k+1}n_{a_{k+1}}) \\ & +0.5R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}\delta{b_{g}} -0.25R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}(n_{w_k}+n_{w_{k+1}}) \end{aligned} \tag{11} δv˙===−0.5(Rk[ak−bak]×δθ+Rk+1[ak+1−bak+1]×δθnew)+0.5(Rk+Rk+1)δba+0.5(Rknak+Rk+1nak+1)−0.5(Rk[ak−bak]×δθ+Rk+1[ak+1−bak+1]×((I−[(2ωk+ωk+1−bgk)]×Δt)δθ−Δtδbg+0.5Δt(nwk+nwk+1))+0.5(Rk+Rk+1)δba+0.5(Rknak+Rk+1nak+1)−0.5(Rk[ak−bak]×+Rk+1[ak+1−bak+1]×((I−[(2ωk+ωk+1−bgk)]×Δt))δθ−0.5(Rk+Rk+1)δba+0.5(Rknak+Rk+1nak+1)+0.5Rk+1[ak+1−bak+1]×Δtδbg−0.25Rk+1[ak+1−bak+1]×Δt(nwk+nwk+1)(11)
得到 δ v n e w \delta{v^{new}} δvnew:
(12) δ v n e w ← δ v + δ v ˙ Δ t \delta{v^{new}}\leftarrow\delta{v}+\dot{\delta{v}}\Delta{t} \tag{12} δvnew←δv+δv˙Δt(12)
再求 δ p ˙ \dot{\delta{p}} δp˙
(13) δ p ˙ = 0.5 ( δ v + δ v n e w ) = δ v + 0.5 δ v ˙ \begin{aligned} \dot{\delta{p}} =&0.5(\delta{v}+\delta{v^{new}}) \\ =&\delta{v}+0.5\dot{\delta{v}} \end{aligned} \tag{13} δp˙==0.5(δv+δvnew)δv+0.5δv˙(13)
求 δ p \delta{p} δp
(14) δ p n e w ← δ p + δ p ˙ Δ t \delta{p^{new}}\leftarrow\delta{p}+\dot{\delta{p}}\Delta{t} \tag{14} δpnew←δp+δp˙Δt(14)
最后更新偏置 δ b a \delta{b_a} δba, δ b g \delta{b_g} δbg
(15) δ b a n e w ← δ b a + n b a Δ t \delta{b_a^{new}}\leftarrow \delta{b_a} + n_{b_a}\Delta{t} \tag{15} δbanew←δba+nbaΔt(15)
(16) δ b g n e w ← δ b g + n b w Δ t \delta{b_g^{new}}\leftarrow \delta{b_g} + n_{b_w}\Delta{t} \tag{16} δbgnew←δbg+nbwΔt(16)
状态转移矩阵为：
F = [ I f 01 I Δ t − 0.25 ( R k + R k + 1 ) Δ t 2 0.25 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 3 0 I − [ 0.5 ( ω k + ω k + 1 ) − b g k ] × Δ t 0 0 − I Δ t 0 f 21 I − 0.5 ( R k + R k + 1 ) Δ t 0.5 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 2 0 0 0 I 0 0 0 0 0 I ] F=\begin{bmatrix} I & f_{01} & I\Delta{t} & -0.25(R_{k}+R_{k+1})\Delta{t}^2 & 0.25R_{k+1}[a_{k+1}-b_{a_{k+1}}]_{\times}\Delta{t}^3\\ 0 & I-[0.5(\omega_k+\omega_{k+1})-b_{g_k}]_{\times}\Delta{t} & 0 & 0 & -I\Delta{t}\\ 0 & f_{21} & I & -0.5(R_{k}+R_{k+1})\Delta{t} & 0.5R_{k+1}[a_{k+1}-b_{a_{k+1}}]_{\times}\Delta{t}^2\\ 0 & 0 & 0 & I & 0\\ 0 & 0 & 0 & 0 & I \end{bmatrix} F=⎣⎢⎢⎢⎢⎡I0000f01I−[0.5(ωk+ωk+1)−bgk]×Δtf2100IΔt0I00−0.25(Rk+Rk+1)Δt20−0.5(Rk+Rk+1)ΔtI00.25Rk+1[ak+1−bak+1]×Δt3−IΔt0.5Rk+1[ak+1−bak+1]×Δt20I⎦⎥⎥⎥⎥⎤
其中
f 01 = − 0.25 ( R k [ a k − b a k ] × Δ t 2 − R k + 1 [ a k + 1 − b a k + 1 ] × ( I − [ 0.5 ( ω k + ω k + 1 ) − b g k ] × Δ t ) ) Δ t 2 f 21 = − 0.5 R k ∗ [ a k − b a k ] × Δ t − 0.5 R k + 1 [ a k + 1 − b a k + 1 ] × ( I − [ 0.5 ( ω k + ω k + 1 ) − b g k ] × Δ t ) Δ t f_{01}=-0.25(R_k[a_k-b_{a_k}]_{\times}\Delta{t}^2-R_{k+1}[a_{k+1}-b_{a_{k+1}}]_{\times}(I-[0.5(\omega_k+\omega_{k+1})-b_{g_k}]_{\times}\Delta{t}))\Delta{t}^2 \\ f_{21}=-0.5R_k*[a_k-b_{a_k}]_{\times}\Delta{t}-0.5R_{k+1}[a_{k+1}-b_{a_{k+1}}]_{\times}(I-[0.5(\omega_k+\omega_{k+1})-b_{g_k}]_{\times}\Delta{t})\Delta{t} f01=−0.25(Rk[ak−bak]×Δt2−Rk+1[ak+1−bak+1]×(I−[0.5(ωk+ωk+1)−bgk]×Δt))Δt2f21=−0.5Rk∗[ak−bak]×Δt−0.5Rk+1[ak+1−bak+1]×(I−[0.5(ωk+ωk+1)−bgk]×Δt)Δt
雅克比矩阵更新：
(17) J n e w ← F J J^{new}\leftarrow FJ \tag{17} Jnew←FJ(17)
上述雅克比矩阵更新是在两帧imu数据之间的更新,在激光SLAM问题中，一般需要求得两帧雷达点云之间状态变换的雅克比矩阵，这只需要累乘 F F F矩阵即可。
噪声的观测矩阵则为：
V = [ 0.25 R k Δ t 2 − 0.125 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 3 0.25 R k + 1 Δ t 2 − 0.125 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 3 0 0 0 0.5 I Δ t 0 0.5 I Δ t 0 0 0.5 R k Δ t − 0.25 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 2 0.5 R k + 1 Δ t − 0.25 R k + 1 [ a k + 1 − b a k + 1 ] × Δ t 2 0 0 0 0 0 0 I Δ t 0 0 0 0 0 0 I Δ t ] V=\begin{bmatrix} 0.25R_k\Delta{t}^2 & -0.125R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}^3 & 0.25R_{k+1}\Delta{t}^2 & -0.125R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}^3 & 0 & 0\\ 0 & 0.5I\Delta{t} & 0 & 0.5I\Delta{t} & 0 & 0\\ 0.5R_k\Delta{t} & -0.25R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}^2 & 0.5R_{k+1}\Delta{t} & -0.25R_{k+1}[a_{k+1}-b_{a_{k+1}}]_\times\Delta{t}^2 & 0 & 0\\ 0 & 0 & 0 & 0 & I\Delta{t} & 0\\ 0 & 0 & 0 & 0 & 0 & I\Delta{t}\end{bmatrix} V=⎣⎢⎢⎢⎢⎡0.25RkΔt200.5RkΔt00−0.125Rk+1[ak+1−bak+1]×Δt30.5IΔt−0.25Rk+1[ak+1−bak+1]×Δt2000.25Rk+1Δt200.5Rk+1Δt00−0.125Rk+1[ak+1−bak+1]×Δt30.5IΔt−0.25Rk+1[ak+1−bak+1]×Δt200000IΔt00000IΔt⎦⎥⎥⎥⎥⎤
则协方差更新：
P n e w ← F P F T + V Q V T P^{new}\leftarrow FPF^T+VQV^T Pnew←FPFT+VQVT
状态更新：
δ X n e w ← F δ X + V Q \delta{X}^{new}\leftarrow F\delta{X}+VQ δXnew←FδX+VQ
![POJ 1284 Primitive Roots (欧拉函数&原根定理)[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)