用ESKF实现IMU/GNSS组合导航（学习记录）

最新推荐文章于 2024-07-05 12:44:52 发布

棒球部前部长

最新推荐文章于 2024-07-05 12:44:52 发布

阅读量1.6k

点赞数 42

文章标签：算法线性代数机器学习

本文链接：https://blog.csdn.net/bangqiubu/article/details/139148772

版权

一、预备知识

1、基本的数学知识和定义：对任意向量，都可以唯一找到对应的反对称矩阵

$\mathbf{a}^\wedge = \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} = \mathbf{A}$

其中a是一个三维向量，A是对应这个向量的反对称矩阵，那么 $A^{\vee }=a$

2、所有的三维旋转矩阵组成了特殊正交群SO(3)[李群]，它是一个3x3的实数矩阵且满足：

旋转矩阵为正交矩阵： $R^{T}=R^{-1}$
旋转矩阵的行列式为1： $det(R)=1$

这两个性质也是李群的满足条件，同时，一个旋转矩阵也可以转换为四元数或者旋转向量（李代数、角速度）来描述。

3、旋转向量( $\omega$ )也称为角轴，是李群SO(3)对应的李代数so(3)，也可用于表达角速度，旋转向量和角速度存在一个导数关系，旋转向量是角度，对角度求时间导数就是角速度。旋转向量的具体公式可以表示为：

$\omega =\theta n$

其中 $\theta$ 是旋转角度， $n$ 是对应的旋转轴。

那么旋转向量到旋转矩阵的转换关系可以由罗德里格斯公式或者SO(3)上的指数映射来确定：

$\mathbf{R} = \cos\theta \mathbf{I} + (1 - \cos\theta) \mathbf{n}\mathbf{n}^\top + \sin\theta \mathbf{n}^\wedge = \exp(\mathbf{\omega }^\wedge)$

其中 $\omega ^{\wedge }$ 代表旋转向量对应的反对称矩阵且记 $Exp(\omega )=exp(\omega ^{\wedge })$

那么：

$\omega =log(R)^{\vee }=LogR$

其中 $log(R)$ 是一个反对称矩阵。

而

$\theta = \arccos\left( \frac{\operatorname{tr}(\mathbf{R}) - 1}{2} \right).$

$Rn=n$

这样就说清楚了旋转矩阵到旋转向量之间的转换关系，而四元数也可以和旋转矩阵旋转向量进行转换。

4、明白了上述转换关系之后，那么对连续的运动又怎么表示旋转呢，下一个旋转矩阵等于这一刻的旋转矩阵乘以一个 $\bigtriangleup R$ 即 $R_{k+1}=\bigtriangleup R*R_{k}$ ，这是在李群下的表示方法，那么在李代数中对应的是什么呢，根据BCH近似，为 $J_l^{-1}(\phi) \Delta \phi + \phi_{\circ}$ ，即：

$\exp(\Delta \Phi^\wedge) \exp(\Phi^\wedge) = \exp\left((\Phi + J_l^{-1}(\Phi) \Delta \Phi)^\wedge\right).$

相反的：

$\exp((\Phi + \Delta \Phi)^\wedge) = \exp((J_l \Delta \Phi)^\wedge) \exp(\Phi^\wedge) = \exp(\Phi^\wedge) \exp((J_r \Delta \Phi)^\wedge).$

其中(a对应角轴的n)：

$J_l = J = \frac{\sin\theta}{\theta} \mathbf{I} + \left(1 - \frac{\sin\theta}{\theta}\right) \mathbf{aa}^\top + \frac{1 - \cos\theta}{\theta} \mathbf{a}^\wedge)$

$J_l^{-1} = \frac{\theta}{2} \cot\frac{\theta}{2} \mathbf{I} + \left(1 - \frac{\theta}{2} \cot\frac{\theta}{2}\right) \mathbf{aa}^\top - \frac{\theta}{2} \mathbf{a}^\wedge)$

$J_r(\phi) = J_l(-\phi)$

值得注意的是，左乘对应左扰动，右乘对应右扰动；至此就明白了李群的乘法对应李代数的加法的关系，在后续推导误差状态转移矩阵具有重要作用。

李代数求导： $\frac{\partial (R \mathbf{p})}{\partial \varphi}=-(R \mathbf{p})^\wedge.$

