一、IMU 传感器模型
1. 坐标系
世界坐标系 , z 轴和重力方向对齐 (The direction of the gravity is aligned with the z-axis of the world frame)
机体坐标系 ,在 vins 中,指的是 imu 坐标系
2. 传感器测量值
1. linear acceleration with noise:
2. angular velocity with noise:
3. 传感器噪声建模
考虑 bias, 高斯白噪声,重力加速度叠加影响
(1)
其中 , 是 imu 测量值
, 是 noise-free 线加速度和角速度(imu 坐标系下)
, 对应 bias
, 对应高斯白噪声, ,
是机体坐标系的旋转矩阵
,代表重力加速度
二、运动模型
1. 微分方程
PVQ 在世界坐标系下表示
(1). 对位置求导
(2)
(2). 对速度求导
(3)
(3). 对四元数求导
(4)
其中,, 为 的叉乘
注意: 在有的参考文献中,四元数的实部在前面, 的表达式会有差异,这里虚部在前是为了和 vins 中的写法保持一致
(4). bais 随机游走
, 被建模为随机游走, 其导数是高斯白噪声
,
, (5)
2. 连续形式积分
(1). 速度积分
更新 V
(6)
(2).位置积分
更新 P
(7)
(3). 四元数积分
更新 Q
(8)
3. 结论
From (6).(7).(8) It can be seen that the IMU state propagation requires rotation, position, and velocity of the frame . When these starting states change, we need to repropagate IMU measurements. Especially, in the optimization-based algorithm, every time we adjust poses, we will need to repropagate IMU measurements between them.
三、预积分
1. 基本概念
After changing the reference frame from the world frame to the local frame , we can only preintegrate the parts that are related to linear acceleration and angular velocity
即更新 PVQ 增量
2. 连续形式
将式 (6). (7). (8) 左乘 ,把积分部分单独拎出来
(9)
其中
(10)
It can be seen that the preintegration terms can be obtained solely with IMU measurements by taking as the reference frame given bias. , , and are only related to IMU biases instead of other states in and .
3. 离散形式
At the beginning, , are 0, and is identity quaternion
(1). 积分方式
对于非线性微分方程:
(11)
通过 龙格-库塔(Runge-Kutta)公式可以得到该微分方程的数值解
<1>. zero-order hold
Euler method: assumes that the derivative is constant over the interval
(12)
Midpoint method: assumes that the derivative is the one at the midpoint of the interval
(13)
<2>. higher-order hold
RK4 integration
(14)
(2). Euler Integration
(15)
where is discrete moment corresponding to a IMU measurement within . is the time interval between two IMU
measurements and .
(3). Midpoint Integration
(16)
4. error state dynamic
(1). 预积分 error state 定义
true state: 考虑噪声、bias 的随机游走,表示为
nominal state: 不考虑噪声、bias 的随机游走,表示为
error state: true state 和 nominal state 的差值,表示为
(17)
(18)
(19)
(20)
(21)
其中 是 true state, 是 nominal state
由于四元数是过参数化,所以用旋转矢量的扰动 来表示 error state
(2). true-state kinematics
(22)
(23)
(24)
(25)
(26)
(3). nominal-state kinematics
(27)
(28)
(29)
(30)
(31)
(4). error-state kinematics
<1>. position error
由式(22)(27), 可得出
(32)
<2>. bias error
由式(25)(30), (26)(31), 可得出
(33)
(34)
<3>. velocity error
由式(23)(28),以及 可得出
(35)
<4>. orientation error
对式(19) 求导,其中
左边: (36)
右边: (37)
(36),(37) 同时消去 ,得到
,那么可以得到 的导数
(38)
令 提取第二行,得到
(39)
<5>. 矩阵形式
(40)
其中, , , , ,
(5). error dynamic 离散形式(欧拉积分)
<1>. 在 时间间隔内,认为一阶导数 恒定,那么离散化后得到的状态转移矩阵为:
(41)
<2>. 高斯噪声积分
连续形式高斯噪声:
(42)
积分得到
(43)
<3>. covariance
初始时刻, 协方差为0, 即
(44)
<4>. 雅可比迭代
初始时刻
, (45)
(6). error dynamic 离散形式(中值积分)
<1>. 误差传播公式
(46)
简写为
其中, , , ,
<2>. 协方差预测
对于协方差:
(47)
初始时刻, 协方差为0, 即 ,
且 (48)
<3>. 雅可比迭代
, 初始时刻
5. correct preintegration results
When the estimation of bias changes, if the change is small,we adjust , , and by their first-order approximations with respect to the bias(参考式(49)), otherwise we do repropagation. This strategy saves a significant amount of computational resources for optimization-based algorithms, since we do not need to propagate IMU measurements repeatedly.
(49)
关于雅可比,,,,,, 可以从 上述离散化求得的雅可比 中提取
四、Vins 源码解析—— class Estimator
1. API
// t: delta_time, 距离上一次 imu data 的时间差
// linear_acceleration: 线加速度
// angular_velocity: 角速度
void Estimator::processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);
2. processIMU 流程
(1). 创建 imu 预积分对象
// 对于视觉帧对应的第一个 imu data, 需要创建 imu 预积分对象
if (!pre_integrations[frame_count]) {
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
(2). imu 预积分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
(3). 缓存原始数据
dt_buf[frame_count].push_back(dt);
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
(4). 依据 imu 观测更新 PVQ
中值积分
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
(5). 更新 acc_0, gyr_0
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
五、Vins 源码解析—— class IntegrationBase
1. 成员变量
const Eigen::Vector3d linearized_acc, linearized_gyr; | 初始时刻的 acc, gyr, 用于 repropagate() |
std::vector<double> dt_buf; std::vector<Eigen::Vector3d> acc_buf; std::vector<Eigen::Vector3d> gyr_buf; | 原始 imu 观测, 用于 repropagate() |
double dt; Eigen::Vector3d acc_0, gyr_0; Eigen::Vector3d acc_1, gyr_1; | 最新的两帧 acc, gyr 观测 |
double sum_dt; Eigen::Vector3d delta_p; Eigen::Quaterniond delta_q; Eigen::Vector3d delta_v; Eigen::Vector3d linearized_ba, linearized_bg; | imu 预积分结果 |
Eigen::Matrix<double, 18, 18> noise | imu 噪声 |
Eigen::Matrix<double, 15, 15> jacobian | jacobian |
Eigen::Matrix<double, 15, 15> covariance | 预积分约束 covariance |
2. 构造函数
// _acc_0, _gyr_0: the first acc, gyr
// _linearized_ba, _linearized_bg: the acc, gyr bias
// 初始化: delta_p, delta_q, delta_v: 设置为 identity/zero
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()}
{
// set noise matrix, 更新 Q
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();
}
3. 处理新的数据
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
// 压入 buffer
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
// 用原始的 bias 预积分
propagate(dt, acc, gyr);
}
4. propagate
imu 预积分
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);
// 更新中值预积分结果
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;
}
5. repropagate(循环调用 propagate)
用新的 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]);
}
6. 预积分(中值积分)
(1). 更新 pvq 增量
// 从 t0 时刻的状态推导 t1 时刻状态
// _dt: t0->t1时间间隔
// _acc0: t0时加速度; _acc1: t1时加速度
// _gyr_0: t0时角速度; _gyr1: t1时角速度
// delta_p: t0时 \alpaha
// delta_q: t0时 \gamma
// delta_v: t0时 \beta
// linearized_ba, linearized_bg: bias
// result_delta_p: t1时 \alpaha
// reulst_delta_q: t1时 \gamma
// reulst_delta_v: t1时 \beta
// update_jacobian: 是否更新雅可比
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)
{
// 中值预积分
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; // bias 没有改变
result_linearized_bg = linearized_bg;
if(update_jacobian) {
// 更新 jacobian 和 covariance
......
}
}
(2). 更新 jacobian 和 Covariance
参考 error state dynamic(covariance)
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();
7. evaluate()
参考 imu factor