KF‐Fusion开发日志 - HIT-K1928-LAB/HIT-K1928-wiki GitHub Wiki

整体目标

搭建基于ESKF的组合导航框架，以松耦合的形式融合GNSS，外部里程计（VIO）或动捕观测信息，支持延迟更新和状态扩展。

关键技术：

通用IMU-ESKF框架，不考虑地球自转带来的非惯性系虚拟力，适用于低成本IMU；
支持以松耦合的形式融合GNSS，外部里程计/动捕，融合里程计或动捕时支持动态标定外参；
基于IMU预积分的延迟更新，相比于传统延迟更新方式更高效；

阶段目标：

1.搭建ESKF基础框架，注意接口设计，便于状态和观测扩展。

2.完成里程计/动捕融合。

3.完成GNSS融合。

设计流水

状态管理逻辑

对每类状态单独定义一个没有继承关系的类，并使用variant对所有状态进行包装实现类型擦除，用一个map存储所有系统状态，使用variant的主要目的是避免使用基于多态的类型擦除，因为这种方法会频繁使用开销较大的dynamic_cast作下行转换，效率较低。

最底层的状态类（一级状态类）是按照状态的数据定义和更新方式区分的，目前主要有两个状态基类，分别是OriBase（旋转）和VecBase（三维向量），状态基类记录该状态的localSize（对应误差状态的维数），OriBase维护的基本数据格式是Sophus::SO3d，更新方式为右扰动，VecBase的基本数据格式是Vector3d。

IMU状态以及外参状态（统称为二级状态类）继承自这些状态基类，其中IMU姿态状态继承自OriBase，外参旋转状态继承自VecBase，其余继承自VecBase，二级状态类需要指定状态类型StateType（枚举值）以及排列序号。

三级状态是利用variant封装的状态类SystemState，也是最终插入到状态池（map）的状态类型。定义用于生成SystemState的模板类，在该类中实现通用的二级状态访问操作（基于visitor）以及工厂函数。

StateBase以及二级状态类:

重要成员变量：

LocalDim：局部维度或误差状态维度，在一级状态中定义，编译常量。

state_value: 状态数值（SO3或Vector），在一级状态中定义。

is_constant：是否是常量状态（涉及到是否会在kf过程中对其进行更新），在一级状态中定义，运行时变量。

start_index：在总状态向量中的索引，在一级状态中定义，编译时确定，运行时赋值。

Order：状态排列序号，在二级状态中定义，编译常量。

关于某个状态在总状态向量中的索引（后称状态索引）如何确定：

在state_utils.h中实现了编译时确定order_index_map，该array数组记录了每个状态对应的状态索引（order_index_map[1]=3，表示排列顺序为1的状态的状态索引是3），外部在调用create创建具体状态时会通过order_index_map给状态的start_index赋值。

重要成员方法：

update()：输入后验误差状态，对状态进行更新。

localSize() / startIndex()：返回对应成员变量的数值。

SystemState/DefineVariantState类

模板类，基于std::variant包装二级状态，定义了一系列编译时确定的常量成员方法，用于实现快捷简便的系统状态增删。

重要成员方法：

orderIndex() / localSize() / update()等：基于visitor模式访问枚举体内部状态的成员变量或方法。

viewAs() / viewConstAs() ：模板函数，模板参数为二级状态类型，通过std::get方法返回二级状态引用。

create() : 工厂函数，传入状态初始值的地址和二级状态类型的枚举，返回创建好的三级状态SystemState。

sumOfLocalSize()：编译常量成员方法，返回总的状态维数（各状态的local size之和）。

StateManager类：

重要成员变量：

map<int, SystemState> state_pool：维护系统状态，键为每个状态的uid，值为variant包装的三级状态类。

成员方法：

insertState()：在系统初始化时调用，输入状态uid，状态类型枚举和初始值地址，构造状态并插入到状态池中。

updateState()：kf完成后调用，输入后验误差状态向量，按顺序调用各状态的更新函数进行更新。

getXXXState()：返回特定状态的状态数值引用。

添加新的状态流程：

1.判断该状态是否属于OriBase或VecBase，若不是则需要自定义新的状态基类，定义方式按照已有状态基类。然后定义二级状态继承自状态基类，指定状态类型和排列序号（均为编译时常量）。