右扰动模型： $\frac{\partial (R \mathbf{a})}{\partial R} = \lim_{\phi \to 0} \frac{R \exp(\phi^\wedge) \mathbf{a} - R \mathbf{a}}{\phi} = \lim_{\phi \to 0} \frac{R (I + \phi^\wedge) \mathbf{a} - R \mathbf{a}}{\phi} = -R \mathbf{a}^\wedge$ 这个公式很有意思，传统的求导定义是 $\lim_{h \to 0} \frac{f(a + h) - f(a)}{h}$ 其实代表的是下一状态量减去此刻状态量，而旋转矩阵的下一个状态量是用 $R \exp(\phi^\wedge) \mathbf{a}$ 代表的

二、用ESKF进行组合导航

1、首先说明一下为什么要用ESKF，在用EKF进行线性化的时候，旋转矩阵的导数是很难求的，需要引入张量，而如果用欧拉角的话会出现万向锁，用四元数的话需要正则化，都会增加计算的复杂度，所以我们需要优雅的使用流形上的方法——将旋转矩阵映射到旋转向量，然后通过BCH展开进行局部线性化。其二，在进行IMU/GNSS导航时，全局坐标数值很大会出现大数吃小数的现象。在使用了流形上的方法以后，ESKF对比KF,EKF有以下优点：

1）、用三维变量表达旋转的增量

2）、由于是误差状态变量的更新，数值总在原点附近，更加的稳定

2、状态变量 $x=[p,v,R,b_{g},b_{a},g]^{T}$ ,其中 $p$ 为平移， $v$ 为速度， $R$ 为旋转矩阵， $b_{g},b_{a}$ 为零偏，g为重力。其中平移，速度，旋转矩阵是导航中必要的消息无需赘述，零偏对传感器误差影响较大且会累积需要补偿，重力加速度在动态环境中是变化的，需要补偿。

3、ESKF包含两个过程：名义状态变量的递推过程，误差状态变量的更新过程，最后将误差状态的后验均值和协方差并入名义状态变量就得到了真值，当然误差状态变量在最后会置零。

名义状态变量的离散时间运动方程：

$p(t + \Delta t) = p(t) + v \Delta t + \frac{1}{2} (R(\tilde{a} - b_a)) \Delta t^2 + \frac{1}{2} g \Delta t^2, \\ v(t + \Delta t) = v(t) + R(\tilde{a} - b_a) \Delta t + g \Delta t, \\ R(t + \Delta t) = R(t) \exp((\tilde{\omega} - b_g) \Delta t), \\ b_g(t + \Delta t) = b_g(t), \\ b_a(t + \Delta t) = b_a(t), \\ g(t + \Delta t) = g(t).$

误差状态变量的离散时间运动方程：

$\delta p(t + \Delta t) = \delta p + \delta v \Delta t, \\ \delta v(t + \Delta t) = \delta v + \left( -R(\tilde{a} - b_a)^\wedge \delta \theta - R \delta b_a + \delta g \right) \Delta t - \eta_v, \\ \delta \theta(t + \Delta t) = \exp \left( -(\tilde{\omega} - b_g) \Delta t \right) \delta \theta - \delta b_g \Delta t - \eta_\theta, \\ \delta b_g(t + \Delta t) = \delta b_g + \eta_{bg}, \\ \delta b_a(t + \Delta t) = \delta b_a + \eta_{ba}, \\ \delta g(t + \Delta t) = \delta g.$

对应的雅可比矩阵F：

$F = \begin{bmatrix} I & I \Delta t & 0 & 0 & 0 & 0 & 0 \\ 0 & I & -R(\tilde{a} - b_a)^\wedge \Delta t & 0 & -R \Delta t & I \Delta t & 0 \\ 0 & 0 & \text{Exp} \left( -(\tilde{\omega} - b_g) \Delta t \right) & -I \Delta t & 0 & 0 & 0 \\ 0 & 0 & 0 & I & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & I & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & I \end{bmatrix}.$

4、至此可以开始进行卡尔曼滤波的经典五个过程了，但是ESKF有一点不一样。

1、预测名义状态变量，在上文中已经给出状态转移方程，无需赘述，只是R会由上一时刻的旋转向量给出。

2、对误差状态变量协方差的预测：

$P_{\text{pred}} = F P F^\top + Q.$

其中F上午已经给出，Q是由随机过程决定的。

3、接下来需要求出观测矩阵H，需要注意的是观测矩阵H应该基于需要更新的状态变量来定，由于需要更新误差状态变量，所以H应该是对误差状态变量求导，即：

