IMU Error Modeling Tutorial (INS state estimation with real-time sensor calibration)

介绍

Kalman Filters on Differentiable Manifolds

IkFoM在Fast-LIO2中的应用

Fast-LIO2中，主要使用了IkFoM作为状态，其中，在use-ikfom.hpp声明了关于各种状态、数据的宏定义

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

#include <IKFoM_toolkit/esekfom/esekfom.hpp>

typedef MTK::vect<3, double> vect3;
typedef MTK::SO3<double> SO3;
typedef MTK::S2<double, 98090, 10000, 1> S2; 
typedef MTK::vect<1, double> vect1;
typedef MTK::vect<2, double> vect2;

MTK_BUILD_MANIFOLD(state_ikfom,
((vect3, pos))
((SO3, rot))
((SO3, offset_R_L_I))
((vect3, offset_T_L_I))
((vect3, vel))
((vect3, bg))
((vect3, ba))
((S2, grav))
);

MTK_BUILD_MANIFOLD(input_ikfom,
((vect3, acc))
((vect3, gyro))
);

MTK_BUILD_MANIFOLD(process_noise_ikfom,
((vect3, ng))
((vect3, na))
((vect3, nbg))
((vect3, nba))
);

...

Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Calibration

摘要

组合多个LIDARS使机器人能够最大化其对环境的感知意识，并获得足够的测量。本文提出了一种实现鲁棒和同步的外参标定，里程计和多个LIDAR的建图系统。

我们的方法从测量预处理开始，从原始测量中提取边缘和平面特征，在运动和外参初始化过程之后，基于滑动窗口的多激光雷达里程计将板载运行，以估计具有在线标定和收敛性检测的位姿。

Large-Scale LiDAR Consistent Mapping using Hierarchical LiDAR Bundle Adjustment

摘要

重建精确一致的大规模激光雷达点云图对机器人应用至关重要。现有的位姿图优化方法虽然效率高，但不能直接优化建图一致性。激光雷达束调整(BA)最近被提出来解决这个问题;然而，这在大尺度地图上太耗时了。为了解决这一问题，本文提出了一种适用于大尺度地图的全局一致且高效的制图方法。

我们提出的工作包括自底向上的层次BA和自顶向下的姿态图优化，结合了两种方法的优点。通过分层设计，我们用比原始BA小得多的Hessian矩阵大小解决了多个BA问题;通过姿态图优化，实现了激光雷达姿态的平稳高效更新。

On the Covariance of ICP-based Scan-matching Techniques

摘要

本文研究迭代最近点(ICP)算法计算旋转平移的协方差估计问题。这个问题与移动机器人和装备了深度传感相机(如Kinect)或激光雷达(如Velodyne)的车辆的定位问题有关。以往文献中提出的封闭形式的协方差公式通常建立在ICP的解是通过最小化线性最小二乘问题得到的这一事实之上。在本文中，我们表明这种方法需要谨慎，因为算法的重新匹配步骤没有显式解释，并将其应用于点到点版本的ICP导致完全错误的协方差。然后，我们给出了一个形式化的数学证明，证明了该方法在点到平面的ICP版本中是有效的，从而验证了从业者的直觉和实验结果。

介绍

Robust and Online LiDAR-inertial Initialization

介绍

摘要：对于大多数LiDAR惯性里程计来说，精确的初始状态，包括时间偏差和LiDAR与6轴IMU之间的外参，起着重要的作用，通常被认为是前提条件。然而，在定制的激光雷达惯性系统中，这样的信息并不总是可用的，本文提出了一种完全在线的激光雷达惯性系统初始化过程，该过程通过校准激光雷达和IMU之间的时间偏差和外参，并通过将激光雷达测量的状态估计值与IMU测量的状态值对齐来校正重力矢量和IMU偏差。我们将所提出的方法实现为一个初始化模块，如果启用该模块，它将自动检测采集数据的激发程度，同时校准时间偏移量、外参偏移量、重力矢量和惯性测量单元偏差，然后将其用作在线激光雷达惯性里程仪系统的高质量初始状态值。对不同类型的激光雷达和激光雷达-惯性组合进行的实验表明了该初始化方法的鲁棒性、适应性和有效性。