2.在commons.h中添加枚举值，包括状态类型StateType，状态排列序号StateOrder，若是需要定义新的状态基类则还需要添加该状态基类的local_dim(local size)；指定该状态的uid，在commons.h中定义内联函数getxxxStateUid()，返回该状态的uid。

3.在SystemState模板参数中添加二级状态类。

4.确定该状态的初始值以及初始协方差，在kf_coordinator的init函数中通过state_manager的接口向状态池中插入该状态。

Kf初始化——StateInitializer类

用于初始化kf状态，主要是导航系下的位姿、速度和零偏，为了实现灵活的初始化（基于odom或基于gps），因此单独将初始化过程封装为一个类。

重要成员变量：

state：StateInit类型，存储了状态的初始值，包括姿态、位置、速度、零偏、初始化时刻的imu信息。

imu_queue：imu缓存队列，用于初始化零偏。

odom_que：里程计队列，用于初始化除零偏以外的状态。

is_bias_init：bool类型，是否完成零偏初始化。

is_state_init：bool类型，是否完成所有状态的初始化。

重要成员方法：

initState()：初始化逻辑主要执行函数，输入观测meas，返回是否初始化成功。若输入meas为imu并且没有完成零偏初始化，则调用initGyroBias()进行零偏初始化；若输入meas为里程计并且已完成零偏初始化，则调用initByOdom()完成其他状态初始化。

initGyroBias()：根据固定时间窗口的imu数据初始化陀螺仪零偏。

initByOdom()：接收odom的位姿和速度，根据odom时间戳以及imu缓存队列，将odom的位姿和速度积分到最新的imu时刻，积分结果作为初始位姿和速度。

Eskf框架——KfCoordinator类

将整个eskf框架封装在一个类里，内部维护状态管理器state_manager和系统状态的协方差，实现了eskf的基本操作，包括初始化、imu状态传播（预测），观测更新等。

KfCoordinator类：

重要成员变量：

imu_que：维护固定时间长度的imu队列，主要用于实现延迟更新。

state_manager：状态管理器，内部维护了系统状态池，kf_coordiantor通过该成员变量获取和操作系统状态信息。

state_cov: 系统状态协方差矩阵，MatrixXd类型，维度取决状态数量和各状态维数。

imu_noise_cov：imu测量的噪声矩阵，包含高斯白噪声和零偏随机游走噪声，由于imu的递推采用中值积分，因此该噪声矩阵维度是18。

obv_residual：观测残差向量，存储VectorXd类型的vector，容器内每个元素对应一个观测信息形成的观测残差。

obv_jocabian：观测雅克比矩阵，存储MatrixXd类型的vector，容器内每个元素存对应一个观测信息对应观测模型的雅克比矩阵，行数为观测残差维度，列数为系统状态维度。注意obv_residual和obv_jocabian里内的元素必须是一一对应关系。

obv_R：观测的噪声矩阵。

is_init：bool类型，是否完成kf的初始化。

重要成员方法：

insertStateWithCov()：插入非常量状态，需指定：该状态uid，状态类型枚举，状态初始值以及初始协方差。

insertConstState()：插入常量状态，不需要输入协方差，默认协方差为极小值。

init()：输入state_initializer的初始化结果，调用insertStateWithCov()或insertConstState()初始化系统状态。

imuStatePropagate()：imu状态递推实现，根据输入的imu信息传播imu状态和协方差，递推方法为中值积分。

processImuMeas()：将新的imu测量push到队列，并控制队列的长度，调用imuStatePropagate()完成imu状态递推。

getDelayUpdateInfo()：输入观测信息的观测时间，返回用于延迟更新的信息（延迟更新雅克比矩阵，延迟预积分结果）。

addResidualJacobian()：输入某次观测信息对应的观测残差、观测雅克比、id_size_pair以及该观测对应的延迟更新雅克比。输入的观测残差直接push到obv_residual；输入的观测雅克比是用vector存储的，每个元素对应该观测残差对某个系统状态的雅克比，通过id_size_pair指明该雅克比关联状态的start_idx和local_size，从而在addResidualJacobian()内部实现对雅克比矩阵的扩展（列扩展为系统状态总维度），最后将扩展后的观测雅克比乘以延迟更新雅克比，push到obv_jacobian中。

