1. 连续时间的PVQ积分
对加速度进行2次积分：一次积分是速度变化量，二次积分是位移的变化量
类似于
δ
x
=
V
o
t
+
1
/
2
a
t
2
\delta x=V_ot+1/2at^2
δx=Vot+1/2at2
此处的a会随时间变化，所以需要把
1
/
2
a
t
2
1/2at^2
1/2at2这一部分变成积分的形式。
为什么a这么复杂：因为在加速度计里面，会掺杂重力的分量。我们在进行积分的时候，需要剔除掉重力分量的部分，这个就是
( R t w ( a ^ t − b a t − n a ) − g w \mathbf{( R}_{t}^{w}\left(\hat{\mathbf{a}}_{t}-\mathbf{b}_{a_{t}}-\mathbf{n}_{a}\right)-\mathbf{g}^{w} (Rtw(a^t−bat−na)−gw =（加速度的观测值 - 零偏 - 随机游走）通过当前的姿态转到世界坐标系下-世界坐标系下的重力
系统维护的全局世界坐标系是与重力对齐的。
速度积分同理。
1.1 铺垫一下知识点
其中四元数左右乘矩阵
也可以写成一下形式
1.2 对旋转四元数的求导进行推导
这里应该是
n
∗
θ
/
δ
t
=
ω
\boldsymbol{n} * \theta / \delta t = \boldsymbol{\omega}
n∗θ/δt=ω 角速度，写错了
在论文中的写法是这样的:
这是因为论文中把虚部写在了前面，实部写在了后面。所以
Ω
Ω
Ω矩阵的形式略有不同.
其中 ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( ω ^ t − b w t − n w ) q t b k \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \boldsymbol{\Omega}\left(\hat{\boldsymbol{\omega}}_{t}-\mathbf{b}_{w_{t}}-\mathbf{n}_{w}\right) \mathbf{q}_{t}^{b_{k}} ∫t∈[tk,tk+1]21Ω(ω^t−bwt−nw)qtbk 相当于 q b k , b k + 1 q_{bk,bk+1} qbk,bk+1
2. 为什么需要预积分？
- 对于滤波问题: IMU积分的作用是对k时刻到k+1时刻机体运动的预测。
- 对于优化问题: 优化问题会优化过去一段时间的位姿，不同于滤波只会优化当前的位姿。在优化问题中,k时刻到k+1时刻的IMU的积分会形成帧间位姿的约束。用于在优化一段时间的位姿时,对积分两端的位姿起一个约束的作用,让它们在优化的过程中被这个积分值给约束住。同时预积分对零偏进行了建模
公式(23)并不能形成一个有效的约束公式,因为作为一个帧间约束，不应该和相邻帧的状态有直接的联系
与观测有关的量主要在以下公式中：
(作为一个帧间约束，不应该和相邻帧的状态有直接的联系,)所以需要在两边乘以
(
R
b
k
W
)
−
1
(R^W_{bk})^{-1}
(RbkW)−1
其中：
被称为预积分量，其中的变量都保证了与相邻帧的位姿无关，
R
t
b
k
R^{b_k}_t
Rtbk 表示为
b
k
b_k
bk帧到
t
t
t时刻的变化。
t
t
t时刻是
b
k
b_k
bk帧到
b
k
+
1
b_{k+1}
bk+1帧之间IMU的一个节点.
如果我们在（25）式中将预积分量放到一边，右边部分就相当于帧间位姿的积分。其他部分则为k时刻到k+1时刻的相关的状态量。
补充一点: 在预积分量中,因为坐标系是相对于 b k b_k bk的,所以 a ^ t \hat{\mathbf{a}}_{t} a^t是包含重力的,所以一段时间下自由落体产生的位移都会存在与 α b k + 1 b k \alpha^{b_k}_{b_{k+1}} αbk+1bk,速度会存在与 β b k + 1 b k \beta^{b_k}_{b_{k+1}} βbk+1bk中
如图黄色部分为预积分的约束，用于约束k时刻和k+1时刻的位姿。
另一个解释的版本是：我们在（25）式如果k时刻的位姿进行了调整，那么k+1时刻的位姿也要调整，这就需要反复积分。
2.1 离散时间预积分量的中值积分 + 代码
因为在实际中,我们获得的IMU测量值都是离散而非实际的。VINS的做法就是采用了中值积分。
- 每次新来了一个IMU数据时,我们就使用(27)式进行递推。(27)式中的 a ^ t \hat{\mathbf{a}}_{t} a^t采用了上一帧和当前帧的IMU的 a ^ t \hat{\mathbf{a}}_{t} a^t取中间值获得,认为在两帧之间是恒定的,不需要再进行积分求得了。
- 我们把 α ^ i b k \hat{\alpha}^{b_k}_{i} α^ibk移到左边,左边就是 δ α \delta\alpha δα,右边就是 v 0 ∗ t + 1 / 2 ∗ a ∗ t 2 v_0*t+1/2*a*t^2 v0∗t+1/2∗a∗t2。
- R ( γ ^ i b k ) \mathbf{R}\left(\hat{\gamma}_{i}^{b_{k}}\right) R(γ^ibk)表示我们把加速度转换到IMU第0帧的坐标系中去。我们这边的 α β γ αβγ αβγ不同于之前在世界坐标系下的PVQ,这里的 α β γ αβγ αβγ都是在 b k b_k bk系下的位置速度和姿态。
- 速度同理
- 旋转的变化:四元数q可以表示为 q = [ c o s ( θ ) / 2 , s i n ( θ / 2 ) ∗ n ] T q=[cos(θ)/2,sin(θ/2)*\mathbf{n}]^T q=[cos(θ)/2,sin(θ/2)∗n]T,因为IMU之间的时间极短,即θ趋近于0, q = [ 1 , θ ∗ n / 2 ] T = [ 1 , ω ∗ Δ t / 2 ] T q=[1,θ*\mathbf{n}/2]^T = [1,ω*Δt/2]^T q=[1,θ∗n/2]T=[1,ω∗Δt/2]T 把 ω ω ω减去零偏,即得到旋转的变化公式
中值积分表现在: a ^ i = ( a i + a i + 1 ) / 2 \hat{\mathbf{a}}_{i} = (a_i+a_{i+1})/2 a^i=(ai+ai+1)/2, ω ^ i = ( ω i + ω i + 1 ) / 2 \hat{\mathbf{ω}}_{i} = (ω_i+ω_{i+1})/2 ω^i=(ωi+ωi+1)/2, R ( γ ^ i b k ) = ( R ( γ i b k ) \mathbf{R}\left(\hat{\gamma}_{i}^{b_{k}}\right)=( \mathbf{R}\left({\gamma}_{i}^{b_{k}}\right) R(γ^ibk)=(R(γibk) + R ( γ i + 1 b k ) ) / 2 \mathbf{R}\left({\gamma}_{i+1}^{b_{k}}\right))/2 R(γi+1bk))/2
// a = R(γ)(ai - ba)
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); // 上一时刻的IMU在初始帧下的加速度的值
// ω = 1/2(ω0 + ω1) - bw
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // k时刻和k+1时刻陀螺仪的值相加/2 - 零偏
// 四元数转化为轴角：q = [cosθ/2,n * sinθ/2] ,当θ趋近于0时： θ -> 0 => q = [1,w*t/2]
// γ_i+1 = γ_i * [1,1/2 * ω * dt ] 中值积分后的旋转
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); // k+1 时刻的加速度值
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;
为什么代码后续这么复杂:因为还涉及到置信度,即协方差矩阵.(我走的步数越多,我对路程的置信度就越低,我走的越少甚至一步都不走,我对这一段路程的置信度就越高)
3. 误差卡尔曼滤波
3.1 什么是误差卡尔曼滤波
- 卡尔曼滤波:以森林走路为例子,我们把森林中走一段路程预估的距离计为x,收到的GPS显示我们前进的距离为y,我们把x和y送入卡尔曼滤波器中,可以得到我们位置的更精确值x’。
- 误差卡尔曼滤波,我们把森林中走一段路程预估的距离计为x,估计的误差为Δx,收到的GPS显示我们前进的距离为y,误差为Δy=y-x.我们将Δx,Δy放入误差卡尔曼滤波器帧中得到误差的更精确值Δx’,x+Δx’即为我们当前位置的更精确值。
我们走的越远,我们的距离的误差就会越大,但是我们本身感觉的距离都是自认为最佳的判断距离。即虽然我们不断前进,我们距离的置信度是在不断下降的,但我们给出的位置判断依然是最佳的位置判断,预测的误差Δx一直都是0。(为什么是0? 答: 如果它不是0,我们把它直接加到x中不就行了么。)
所以在误差卡尔曼中预测给出的误差一般都是0,但是Δx的协方差矩阵一直是在增加的,同理Δy也有一个置信度。我们把x,y的误差和协方差矩阵都送入误差卡尔曼滤波模型中,可以得到当前位置的最佳估计值误差Δx’。我们把Δx’补偿到x中,就可以得到置信度更高的位置估计值,此时Δx的协方差矩阵相比与之前变低了。
3.2 为什么需要误差卡尔曼滤波
- 旋转的误差状态通常是个很小的值,它的参数应该和旋转的自由度相等.(在误差卡尔曼滤波里,通常用旋转向量表示旋转的误差,而不会使用四元数),来避免过参数化的表示方式(因为四元数有四个量,本身存在 ∣ ∣ q ∣ ∣ = 1 ||q||=1 ∣∣q∣∣=1的约束,所以才能描述一个三自由度的旋转)
- 使用4 * 4 的协方差矩阵来描述三自由度的旋转是存在问题的,所以我们引入误差卡尔曼滤波使用 3 * 3的协方差矩阵来描述旋转的置信度,此时是没有歧义的.
- 虽然旋转向量有周期性问题,但是误差是很小的值,一定在- π π π~ π π π之间,或者说是在0附近,可以避免周期性.
- 误差量系统一般很接近0,(误差一般都来自于观测),可以避免万向节死锁.
- 因为误差量非常小,所以可以忽略二阶雅克比计算,计算一阶雅克比非常容易
- 误差的变换很慢,我们可以在非常低的速度下,对它进行修正.比如在森林走路,因为误差很小,我们可以走几步再等一个GPS信号,而不用走一步就停下来等待.
3.3 误差卡尔曼滤波相关概念
- True: 真实值
- Nominal:名义值(位姿的估计结果一定是个名义值而非真值,包含了噪声量)
- Error: 误差量
- 真值 = 名义值+误差量,相当于对实际得到的数据进行误差量的补偿,就得到了真实值.
类比一下加速度计:
a
=
a
t
+
b
a
+
n
a
a = a_t +b_a + n_a
a=at+ba+na
真实值
a
t
a_t
at = 名义值
a
a
a + 补偿量(
−
b
a
−
n
a
-b_a-n_a
−ba−na)
3.4 VINS论文对误差卡尔曼的描述
-
当我们要处理协方差的传递时,四自由度的四元数旋转描述是过参数化的,我们用一个扰动来定义了他的误差项
-
真实值 = 名义值 +(广义的加法) 误差值
-
误差状态帮助IMU预积分计算协方差时避免过参数化
4. 连续时间IMU误差状态传递的推导
- 推导 δ α ˙ t b k \delta \dot{\boldsymbol{\alpha}}_{t}^{b_{k}} δα˙tbk
- 推导 δ β ˙ t b k \delta \dot{\boldsymbol{\beta}}_{t}^{b_{k}} δβ˙tbk
- 推导
δ
θ
˙
t
b
k
\delta \dot{\boldsymbol{\theta}}_{t}^{b_{k}}
δθ˙tbk
- 推导 δ b ˙ t b k \delta \dot{\boldsymbol{b}}_{t}^{b_{k}} δb˙tbk
通过将极小时间内的零偏导数建模为高斯白噪声，可以在惯性导航系统或其他相关应用中进行滤波、校准和补偿。这样的假设允许将零偏的导数视为测量噪声，并利用滤波算法来消除或减小其影响，从而提高系统的精度和准确性。
5. 离散时间预积分误差传递 + 代码
在我们实际的系统中，不可能存在连续时间的IMU数据，其必然是离散的。
如何从连续时间IMU误差状态传递=》离散时间下IMU误差状态传递？
基于中值积分的离散时间预积分误差传递
展开来就变成了：
这个式子表示了如何由
δ
x
k
\delta x_k
δxk求得
δ
x
k
+
1
\delta x_{k+1}
δxk+1
// 更新方差矩阵和雅克比
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;
// 协方差矩阵 (见博客5.0) δx_k+1 = F * δ_k + V * n_t
/*[ δα_k+1 ] [ I f01 δt f03 f04 ] [ δα_k ] [ v00 v01 v02 v03 0 0 ][ n_ak ]
*[ δθ_k+1 ] [ 0 f11 0 0 -δt ] [ δθ_k ] [ 0 -δt/2 0 -δt/2 0 0 ][ n_ωk ]
*[ δβ_k+1 ] = [ 0 f21 I f23 f24 ] [ δβ_k ] + [ -R_k*δt/2 v21 -R_k+1*δt/2 v23 0 0 ][ n_ak+1]
*[ δb_a_k+1] [ 0 0 0 0 0 ] [ δb_a_k] [ 0 0 0 0 0 0 ][ n_ωk+1]
*[ δb_ω_k+1] [ 0 0 0 0 0 ] [ δb_ω_k] [ 0 0 0 0 0 δt][ n_ba ]
* [ n_bw ]
*/
// F矩阵
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;
// 都与博客相差一个负号,因为噪声都是0均值的高斯白噪声,所以没有影响
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;
由4.连续时间IMU状态的推导=>5.1,5.2, 5.3, 5.4
5.1 离散时间角度误差的推导
5.2 离散时间速度误差的推导
5.3 离散时间位置误差的推导
5.4 离散时间零偏误差的推导
已知连续时间零偏误差的推导:
δ b ˙ a = n b a \delta \dot{\boldsymbol{b}}_{a} = n_{ba} δb˙a=nba
=> δ b k + 1 = δ b k + n b a ∗ δ t \delta{\boldsymbol{b}}_{k+1} = \delta{\boldsymbol{b}}_{k} + n_{ba}*\delta{{t}} δbk+1=δbk+nba∗δt
δ b ˙ w = n b w \delta \dot{\boldsymbol{b}}_{w} = n_{bw} δb˙w=nbw
=> δ b k + 1 = δ b k + n b w ∗ δ t \delta{\boldsymbol{b}}_{k+1} = \delta{\boldsymbol{b}}_{k} + n_{bw}*\delta{{t}} δbk+1=δbk+nbw∗δt
6. 预积分零偏建模方式
6.1 预积分中关于IMU零偏的建模 + 代码
IMU连续时间积分的表达式:
- 都会涉及到零偏。
- 因为VINS-MONO紧耦合的VIO,其零偏也是一个待估计量,这意味着在优化问题求解过程中,每次优化都可能会改变零偏使满足整体优化最小的条件。
- 这会带来一个问题:每一次优化都需要重新计算积分。
所以我们利用一阶近似来解决这个问题:
f ( x + Δ x ) = f ( x ) + J ( x ) Δ x = f(x+Δx) = f(x)+J(x)Δx = f(x+Δx)=f(x)+J(x)Δx= 原先预积分的结果 + 相对于零偏的一阶雅克比 * 补偿量
/// 求bias改变后的预积分量
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
// 对零偏雅克比线性展开
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
其中 J b a α = δ α b k + 1 b k δ b a k J b w α = δ α b k + 1 b k δ b w k J b a β = δ β b k + 1 b k δ b a k J b w β = δ β b k + 1 b k δ b w k J b w γ = δ γ b k + 1 b k δ b w k \begin{aligned} \mathbf{J}_{b_{a}}^{\alpha} &=\frac{\delta \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}}}{\delta \mathbf{b}_{a_{k}}} \\ \mathbf{J}_{b_{w}}^{\alpha} &=\frac{\delta \boldsymbol{\alpha}_{b_{k+1}}^{b_{k}}}{\delta \mathbf{b}_{w_{k}}} \\ \mathbf{J}_{b_{a}}^{\beta} &=\frac{\delta \boldsymbol{\beta}_{b_{k+1}}^{b_{k}}}{\delta \mathbf{b}_{a_{k}}} \\ \mathbf{J}_{b_{w}}^{\beta} &=\frac{\delta \boldsymbol{\beta}_{b_{k+1}}^{b_{k}}}{\delta \mathbf{b}_{w_{k}}} \\ \mathbf{J}_{b_{w}}^{\gamma} &=\frac{\delta \boldsymbol{\gamma}_{b_{k+1}}^{b_{k}}}{\delta \mathbf{b}_{w_{k}}} \end{aligned} JbaαJbwαJbaβJbwβJbwγ=δbakδαbk+1bk=δbwkδαbk+1bk=δbakδβbk+1bk=δbwkδβbk+1bk=δbwkδγbk+1bk
// 获取P V Q 关于 ba，bg的雅克比
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
enum StateOrder
{
O_P = 0,
O_R = 3,
O_V = 6,
O_BA = 9,
O_BG = 12
};
6.2 如何计算预积分量关于零偏的雅克比
// 雅克比的更新
jacobian = F * jacobian;
初始时，即IMU数据还没有来之前，雅克比矩阵（J）是单位阵
其中（29）式：
6.3 预积分协方差矩阵的传递
假设a符合高斯分布,其方差为A 对a乘以系数k, 则 k a ka ka的方差为 k 2 A k^2A k2A
若a为向量,则其协方差为A的矩阵
若F矩阵乘以a向量,其协方差为 F A F T FAF^T FAFT
若向量a的协方差矩阵为A,向量b的协方差矩阵为B,则向量a+向量b的协方差矩阵为A+B矩阵
// 协方差的更新
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
7. 代码
文件位置在vins_estimator/src/factor/integration_base.h
/**
* @class IntegrationBase IMU预积分类 ：加速度计、陀螺仪、线性加速度计ba、陀螺仪bg、雅克比矩阵初始化、协方差矩阵15*15、dt、PVQ
* @Description
*/
class IntegrationBase
{
public:
IntegrationBase() = delete;
IntegrationBase(const Eigen::Vector3d &_acc_0, // 初始时刻的加速度
const Eigen::Vector3d &_gyr_0, // 初始时刻的角速度
const Eigen::Vector3d &_linearized_ba, // 初始时刻加速计的零偏
const Eigen::Vector3d &_linearized_bg) // 初始时刻角速度计的零偏
: acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0},
linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg},
jacobian{Eigen::Matrix<double, 15, 15>::Identity()}, // 预积分量对自身的雅克比矩阵(见博客6.2)
covariance{Eigen::Matrix<double, 15, 15>::Zero()}, // 协方差矩阵,没有数据置信度自然为0
sum_dt{0.0},
delta_p{Eigen::Vector3d::Zero()}, // 初始的位移α
delta_q{Eigen::Quaterniond::Identity()}, // 初始的旋转γ
delta_v{Eigen::Vector3d::Zero()} // 初始的速度β
{
// ΔXk+1 = F * ΔXk + G * nk ,这里的噪声项nk为18*1的矩阵
// 则ΔXk+1的协方差矩阵为 Pk+1 = F*Pk*F^T + G*V*G^T, 这里噪声的协方差矩阵为18*18的矩阵
// 具体见博客6.3
noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); // k时刻k+1时刻加速度计的噪声是相同的
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); // k时刻k+1时刻陀螺仪的噪声是相同的
noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); // 加速度计零偏的噪声
noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); // 陀螺仪零偏的噪声
}
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
// 方便后续做repropagate
dt_buf.push_back(dt); // 时间戳
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
// IMU预积分传播方程
propagate(dt, acc, gyr);
}
//优化过程中Bias会更新，需要根据新的bias重新计算预积分
void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg)
{
sum_dt = 0.0;
acc_0 = linearized_acc;
gyr_0 = linearized_gyr;
delta_p.setZero();
delta_q.setIdentity();
delta_v.setZero();
linearized_ba = _linearized_ba;
linearized_bg = _linearized_bg;
jacobian.setIdentity();
covariance.setZero();
for (int i = 0; i < static_cast<int>(dt_buf.size()); i++)
propagate(dt_buf[i], acc_buf[i], gyr_buf[i]);
}
/**
* @brief IMU预积分中采用中值积分递推Jacobian和Covariance，构造误差的线性化递推方程，得到Jacobian和Covariance递推公式-> Paper 式27
* @return void
*/
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");
/// step1 中值积分更新状态量(见博客2.1) 公式27
// a = R(γ)(ai - ba) 公式（27）
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); // 上一时刻的IMU在初始帧下的加速度的值
// ω = 1/2(ω0 + ω1) - bw
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; // k时刻和k+1时刻陀螺仪的值相加/2 - 零偏
// 四元数转化为轴角：q = [cosθ/2,n * sinθ/2] ,当θ趋近于0时： θ -> 0 => q = [1,w*t/2]
// γ_i+1 = γ_i * [1,1/2 * ω * dt ] 中值积分后的旋转
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); // k+1 时刻的加速度值
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;
/// step2 更新方差矩阵和雅克比
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;
// 协方差矩阵 (见博客5.0) δx_k+1 = F * δ_k + V * n_t
/*[ δα_k+1 ] [ I f01 δt f03 f04 ] [ δα_k ] [ v00 v01 v02 v03 0 0 ][ n_ak ]
*[ δθ_k+1 ] [ 0 f11 0 0 -δt ] [ δθ_k ] [ 0 -δt/2 0 -δt/2 0 0 ][ n_ωk ]
*[ δβ_k+1 ] = [ 0 f21 I f23 f24 ] [ δβ_k ] + [ -R_k*δt/2 v21 -R_k+1*δt/2 v23 0 0 ][ n_ak+1]
*[ δb_a_k+1] [ 0 0 0 0 0 ] [ δb_a_k] [ 0 0 0 0 0 0 ][ n_ωk+1]
*[ δb_ω_k+1] [ 0 0 0 0 0 ] [ δb_ω_k] [ 0 0 0 0 0 δt][ n_ba ]
* [ n_bw ]
*/
// F矩阵
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;
// 都与博客相差一个负号,因为噪声都是0均值的高斯白噪声,所以没有影响
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;
// 雅克比的更新(博客6.2)
jacobian = F * jacobian;
// 协方差的更新(博客6.3)
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}
/**
* @brief IMU预积分传播方程
* @Description 积分计算两个关键帧之间IMU测量的变化量：
* 旋转delta_q 速度delta_v 位移delta_p
* 加速度的biaslinearized_ba 陀螺仪的Bias linearized_bg
* 同时维护更新预积分的Jacobian和Covariance,计算优化时必要的参数
* @param[in] _dt 时间间隔
* @param[in] _acc_1 线加速度
* @param[in] _gyr_1 角速度
* @return void
*/
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
// 中值积分
midPointIntegration(_dt, acc_0, gyr_0, // k时刻加速度计和陀螺仪的值
_acc_1, _gyr_1, // k+1时刻加速度计和陀螺仪的值
delta_p, delta_q, delta_v, // k时刻的α,γ,β
linearized_ba, linearized_bg, // 两帧图像帧之间零偏不变
result_delta_p, result_delta_q, result_delta_v, // k+1时刻的α,γ,β
result_linearized_ba, result_linearized_bg, // k+1时刻的零偏
1);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
sum_dt += dt;
acc_0 = acc_1;
gyr_0 = gyr_1;
}
/**
* 计算和给定相邻帧状态量的残差
* @param Pi，Qi，Vi，Bai，Bgi [前一次预积分结果]
* @param Pj，Qj，Vj，Baj，Bgj [后一次预积分结果]
* @return
*/
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
// 获取P V Q 关于 ba，bg的雅克比 见博客6.1
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
/// 求bias改变后的预积分量
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
// 对零偏雅克比线性展开
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
/// 计算IMU的残差
// 位置的残差
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
// 旋转的残差 ，.vec()：取出虚部
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
// 速度的残差
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
// 零偏
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
double dt;
Eigen::Vector3d acc_0, gyr_0;
Eigen::Vector3d acc_1, gyr_1;
const Eigen::Vector3d linearized_acc, linearized_gyr;
Eigen::Vector3d linearized_ba, linearized_bg;
Eigen::Matrix<double, 15, 15> jacobian, covariance;
Eigen::Matrix<double, 15, 15> step_jacobian;
Eigen::Matrix<double, 15, 18> step_V;
Eigen::Matrix<double, 18, 18> noise;
double sum_dt;
//预积分值
Eigen::Vector3d delta_p;
Eigen::Quaterniond delta_q;
Eigen::Vector3d delta_v;
std::vector<double> dt_buf;
std::vector<Eigen::Vector3d> acc_buf;
std::vector<Eigen::Vector3d> gyr_buf;
};
870
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
