高翔【自动驾驶与机器人中的SLAM技术】学习笔记（十）高翔书中的细节：参考链接；卫星导航；ESKF

一、参考链接

我认真查找了好多地方：结果在最后一页。

作者GITHUB链接如下：
https://github.com/gaoxiang12/slam_in_autonomous_driving

全书所有参考链接：如下

1 https://www.sae.org/standards/content/j3016_202104
2 http://www.evinchina.com/articleshow-217.html
3 Arcgis: https://www.arcgis.com/
4 Autoware: https://github.com/autowarefoundation/autoware
5 工具地址见：https://github.com/hobu/mgrs
6 ÀEPFL雕像数据集：https://lgg.epfl.ch/statues_dataset.php
7 https://github.com/HKUST-Aerial-Robotics/A-LOAM
8 https://github.com/wh200720041/floam

二、卫星导航

全球卫星导航系统（Global Navigation Satellite System，GNSS），简称卫星导航。

GNSS 通过测量自身与地球周围各卫星的距离来确定自身的位置，而与卫星的距离主要是通过测量时间间隔来确定的。一个卫星信号从卫星上发出时，带有一个发送时间。而GNSS接收机接收到它时，又有一个接收时间。比较接收时间与卫星发送时间，就能估算各卫星离我们的距离。而各种 GNSS 和测量方法的主要差异，就是如何减少这个时间测量的误差。从这种角度来看，GNSS本质上可以看成一种高精度的授时系统。

目前，世界范围内，我们可以接收到的卫星信号主要来自四个系统：

美国的全球定位系统Global Positioning System,GPS )、
中国的北斗卫星导航系统(Beidou Navigation Satellite System，BDS)、
俄罗斯的格洛纳斯系统(GLONASS)、
欧盟的伽利略系统(GALILEO)。

对于自动驾驶车辆来说，最常用的卫星定位技术包括以下几种。

单点 GNSS 定位，即传统的米级精度卫星定位。这种定位方式价格低廉，应用广泛。大多数手机、车机等终端都具备单点卫星定位能力。在普通车辆的道路级导航中，单点定位的精度足以让驾驶人员辨认出车辆位于哪条道路，但在多条道路并排（车道级）时，它的精度又往往不足以区分车辆是在高速路上还是在辅路上，或者是在主路上还是在匝道上。
RTK 定位。由于卫星定位信号在传输过程中可能产生误差，人们发展了差分定位技术，即通过地面上的一个已知精确位置的基站与车辆通信，校正车辆卫星接收机的信号。差分定位又可进一步分为位置、伪距及载波相位差分定位。其中最广泛使用的，是基于载波相位差分的 RTK 技术。RTK 通过与一个或多个基站进行通信，可以实时地获取校正后的卫星导航位置。

自动驾驶通常需要车道级导航而非道路级导航。车道级导航可以指出车辆位于道路当中哪个车道，比道路级导航更稳定。

世界坐标系

地理坐标系：经纬度
UTM坐标系：米制坐标

GNSS代码：src/common/gnss.h

//
// Created by xiang on 2022/1/4.
//

#ifndef SLAM_IN_AUTO_DRIVING_GNSS_H
#define SLAM_IN_AUTO_DRIVING_GNSS_H

#include "common/eigen_types.h"
#include "common/message_def.h"

namespace sad {

/// GNSS状态位信息
/// 通常由GNSS厂商提供，这里使用千寻提供的状态位
enum class GpsStatusType {
    GNSS_FLOAT_SOLUTION = 5,         // 浮点解（cm到dm之间）
    GNSS_FIXED_SOLUTION = 4,         // 固定解（cm级）
    GNSS_PSEUDO_SOLUTION = 2,        // 伪距差分解（分米级）
    GNSS_SINGLE_POINT_SOLUTION = 1,  // 单点解（10m级）
    GNSS_NOT_EXIST = 0,              // GPS无信号
    GNSS_OTHER = -1,                 // 其他
};

/// UTM 坐标
struct UTMCoordinate {
    UTMCoordinate() = default;
    explicit UTMCoordinate(int zone, const Vec2d& xy = Vec2d::Zero(), bool north = true)
        : zone_(zone), xy_(xy), north_(north) {}

    int zone_ = 0;              // utm 区域
    Vec2d xy_ = Vec2d::Zero();  // utm xy
    double z_ = 0;              // z 高度（直接来自于gps）
    bool north_ = true;         // 是否在北半球
};

/// 一个GNSS读数结构
struct GNSS {
    GNSS() = default;
    GNSS(double unix_time, int status, const Vec3d& lat_lon_alt, double heading, bool heading_valid)
        : unix_time_(unix_time), lat_lon_alt_(lat_lon_alt), heading_(heading), heading_valid_(heading_valid) {
        status_ = GpsStatusType(status);
    }