solveAndUpdate()：执行卡尔曼滤波的更新流程，计算卡尔曼因子k和后验误差状态，并更新系统（名义）状态和系统协方差。

updateWithOdom()：输入odom观测信息，首先通过getDelayUpdateInfo得到延迟更新信息，然后计算观测残差和观测雅克比并调用addResidualJacobian添加到obv_res和obv_jacobian中，设置本次观测的噪声矩阵，最后调用solveAndUpdate()实现kf更新。

updateWithZUPT()：输入imu观测，构建零速更新的残差项和雅克比，从而实现零速更新。

系统Pipeline——KfInterface类

将整个系统的pipline进行封装，实现顶层数据通信、数据同步以及各算法模块的调用。传感器或其他观测数据通过多态实现类型擦除，用一个queue进行统一管理，从而实现顺序更新。

SystemCtrl类

重要成员变量：

thrd_hdl：C++11线程句柄。

meas_queue：缓存队列，存储接收到的观测数据。

kf_coordinator：封装了eskf相关操作。

state_initializer：封装了kf初始化相关操作。

static_check：零速检测器，目前实现了基于imu滑窗的静止判定，后续会被vins零速检测替代。

pub_xxx / sub_xxx：ros发布和订阅句柄。

重要成员方法：

receiveImuTopic()：imu话题订阅回调函数，包装数据push到队列，通过条件变量唤醒处理线程。

receiveOdomTopic()：odom话题订阅回调函数，包装数据push到队列，通过条件变量唤醒处理线程。

publishxxx()：通过ros发布相关处理结果。

processMeasurements()：作为处理线程入口函数，当被唤醒时，处理观测数据(while遍历处理队列里的所有数据)，具体逻辑：

1.判定是否kf完成初始化，若没有初始化则调用state_initializer.initState()以及kf_coordiantor_.init()方法完成初始化。

2.若已完成初始化，如果当前测量数据是imu，则调用kf_coordiantor.imuStatePropagate()递推状态和协方差，并调用static_check.inputImu()检测是否检测到静止，若检测到静止则调用kf_coordiantor.updateWithZUPT()进行零速更新；若是odom或gps数据，调用相关update函数实现kf更新。

3.通过ros广播相关处理结果。

测试项：

1.检查陀螺仪零偏能否正常初始化，测试初始化逻辑是否正常，先完成零偏初始化，再完成状态初始化。

2.检查插入状态的信息是否预期，主要是start_index和local_size。

3.验证基于预计分链式求导的延迟更新法有效性：如果延迟更新有效，则两条轨迹应该表现出较明显的相移，且kf输出轨迹没有明显的突刺（有少量小突刺是合理的）。

延迟更新：有明显的相移

odom延迟可视化：

yaw轴融合效果：

无延迟更新：imu轨迹非常贴合odom结果，没有达到延迟消除效果。

4.测试融合vins后的位置和速度曲线是否正常且平滑。

问题记录：

1.kf直接发散，原因：存储观测残差和雅克比的vector没有在每次update后清空导致。

2.kf有概率发散，原因：外参估计发散导致。

3.锯齿状轨迹：可能原因：零偏估计误差大，roll和pitch估计误差。

静止时尤其明显：

锯齿问题分析：

1.vins的速度估计误差较大：

实际静止时，vins输出的速度不为0且反而愈发增大，kf输出的速度呈现锯齿状，在观测更新时刻融合后的速度反而下降了，是因为位置约束更强（观测噪声设置情况是位置噪声小于速度噪声），位置观测误差传播到速度上的权重更大，因此耦合后的速度会向0变化。但是在下一次观测更新前速度又呈现上升，说明是惯性系加速度误差较大导致速度积分误差大。

实际静止时，vins输出的位置变换幅度很小，即使存在较大的速度估计误差，但是vins内部有许多视觉观测约束项（对位置强约束），使得对位置的估计更准确，而速度估计只有imu预积分项直接约束（视觉观测约束对速度的约束是间接的，不强），静止时由于边缘化次新帧策略，vins滑窗的次新帧和最新帧之间的时间差会越来越大，预计分项误差也会越打越大，导致速度估计越来越差。因此在视觉惯性里程计中加入zupt是十分有必要的。对于kf，速度误差和零偏误差导致位置误差持续增大，但是有里程计的位置约束，因此出现这种锯齿的轨迹。

