SAD notes

预积分总结

在这里插入图片描述

这里我们要求的状态变量是每一帧的

为了让变量可叠加, 这里将要求的变量转化成预积分变量
在这里插入图片描述
预积分变量 = 预积分观测变量 - 噪声

预积分观测变量定义为:

预积分观测变量可以通过观测直接得到或者推导出

$\Delta \tilde R_{ij}$ 的求法
在这里插入图片描述


// 去掉零偏的测量
Vec3d gyr = imu.gyro_ - bg_;  // 陀螺, tj : imu.gyro_是当前帧的g, 而公式里面是前一帧的g, 有点不一样
...
// 旋转部分
Vec3d omega = gyr * dt;         // 转动量
Mat3d rightJ = SO3::jr(omega);  // 右雅可比
SO3 deltaR = SO3::exp(omega);   // exp后
dR_ = dR_ * deltaR;             // (4.9)
1
2
3
4
5
6
7
8
9

$\Delta \tilde v_{ij}$ 的求法
在这里插入图片描述

Vec3d acc = imu.acce_ - ba_;  // 加计, tj : imu.acce_是当前帧的a, 而公式里面是前一帧的a, 有点不一样
...
dv_ = dv_ + dR_ * acc * dt;
1
2
3

$\Delta \tilde p_{ij}$ 的求法
在这里插入图片描述

dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc * dt * dt;
1

下面我们更新噪声,

在这里插入图片描述

Vec3d acc = imu.acce_ - ba_;  // 加计, tj : imu.acce_是当前帧的a, 而公式里面是前一帧的a, 有点不一样
...
 Mat3d acc_hat = SO3::hat(acc);
  double dt2 = dt * dt;

  // NOTE A, B左上角块与公式稍有不同
  A.block<3, 3>(3, 0) = -dR_.matrix() * dt * acc_hat;
  A.block<3, 3>(6, 0) = -0.5f * dR_.matrix() * acc_hat * dt2;
  A.block<3, 3>(6, 3) = dt * Mat3d::Identity();

  B.block<3, 3>(3, 3) = dR_.matrix() * dt;
  B.block<3, 3>(6, 3) = 0.5f * dR_.matrix() * dt2;

... // 更新先验dR_到当前帧
  A.block<3, 3>(0, 0) = deltaR.matrix().transpose();
  B.block<3, 3>(0, 0) = rightJ * dt;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

将噪声项写成协方差的形式

在这里插入图片描述

// 更新噪声项
cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();
1
2

算这个协方差主要是为了可以设置后面预积分边优化中的information矩阵

EdgeInertial::EdgeInertial(std::shared_ptr<IMUPreintegration> preinteg, const Vec3d& gravity, double weight)
    : preint_(preinteg), dt_(preinteg->dt_) {
    resize(6);  // 6个关联顶点
    grav_ = gravity;
    setInformation(preinteg->cov_.inverse() * weight);
}
1
2
3
4
5
6

从而可以算它的hessian 矩阵

Eigen::Matrix GetHessian() {
linearizeOplus();
Eigen::Matrix J;
J.block<9, 6>(0, 0) = _jacobianOplus[0];
J.block<9, 3>(0, 6) = _jacobianOplus[1];
J.block<9, 3>(0, 9) = _jacobianOplus[2];
J.block<9, 3>(0, 12) = _jacobianOplus[3];
J.block<9, 6>(0, 15) = _jacobianOplus[4];
J.block<9, 3>(0, 21) = _jacobianOplus[5];
return J.transpose() * information() * J;
}

这里观测变量有三个: $r_{\Delta R_{ij}}$ , $r_{\Delta v_{ij}}$ , $r_{\Delta p_{ij}}$ , 9维

优化变量(也就是顶点变量)有八个: $R_i$ , $v_i$ , $p_i$ , $b_{a,i}$ , $b_{g,i}$ , $R_j$ , $v_j$ , $p_j$ , 24维

信息矩阵是9x9, J是9x24, H是24x24

$H = J^T*information*J$

此时预积分观测变量都假设 $i$ 时刻的IMU零偏不变(可以理解为先验), 如果考虑零偏的变化, 就要修正预积分观测变量(可以理解为后验)
在这里插入图片描述

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

