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;
1

将在当地坐标系的位置信息转换到ECEF地心地固坐标系后，在转换成大地坐标系中

Vector3d blh1  = ecef2blh(ecef1);
1

下段代码的大致含义：

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;
    }
1
2
3
4
5
6
7
8

设置噪声矩阵

最终获得了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;
}

1
2
3
4
5
6
7
8

这里先进行偏差补偿

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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63