预积分相关处理文件:integration_base.h
class IntegrationBase
构造函数
参数列表
IntegrationBase(
const Eigen::Vector3d &_acc_0,
const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_linearized_ba,
const Eigen::Vector3d &_linearized_bg
)
//_acc_0:初始时刻加速度;
//_gyr_0:初始时刻角加速度;
//_linearized_ba:加速度计临偏;
//_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()}
//jacobian:雅可比矩阵,初始化为单位矩阵,15 x 15
//covariance:协方差矩阵,初始化为零矩阵,15 x 15
//delta_p:对应于预积分量α,初始化为0向量,3 x 1
//delta_q:对应于预积分量γ,初始化为单位四元数, ,3 x 1
//delta_v:对应于预积分量β,初始化为零向量,3 x 1
构造函数
函数体
//Δx{k+1} = FΔx{k }+ Gn{k}
//P{k+1}=FP{K}F^T + GvG^T;
//P和v分别是Δx和n的协方差矩阵
//噪声;k时刻加速度计噪声和陀螺仪噪声、k+1时刻加速度计噪声和陀螺仪噪声、两个临偏的随机游走,即18 x 1;
//noise:此处noise指噪声的协方差矩阵v,18 x 18;
//初始化时,各变量仅与自身有关
//参考eigen中block的使用
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();
噪声的协方差矩阵noise:
函数1
push_back:
函数作用:
IntegrationBase类的接口,每来一个imu数据,调用该接口将参数存入对应buf,并通过函数propagate()进行处理;
函数声明:
void push_back(
double dt,
const Eigen::Vector3d &acc,
const Eigen::Vector3d &gyr
)
函数参数
// dt :时间戳
// acc:该时间戳加速度计值
// gyr:该时间戳陀螺仪值
propagate(dt, acc, gyr);//调用propagate函数进行中值积分
函数2
propagate
函数作用:
进行中值积分,然后更新上一时刻状态量;
函数声明:
void propagate(
double _dt,
const Eigen::Vector3d &_acc_1,
const Eigen::Vector3d &_gyr_1
)
函数参数
// _dt :时间戳
// _acc_1:该时间戳加速度计值
// _gyr_1:该时间戳陀螺仪值
midPointIntegration(
_dt, acc_0, gyr_0, _acc_1, _gyr_1,
//时间戳i时刻加速度计值和陀螺仪值、i+1时刻加速度计值和陀螺仪值
delta_p, delta_q, delta_v,
//i时候预积分α、β、γ
linearized_ba, linearized_bg,
//加速度计和陀螺仪临偏
result_delta_p, result_delta_q, result_delta_v,
//求得i+1时刻预积分α、β、γ
result_linearized_ba, result_linearized_bg, 1
//求得加速度计和陀螺仪临偏
);
//将当前时刻更新为上一时刻
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;
函数3
midPointIntegration
函数作用:
中值积分的具体实现过程;
函数声明:
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
)
函数参数
//_dt:时间戳
//_acc_0:i时刻加速度计值
//_gyr_0,:i时刻陀螺仪值
//_acc_1:i+1时刻加速度计值
//_gyr_1:i+1时刻陀螺仪值
//delta_p:i时刻预积分α
//delta_q:i时刻预积分γ,即Rki:第i帧IMU变换到第k帧图像的旋转
//delta_v,:i时刻预积分β
//linearized_ba:i时刻加速度计临偏
//linearized_bg,:i时刻陀螺仪临偏
//result_delta_p:出参,i+1时刻预积分α
//result_delta_q:出参,i+1时刻预积分γ
//result_delta_v,:出参,i+1时刻预积分β
//result_linearized_ba:出参,i+1时刻加速度计临偏
//result_linearized_bg:出参,i+1时刻陀螺仪临偏
//update_jacobian:是否更新雅可比矩阵
第一步:更新中值积分中的状态量
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
// a{k}=R{ki}*(a{i}-b{a}),将第i帧imu加速度计值转换到第k帧图像中;
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
//中值积分,用i和i+1时刻陀螺仪均值表示整段时间
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
//i+1时刻imu到第k帧图像的旋转=i时刻imu到第k帧旋转*第i时刻imu到第i+1时刻imu的旋转
//[cos(theta/2) n*sin(theta/2)],当角度趋近于0,[1 n*theta/2]=[1 w*dt/2],w是陀螺仪值;
//R{k<i+1}=R{ki}*R{i+1<-i}
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
// a{k}=R{ki+1}*(a{i}-ba),将第i+1帧imu加速度计值转换到第k帧图像中;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//中值积分,用i和i+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矩阵
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
//(w{k}+w{k+1})/2-b{wk},对应上面绿色框
Vector3d a_0_x = _acc_0 - linearized_ba;
//a{k}-b{ak},对应上面红色框
Vector3d a_1_x = _acc_1 - linearized_ba;
//a{k+1}-b{ak},对应上面黄色框
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;
//计算反对称矩阵
//开始更新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();
//更新V矩阵
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;
//更新雅可比矩阵和协方差矩阵
jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
联系客服