注意修正后的预积分观测变量跟当前帧的零偏有关

其中偏导由雅可比给出
在这里插入图片描述

// 更新各雅可比，见式(4.39)
dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2;                      // (4.39d)
dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * acc_hat * dR_dbg_;  // (4.39e)
dV_dba_ = dV_dba_ - dR_.matrix() * dt;                                             // (4.39b)
dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * acc_hat * dR_dbg_;                         // (4.39c)
1
2
3
4
5

至此, 在imu_preintegration.cc 文件中, 我们求得了修正后的预积分变量, 他们会与当前帧的零偏有关

求得了预积分我们可以就可以反推得到状态变量. 根据修正前的预积分变量(先验), 我们可以更新当前帧的状态(先验)
在这里插入图片描述

根据4.7a, 我们得到
$R_j = R_i \Delta R_{ij}$
根据4.7b,我们得到
$v_j = v_i + g\Delta t_{ij}+R_i\Delta v_{ij}$
根据4.7c,我们得到
$p_j = p_i + v_i\Delta t_{ij} + 1/2g\Delta t_{ij}^2 + R_i\Delta p_{ij}$

注意公式4.7d中 $\Delta v_{ik}$ 的定义就是简单的 $v_k - v_i$ , 跟 4.7b中的 $\Delta V_{ij}$ 不相干

NavStated IMUPreintegration::Predict(const sad::NavStated &start, const Vec3d &grav) const { // tj : 通过先验预积分更新当前帧先验状态变量, 参考 4.7

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

接下来我们就要调用gins_pre_integ.cc文件中的optimize()来求上时刻和本时刻的状态变量

首先设置optimizer的变量

// 上时刻顶点， pose, v, bg, ba
auto v0_pose = new VertexPose();
v0_pose->setId(0);
v0_pose->setEstimate(last_frame_->GetSE3());
optimizer.addVertex(v0_pose);

auto v0_vel = new VertexVelocity();
v0_vel->setId(1);
v0_vel->setEstimate(last_frame_->v_);
optimizer.addVertex(v0_vel);

auto v0_bg = new VertexGyroBias();
v0_bg->setId(2);
v0_bg->setEstimate(last_frame_->bg_);
optimizer.addVertex(v0_bg);

auto v0_ba = new VertexAccBias();
v0_ba->setId(3);
v0_ba->setEstimate(last_frame_->ba_);
optimizer.addVertex(v0_ba);

// 本时刻顶点，pose, v, bg, ba
auto v1_pose = new VertexPose();
v1_pose->setId(4);
v1_pose->setEstimate(this_frame_->GetSE3());
optimizer.addVertex(v1_pose);

auto v1_vel = new VertexVelocity();
v1_vel->setId(5);
v1_vel->setEstimate(this_frame_->v_);
optimizer.addVertex(v1_vel);

auto v1_bg = new VertexGyroBias();
v1_bg->setId(6);
v1_bg->setEstimate(this_frame_->bg_);
optimizer.addVertex(v1_bg);

auto v1_ba = new VertexAccBias();
v1_ba->setId(7);
v1_ba->setEstimate(this_frame_->ba_);
optimizer.addVertex(v1_ba);
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

预积分边, 也就是图中IMU的方块

在这里插入图片描述

    // 预积分边
    auto edge_inertial = new EdgeInertial(pre_integ_, options_.gravity_);
    edge_inertial->setVertex(0, v0_pose);
    edge_inertial->setVertex(1, v0_vel);
    edge_inertial->setVertex(2, v0_bg);
    edge_inertial->setVertex(3, v0_ba);
    edge_inertial->setVertex(4, v1_pose);
    edge_inertial->setVertex(5, v1_vel);
    auto* rk = new g2o::RobustKernelHuber();
    rk->setDelta(200.0);
    edge_inertial->setRobustKernel(rk);
    optimizer.addEdge(edge_inertial);
1
2
3
4
5
6
7
8
9
10
11
12

void EdgeInertial::computeError() {
    auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
    auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
    auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
    auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
    auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
    auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);

    Vec3d bg = bg1->estimate();
    Vec3d ba = ba1->estimate();

    //tj : 后验观测变量, 凡是跟变量estimate相关都当做后验变量
    const SO3 dR = preint_->GetDeltaRotation(bg);
    const Vec3d dv = preint_->GetDeltaVelocity(bg, ba);
    const Vec3d dp = preint_->GetDeltaPosition(bg, ba);

    /// 预积分误差项（4.41）
    const Vec3d er = (dR.inverse() * p1->estimate().so3().inverse() * p2->estimate().so3()).log();
    Mat3d RiT = p1->estimate().so3().inverse().matrix();
    const Vec3d ev = RiT * (v2->estimate() - v1->estimate() - grav_ * dt_) - dv;
    const Vec3d ep = RiT * (p2->estimate().translation() - p1->estimate().translation() - v1->estimate() * dt_ -
                            grav_ * dt_ * dt_ / 2) -
                     dp;
    _error << er, ev, ep;
}
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

