[学习SLAM]VINS中IMU预积分的误差推到公式与代码雅克比(协防差/信息矩阵)构建

96 阅读 0 评论 64 点赞

我是靠谱客的博主含蓄冬天，最近开发中收集的这篇文章主要介绍[学习SLAM]VINS中IMU预积分的误差推到公式与代码雅克比(协防差/信息矩阵)构建，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

//todo
/**
*IMU预积分中采用中值积分地推jacobian和covariance
**/
void midPointIntegration(double _dt, 
                        const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
                        const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
                        const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
                        const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
                        Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
                        Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)

1. 中值法计算预积分

处理IMU数据的函数。采用的是中值法计算预积分，对应的公式为

Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);//(4-1)
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//(5) w
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);//(3)--->un_gyr(0) * _dt / 2 (5)
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);//(4-2)
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);//(4)
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;//(1)
result_delta_v = delta_v + un_acc * _dt;//(2) --> un_acc(4) 
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;

2. 推jacobian和covariance

jacobian.setIdentity();
covariance.setZero();

R_w_x<<0, -w_x(2), w_x(1),
    w_x(2), 0, -w_x(0),
    -w_x(1), w_x(0), 0;//反对称矩阵
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
    a_0_x(2), 0, -a_0_x(0),
    -a_0_x(1), a_0_x(0), 0;//反对称矩阵
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
    a_1_x(2), 0, -a_1_x(0),
    -a_1_x(1), a_1_x(0), 0;//反对称矩阵

MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 
                      -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 
                      -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;

MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) =  V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) =  0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) =  0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) =  0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) =  0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) =  V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

jacobian = F * jacobian;
covariance = F * covariance * F.transpose() + V * noise * V.transpose();

noise = Eigen::Matrix<double, 18, 18>::Zero();
noise.block<3, 3>(0, 0) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(3, 3) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(6, 6) =  (ACC_N * ACC_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(9, 9) =  (GYR_N * GYR_N) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(12, 12) =  (ACC_W * ACC_W) * Eigen::Matrix3d::Identity();
noise.block<3, 3>(15, 15) =  (GYR_W * GYR_W) * Eigen::Matrix3d::Identity();

3. 雅克比矩阵的使用

void EdgeImu::ComputeJacobians() {
...............
}
jacobians_[3]    Eigen::Quaterniond corrected_delta_q /jacobian_speedbias_i

4. covariance的使用

评估残差: residual = pre_integration->evaluate;

covariance = F * covariance * F.transpose() + V * noise * V.transpose();

        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);

        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
        residual = sqrt_info * residual;

计算残差中使用 (设置信息矩阵)

void EdgeImu::ComputeResidual() {

   .....................

    residual_ = pre_integration_->evaluate(Pi, Qi, Vi, Bai, Bgi,
                              Pj, Qj, Vj, Baj, Bgj);
//    Mat1515 sqrt_info  = Eigen::LLT< Mat1515 >(pre_integration_->covariance.inverse()).matrixL().transpose();
    SetInformation(pre_integration_->covariance.inverse());
}

edge.h

    /// 返回残差
    VecX Residual() const { return residual_; }

    /// 返回雅可比
    std::vector<MatXX> Jacobians() const { return jacobians_; }

    /// 设置信息矩阵
    void SetInformation(const MatXX &information) {
        information_ = information;
        // sqrt information
        sqrt_information_ = Eigen::LLT<MatXX>(information_).matrixL().transpose();
    }

Eigen::LLT<MatXX>(information_)

LLT.h

    template<typename InputType>
    explicit LLT(const EigenBase<InputType>& matrix)
      : m_matrix(matrix.rows(), matrix.cols()),
        m_isInitialized(false)
    {
      compute(matrix.derived());
    }

5. 信息矩阵

5.1. 协方差矩阵

在统计学中，方差是用来度量单个随机变量的离散程度，而协方差则一般用来刻画两个随机变量的相似程度. 其中，方差的计算公式为
$sigma _{x}^{2} = frac{1}{n-1}sum_{i=1}{n}(x_{i}-bar{x}){2}$
其中，n 表示样本量，符号 $bar{x}$ 表示观测样本的均值。

在此基础上，协方差的计算公式被定义为 $sigma _{x}^{2} = frac{1}{n-1}sum_{i=1}^{n}(x_{i}-bar{x})(y_{i} - bar{y})$
在公式中，符号 $bar{x}$ , $bar{y}$ 分别表示两个随机变量所对应的观测样本均值，据此，我们发现：方差 $sigma _{x}^{2}$ 可视作随机变量 $x$ 关于其自身的协方差 .

