男女做那事视频免费网站,开网络公司做网站挣钱么,怎么制作论坛,wordpress注册跳过邮箱验证IMU状态预积分代码实现 —— IMU状态预积分类 实现IMU状态预积分类 实现IMU状态预积分类
首先#xff0c;实现预积分自身的结构。一个预积分类应该存储一下数据#xff1a;
预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtrian… IMU状态预积分代码实现 —— IMU状态预积分类 实现IMU状态预积分类 实现IMU状态预积分类
首先实现预积分自身的结构。一个预积分类应该存储一下数据
预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij} △R~ij,△v~ij,△p~ij预积分开始时的IMU零偏 b g , b a b_{g},b_{a} bg,ba在积分时期内的测量噪声 Σ i , k 1 \Sigma _{i,k1} Σi,k1各积分量对IMU零偏的雅克比矩阵整个积分时间 △ t i j \bigtriangleup t_{ij} △tij
以上都是必要的信息。除此之外也可以将IMU的读数记录在预积分类中当然也可以不记录因为都已经积分过了。同时IMU的测量噪声和零偏随机游走噪声也可以作为配置参数写在预积分类中。
声明这个类
class IMUPreintegration {参数配置项 其中包括
陀螺仪初始零偏加速度计初始零偏陀螺噪声加计噪声 /// 参数配置项/// 初始的零偏需要设置其他可以不改struct Options {Options() {}Vec3d init_bg_ Vec3d::Zero(); // 初始零偏Vec3d init_ba_ Vec3d::Zero(); // 初始零偏double noise_gyro_ 1e-2; // 陀螺噪声标准差double noise_acce_ 1e-1; // 加计噪声标准差};构造函数
IMUPreintegration(Options options Options());中间省略函数的声明之后再写。
下面完成类的成员变量定义 整体预积分时间 double dt_ 0; // 整体预积分时间噪声矩阵累积噪声矩阵 Σ i , k 1 \Sigma _{i,k1} Σi,k1 测量噪声矩阵 C o v ( η d , k ) Cov(\eta_{d,k} ) Cov(ηd,k) Mat9d cov_ Mat9d::Zero(); // 累计噪声矩阵Mat6d noise_gyro_acce_ Mat6d::Zero(); // 测量噪声矩阵预积分开始时的IMU零偏 b g , b a b_{g},b_{a} bg,ba // 零偏Vec3d bg_ Vec3d::Zero();Vec3d ba_ Vec3d::Zero();预积分的观测量 △ R ~ i j , △ v ~ i j , △ p ~ i j \bigtriangleup \tilde{R} _{ij},\bigtriangleup \tilde{v} _{ij},\bigtriangleup \tilde{p} _{ij} △R~ij,△v~ij,△p~ij // 预积分观测量SO3 dR_;Vec3d dv_ Vec3d::Zero();Vec3d dp_ Vec3d::Zero();各积分量对IMU零偏的雅克比矩阵 // 雅可比矩阵Mat3d dR_dbg_ Mat3d::Zero();Mat3d dV_dbg_ Mat3d::Zero();Mat3d dV_dba_ Mat3d::Zero();Mat3d dP_dbg_ Mat3d::Zero();Mat3d dP_dba_ Mat3d::Zero();因为IMU零偏相关的噪声项并不直接和预积分类有关所以将它们挪到优化类当中。这个类主要完成对IMU数据进行预积分操作然后提供积分之后的观测量与噪声值。
下面来看单个IMU的积分函数首先在类中进行声明。 /*** 插入新的IMU数据* param imu imu 读数* param dt 时间差*/void Integrate(const IMU imu, double dt);来看函数具体实现
整体而言它按照以下顺序更新内部的成员变量
更新位置和速度的测量值更新运动模型的噪声矩阵更新观测量对零偏的各雅克比矩阵更新旋转部分的测量值更新积分时间在这里插入代码片
void IMUPreintegration::Integrate(const IMU imu, double dt) {去掉零偏的测量 Vec3d gyr imu.gyro_ - bg_; // 陀螺Vec3d acc imu.acce_ - ba_; // 加计更新预积分速度观测量和位置观测量 // 更新dv, dpdp_ dp_ dv_ * dt 0.5f * dR_.matrix() * acc * dt * dt;dv_ dv_ dR_ * acc * dt;对应公式为 预积分旋转观测 dR先不更新因为A, B阵还需要现在的dR
下面计算运动方程雅克比矩阵系数A、B阵用于更新噪声项 // 运动方程雅可比矩阵系数A,B阵// 另外两项在后面Eigen::Matrixdouble, 9, 9 A;A.setIdentity();Eigen::Matrixdouble, 9, 6 B;B.setZero();加速度计的伴随矩阵和t的平方 Mat3d acc_hat SO3::hat(acc);double dt2 dt * dt;公式中的这个地方有用到避免重复计算 A.block3, 3(3, 0) -dR_.matrix() * dt * acc_hat;A.block3, 3(6, 0) -0.5f * dR_.matrix() * acc_hat * dt2;A.block3, 3(6, 3) dt * Mat3d::Identity();计算A矩阵中对应的各个块分别对应公式如下A矩阵中的A.block3, 3(0, 0)块之后用更新完的dR 更新 B.block3, 3(3, 3) dR_.matrix() * dt;B.block3, 3(6, 3) 0.5f * dR_.matrix() * dt2;更新B矩阵的各块分别对应公式如下 // 更新各雅可比dP_dba_ dP_dba_ dV_dba_ * dt - 0.5f * dR_.matrix() * dt2; dP_dbg_ dP_dbg_ dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_; dV_dba_ dV_dba_ - dR_.matrix() * dt; dV_dbg_ dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_; 更新各雅克比矩阵对应公式依次为 下面更新预积分旋转部分观测量 // 旋转部分Vec3d omega gyr * dt; // 转动量Mat3d rightJ SO3::jr(omega); // 右雅可比SO3 deltaR SO3::exp(omega); // exp后dR_ dR_ * deltaR; // 更新预积分旋转部分观测量对应公式 其中右雅克比矩阵的计算是为了更新上面的B矩阵 A.block3, 3(0, 0) deltaR.matrix().transpose();B.block3, 3(0, 0) rightJ * dt;利用更新完的dR和右雅克比矩阵更新A、B阵中对应的块 对应公式 // 更新噪声项cov_ A * cov_ * A.transpose() B * noise_gyro_acce_ * B.transpose();利用填充好的A阵和B阵来更新噪声项 对应公式如下 其中 C o v ( η d , k ) Cov(\eta_{d,k} ) Cov(ηd,k)即代码中的noise_gyro_acce_的构成就是陀螺仪和加计的噪声构成的对角矩阵在构造函数中构成的 const float ng2 options.noise_gyro_ * options.noise_gyro_;const float na2 options.noise_acce_ * options.noise_acce_;noise_gyro_acce_.diagonal() ng2, ng2, ng2, na2, na2, na2;下则继续更新预积分旋转观测量对陀螺仪零偏的雅克比矩阵 // 更新dR_dbgdR_dbg_ deltaR.matrix().transpose() * dR_dbg_ - rightJ * dt; 对应公式如下
最后增加积分时间 // 增量积分时间dt_ dt;这样就完成了一次对IMU数据的操作。需要注意的是如果不进行优化则预积分和直接积分的效果是完全一致的都是将IMU数据一次性地积分。在预积分之后也可以向ESKF一样从起始状态向最终状态进行预测。
预测函数实现如下 /*** 从某个起始点开始预测积分之后的状态* param start 起始时时刻状态* return 预测的状态*/NavStated IMUPreintegration::Predict(const sad::NavStated start, const Vec3d grav) const {SO3 Rj start.R_ * dR_;Vec3d vj start.R_ * dv_ start.v_ grav * dt_;Vec3d pj start.R_ * dp_ start.p_ start.v_ * dt_ 0.5f * grav * dt_ * dt_;auto state NavStated(start.timestamp_ dt_, Rj, pj, vj);state.bg_ bg_;state.ba_ ba_;return state;}与ESKF不同的是预积分可以对多个IMU数据进行预测可以从任意起始时刻向后预测而ESKF通常只在当前状态下针对单个IMU数据向下一时刻预测。
获取修正之后的观测量bias可以与预积分时期的不同会有一阶修正
// 预积分旋转零偏更新修正后测量值
SO3 IMUPreintegration::GetDeltaRotation(const Vec3d bg) { return dR_ * SO3::exp(dR_dbg_ * (bg - bg_)); }对应公式为 预积分速度零偏更新修正后测量值 // 预积分速度零偏更新修正后测量值Vec3d IMUPreintegration::GetDeltaVelocity(const Vec3d bg, const Vec3d ba) {return dv_ dV_dbg_ * (bg - bg_) dV_dba_ * (ba - ba_);}对应公式为 预积分位置零偏更新修正后测量值 // 预积分位置零偏更新修正后测量值Vec3d IMUPreintegration::GetDeltaPosition(const Vec3d bg, const Vec3d ba) {return dp_ dP_dbg_ * (bg - bg_) dP_dba_ * (ba - ba_);}对应公式为