然后就是算雅可比

注意在代码中的索引位置

\begin{aligned} j a c o b i a n O p l u s [ & 0] . b l o c k < 3, 3 > ( & 0, & 0) \\ 顶 & 点 的 & 误 差 的 & 顶 点 内 部 的 \end{aligned}

ja co bian Opl u s [顶 0] . b l oc k < 3, 3 > (点的 0, 误差的 0) 顶点内部的

//                 | R1 | p1 | v1 | bg1 | ba1 | R2 | p2 | v2 |
//  vert           | 0       | 1  | 2   | 3   | 4       | 5  |
//  vert offset    | 0    3  | 0  | 0   | 0   | 0    3  | 0  |
//    row
//  eR 0           |
//  ev 3           |
//  ep 6           |
1
2
3
4
5
6
7

在这里插入图片描述

// dR/dR1, 4.42
_jacobianOplus[0].block<3, 3>(0, 0) = -invJr * (R2.inverse() * R1).matrix();
1
2

在这里插入图片描述

// dv/dR1, 4.47
_jacobianOplus[0].block<3, 3>(3, 0) = SO3::hat(R1T * (vj - vi - grav_ * dt_));
1
2

在这里插入图片描述

// dp/dR1, 4.48d
_jacobianOplus[0].block<3, 3>(6, 0) = SO3::hat(R1T * (pj - pi - v1->estimate() * dt_ - 0.5 * grav_ * dt_ * dt_));
1
2

在这里插入图片描述

/// 残差对p1
// dp/dp1, 4.48a
_jacobianOplus[0].block<3, 3>(6, 3) = -R1T.matrix();
1
2
3

在这里插入图片描述

// dv/dv1, 4.46a
_jacobianOplus[1].block<3, 3>(3, 0) = -R1T.matrix();
1
2

在这里插入图片描述

// dp/dv1, 4.48c
_jacobianOplus[1].block<3, 3>(6, 0) = -R1T.matrix() * dt_;
1
2

在这里插入图片描述

// dR/dbg1, 4.45
_jacobianOplus[2].block<3, 3>(0, 0) = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;
1
2

根据
在这里插入图片描述
得到

\begin{aligned} \frac{\partial r_{Δ v_{i j}}}{\partial b_{g, i}} = - \frac{Δ {\tilde{v}}_{i j}}{\partial b_{g, i}} \\ \frac{\partial r_{Δ p_{i j}}}{\partial b_{g, i}} = - \frac{Δ {\tilde{p}}_{i j}}{\partial b_{g, i}} \end{aligned}

\frac{\partial r _{Δ v_{ij}}}{\partial b _{g, i}} = - \frac{Δ v ~ _{ij}}{\partial b _{g, i}} \frac{\partial r _{Δ p_{ij}}}{\partial b _{g, i}} = - \frac{Δ p ~ _{ij}}{\partial b _{g, i}}

// dv/dbg1
_jacobianOplus[2].block<3, 3>(3, 0) = -dv_dbg;
// dp/dbg1
_jacobianOplus[2].block<3, 3>(6, 0) = -dp_dbg;
1
2
3
4

同理