$H = \left. \frac{\partial h}{\partial \delta x} \right|_{x_{\text{pred}}}$

然后计算卡尔曼增益，更新误差状态变量，接着并入名义状态变量，最后在更新误差状态的协方差矩阵，即：

$K = P_{\text{pred}} H^{\top} \left( H P_{\text{pred}} H^{\top} + V \right)^{-1}$

$\delta x = K \left( z - h(x_{\text{pred}}) \right)$

$x = x_{\text{pred}} + \delta x$

$P = (I - K H) P_{\text{pred}}$

由于大部分观测数据是观测名义状态变量的，所以提供一个通用的链式求导法则来对误差状态变量求导：

$H = \frac{\partial h}{\partial x} \frac{\partial x}{\partial \delta x}$

第一项不用赘述，第二项求导：

$\frac{\partial x}{\partial \delta x} = \text{diag}\left(I_3, I_3, I_3, \frac{\partial \text{Log}\left( \mathbb{R}(\text{Exp}(\delta \theta)) \right)}{\partial \delta \theta}, I_3, I_3, I_3 \right)$

其中：

$\frac{\partial \text{Log}\left( \mathbb{R}(\text{Exp}(\delta \theta)) \right)}{\partial \delta \theta} = J_r^{-1}(\mathbb{R})$

至此，给出了基于ESKF的组合导航通用步骤，总结一下就是首先数学推导出名义状态变量的状态转移方程，通过流形的方法推导出误差状态的雅可比矩阵F，接着通过求出的状态转移方程对名义状态变量进行预测，然后用F预测误差状态变量的协方差，接着用F和预测协方差还有观测矩阵的雅可比矩阵H计算K，然后更新误差状态变量并且并入预测的名义状态变量中，最后更新协方差矩阵并清零误差状态变量。

4、接下来通过具体的观测源GNSS进行观测，假设GNSS能观测姿态和位置六维信息，那么旋转观测方程如下：

$R_{\text{gnss}} = R \text{Exp}(\delta \theta)$

这里的观测应该是对全量的观测，所以需要将名义旋转矩阵广义加上误差状态矩阵，其中 $R$ 为名义状态变量， $\delta \theta$ 为误差状态，在此基础上做些变换：

$z_{\delta \theta} = h(\delta \theta) = \text{Log} \left( R^{\top} R_{\text{gnss}} \right)=\delta \theta$

那么则有：

$\frac{\partial z_{\delta \theta}}{\partial \delta \theta} = I$

这样就避免了通用的链式求导，可以直接得到对误差状态的雅可比矩阵分量。

位置的观测是平凡的，之间给出位置观测方程和对应雅可比矩阵分量：

$p_{\text{gnss}} = p + \delta p$

$\frac{\partial p_{\text{gnss}}}{\partial \delta p} = I_{3 \times 3}$

此时残差：

$z - h(x) = \left[ p_{\text{gnss}} - p, \, \text{Log}\left( R^{\top} R_{\text{gnss}} \right) \right]^{\top}$

至此基于ESKF的IMU/GNSS导航系统理论部分结束，接下来通过Ubuntu20环境下的cpp代码复现该组合导航。

#include "eigen_types.h"
#include "gnss.h"
#include "imu.h"
#include "math_utils.h"
#include "nav_state.h"
#include "odom.h"

#include <glog/logging.h>
#include <iomanip>

namespace sad {

template <typename S = double>
class ESKF {
    public:
    using SO3 = Sophus::SO3<S>;                         //定义旋转矩阵类型
    using Vec3T = Eigen::Matrix<S, 3, 1>;               //定义三维向量类型
    using Vec18T = Eigen::Matrix<S, 18, 1>;             //定义18维向量类型
    using Mat3T = Eigen::Matrix<S, 3, 3>;               //定义3x3矩阵类型
    using MotionNoiseT = Eigen::Matrix<S, 18 ,18>;      //定义18x18状态变量噪声协方差类型
    using OdomNoiseT = Eigen::Matrix<S, 3, 3>;          //定义3x3里程计噪声协方差类型
    using GnssNoiseT = Eigen::Matrix<S, 6, 6>;          //定义6x6GNSS噪声协方差类型
    using Mat18T = Eigen::Matrix<S, 18, 18>;            //定义18x18协方差类型
    using NavStateT = NavState<S>;                      //定义名义状态变量类型

