OB_GINS_day3

实现当前状态初始化

这个state_curr的主要功能是初始化GNSS现在时刻的状态参数

实现预积分的初始化

由于此时preintegration_options 是3（也就是考虑odo以及earth rotation）

这里值的注意的问题在于：此时emplace_back的类是基于PreintegrationEarthOdo的类，所以在下面这个函数中，返回的preintegration最终是PreintegrationEarthOdo类型的变量
注意：这里需要调用PreintegrationEarthOdo的类默认构造函数
这个默认构造函数

1. 首先将初始的状态进行重置
   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_ptr<PreintegrationBase>  creatPreintegration(const std::shared_ptr<IntegrationParameters> &parameters, const IMU &imu0,  const IntegrationState &state, PreintegrationOptions options)
{if (options == PREINTEGRATION_EARTH_ODO) {//最终执行这个if条件语句preintegration = std::make_shared<PreintegrationEarthOdo>(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);
}