\begin{aligned} \frac{\partial r_{Δ v_{i j}}}{\partial b_{a, i}} = - \frac{Δ {\tilde{v}}_{i j}}{\partial b_{a, i}} \\ \frac{\partial r_{Δ p_{i j}}}{\partial b_{a, i}} = - \frac{Δ {\tilde{p}}_{i j}}{\partial b_{a, i}} \end{aligned}

\frac{\partial r _{Δ v_{ij}}}{\partial b _{a, i}} = - \frac{Δ v ~ _{ij}}{\partial b _{a, i}} \frac{\partial r _{Δ p_{ij}}}{\partial b _{a, i}} = - \frac{Δ p ~ _{ij}}{\partial b _{a, i}}

// dv/dba1
_jacobianOplus[3].block<3, 3>(3, 0) = -dv_dba;
// dp/dba1
_jacobianOplus[3].block<3, 3>(6, 0) = -dp_dba;
1
2
3
4

在这里插入图片描述

// dr/dr2, 4.43
_jacobianOplus[4].block<3, 3>(0, 0) = invJr;
1
2

在这里插入图片描述

// dp/dp2, 4.48b
_jacobianOplus[4].block<3, 3>(6, 3) = R1T.matrix();
1
2

在这里插入图片描述

// dv/dv2, 4,46b
_jacobianOplus[5].block<3, 3>(3, 0) = R1T.matrix();  

1
2
3

写在一起

void EdgeInertial::linearizeOplus() {
    auto* p1 = dynamic_cast<const VertexPose*>(_vertices[0]);
    auto* v1 = dynamic_cast<const VertexVelocity*>(_vertices[1]);
    auto* bg1 = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
    auto* ba1 = dynamic_cast<const VertexAccBias*>(_vertices[3]);
    auto* p2 = dynamic_cast<const VertexPose*>(_vertices[4]);
    auto* v2 = dynamic_cast<const VertexVelocity*>(_vertices[5]);

    Vec3d bg = bg1->estimate();
    Vec3d ba = ba1->estimate();
    Vec3d dbg = bg - preint_->bg_;

    // 一些中间符号
    const SO3 R1 = p1->estimate().so3();
    const SO3 R1T = R1.inverse();
    const SO3 R2 = p2->estimate().so3();

    auto dR_dbg = preint_->dR_dbg_;
    auto dv_dbg = preint_->dV_dbg_;
    auto dp_dbg = preint_->dP_dbg_;
    auto dv_dba = preint_->dV_dba_;
    auto dp_dba = preint_->dP_dba_;

    // 估计值
    Vec3d vi = v1->estimate();
    Vec3d vj = v2->estimate();
    Vec3d pi = p1->estimate().translation();
    Vec3d pj = p2->estimate().translation();

    const SO3 dR = preint_->GetDeltaRotation(bg);
    const SO3 eR = SO3(dR).inverse() * R1T * R2;
    const Vec3d er = eR.log();
    const Mat3d invJr = SO3::jr_inv(eR);

    /// 雅可比矩阵
    /// 注意有3个index, 顶点的，自己误差的，顶点内部变量的
    /// 变量顺序：pose1(R1,p1), v1, bg1, ba1, pose2(R2,p2), v2
    /// 残差顺序：eR, ev, ep，残差顺序为行，变量顺序为列

    //       | R1 | p1 | v1 | bg1 | ba1 | R2 | p2 | v2 |
    //  vert | 0       | 1  | 2   | 3   | 4       | 5  |
    //  col  | 0    3  | 0  | 0   | 0   | 0    3  | 0  |
    //    row
    //  eR 0 |
    //  ev 3 |
    //  ep 6 |

    /// 残差对R1
    _jacobianOplus[0].setZero();
    // dR/dR1, 4.42
    _jacobianOplus[0].block<3, 3>(0, 0) = -invJr * (R2.inverse() * R1).matrix();
    // dv/dR1, 4.47
    _jacobianOplus[0].block<3, 3>(3, 0) = SO3::hat(R1T * (vj - vi - grav_ * dt_));
    // dp/dR1, 4.48d
    _jacobianOplus[0].block<3, 3>(6, 0) = SO3::hat(R1T * (pj - pi - v1->estimate() * dt_ - 0.5 * grav_ * dt_ * dt_));

    /// 残差对p1
    // dp/dp1, 4.48a
    _jacobianOplus[0].block<3, 3>(6, 3) = -R1T.matrix();

    /// 残差对v1
    _jacobianOplus[1].setZero();
    // dv/dv1, 4.46a
    _jacobianOplus[1].block<3, 3>(3, 0) = -R1T.matrix();
    // dp/dv1, 4.48c
    _jacobianOplus[1].block<3, 3>(6, 0) = -R1T.matrix() * dt_;

    /// 残差对bg1
    _jacobianOplus[2].setZero();
    // dR/dbg1, 4.45
    _jacobianOplus[2].block<3, 3>(0, 0) = -invJr * eR.inverse().matrix() * SO3::jr((dR_dbg * dbg).eval()) * dR_dbg;
    // dv/dbg1
    _jacobianOplus[2].block<3, 3>(3, 0) = -dv_dbg;
    // dp/dbg1
    _jacobianOplus[2].block<3, 3>(6, 0) = -dp_dbg;

    /// 残差对ba1
    _jacobianOplus[3].setZero();
    // dv/dba1
    _jacobianOplus[3].block<3, 3>(3, 0) = -dv_dba;
    // dp/dba1
    _jacobianOplus[3].block<3, 3>(6, 0) = -dp_dba;

    /// 残差对pose2
    _jacobianOplus[4].setZero();
    // dr/dr2, 4.43
    _jacobianOplus[4].block<3, 3>(0, 0) = invJr;
    // dp/dp2, 4.48b
    _jacobianOplus[4].block<3, 3>(6, 3) = R1T.matrix();

    /// 残差对v2
    _jacobianOplus[5].setZero();
    // dv/dv2, 4,46b
    _jacobianOplus[5].block<3, 3>(3, 0) = R1T.matrix();  // OK
}
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
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95