    //包含各类噪声的结构体
    struct Options {
        Options() = default;                            //构造函数

        double imu_dt_ = 0.005;                         //IMU传输数据频率

        double gyro_var_ = 1e-5;                        //陀螺仪测量方差
        double acce_var_ = 1e-2;                        //加速度计测量方差
        double bias_gyro_var_ = 1e-6;                   //陀螺仪零偏方差
        double bias_acce_var_ = 1e-4;                   //加速度计零偏方差

        double odom_var_ = 0.5;                         //里程计测量标准差
        double odom_span_ = 0.1;                        //里程计测量间隔
        double wheel_radius_ = 0.155;                   //轮子半径
        double circle_pulse_ = 1024.0;                  //编码器每圈脉冲数

        double gnss_pos_noise_ = 0.1;                   //GNSS位置噪声标准差
        double gnss_height_noise_ = 0.1;                //GNSS高度噪声标准差
        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  //GNSS旋转噪声标准差

        bool update_bias_gyro_ = true;                  //是否更新陀螺零偏
        bool update_bias_acce_ = true;                  //是否更新加速度计零偏
    };

    ESKF(Options option = Options()) : options_(option) {
        BuildNoise(option);
    }

    //设置初始条件，噪声协方差项配置、初始零偏、重力、初始误差状态变量协方差
    void SetInitialConditions(Options options, const Vec3T& init_bg, const Vec3T& init_ba, const Vec3T& gravity = Vec3T(0, 0, -9.8)) {
        BuildNoise(options);
        options_ = options;
        bg_ = init_bg;
        ba_ = init_ba;
        g_ = gravity;
        cov_P = Mat18T::Identity() * 1e-4; 
    }

    //预测过程函数的声明
    bool Predict(const IMU& imu);

    //里程计观测和更新过程函数的声明
    bool ObserveWheelSpeed(const Odom& odom);

    //GPS观测和更新过程函数的声明
    bool ObserveGPS(const GNSS& gnss);
    bool ObserveSE3(const SE3& pose);

    //获取名义状态变量（上一时刻名义状态变量和上一时刻的误差状态变量之和）
    NavStateT GetNominalState() const {
        return NavState(current_time_, R_, p_, v_, bg_, ba_);
    }

    //获取变换矩阵
    SE3 GetNominalSE3() const {
        return SE3(R_,p_);
    }

    //设置名义状态变量状态
    void SetX(const NavStated& x, const Vec3d& grav){
        current_time_ = x.timestamp_;
        R_ = x.R_;
        p_ = x.p_;
        v_ = x.v_;
        bg_ = x.bg_;
        ba_ = x.ba_;
        g_ = grav;
    }

    //设置误差状态变量协方差矩阵
    void SetCov(const Mat18T& cov) {
        cov_P = cov;
    }

    //获取重力
    Vec3d GetGravity() const {
        return g_;
    }

    private:
    //创建初始状态变量协方差矩阵P、里程计噪声协方差矩阵、GNSS噪声协方差矩阵
    void BuildNoise(const Options& options) {
        double ev = options.acce_var_;
        double et = options.gyro_var_;
        double eg = options.bias_gyro_var_;
        double ea = options.bias_acce_var_;

        Q_.diagonal() << 0, 0, 0, ev, ev, ev, et, et, et, eg, eg, eg, ea, ea, ea, 0, 0, 0;

        double o2 = options_.odom_var_ * options_.odom_var_;
        odom_noise_cov.diagonal() << o2, o2, o2;

        double gp2 = options.gnss_pos_noise_ * options.gnss_pos_noise_;
        double gh2 = options.gnss_height_noise_ * options.gnss_height_noise_;
        double ga2 = options.gnss_ang_noise_ * options.gnss_ang_noise_;
        gnss_noise_cov.diagonal() << gp2, gp2, gh2, ga2, ga2, ga2;    
    }

