文章目录
- Imu 预积分
-
- Imu 推导 --部分直接给出答案
- 代码实现
-
- Imu 预积分
-
- 构造
- 添加 Imu 数据
- IMU预积分传播方程
- IMU预积分采用中值积分Jacobian和Covariance
- 计算 残差
- Ceres 求解器
- 旋转矩阵向其角度求导
Imu 预积分
imu自我递推即根据imu输入，计算位置(输入:线加速度 角速度)- IMU 积分基于起始位置( b t w P {_{bt}^{w}\textrm{P}} btwP)递推导致每次更新姿势都需要重新定位，所以 IMU积分 改为预积分模式仅与当前观测值有关，需记录：IMU的积分项 α b i b j {\alpha _{b_ib_j}} αbibj、 β b i b j {\beta _{b_ib_j}} βbibj、 q b i b j {q _{b_ib_j}} qbibj
- IMU的积分项 通过前一时刻的积分项递推而得，且递推中若采用中值积分，则与前一时刻位姿+当前在时刻位姿+角速度偏置+加速度偏置有关，因此可根据其进行方差传递。
- 对于残差：一段时间内IMU构建的预积分量作为测量值，对两时刻之间状态进行约束。
- 求解Ceres Jacobian：
- 即残差分别对其变量的求导，
- 其中偏差对bias的求导，偏差对预积分项求导*预积分项对偏差的求导。
Imu 推导 --部分直接给出答案
代码实现
Imu 预积分
构造
- 有参构造：初始加速度，初始角速度，初始bias偏差，其中Jacobian单位矩阵，协方差为0矩阵
- 记录了第一帧的加速度与角速度，方便更新bias重新进行预积分。
- 中值积分 中的固有 噪声。
- n k a , n k g , n k + 1 a , n k + 1 g , n b k a , n b k + 1 a {n^a_k,n^g_k,n^a_{k+1},n^g_{k+1},n^a_{b_k},n^a_{b_{k+1}}} nka,nkg,nk+1a,nk+1g,nbka,nbk+1a
- 其：如下 18*18矩阵所示
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()}, covariance{
Eigen::Matrix<double, 15, 15>::Zero()},
sum_dt{
0.0}, delta_p{
Eigen::Vector3d::Zero()}, delta_q{
Eigen::Quaterniond::Identity()}, delta_v{
Eigen::Vector3d::Zero()}
{
noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
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();
}
添加 Imu 数据
- 将 delta_t，线加速度，角速度 放入各自的队列中
- 目的：Bias会更新，需要根据新的bias重新计算预积分
- 进行 IMU预积分传播方程
IMU预积分传播方程
- 积分计算两个关键帧之间IMU测量的变化量：
- 旋转delta_q 速度delta_v 位移delta_p
- 加速度的
biaslinearized_ba陀螺仪的Biaslinearized_bg - 同时维护更新预积分的
Jacobian和Covariance,计算优化时必要的参数
- 全局变量：
- delta_p 对应公式： α b i b k { \alpha_{b_ib_{k}}} αbibk
- delta_q 对应公式： q b i b k {q_{b_ib_{k}}} qbibk
- delta_v 对应公式： β b i b k {\beta_{b_ib_{k}}} βbibk
- linearized_ba 对应公式： b k a {b_k^a} bka
- linearized_bg 对应公式： b k g {b_k^g} bkg
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, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 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;
}
IMU预积分中采用中值积分递推Jacobian和Covariance
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;
// 计算F_k 和 G_k ，与公式一样
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.bl