接下来算图中两个Bias的边

// 两个零偏随机游走
auto* edge_gyro_rw = new EdgeGyroRW();
edge_gyro_rw->setVertex(0, v0_bg);
edge_gyro_rw->setVertex(1, v1_bg);
edge_gyro_rw->setInformation(options_.bg_rw_info_);
optimizer.addEdge(edge_gyro_rw);

auto* edge_acc_rw = new EdgeAccRW();
edge_acc_rw->setVertex(0, v0_ba);
edge_acc_rw->setVertex(1, v1_ba);
edge_acc_rw->setInformation(options_.ba_rw_info_);
optimizer.addEdge(edge_acc_rw);
1
2
3
4
5
6
7
8
9
10
11
12

class EdgeGyroRW : public g2o::BaseBinaryEdge<3, Vec3d, VertexGyroBias, VertexGyroBias> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeGyroRW() {}

    virtual bool read(std::istream& is) { return false; }
    virtual bool write(std::ostream& os) const { return false; }

    void computeError() {
        const auto* VG1 = dynamic_cast<const VertexGyroBias*>(_vertices[0]);
        const auto* VG2 = dynamic_cast<const VertexGyroBias*>(_vertices[1]);
        _error = VG2->estimate() - VG1->estimate();
    }

    virtual void linearizeOplus() {
        _jacobianOplusXi = -Mat3d::Identity();
        _jacobianOplusXj.setIdentity();
    }

    Eigen::Matrix<double, 6, 6> GetHessian() {
        linearizeOplus();
        Eigen::Matrix<double, 3, 6> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 3>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }
};
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

接下来算图中prior的边

// 上时刻先验
auto* edge_prior = new EdgePriorPoseNavState(*last_frame_, prior_info_);
edge_prior->setVertex(0, v0_pose);
edge_prior->setVertex(1, v0_vel);
edge_prior->setVertex(2, v0_bg);
edge_prior->setVertex(3, v0_ba);
optimizer.addEdge(edge_prior);
1
2
3
4
5
6
7

EdgePriorPoseNavState::EdgePriorPoseNavState(const NavStated& state, const Mat15d& info) {
    resize(4);
    state_ = state;
    setInformation(info);
}