    //更新名义状态变量，重置误差状态变量为0
    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));
        bg_ += dx_.template block<3, 1>(9, 0);
        ba_ += dx_.template block<3, 1>(12, 0);
        g_ += dx_.template block<3, 1>(15, 0);

        dx_.setZero();
    }

    //一些成员变量

    名义状态变量
    double current_time_ = 0.0;
    Vec3T p_ = Vec3T::Zero();
    Vec3T v_ = Vec3T::Zero();
    SO3 R_;
    Vec3T bg_ = Vec3T::Zero();
    Vec3T ba_ = Vec3T::Zero();
    Vec3T g_{0, 0, -9.8};

    Vec18T dx_ = Vec18T::Zero();                    //误差状态
    Mat18T cov_P = Mat18T::Identity();              //误差状态协方差矩阵
    MotionNoiseT Q_ = MotionNoiseT::Zero();         //误差状态噪声协方差矩阵
    OdomNoiseT odom_noise_cov = OdomNoiseT::Zero(); //里程计噪声协方差矩阵
    GnssNoiseT gnss_noise_cov = GnssNoiseT::Zero(); //GNSS噪声协方差矩阵

    bool first_gnss_ = true;

    Options options_;
};

using ESKFD = ESKF<double>;
using ESKFF = ESKF<float>;

//名义状态和误差状态协方差矩阵的预测
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) {
        LOG(INFO) << "skip this imu because dt_ = " << dt;
        current_time_ = imu.timestamp_;
        return false;
    }

    //名义状态变量的预测过程
    Vec3T new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
    Vec3T 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;

    //误差状态雅可比矩阵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

    //误差状态变量协方差矩阵的预测过程
    cov_P = F * cov_P.eval() * F.transpose() + Q_;

    //更新时间
    current_time_ = imu.timestamp_;
    return true;
}

    //里程计观测更新过程
template <typename S>
bool ESKF<S>::ObserveWheelSpeed(const Odom& odom) {
    assert(odom.timestamp_ >= current_time_);

    Eigen::Matrix<S, 3, 18> H = Eigen::Matrix<S, 3, 18>::Zero();
    H.template block<3, 3>(0, 3) = Mat3T::Identity();

    // 卡尔曼增益
    Eigen::Matrix<S, 18, 3> K = cov_P * H.transpose() * (H * cov_P * H.transpose() + odom_noise_cov).inverse();

    // 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);
    //载体坐标系下的速度，只有车辆前进方向
    Vec3T vel_odom(average_vel, 0.0, 0.0);
    //转换到导航坐标系下
    Vec3T vel_world = R_ * vel_odom;

    //更新误差状态变量
    dx_ = K * (vel_world - v_);

    //更新误差状态变量协方差矩阵
    cov_P = (Mat18T::Identity() - K * H) * cov_P;

    //将误差状态变量并入名义状态变量，并将误差状态变量置零
    UpdateAndReset();
    return true;
}

template <typename S>
bool ESKF<S>::ObserveGPS(const GNSS& gnss) {
    assert(gnss.unix_time_ >= current_time_);

    //用第一个GNSS数据初始化旋转矩阵和位置
    if (first_gnss_) {
        R_ = gnss.utm_pose_.so3();
        p_ = gnss.utm_pose_.translation();
        first_gnss_ = false;
        current_time_ = gnss.unix_time_;
        return true;
    }

    assert(gnss.heading_valid_);
    ObserveSE3(gnss.utm_pose_);
    current_time_ = gnss.unix_time_;

    return true;
}

template <typename S>
bool ESKF<S>::ObserveSE3(const SE3& pose){
    //观测噪声协方差矩阵
    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();
    H.template block<3, 3>(3, 6) = Mat3T::Identity();

    //GNSS观测卡尔曼增益
    Eigen::Matrix<S, 18, 6> K = cov_P * H.transpose() * (H * cov_P * H.transpose() + gnss_noise_cov).inverse();

    //残差
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);
    innov.template tail<3>() = (R_.inverse() * pose.so3()).log();

    //更新误差状态变量
    dx_ = K * innov;

    //更新误差状态变量协方差矩阵
    cov_P = (Mat18T::Identity() - K * H) * cov_P * (Mat18T::Identity() - K * H).transpose() + K * gnss_noise_cov * K.transpose();

    //将误差状态变量并入名义状态变量，并将误差状态变量置零
    UpdateAndReset();
    return true;
}
}

这个是ESKF的头文件，包含了ESKF的所有功能。

#include "eskf.h"
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <iomanip>
#include <fstream>

