高翔【自动驾驶与机器人中的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)。

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

1. 单点 GNSS 定位，即传统的米级精度卫星定位。这种定位方式价格低廉，应用广泛。大多数手机、车机等终端都具备单点卫星定位能力。在普通车辆的道路级导航中，单点定位的精度足以让驾驶人员辨认出车辆位于哪条道路，但在多条道路并排（车道级）时，它的精度又往往不足以区分车辆是在高速路上还是在辅路上，或者是在主路上还是在匝道上。
2. 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 xydouble 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 sadusing 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观测视为观测模型，推导整个滤波器。

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

状态变量为：

所有变量都默认取下标，其中p为平移，v为速度，R为旋转，为零偏，g为重力。

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

公式3.1：

公式3.22

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

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

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

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

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

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

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

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

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

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

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

• A有一个数据，B有一个数据，真实数据应该是多少。
• 这俩数据之间的差异，通过其协方差（可信度）来分配更倾向于A还是B。

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

• 矩阵H是从预测空间转换到观测空间的变换矩阵。
• Hx就将递推预测的数据转换到观测空间，
• 与观测数据进行比较。获取差异。

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

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

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

个人理解哈：只考虑扰动模型的情况下，有各种好处。直白点：累计误差不会在更新中因累计放大而影响彼此。只考虑观察和预测的扰动模型即可。

---

在ESKF中，通常把原状态变量称为名义状态变量(Nominal State)，把ESKF里的状态变量称为误差状态变量(Error State)。

<