结论：位置锯齿的原因是速度误差大导致，速度误差的原因与惯性系加速度有关，惯性系加速度与姿态和零偏有关。

需要确定速度误差（静止时持续变换），是由于姿态误差导致还是零偏导致：（每次接收到imu进行积分时，打印以下值：原始加速度转到惯性系，零偏转到惯性系）。下面结果显示实际为零偏的大误差导致。

2.kf零偏估计异常：

加速度计的x，y轴零偏估计异常，值达到了±0.6左右，结合静止时的位置和速度变化，以x轴为例，当零偏为-0.6时，即使pitch和roll都是负的，原始加速度投影到惯性系后在x轴的值应该也是负的，但是减去-0.6的零偏后，投影到惯性系在x轴上的值直接大于0，导致速度呈增大趋势。

3.姿态估计误差影响速度：

问题：kf和vins谁输出的零偏姿态更准？

px4结果：

kf和vins：

vins输出的pitch和roll更接近px4结果，因此vins的roll和pitch更准。因此需要提高kf的姿态融合权重。

4.vins估计的零偏估计与kf估计的相近：

即使kf没有直接融合vins的零偏，但通过融合vins的速度和位置也会使得kf估计的零偏收敛于vins估计的零偏。

综上，得出以下结论：

位置锯齿由于速度误差导致，速度锯齿由于惯性系加速度误差导致，解决问题的出发点是：提高kf与vins的速度一致性和惯性系加速度一致性，根本是要提高惯性系加速度一致性。

**有效方案：**提高vins的速度和零偏估计精度，若vins零偏估计精度高，从上面的实验看出kf估计的零偏会收敛于vins零偏，因此kf估计的零偏精度也会随之提高。从而使得速度一致性好，同时误差也小。

因此需要提高vins速度和零偏估计精度，同时增大kf融合速度的权重。

5.考虑到短时间没法提高vins精度，通过减小vins速度观测协方差（更加相信vins速度），强行提高速度一致性，对比kf和vins输出位置和速度曲线：

提高速度融合权重后，发现新的现象：

提高速度融合权重之前：

提高速度融合权重之后：

发现提高速度和姿态融合权重之后，kf和vins零偏出现较大偏差，同时kf估计的姿态不再贴合vins的姿态，但两者的速度曲线基本贴合，进一步两者的速度微分即惯性系加速度也基本贴合；相同的加速度测量值，惯性系加速度取决于姿态（roll pitch）和零偏，两者耦合在一起，因此这里的现象是kf估计的零偏较大偏离了vins估计的零偏，但是为了具有与vins相同的惯性系加速度，kf的pitch和roll估计也与vins的估计出现了较明显偏置，相当于通过改变pitch和roll补偿了与vins的零偏偏差，从而使得两者的惯性系加速度相近，进而使得速度估计相近。

解决方法：进一步提高姿态融合权重，增大加速度零偏的初始化协方差。

之前：

现在：

相较于之前，位置锯齿明显减少，速度曲线仍然存在锯齿但整体的一致性有提升。

但是速度仍然存在的锯齿，速度不一致性的原因与姿态、加速度计零偏以及速度初值有关。

结论：提高速度融合权重有利于消除位置锯齿，速度一致性有小幅提升，但是静止时vins的速度误差大，导致kf在静止情况下的位置误差大。因此还是需要解决vins的速度估计误差的问题。

6.若vins速度和零偏估计精度无法有效提升，则不融合vins速度。

不融合速度实验：

若不融合速度，速度不一致性严重，位置出现频繁锯齿。因此必须融合速度。

不融合速度的情况下，可以更好的分析速度不一致的原因，如上图所示，可以看到kf和vins曲线的形状大体一致，造成两者没有贴合的原因主要有：速度初始值不同，速度导数也就是惯性系加速度不同，惯性系加速度与姿态、加速度测量以及零偏有关，加速度测量都是一样的，由于加了强姿态约束，因此姿态也基本一致，所以问题就出在零偏上，在上图中零偏大致差了0.6左右。

7.融合vins零偏实验：

融合vins零偏后，速度一致性显著提高。