我们的LiDAR-惯性初始化法的主要目的之一是在没有任何初始估计的情况下校准LiDAR和IMU之间的外特性。现有的一些外部标定方法是基于批量优化的，数据关联性很强，耗时较大。例如，Lveal。[14]提出了一种基于连续时间批优化的标定方法。B样条的使用使得需要估计的参数较多，计算量也较大。[15]使用扩展卡尔曼过滤估计运动补偿复杂的外域变换，收敛速度有限。与这些方法相比，我们的方法更轻量级，能够快速运行，同时仍然可以获得足够精确的外部校准，以便后续的在线估计(例如，通过[1])。我们的方法还校准了[14，15]中没有考虑的时间偏移。此外，文献[14，15]所采用的基于NDT的扫描-扫描匹配通常不适用于具有非重复扫描模式的激光雷达。相比之下，我们的方法采用了扫描到地图的匹配策略，可以很容易地应用重复扫描和非重复扫描的LiDAR。

Performance evaluation of iterated extended Kalman filterwith variable step-length

介绍

优化方法主要基于最小化均方误差准则，对于线性高斯系统，导致了著名的卡尔曼滤波。与提供状态的条件PDF的贝叶斯方法不同，最优化方法提供状态的点估计和估计误差的相应协方差矩阵(CM)。与贝叶斯方法一样，基于最优化方法的状态估计问题只能在少数特殊情况下得到解析解。对于其他情况，估计方法通常遵循卡尔曼滤波框架，并利用非线性函数的线性化等逼近技术，例如，扩展(EKF)和二阶扩展卡尔曼滤波器[6]、[1]分别通过围绕当前估计到一阶或二阶的泰勒级数展开来逼近非线性函数，当线性化误差的影响往往会扰乱过滤的性能或其收敛性时，在更新状态附近重新线性化测量方程可能会减轻困难。

这种方法被称为迭代扩展卡尔曼过滤(IEKF)[6]。

IEKF计算状态估计不是作为近似条件平均值(就像EKF那样)，而是作为最大后验(MAP)估计[7]。文献[8]证明了IEKF量测更新是高斯-牛顿(GN)方法的应用，而EKF是仅用一次GN方法迭代的IEKF的特例。

Kalman Filters on Differentiable Manifolds

摘要

卡尔曼滤波是现代控制系统中最重要和应用最广泛的滤波技术之一。然而，几乎所有当前的卡尔曼滤波器的变体都是在欧几里得空间Rn中制定的，而许多现实世界的系统(例如，机器人系统)实际上是在流形上进化的。

在本文中，我们提出了一种为这类流形上系统设计卡尔曼滤波器的方法。利用运算\(\boxplus \backslash \boxminus\)，并进一步定义了相应流形上的⊕运算，给出了流形上系统的规范表示。这种非正则形式使我们能够在卡尔曼过滤的每一步中将多种约束从系统行为中分离出来，最终产生一个在流形上自然演化的通用的和象征性的卡尔曼过滤框架。

前言

协程是Cyber-RT的实现任务轮转的最小单位，是处理数据回调的运行模块。协程可以理解为“可以暂停”的函数，相比于线程，其具有中断可恢复的特性，那么只需要在开一个全局的数组存储所有的协程，在协程中断时，不断轮转调用下一个协程继续运行即可达到类似线程的效果。

为什么选用协程，因为基于协程的特性再加上Cyber中枢调度Scheduler的线程调度，可以避免回调时由于阻塞导致其他回调不能被执行的情况；此外，协程是在用户态来完成上下文切换的，所以切换耗时只有区区100ns多一些，比进程切换要高30倍。

Cyber/croutine目录

1
2
3
4
5
6
7
8
9
10

├── BUILD
├── CMakeLists.txt
├── croutine.cc
├── croutine.h
├── detail
│   ├── routine_context.cc
│   ├── routine_context.h
│   ├── swap_aarch64.S
│   └── swap_x86_64.S
└── routine_factory.h