    /// 从ros的NavSatFix进行转换
    /// NOTE 这个只有位置信息而没有朝向信息，UTM坐标请从ch3的代码进行转换
    GNSS(sensor_msgs::NavSatFix::Ptr msg) {
        unix_time_ = msg->header.stamp.toSec();
        // 状态位
        if (int(msg->status.status) >= int(sensor_msgs::NavSatStatus::STATUS_FIX)) {
            status_ = GpsStatusType::GNSS_FIXED_SOLUTION;
        } else {
            status_ = GpsStatusType::GNSS_OTHER;
        }
        // 经纬度
        lat_lon_alt_ << msg->latitude, msg->longitude, msg->altitude;
    }

    double unix_time_ = 0;                                  // unix系统时间
    GpsStatusType status_ = GpsStatusType::GNSS_NOT_EXIST;  // GNSS 状态位
    Vec3d lat_lon_alt_ = Vec3d::Zero();                     // 经度、纬度、高度，前二者单位为度
    double heading_ = 0.0;                                  // 双天线读到的方位角，单位为度
    bool heading_valid_ = false;                            // 方位角是否有效

    UTMCoordinate utm_;       // UTM 坐标（区域之类的也在内）
    bool utm_valid_ = false;  // UTM 坐标是否已经计算（若经纬度给出错误数值，此处也为false）

    SE3 utm_pose_;  // 用于后处理的6DoF Pose
};

}  // namespace sad

using GNSSPtr = std::shared_ptr<sad::GNSS>;

#endif  // SLAM_IN_AUTO_DRIVING_GNSS_H

给出了GNSS的状态枚举；以及UTM坐标的数据结构和GNSS的读数数据结构。

其中GNSS的数据结构与ros中的NavSatStatus消息可以转换。

注意GNSS中，有几个属性字段：一个是通过utm_convert转换之后的坐标。另一个是基于UTM坐标的6自由度pose。

antenna：天线

//
// Created by xiang on 2022/1/4.
//

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

#include "common/gnss.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"
#include "utm_convert.h"

DEFINE_string(txt_path, "./data/ch3/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, "是否显示图形界面");

/**
 * 本程序演示如何处理GNSS数据
 * 我们将GNSS原始读数处理成能够进行后续处理的6自由度Pose
 * 需要处理UTM转换、RTK天线外参、坐标系转换三个步骤
 *
 * 我们将结果保存在文件中，然后用python脚本进行可视化
 */

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

    sad::TxtIO io(fLS::FLAGS_txt_path);

    std::ofstream fout("./data/ch3/gnss_output.txt");
    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);

    auto save_result = [](std::ofstream& fout, double timestamp, const SE3& pose) {
        auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
        auto save_quat = [](std::ofstream& fout, const Quatd& q) {
            fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
        };

        fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
        save_vec3(fout, pose.translation());
        save_quat(fout, pose.unit_quaternion());
        fout << std::endl;
    };

    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
    if (FLAGS_with_ui) {
        ui = std::make_shared<sad::ui::PangolinWindow>();
        ui->Init();
    }

    bool first_gnss_set = false;
    Vec3d origin = Vec3d::Zero();
    io.SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
          sad::GNSS gnss_out = gnss;
          if (sad::ConvertGps2UTM(gnss_out, antenna_pos, FLAGS_antenna_angle)) {
              if (!first_gnss_set) {
                  origin = gnss_out.utm_pose_.translation();
                  first_gnss_set = true;
              }

              /// 减掉一个原点
              gnss_out.utm_pose_.translation() -= origin;

              save_result(fout, gnss_out.unix_time_, gnss_out.utm_pose_);

              if (ui) {
                  ui->UpdateNavState(
                      sad::NavStated(gnss_out.unix_time_, gnss_out.utm_pose_.so3(), gnss_out.utm_pose_.translation()));
                  usleep(1e3);
              }
          }
      }).Go();

    if (ui) {
        while (!ui->ShouldQuit()) {
            usleep(1e5);
        }
        ui->Quit();
    }

    return 0;
}

正如代码注释中所写的：将txt中的GNSS数据读取，然后转换成UTM数据，基于天线外参，坐标转换为6自由度的pose。将pose记录gnss_out。最后用gnss_out转换成导航状态量NavStated类。

此处是从txt读取数据，真实应用更可能订阅某个ROS话题节点。