8.vins在静止时由于较大的零偏估计误差，导致估计的速度不为0，进而导致融合vins速度和零偏的kf估计的速度不为零，导致位置和速度锯齿。解决方法：加入静止检测，成功检测到静止时加入角速度、惯性系加速度和速度约束项，以矫正姿态（roll和pitch）、速度和零偏。

静止检测复用初始化陀螺仪零偏的静止检测逻辑，效果：

静止时kf的速度和位置曲线：

kf的轨迹平滑效果：

rviz可视化：

融合动捕

动捕输出进行数据同步后输出动捕机体系的位姿速度，kf将动捕作为观测进行融合，并且会将动捕机体系与imu系之间的外参进行在线估计。

融合效果：

kf输出整体比较平滑，但在刚开始时出现kf位置与动捕位置分离较明显的情况：

发现是外参估计错误导致：

同时估计外参的旋转和位移：

不估计外参：

总结：

单点融合里程计的情况下，外参是不可观的，导致估计结果较差，影响融合后精度，因此不再进行外参在线标定。

融合动捕，零偏估计情况:

与之前融合vins估计的零偏在xy上正好差了个正负号，但是模长很接近。

imu系与外部里程计机体系外参标定

主要用于标定动捕定义的机体系与无人机imu系之间的外参，利用vins和动捕输出的里程计数据，采用手眼标定法对外参进行离线标定。

标定的基本逻辑是对输入的动捕和vins数据进行同步后，利用相邻帧的动捕和vins位姿构建线性方程组，分别求解外参旋转和位移。需要对相邻帧进行筛选从而保证方程组有解。

算法pipline设计：

1.维护两个队列分别缓存动捕里程计和vins里程计，程序一开始处于就绪状态，当接收到第一帧vins里程计后，判断能否从动捕里程计队列里插值得到vins里程计时刻同步的动捕位姿，若可以则进入数据收集状态，并将同步动捕位姿和vins位姿push到求解位姿队列。

2.数据收集状态下，每次从vins里程计队列pop出队首元素，首先判断该帧距离求解队列的最后一帧的旋转角度差和位移差，如果两者都大于阈值，则将该vins里程计与动捕位姿进行同步（线性插值），成功同步后push到求解队列。

3.数据收集状态下，超过3s没有push新的位姿到求解队列中，则会终止数据收集状态进入求解状态。

4.求解状态下，检查求解队列长度是否超过阈值，若没有则求解失败，程序终止。否则开始标定程序，基于手眼标定法，构建超定方程组先求解旋转外参，然后固定旋转外参，再构建方程求解位移外参。

测试：

同一个无人机（orin03），外参标定结果：

kf_calib_01.bag:

kf_calib_02.bag:

kf_calib_03.bag:

kf_calib_04.bag:

旋转外参估计结果的一致性较高，平移外参最多有1~2cm的偏差，从误差比例来看不是很理想。

原因分析：

手眼标定法的精度依赖于两个坐标系的全局运动轨迹（位姿）估计精度和标定时运动是否充分，目前程序的设计保证了用于求解外参的运动是充分的，导致标定误差的主要原因是vins的误差，初步估计vins在10cm相对位移的估计误差在2~~3cm，因此外参的估计精度在1~~2cm也合理，建议多标定几次（录制不同的数据），对平移外参取平均作为最终标定结果。

vins的旋转估计精度较高，因此旋转外参估计精度的比较可靠。

ToF对地高约束

为解决vins在z轴累计误差显著的问题，利用tof传感器测量对地高并作为观测被kf_fusion融合。

设计流水：

完成初步融合功能，目前存在问题：
1.tof测量的对地高噪声明显，会出现尖刺：

后续改善：

1.考虑到无人机高度不会发生突变，对tof测量的对地高进行跳变检测，对测量值计算导数，如果导数大于一定阈值则跳过此次融合。

2.对地高假设地面平坦，如果出现凸起或凹陷会导致对地高测量误差。解决方法复用第一点中的导数检测。

3.目前tof融合频率过高（50hz），为降低测量噪声的影响，需降低融合tof频率（5hz）。（设计一个tof质量检测器，只有当tof测量平滑且距离上一次融合时间大于阈值时才进行后续融合）

4.里程计世界系原点是Imu初始位置，而对地高实际测量的是真实水平地面的高度，因此测量的对地高需要减去一个固定偏置，偏置由实际测量给出。

改善后：