5.2. 信息矩阵和协方差矩阵

1：运动方程有协方差矩阵R，观测方程有协方差矩阵Q。它们表示的意义是，到当前时刻t为止，所有测量的样本的协方差矩阵，用来衡量本次测量的不确定性。

2：信息矩阵是协方差矩阵的逆，用来表示本次测量的可靠性，即不确定越小，则可靠性就越大。

3：因此，公式推导里出现的相邻两个状态之间的协方差矩阵，实际上是到当前状态为止，之前所有样本的协方差矩阵。也就是说协方差矩阵随着样本的增加在不断的更新。

5.3. 协方差矩阵和信息矩阵在SLAM中的应用

经过优化后，误差只是减少并不是完全消除，不能消除的误差去那了呢，当然是每条边分摊喽，那么问题来了，每条边误差分担多少呢,难道每条边都分担一样多的误差吗，当然也是可以的，但这样明显不科学，因为有些边在构建的时候已经很准确了，所以这个时候就有了信息矩阵，它表达的是每条边要分摊总误差的多少，嗯，这样就科学多了。提一句，如果你的信息矩阵是单位矩阵，那么误差就是均摊啦。(误差项的权重)

但是信息矩阵是协方差矩阵的一个逆矩阵，这个怎么理解呢？没什么意义，香农形式，这里是推导出来的，只是换了一种数学表达形式**，那为什么需要信息矩阵呢？**

因为信息矩阵在计算条件概率分布明显比协方差矩阵要方便，显然，协方差矩阵要求逆矩阵，所以时间复杂度是O(n^3). 之后我们可以在图优化slam中可以看到，因为图优化优化后的解是无穷多个的，比如说x1->x2->x3, 每个xi相隔1m这是我们实际观测出来的，优化后，我们会得出永远得不出x1 x2 x3的唯一解，因为他们有可能123 可能是234 blabla 但是如果我们提供固定值比如说x2 坐标是3那么解那么就有唯一解234，提供固定值x2这件事情其实就是个先验信息，提供先验信息，求分布，那就是条件分布，也就是这里我们要用到信息矩阵。
接下来看看如何通过信息矩阵来处理要移除的项——通过舒尔补（因为对于滑动窗口法而言，随着时间的变化，有一些新的量要加入，旧的量要移除，本来是一个联合分布，丢掉一部分信息后，协方差矩阵、信息矩阵如何变化~ ）

6.最小二乘计算误差

另外，因为是最小二乘计算误差，误差项取平方，所以误差项的权重（信息矩阵）也要对应σ的平方。

极大似然估计中，信息矩阵、Hessian矩阵和协方差矩阵的关系

1. Fisher Information Matrix 和 Hessian of Log Likelihood

这个博客根据Fisher Information的定义，非常清晰地证明了为什么Fisher Information Matrix和负的Hessian of log likelihood是相等的（关键步骤是二阶导运算符和积分可以互换位置！）。

2. Hessian of Negative Log Likelihood 和 Covariance Matrix

高斯分布假设下，maximum likelihood的等效结果是minimize negative log likelihood（根据高斯分布的概率密度函数可以看出）。同时注意到，negative log likelihood的二阶导数（也就是其Hessian），正好是协方差的逆，也就是说此Hessian of Negative Log Likelihood即Inverse of Covariance Matrix。
这个结论可以继续往下推广：
当高斯分布的均值是关于状态的线性函数时，negative log likelihood的二阶导数（也就是其Hessian），正好是这个线性变换后的新状态的的协方差的逆，此时也有Hessian of Negative Log Likelihood (about the original state)等于Inverse of (new) Covariance Matrix。
当高斯分布的均值是关于状态的非线性函数时，可以做一个线性化将其展开乘线性形式，于是根据上一段的结论，此时Approximate Hessian of Negative Log Likelihood (about the original state)等于Approximate Inverse of (new) Covariance Matrix近似于Inverse of (new) Covariance Matrix。
另外，这里也有一份pdf阐述了我的上述理解。

3. 总结

注意到negative log likelihood其实就得到了我们非常熟悉的指标函数了，在高斯牛顿法中，指标函数做线性展开时得到的Hessian，实际就是前面所说的Approximate Hessian of Negative Log Likelihood (about the original state)，这个Hessian，从一个方向近似等于Fisher信息矩阵，从另一个方向则近似等于协方差矩阵的逆。