企业官方网站开发平台,网站快速收录教程,国际战事最新消息,wordpress搭建邮箱这里写目录标题 实现当前状态初始化实现预积分的初始化由于此时preintegration_options 是3#xff08;也就是考虑odo以及earth rotation#xff09;为预积分的容器添加需要积分的IMU积分因子接下来是添加新的IMU到preintegration中 实现当前状态初始化
这个state_curr的主要… 这里写目录标题 实现当前状态初始化实现预积分的初始化由于此时preintegration_options 是3也就是考虑odo以及earth rotation为预积分的容器添加需要积分的IMU积分因子接下来是添加新的IMU到preintegration中 实现当前状态初始化
这个state_curr的主要功能是初始化GNSS现在时刻的状态参数
实现预积分的初始化 由于此时preintegration_options 是3也就是考虑odo以及earth rotation
这里值的注意的问题在于此时emplace_back的类是基于PreintegrationEarthOdo的类所以在下面这个函数中返回的preintegration最终是PreintegrationEarthOdo类型的变量 注意这里需要调用PreintegrationEarthOdo的类默认构造函数 这个默认构造函数 首先将初始的状态进行重置 resetState(current_state_, NUM_STATE);//NUM_STATE 19;此处不明白pn_代表什么 函数iewn实现的主要功能是地球自转角速度在n系上的投影 其中Earth::iewn()函数——是将基站的坐标初始状态以及在当地坐标中的位置在局部坐标系转换成全局坐标系 将大地坐标系BLH转换为ECEF地心地固坐标系 然后将地心地固坐标系ECEF0转换为导航坐标系CNE0实现地心地固坐标系到导航系的转换 这里导航系n系——e系的转换矩阵 所以在OB_GINS中选取北东地坐标系为导航系 当前位置的local pos信息转换为在ECEF坐标系中的位置信息
Vector3d ecef1 ecef0 cn0e * local;将在当地坐标系的位置信息转换到ECEF地心地固坐标系后在转换成大地坐标系中
Vector3d blh1 ecef2blh(ecef1);下段代码的大致含义
iewn_skew_ Rotation::skewSymmetric(iewn_);
//这段代码目的是求出地球自转角速度的反对陈矩阵static Matrix3d skewSymmetric(const Vector3d vector) {Matrix3d mat;mat 0, -vector(2), vector(1), vector(2), 0, -vector(0), -vector(1), vector(0), 0;return mat;}设置噪声矩阵
最终获得了global的全局坐标系
为预积分的容器添加需要积分的IMU积分因子
此时的back()是取出preintegrationlist的最后一个元素然后向这个元素中加入NewImu 这个最后一个元素的类型是std::shared_ptr preintegrationlist的类型是PreintegrationBase类型
接下来是添加新的IMU到preintegration中 此时值得注意的问题在于integrationrocess是一个重写的函数 最终执行的是PreintegrationEarthOdo——因为之前的preintegration返回的类型是PreintegrationEarthOdo类型的变量 这里需要明确——返回的类型是基类而非派生类但是返回的变量的类型preintegration要定义为派生类类型
static std::shared_ptrPreintegrationBase creatPreintegration(const std::shared_ptrIntegrationParameters parameters, const IMU imu0, const IntegrationState state, PreintegrationOptions options)
{if (options PREINTEGRATION_EARTH_ODO) {//最终执行这个if条件语句preintegration std::make_sharedPreintegrationEarthOdo(parameters, imu0, state);}return preintegration;
}
这里先进行偏差补偿
void PreintegrationEarthOdo::integrationProcess(unsigned long index) {IMU imu_pre compensationBias(imu_buffer_[index - 1]);IMU imu_cur compensationBias(imu_buffer_[index]);// 区间时间累积double dt imu_cur.dt;delta_time_ dt;end_time_ imu_cur.time;current_state_.time imu_cur.time;// 连续状态积分, 先位置速度再姿态// 位置速度Vector3d dvfb imu_cur.dvel 0.5 * imu_cur.dtheta.cross(imu_cur.dvel) 1.0 / 12.0 * (imu_pre.dtheta.cross(imu_cur.dvel) imu_pre.dvel.cross(imu_cur.dtheta));// 哥氏项和重力项Vector3d dv_cor_g (gravity_ - 2.0 * iewn_.cross(current_state_.v)) * dt;// 地球自转补偿项, 省去了enwn项Vector3d dnn -iewn_ * dt;Quaterniond qnn Rotation::rotvec2quaternion(dnn);Vector3d dvel 0.5 * (Matrix3d::Identity() qnn.toRotationMatrix()) * current_state_.q.toRotationMatrix() * dvfb dv_cor_g;// 前后历元平均速度计算位置current_state_.p dt * current_state_.v 0.5 * dt * dvel;current_state_.v dvel;// 缓存IMU时刻位置, 时间间隔为两个历元的间隔pn_.emplace_back(std::make_pair(dt, current_state_.p));// 姿态Vector3d dtheta imu_cur.dtheta 1.0 / 12.0 * imu_pre.dtheta.cross(imu_cur.dtheta);current_state_.q qnn * current_state_.q * Rotation::rotvec2quaternion(dtheta);current_state_.q.normalize();// 预积分// 中间时刻的地球自转等效旋转矢量dnn -(delta_time_ - 0.5 * dt) * iewn_;Matrix3d cbbe (q0_.inverse() * Rotation::rotvec2quaternion(dnn) * q0_ * delta_state_.q).toRotationMatrix();// 里程增量//相比于earth 多了里程增量Vector3d dsodo Vector3d(imu_cur.odovel, 0, 0);delta_state_.s cbbe * (cvb_ * dsodo * (1 delta_state_.sodo) -Rotation::rotvec2quaternion(imu_cur.dtheta).toRotationMatrix() * lodo_ lodo_);// 前后历元平均速度计算位置dvel cbbe * dvfb;delta_state_.p dt * delta_state_.v 0.5 * dt * dvel;delta_state_.v dvel;// 姿态delta_state_.q * Rotation::rotvec2quaternion(dtheta);delta_state_.q.normalize();// 更新系统状态雅克比和协方差矩阵updateJacobianAndCovariance(imu_pre, imu_cur);
}
