fast-lio代码解析(二）
文章目录
- 1. 状态解释
- 1. 状态变量定义
- 2. 输入定义
- 3. 状态转移函数 f f f
- 4. 雅可比矩阵 ∂ f ∂ x \frac{\partial f}{\partial \mathbf{x}} ∂x∂f
- 5. 雅可比矩阵 ∂ f ∂ w \frac{\partial f}{\partial \mathbf{w}} ∂w∂f
- 6. 广义加法boxplus
- 7. 前向传播predict
注，本代码解释的为简化版fast-lio：github
1. 状态解释
struct state_ikfom
{
// 位置向量，表示系统的全局位置 (x, y, z)
Eigen::Vector3d pos = Eigen::Vector3d(0,0,0);
// 旋转矩阵，使用 Sophus::SO3 表示旋转，初始化为单位矩阵 (无旋转)
Sophus::SO3 rot = Sophus::SO3(Eigen::Matrix3d::Identity());
// LiDAR 到 IMU 的旋转偏移，初始化为单位旋转
Sophus::SO3 offset_R_L_I = Sophus::SO3(Eigen::Matrix3d::Identity());
// LiDAR 到 IMU 的平移偏移，初始化为零向量
Eigen::Vector3d offset_T_L_I = Eigen::Vector3d(0,0,0);
// 速度向量，表示系统的线速度 (vx, vy, vz)
Eigen::Vector3d vel = Eigen::Vector3d(0,0,0);
// 陀螺仪偏置 (gyroscope bias)，用于补偿陀螺仪测量误差
Eigen::Vector3d bg = Eigen::Vector3d(0,0,0);
// 加速度计偏置 (accelerometer bias)，用于补偿加速度计测量误差
Eigen::Vector3d ba = Eigen::Vector3d(0,0,0);
// 重力向量，通常设定为 (0, 0, -9.81) m/s²
Eigen::Vector3d grav = Eigen::Vector3d(0,0,-G_m_s2);
};
1. 状态变量定义
状态变量 x x x 包括以下部分：
- 位置信息 p p p： p ∈ R 3 \mathbf{p} \in \mathbb{R}^3 p∈R3，表示全局坐标系中的位置。
- 旋转信息 R R R： R ∈ S O ( 3 ) \mathbf{R} \in SO(3) R∈SO(3)，表示从局部到全局的旋转矩阵。
- LiDAR 相对偏移（旋转 Δ R \Delta R ΔR 和平移 Δ T \Delta T ΔT）： Δ R ∈ S O ( 3 ) , Δ T ∈ R 3 \Delta \mathbf{R} \in SO(3), \Delta \mathbf{T} \in \mathbb{R}^3 ΔR∈SO(3),ΔT∈R3。
- 速度信息 v \mathbf{v} v： v ∈ R 3 \mathbf{v} \in \mathbb{R}^3 v∈R3，表示在当前时刻的线速度。
- 陀螺仪偏差 b g \mathbf{b}_g bg 和加速度计偏差 b a \mathbf{b}_a ba： b g , b a ∈ R 3 \mathbf{b}_g, \mathbf{b}_a \in \mathbb{R}^3 bg,ba∈R3。
- 重力方向 g g g： g ∈ S 2 g \in S^2 g∈S2，定义在单位球面上。
公式表示为：
x
=
{
p
,
R
,
Δ
R
,
Δ
T
,
v
,
b
g
,
b
a
,
g
}
\mathbf{x} = \{\mathbf{p}, \mathbf{R}, \Delta \mathbf{R}, \Delta \mathbf{T}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a, g\}
x={p,R,ΔR,ΔT,v,bg,ba,g}
2. 输入定义
// 输入结构体，表示IMU的输入数据
struct input_ikfom
{
// 加速度计测量值，单位通常为 m/s²
Eigen::Vector3d acc = Eigen::Vector3d(0,0,0);
// 陀螺仪测量值，单位通常为 rad/s
Eigen::Vector3d gyro = Eigen::Vector3d(0,0,0);
};
系统的输入包括：
- 加速度 a \mathbf{a} a： a ∈ R 3 \mathbf{a} \in \mathbb{R}^3 a∈R3。
- 角速度 ω \boldsymbol{\omega} ω： ω ∈ R 3 \boldsymbol{\omega} \in \mathbb{R}^3 ω∈R3。
公式表示为：
u
=
{
a
,
ω
}
\mathbf{u} = \{\mathbf{a}, \boldsymbol{\omega}\}
u={a,ω}
3. 状态转移函数 f f f
Eigen::Matrix<double, 24, 1> get_f(state_ikfom s, input_ikfom in)
{
// 对应顺序为速度(3)，角速度(3),外参T(3),外参旋转R(3)，加速度(3),角速度偏置(3),加速度偏置(3),位置(3)，与论文公式顺序不一致
Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
Eigen::Vector3d omega = in.gyro - s.bg; // 输入的imu的角速度(也就是实际测量值) - 估计的bias值(对应公式的第1行)
Eigen::Vector3d a_inertial = s.rot.matrix() * (in.acc - s.ba); // 输入的imu的加速度，先转到世界坐标系（对应公式的第3行）
for (int i = 0; i < 3; i++)
{
res(i) = s.vel[i]; //速度（对应公式第2行）
res(i + 3) = omega[i]; //角速度（对应公式第1行）
res(i + 12) = a_inertial[i] + s.grav[i]; //加速度（对应公式第3行）
}
return res;
}
状态转移函数
f
f
f 表示系统在状态
x
x
x 和输入
u
u
u 下的演化方为：
f
(
x
,
u
)
=
[
v
ω
−
b
g
0
0
R
(
a
−
b
a
)
+
g
0
0
0
]
\mathbf{f}(\mathbf{x}, \mathbf{u}) = \begin{bmatrix} \mathbf{v} \\ \boldsymbol{\omega} - \mathbf{b}_g \\ \mathbf{0} \\ \mathbf{0} \\ \mathbf{R} (\mathbf{a} - \mathbf{b}_a) + g \\ \mathbf{0} \\ \mathbf{0}\\ \mathbf{0} \end{bmatrix}
f(x,u)=
vω−bg00R(a−ba)+g000
- 第 1-3 维为 v \mathbf{v} v（线速度）。
- 第 4-6 维为 ω − b g \boldsymbol{\omega} - \mathbf{b}_g ω−bg（角速度减去陀螺仪偏差）。
- 第 7-12 维初始化为零。
- 第 13-15 维为 a inertial \mathbf{a}_\text{inertial} ainertial（加速度的全局投影）。
- 第 16-24 维初始化为零。
4. 雅可比矩阵 ∂ f ∂ x \frac{\partial f}{\partial \mathbf{x}} ∂x∂f
// 定义函数：计算状态方程对状态变量的雅可比矩阵（df/dx）
//对应公式(7)的Fx 注意该矩阵没乘dt，没加单位阵
Eigen::Matrix<double, 24, 24> df_dx(state_ikfom s, input_ikfom in)
{
Eigen::Matrix<double, 24, 24> cov = Eigen::Matrix<double, 24, 24>::Zero();
cov.block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); //对应公式(7)第2行第3列 I
Eigen::Vector3d acc_ = in.acc - s.ba; //测量加速度 = a_m - bias
cov.block<3, 3>(12, 3) = -s.rot.matrix() * Sophus::SO3::hat(acc_); //对应公式(7)第3行第1列
cov.block<3, 3>(12, 18) = -s.rot.matrix(); //对应公式(7)第3行第5列
cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); //对应公式(7)第3行第6列 I
cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); //对应公式(7)第1行第4列 (简化为-I)
return cov;
}
对应论文中的公式(7)为
F
x
=
[
Exp
(
−
ω
^
i
Δ
t
)
0
0
−
A
(
ω
^
i
Δ
t
)
T
Δ
t
0
0
0
I
I
Δ
t
0
0
0
−
G
R
^
i
[
a
^
i
]
∧
Δ
t
0
I
0
−
G
R
^
i
Δ
t
I
Δ
t
0
0
0
I
0
0
0
0
0
0
I
0
0
0
0
0
0
I
]
\mathbf{F}_{\mathbf{x}} = \begin{bmatrix} \text{Exp}(-\hat{\boldsymbol{\omega}}_i \Delta t) & \mathbf{0} & \mathbf{0} & -\mathbf{A}(\hat{\boldsymbol{\omega}}_i \Delta t)^T \Delta t & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & \mathbf{I} \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ -G \hat{\mathbf{R}}_i [\hat{\mathbf{a}}_i]^\wedge \Delta t & \mathbf{0} & \mathbf{I} & \mathbf{0} & -G \hat{\mathbf{R}}_i \Delta t & \mathbf{I} \Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \end{bmatrix}
Fx=
Exp(−ω^iΔt)0−GR^i[a^i]∧Δt0000I00000IΔtI000−A(ω^iΔt)TΔt00I0000−GR^iΔt0I000IΔt00I
5. 雅可比矩阵 ∂ f ∂ w \frac{\partial f}{\partial \mathbf{w}} ∂w∂f
//对应公式(7)的Fw 注意该矩阵没乘dt
Eigen::Matrix<double, 24, 12> df_dw(state_ikfom s, input_ikfom in)
{
Eigen::Matrix<double, 24, 12> cov = Eigen::Matrix<double, 24, 12>::Zero();
cov.block<3, 3>(12, 3) = -s.rot.matrix(); //对应公式(7)第3行第2列 -R
cov.block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); //对应公式(7)第1行第1列 -A(w dt)简化为-I
cov.block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); //对应公式(7)第4行第3列 I
cov.block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); //对应公式(7)第5行第4列 I
return cov;
}
对应论文中的公式(7)为
F
w
=
[
−
A
(
ω
^
i
Δ
t
)
T
Δ
t
0
0
0
0
0
0
0
0
0
0
0
−
G
R
^
i
Δ
t
0
0
0
0
0
I
Δ
t
0
0
0
0
0
I
Δ
t
0
0
0
0
0
]
\mathbf{F}_{\mathbf{w}} = \begin{bmatrix} -\mathbf{A}(\hat{\boldsymbol{\omega}}_i \Delta t)^T \Delta t & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -G \hat{\mathbf{R}}_i \Delta t & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \Delta t & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \Delta t \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{bmatrix}
Fw=
−A(ω^iΔt)TΔt0000000000000−GR^iΔt000000IΔt000000IΔt0
6. 广义加法boxplus
//广义加法 公式(4)
state_ikfom boxplus(state_ikfom x, Eigen::Matrix<double, 24, 1> f_)
{
state_ikfom x_r;
x_r.pos = x.pos + f_.block<3, 1>(0, 0);
x_r.rot = x.rot * Sophus::SO3::exp(f_.block<3, 1>(3, 0));
x_r.offset_R_L_I = x.offset_R_L_I * Sophus::SO3::exp(f_.block<3, 1>(6, 0));
x_r.offset_T_L_I = x.offset_T_L_I + f_.block<3, 1>(9, 0);
x_r.vel = x.vel + f_.block<3, 1>(12, 0);
x_r.bg = x.bg + f_.block<3, 1>(15, 0);
x_r.ba = x.ba + f_.block<3, 1>(18, 0);
x_r.grav = x.grav + f_.block<3, 1>(21, 0);
return x_r;
}
给定状态 x \boldsymbol{x} x 和增量向量 ξ ∈ R 24 \boldsymbol{\xi} \in \mathbb{R}^{24} ξ∈R24，更新后的状态 x ′ \boldsymbol{x}' x′ 表达如下：
x pos ′ = x pos + ξ 0 : 3 \boldsymbol{x}'_{\text{pos}} = \boldsymbol{x}_{\text{pos}} + \boldsymbol{\xi}_{0:3} xpos′=xpos+ξ0:3
R ′ = R ⋅ exp ( ξ 3 : 6 ) \boldsymbol{R}' = \boldsymbol{R} \cdot \exp\left(\boldsymbol{\xi}_{3:6}\right) R′=R⋅exp(ξ3:6)
R L I ′ = R L I ⋅ exp ( ξ 6 : 9 ) \text{R}_{LI}' =\text{R}_{LI}\cdot \exp\left(\boldsymbol{\xi}_{6:9}\right) RLI′=RLI⋅exp(ξ6:9)
T L I ′ = T L I + ξ 9 : 12 \text{T}_{LI}'= \text{T}_{LI} + \boldsymbol{\xi}_{9:12} TLI′=TLI+ξ9:12
vel
′
=
vel
+
ξ
12
:
15
\text{vel}' = \text{vel} + \boldsymbol{\xi}_{12:15}
vel′=vel+ξ12:15
广义减法其他同理。
//广义减法
vectorized_state boxminus(state_ikfom x1, state_ikfom x2)
{
vectorized_state x_r = vectorized_state::Zero();
x_r.block<3, 1>(0, 0) = x1.pos - x2.pos;
x_r.block<3, 1>(3, 0) = Sophus::SO3(x2.rot.matrix().transpose() * x1.rot.matrix()).log();
x_r.block<3, 1>(6, 0) = Sophus::SO3(x2.offset_R_L_I.matrix().transpose() * x1.offset_R_L_I.matrix()).log();
x_r.block<3, 1>(9, 0) = x1.offset_T_L_I - x2.offset_T_L_I;
x_r.block<3, 1>(12, 0) = x1.vel - x2.vel;
x_r.block<3, 1>(15, 0) = x1.bg - x2.bg;
x_r.block<3, 1>(18, 0) = x1.ba - x2.ba;
x_r.block<3, 1>(21, 0) = x1.grav - x2.grav;
return x_r;
}
7. 前向传播predict
//前向传播 公式(4-8)
void predict(double &dt, Eigen::Matrix<double, 12, 12> &Q, const input_ikfom &i_in)
{
Eigen::Matrix<double, 24, 1> f_ = get_f(x_, i_in); //公式(3)的f
Eigen::Matrix<double, 24, 24> f_x_ = df_dx(x_, i_in); //公式(7)的df/dx
Eigen::Matrix<double, 24, 12> f_w_ = df_dw(x_, i_in); //公式(7)的df/dw
x_ = boxplus(x_, f_ * dt); //前向传播 公式(4)
f_x_ = Matrix<double, 24, 24>::Identity() + f_x_ * dt; //之前Fx矩阵里的项没加单位阵，没乘dt 这里补上
P_ = (f_x_)*P_ * (f_x_).transpose() + (dt * f_w_) * Q * (dt * f_w_).transpose(); //传播协方差矩阵，即公式(8)
}
前向传播：前向传播在接收到IMU输入后执行。更具体地说，状态通过设置过程噪声 w i w_i wi 为零来传播：
x ^ i + 1 = x ^ i ⊞ ( Δ t f ( x ^ i , u i , 0 ) ) ; x ^ 0 = x ˉ k − 1 . (4) \hat{x}_{i+1} = \hat{x}_i \boxplus (\Delta t f(\hat{x}_i, u_i, 0)); \quad \hat{x}_0 = \bar{x}_{k-1}. \tag{4} x^i+1=x^i⊞(Δtf(x^i,ui,0));x^0=xˉk−1.(4)
为了传播协方差，我们使用获得的误差状态动态模型：
x ~ i + 1 = x i + 1 ⊟ x ^ i + 1 = ( x i ⊞ Δ t f ( x i , u i , w i ) ) ⊞ ( x ^ i ⊞ Δ t f ( x ^ i , u i , 0 ) ) = ≃ ( 23 ) F x ~ x ~ i + F w w i . ( 5 ) \tilde{x}_{i+1} = x_{i+1} \boxminus \hat{x}_{i+1} = \left(x_{i} \boxplus \Delta t f(x_{i}, u_{i}, w_{i})\right) \boxplus \left(\hat{x}_{i} \boxplus \Delta t f(\hat{x}_{i}, u_{i}, 0)\right)= \stackrel{(23)}{\simeq} F_{\tilde{x}} \tilde{x}_{i} + F_{w} w_{i}. \quad (5) x~i+1=xi+1⊟x^i+1=(xi⊞Δtf(xi,ui,wi))⊞(x^i⊞Δtf(x^i,ui,0))=≃(23)Fx~x~i+Fwwi.(5)
公式(5)中的矩阵 F x ~ F_{\tilde{x}} Fx~ 和 F w F_w Fw根据附录V-A计算。结果在本页底部的公式(7)中显示，其中 ω ^ i = ω m i − b ^ ω i \hat{\omega}_i = \omega_{m_i} - \hat{b}_{\omega_i} ω^i=ωmi−b^ωi， a ^ i = a m i − b ^ a i \hat{a}_i = a_{m_i} - \hat{b}_{a_i} a^i=ami−b^ai，并且 A ( u ) − 1 A(u)^{-1} A(u)−1 遵循[26]中相同的定义，如下所示：
A
(
u
)
−
1
=
I
−
1
2
[
u
]
∧
+
(
1
−
α
(
∥
u
∥
)
)
[
u
]
∧
2
∥
u
∥
2
A(u)^{-1} = I - \frac{1}{2}[u]_{\wedge} + \left(1 - \alpha(\|u\|)\right)\frac{[u]_{\wedge}^2}{\|u\|^2}
A(u)−1=I−21[u]∧+(1−α(∥u∥))∥u∥2[u]∧2
α
(
m
)
=
m
2
cot
(
m
2
)
=
m
2
cos
(
m
/
2
)
sin
(
m
/
2
)
(6)
\alpha(m) = \frac{m}{2} \cot\left(\frac{m}{2}\right) = \frac{m}{2} \frac{\cos(m/2)}{\sin(m/2)} \tag{6}
α(m)=2mcot(2m)=2msin(m/2)cos(m/2)(6)
将白噪声 w w w的协方差记为 Q Q Q，则传播的协方差 P ^ i \hat{P}_i P^i 可以通过以下方程迭代计算：
P ^ i + 1 = F x ~ P ^ i F x ~ T + F w Q F w T ; P ^ 0 = P ˉ k − 1 . (8) \hat{P}_{i+1} = F_{\tilde{x}} \hat{P}_i F_{\tilde{x}}^T + F_w Q F_w^T; \quad \hat{P}_0 = \bar{P}_{k-1}. \tag{8} P^i+1=Fx~P^iFx~T+FwQFwT;P^0=Pˉk−1.(8)