void EdgePriorPoseNavState::computeError() {
    auto* vp = dynamic_cast<const VertexPose*>(_vertices[0]);
    auto* vv = dynamic_cast<const VertexVelocity*>(_vertices[1]);
    auto* vg = dynamic_cast<const VertexGyroBias*>(_vertices[2]);
    auto* va = dynamic_cast<const VertexAccBias*>(_vertices[3]);

    const Vec3d er = SO3(state_.R_.matrix().transpose() * vp->estimate().so3().matrix()).log();
    const Vec3d ep = vp->estimate().translation() - state_.p_;
    const Vec3d ev = vv->estimate() - state_.v_;
    const Vec3d ebg = vg->estimate() - state_.bg_;
    const Vec3d eba = va->estimate() - state_.ba_;

    _error << er, ep, ev, ebg, eba;
}

void EdgePriorPoseNavState::linearizeOplus() {
    const auto* vp = dynamic_cast<const VertexPose*>(_vertices[0]);
    const Vec3d er = SO3(state_.R_.matrix().transpose() * vp->estimate().so3().matrix()).log();

    /// 注意有3个index, 顶点的，自己误差的，顶点内部变量的
    _jacobianOplus[0].setZero();
    _jacobianOplus[0].block<3, 3>(0, 0) = SO3::jr_inv(er);    // dr/dr
    _jacobianOplus[0].block<3, 3>(3, 3) = Mat3d::Identity();  // dp/dp
    _jacobianOplus[1].setZero();
    _jacobianOplus[1].block<3, 3>(6, 0) = Mat3d::Identity();  // dv/dv
    _jacobianOplus[2].setZero();
    _jacobianOplus[2].block<3, 3>(9, 0) = Mat3d::Identity();  // dbg/dbg
    _jacobianOplus[3].setZero();
    _jacobianOplus[3].block<3, 3>(12, 0) = Mat3d::Identity();  // dba/dba
}

}  // namespace sad
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

这里dr/dr可以参考 4.43算出来
在这里插入图片描述

接下来,算图中GNSS边

// GNSS边
auto edge_gnss0 = new EdgeGNSS(v0_pose, last_gnss_.utm_pose_);
edge_gnss0->setInformation(options_.gnss_info_);
optimizer.addEdge(edge_gnss0);

auto edge_gnss1 = new EdgeGNSS(v1_pose, this_gnss_.utm_pose_);
edge_gnss1->setInformation(options_.gnss_info_);
optimizer.addEdge(edge_gnss1);
1
2
3
4
5
6
7
8

/**
 * 6 自由度的GNSS
 * 误差的角度在前，平移在后
 */
class EdgeGNSS : public g2o::BaseUnaryEdge<6, SE3, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeGNSS() = default;
    EdgeGNSS(VertexPose* v, const SE3& obs) {
        setVertex(0, v);
        setMeasurement(obs);
    }

    void computeError() override {
        VertexPose* v = (VertexPose*)_vertices[0];
        _error.head<3>() = (_measurement.so3().inverse() * v->estimate().so3()).log();
        _error.tail<3>() = v->estimate().translation() - _measurement.translation();
    };

    void linearizeOplus() override {
        VertexPose* v = (VertexPose*)_vertices[0];
        // jacobian 6x6
        _jacobianOplusXi.setZero();
        _jacobianOplusXi.block<3, 3>(0, 0) = (_measurement.so3().inverse() * v->estimate().so3()).jr_inv();  // dR/dR
        _jacobianOplusXi.block<3, 3>(3, 3) = Mat3d::Identity();                                              // dp/dp
    }

    Mat6d GetHessian() {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }

   private:
};
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

接下来算图中odom边