三、误差状态卡尔曼滤波器（Error State Kalman Filter,ESKF）

RTK 设备为我们提供了一个不太稳定的位姿观测源。我们可以将它视为定位滤波器的一种观测。本节将RTK与IMU 结合，使用拓展卡尔曼滤波器形成传统的组合导航算法，以供后续的算法对比。严格来说，笔者向读者介绍的是误差状态卡尔曼滤波器(Error State Kalman Filter，ESKF)。ESKF 的应用十分广泛，从GINS组合导航到视觉SLAM[58-60]、外参自标定[61-62]等任务中都有应用。

这里提到了一个外参自动标定。

Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration.
A kalman flter-based algorithm for imu-camera calibration：Observability analysis and performance evaluation.

1、ESKF的数学推导演变

之前是把IMU视为观测模型。现在我们IMU视为运动模型，并把GNSS观测视为观测模型，推导整个滤波器。

状态估计问题嘛，先明确状态变量。

状态变量为：

$\mathbf{x} = [\mathbf{p}, \mathbf{v},\mathbf{R},\mathbf{b}_g, \mathbf{b}_a, \mathbf{g}]^\top$

所有变量都默认取下标 $()_{WB}$ ，其中p为平移，v为速度，R为旋转， $\mathbf{b}_g,\mathbf{b}_a$ 为零偏，g为重力。

将IMU测量值带入公式3.1。状态变量在连续时间下的运动方程为公式3.22：

公式3.1：

公式3.22

直白点说：搞不定了，或者说不好搞了。得想个其他法子。

于是，能否避免直接使用x和P来表达状态的均值和协方差，推导运动和观察方程呢？能否使用原先卡尔曼滤波器中的更新量来推导这两个方程？

说直白点：状态量用x和P来表示均值和协方差，来推导运动方程和观察方程这个方式有困难推不动了，改用更新量来重新推导运动方程和观察方程。

回忆卡尔曼滤波器中的观测部分。咱们把运动方程和观察方程，以及“黄金五公式”一起回忆下。

在卡尔曼滤波器观测部分，我们看到在用卡尔曼增益更新状态时，（C和H看做等效）。

$\boldsymbol{x}_k=\boldsymbol{x}_{k,\mathrm{pred}}+\boldsymbol{K}_k\underbrace{(\boldsymbol{z}_k-\boldsymbol{H}_k\boldsymbol{x}_{k,\mathrm{pred}})}_\text{Updates Value}.$

之前提到这个公式有大用，基于此可以做一些操作了。

在流形意义下，右侧的更新量（Updates Value）（CSDN富文本编辑模式下，公式不支持中文，此处用了Updates Value）。原文如下：

应是位于切空间中的矢量，中间的加法应为流形与切空间指数映射的广义加法。但也可以将更新量(或者称为误差状态)视为滤波器的状态变量，来推导运动和观测模型。

这就引出了误差状态卡尔曼滤波器。

到这意思就是说，对于我们的状态估计问题，我们要估计的状态变为了误差状态。

这个更新量：其实就是观测结果与递推预测结果的差异。

A有一个数据，B有一个数据，真实数据应该是多少。

这俩数据之间的差异，通过其协方差（可信度）来分配更倾向于A还是B。

这个之前理解KF时提及过。如何分配这个差异。

矩阵H是从预测空间转换到观测空间的变换矩阵。

Hx就将递推预测的数据转换到观测空间，

与观测数据进行比较。获取差异。

更进一步，不光是平移和旋转，把所有的状态都用误差状态来表达，这就是典型ESKF的做法。
ESKF是许多传统的、现代的系统里都广泛使用的状态估计方法，既可以作为组合导航的滤波器，也可以用来实现LIO、VIO 等复杂系统。

相比于传统KF，ESKF的优点可以总结如下：

在旋转的处理上，ESKF的状态变量可以采用最小化的参数表达，也就是使用三维变量来表达旋转的增量。该变量位于切空间中，而切空间是一个矢量空间。传统KF需要用到四元数(4维)或者更高维的变量来表达状态(旋转矩阵，9维)，要不就得采用带有奇异性的表达方式(欧拉角)。
ESKF总是在原点附近，离奇异点较远，数值方面更稳定，并且不会产生离工作点太远而导致线性化近似不够的问题。
ESKF的状态量为小量，其二阶变量相对来说可以忽略。同时，大多数雅可比矩阵在小量
情况下变得非常简单，甚至可以用单位阵代替。
误差状态的运动学相比原状态变量更小(小量的运动学)，因此可以把更新部分归入原状
态变量中。