#include "io_utils.h"
#include "static_imu_init.h"
#include "utm_convert.h"
DEFINE_string(txt_path, "../data/10.txt", "数据文件路径");
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角(角度)");
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
DEFINE_bool(with_ui, true, "是否显示图形界面");
DEFINE_bool(with_odom, false, "是否加入轮速计信息");

    //存p，v，ba，bg数据
    void save_vec3(std::ofstream& fout, const Vec3d& v) {
    fout << v[0] << " " << v[1] << " " << v[2] << " ";
    };

    //存四元数
    void save_quat(std::ofstream& fout, const Quatd& q) {
    fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
    };

    void save_result(std::ofstream& fout, const sad::NavStated& save_state) {
    fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);//时间戳设置为18位有效数字，其他数据设置为9位有效数字
    save_vec3(fout, save_state.p_);
    save_quat(fout, save_state.R_.unit_quaternion());
    save_vec3(fout, save_state.v_);
    save_vec3(fout, save_state.bg_);
    save_vec3(fout, save_state.ba_);
    fout << std::endl;
    };
   
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc,&argv,true);

    if (fLS::FLAGS_txt_path.empty()) {
        return -1;
    };

    //实例化一些需要用到的类，初始化imu，eskf，回调函数，RTK安装角
    sad::StaticIMUInit imu_init;  
    sad::ESKFD eskf;
    sad::TxtIO io(FLAGS_txt_path);
    Vec2d antenna_pos(FLAGS_antenna_pox_x,FLAGS_antenna_pox_y);

    //导航结果保存路径
    std::ofstream fout("../data/gins.txt");

    bool imu_inited = false, gnss_inited = false;

    bool first_gnss_set = false;
    Vec3d origin = Vec3d::Zero();

    
    //匿名函数作为实参传入并进行预测过程，接着进行观测
    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
        
        if (!imu_init.InitSuccess()) {
            imu_init.AddIMU(imu);
            return;
        }

        if (!imu_inited) {
            // 读取初始零偏，设置ESKF
            sad::ESKFD::Options options;
            // 噪声由初始化器估计
            options.gyro_var_ = sqrt(imu_init.GetCovGyro()[0]);
            options.acce_var_ = sqrt(imu_init.GetCovAcce()[0]);
            eskf.SetInitialConditions(options, imu_init.GetInitBg(),imu_init.GetInitBa(), imu_init.GetGravity());
            imu_inited = true;
            return;
        }

        if (!gnss_inited) {
            //等待有效的RTK数据
            return;
        }

        //接收到有效GNSS信号后开始预测
        eskf.Predict(imu);  

        //存入数据
        auto state = eskf.GetNominalState();   
 
        save_result(fout,state);

        usleep(1e3);
    })
    .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
        
        if (!imu_inited) {
                return;
            }
        sad::GNSS gnss_convert = gnss;
        if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos,FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
            return;
        }

        //去掉原点
        if (!first_gnss_set) {
            origin = gnss_convert.utm_pose_.translation();
            first_gnss_set = true;
        }
        gnss_convert.utm_pose_.translation() -= origin;
        eskf.ObserveGPS(gnss_convert);
        auto state = eskf.GetNominalState();
        save_result(fout,state);
        gnss_inited = true;
    })
    .SetOdomProcessFunc([&](const sad::Odom& odom) {
        //主要用于初始化
        imu_init.AddOdom(odom);
        if (FLAGS_with_odom && imu_inited && gnss_inited) {
            eskf.ObserveWheelSpeed(odom);
            //这两行也可以不用，低频里程计信号观测更新信息会融入到高频imu和GNSS观测更新信息中
            auto state = eskf.GetNominalState();
            save_result(fout,state);
        }
    })
    .Go();

    return 0;
}

这个是运行ESKF的源文件，包含了读取传感器数据，调用ESKF功能，存储导航数据等。

主要参考文献：高翔自动驾驶与机器人中的SLAM技术

棒球部前部长

关注

42
点赞
踩
45

收藏

觉得还不错? 一键收藏
0
评论
用ESKF实现IMU/GNSS组合导航（学习记录）

至此，给出了基于ESKF的组合导航通用步骤，总结一下就是首先数学推导出名义状态变量的状态转移方程，通过流形的方法推导出误差状态的雅可比矩阵F，接着通过求出的状态转移方程对名义状态变量进行预测，然后用F预测误差状态变量的协方差，接着用F和预测协方差还有观测矩阵的雅可比矩阵H计算K，然后更新误差状态变量并且并入预测的名义状态变量中，最后更新协方差矩阵并清零误差状态变量。其中平移，速度，旋转矩阵是导航中必要的消息无需赘述，零偏对传感器误差影响较大且会累积需要补偿，重力加速度在动态环境中是变化的，需要补偿。
复制链接

扫一扫