// Odom边
    EdgeEncoder3D* edge_odom = nullptr;
    Vec3d vel_world = Vec3d::Zero();
    Vec3d vel_odom = Vec3d::Zero();
    if (last_odom_set_) {
        // velocity obs
        double velo_l =
            options_.wheel_radius_ * last_odom_.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double velo_r =
            options_.wheel_radius_ * last_odom_.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
        double average_vel = 0.5 * (velo_l + velo_r);
        vel_odom = Vec3d(average_vel, 0.0, 0.0);
        vel_world = this_frame_->R_ * vel_odom;

        edge_odom = new EdgeEncoder3D(v1_vel, vel_world);
        edge_odom->setInformation(options_.odom_info_);
        optimizer.addEdge(edge_odom);

        // 重置odom数据到达标志位，等待最新的odom数据
        last_odom_set_ = false;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

/**
 * 3维 轮速计观测边
 * 轮速观测世界速度在自车坐标系下矢量, 3维情况下假设自车不会有y和z方向速度
 */
class EdgeEncoder3D : public g2o::BaseUnaryEdge<3, Vec3d, VertexVelocity> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeEncoder3D() = default;

    /**
     * 构造函数需要知道世界系下速度
     * @param v0
     * @param speed
     */
    EdgeEncoder3D(VertexVelocity* v0, const Vec3d& speed) {
        setVertex(0, v0);
        setMeasurement(speed);
    }

    void computeError() override {
        VertexVelocity* v0 = (VertexVelocity*)_vertices[0];
        _error = v0->estimate() - _measurement;
    }

    void linearizeOplus() override { _jacobianOplusXi.setIdentity(); }

    virtual bool read(std::istream& in) { return true; }
    virtual bool write(std::ostream& out) const { return true; }
};
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

至此, 优化部分搞定

ESKF 总结

prediction

更新误差先验
在这里插入图片描述

$F$ 通过3.42来算
在这里插入图片描述
得到

这里有点绕的一点是: 误差状态的 $F$ 牵涉到名义状态, 而名义状态又需要在时间上推进更新

其中, F中的名义状态的推进通过公式3.41得到,

(名义状态不考虑误差, 这一点从3.41d, 3.41e可以看出, 误差状态只考虑误差, 真实状态为名义+误差)

在这里插入图片描述代码的实现

template <typename S>
bool ESKF<S>::Predict(const IMU& imu) {
    assert(imu.timestamp_ >= current_time_);

    double dt = imu.timestamp_ - current_time_;
    if (dt > (5 * options_.imu_dt_) || dt < 0) {
        // 时间间隔不对，可能是第一个IMU数据，没有历史信息
        LOG(INFO) << "skip this imu because dt_ = " << dt;
        current_time_ = imu.timestamp_;
        return false;
    }

    // nominal state 递推
    VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
    VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
    SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);

    R_ = new_R;
    v_ = new_v;
    p_ = new_p;
    // 其余状态维度不变

    // error state 递推
    // 计算运动过程雅可比矩阵 F，见(3.47)
    // F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta
    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta
    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg

    // mean and cov prediction
    dx_ = F * dx_;  // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
    cov_ = F * cov_.eval() * F.transpose() + Q_; 
    current_time_ = imu.timestamp_;
    return true;
}
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

$Q$ 的算法

注意, 在离散情况下

\begin{aligned} η_{v} = η_{a} Δ t \\ η_{θ} = η_{g} Δ t \\ η_{g} = η_{b g} Δ t \\ η_{a} = η_{b a} Δ t \end{aligned}

η_{v} = η_{a} Δ t η_{θ} = η_{g} Δ t η_{g} = η_{b g} Δ t η_{a} = η_{ba} Δ t

可以根据3.40将3.42进行一阶泰勒展开
在这里插入图片描述

代码中的实现

        double ev = options.acce_var_;
        double et = options.gyro_var_;
        double eg = options.bias_gyro_var_;
        double ea = options.bias_acce_var_;

        double ev2 = ev;  // * ev;   // tj : 为什么没平方? Q里面应该是方差
        double et2 = et;  // * et;
        double eg2 = eg;  // * eg;
        double ea2 = ea;  // * ea;

        // 设置过程噪声
        Q_.diagonal() << 0, 0, 0, ev2, ev2, ev2, et2, et2, et2, eg2, eg2, eg2, ea2, ea2, ea2, 0, 0, 0;
1
2
3
4
5
6
7
8
9
10
11
12

