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(...)
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