其中options的定义

    struct Options {
        Options() = default;

        /// IMU 测量与零偏参数
        double imu_dt_ = 0.01;  // IMU测量间隔
        // NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
        double gyro_var_ = 1e-5;       // 陀螺测量标准差
        double acce_var_ = 1e-2;       // 加计测量标准差
        double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差
        double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差
1
2
3
4
5
6
7
8
9
10

correction

更新误差后验

在这里插入图片描述

$H$ 是观测值对误差状态变量的jacob

先看GNSS, 它观测值有位移和旋转, 根据3.66和3.70可得 $H$ ,
在这里插入图片描述

GNSS对旋转的观测是总体的旋转, 然而名义上的 $R$ 是知道的, 所以我们可以将观测值变以成对于旋转误差状态的直接观测, 这样一来, 它对自己求导就为 $I$

同理, GNSS对位移的观测是总体的位移, 然后名义上的 $p$ 是知道, 所以我们将对总体位移的观测转到对于误差位移的直接观测

    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分 (3.70)
    H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)
1
2
3

然后求 $V$

 // 卡尔曼增益和更新过程
    Vec6d noise_vec;
    noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;

    Mat6d V = noise_vec.asDiagonal();
1
2
3
4
5

其中trans_noise, ang_noise在option中定义

        /// RTK 观测参数
        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声
1
2
3
4

然后更新后验3.51b, 3.51d

    // 更新x和cov
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
    innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)

    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;
1
2
3
4
5
6
7

有了误差状态, 就可以更新名义状态3.51c

   /// 更新名义状态变量，重置error state
    void UpdateAndReset() {
        p_ += dx_.template block<3, 1>(0, 0);
        v_ += dx_.template block<3, 1>(3, 0);
        R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));

        if (options_.update_bias_gyro_) {
            bg_ += dx_.template block<3, 1>(9, 0);
        }

        if (options_.update_bias_acce_) {
            ba_ += dx_.template block<3, 1>(12, 0);
        }

        g_ += dx_.template block<3, 1>(15, 0);

        ProjectCov();
        dx_.setZero();
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

注意这里有一步需要投影, 参考3.63

在这里插入图片描述

/// 对P阵进行投影，参考式(3.63)
void ProjectCov() {
    Mat18T J = Mat18T::Identity();
    J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
    cov_ = J * cov_ * J.transpose();
}
1
2
3
4
5
6

再看odom, 它观测值只有速度

首先求H, 注意这里观测值为本地速度, 但是名义变量 $R$ 这时是知道的, 所以可以把观测值从本地速度转化到世界速度, 然后世界速度对世界速度求导就为 $I$

在这里插入图片描述

// odom 修正以及雅可比
// 使用三维的轮速观测，H为3x18，大部分为零
Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
H.template block<3, 3>(0, 3) = Mat3T::Identity();
1
2
3
4

然后算V

// 设置里程计噪声
double o2 = options_.odom_var_ * options_.odom_var_;
odom_noise_.diagonal() << o2, o2, o2;
1
2
3

option里面的定义

/// 里程计参数
double odom_var_ = 0.5;
double odom_span_ = 0.1;        // 里程计测量间隔
double wheel_radius_ = 0.155;   // 轮子半径
double circle_pulse_ = 1024.0;  // 编码器每圈脉冲数
1
2
3
4
5

然后更新后验

本地速度观测值的算法参见 3.76

// velocity obs
double velo_l = options_.wheel_radius_ * odom.left_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double velo_r =
    options_.wheel_radius_ * odom.right_pulse_ / options_.circle_pulse_ * 2 * M_PI / options_.odom_span_;
double average_vel = 0.5 * (velo_l + velo_r);

VecT vel_odom(average_vel, 0.0, 0.0);
VecT vel_world = R_ * vel_odom;

dx_ = K * (vel_world - v_);

// update cov
cov_ = (Mat18T::Identity() - K * H) * cov_;
UpdateAndReset();
1
2
3
4
5
6
7
8
9
10
11
12
13
14

相关阅读:
初识设计模式 - 工厂模式
先验 & 后验 & 似然估计
java-net-php-python-ssm电子班牌系统计算机毕业设计程序
Cascade R-CNN详解
[网络安全] PKI
Spring Cloud Alibaba实现服务的无损下线功能
Spring-boot-starter-actuator的可视化spring-boot-admin
flask学习
【Android】-- 如何使用按钮和图片（点击事件、长按点击、同时展示文本和图像、ImageView）
红黑树详解

原文地址：https://blog.csdn.net/seamanj/article/details/134045343