1.组合导航卡尔曼滤波噪声协方差矩阵调参

在组合导航卡尔曼滤波算法中，主要涉及两个噪声协方差矩阵，过程噪声协方差矩阵Q，测量噪声协方差矩阵R，具体来说：

R表示测量噪声协方差，它是一个数值，这是和仪器或设备比如IMU直接相关的一个特性，作为已知条件输入滤波器。需要注意的是这个值过大过小都会使滤波效果变差，且R取值越小收敛越快，所以可以通过实验手段并辅之以适当的数学建模寻找合适的R值再利用它进行真实的滤波。

Q表示过程激励噪声的协方差，它是状态转移矩阵与实际过程之间的误差。这个矩阵是卡尔曼滤波中比较难确定的一个量，一般有两种思路：

一是在某些稳定的过程可以假定它是固定的矩阵，通过寻找最优的Q值使滤波器获得更好的性能，这是调整滤波器参数的主要手段，Q一般是对角阵，且对角线上的值很小，便于快速收敛；

二是在自适应卡尔曼滤波（AKF）中Q矩阵是随时间变化的。

实际操作过程中，要有如下几种方法：
a. 通过数学建模，比如白噪声和随机游走等确定IMU传感器噪声模型后，先通过GPS和IMU的性能参数和频率确定一个米级单位下的噪声。之后在该噪声参数下得到融合的轨迹，然后分别对两个噪声增大缩小分成几组进行调节，观察轨迹。最终选最平滑的一组轨迹的噪声参数，或者选跟真值比精度最高的一组的噪声参数。

b.对于测量噪声，除了采用调试法，还可以采用CNN学习的自适应调节，具体可参考AI-IMU Dead-Reckoning

2. 组合导航卡尔曼滤波噪声来源

（1）量化噪声
量化噪声是数字传感器必然出现的噪声，我们通过AD采集把连续时间信号采集成离散信号，在这个过程中，精度就会损失，损失的精度大小和AD采样的精度有关（这里具体指的是模数转换时，AD器件的位数，位数越高采样越精确），精度越高，量化噪声越小。
（2）角度随机游走
陀螺敏感角速率并输出时是有噪声的，这个噪声里面的白噪声成分叫宽带角速率白噪声，我们计算姿态时，本质上是对角速率做积分，这必然会对噪声也做了积分。白噪声的积分并不是白噪声，而是一个马尔可夫过程，即这一次的误差是在上一次误差的基础上累加一个随机白噪声得到的。角度误差所包含的这种马尔可夫性质的误差就叫做角度随机游走。
（3）角速率随机游走
从理解上和角度随机游走一样，角速率里面并不全是白噪声，它也有马尔可夫性质的误差成分，而这个误差是由宽带角加速率白噪声累积的结果。
（4）零偏不稳定性噪声
这应该是大家再熟悉不过的一个误差项了，如果一个陀螺只让你用一个指标来体现精度，那必然就是它了。但是这个指标的理解上却不像前几个参数那样直白。
我们可以先把它理解为零偏随时间的缓慢变化，假设在刚开始时零偏大小是某个值，那么过一段时间之后，零偏便发生了变化，具体变化成了多少，无法预估，所以就要给他一个概率区间，来描述它有多大的可能性落在这个区间内，时间越长，区间越大。
实际上，如果你真的测的时间足够长，会发现它也不会无限制增长下去，所以，这个对概率区间的描述只是近似有效，或者一定时间内有效，由于这个有效时间比较长，所以我们一般仍然使用这种方式来描述，只是在理解上要知道这一点的存在。
（5）速率斜坡
看到斜坡这种描述词，我们一般会想它是不是一种趋势项。实际上，它确实是趋势性误差，而不是随机误差。所谓随机误差，是指你无法用确定性模型去拟合并消除它，最多只能用概率模型去描述它，这样得到的预测结果也是概率性质的。而趋势性误差是可以直接拟合消除的，在陀螺里，这种误差最常见的原因是温度引起零位变化，可以通过温补来消除。
加速度计同样具有这5项误差，而且原理一致，因此不再赘述。

3.GNSS与IMU的时间同步

GNSS与IMU同步主要需解决两个传感器在统一的时间线下的采样时间。系统处理NMEA信号有几十毫秒的延时，需要进行精确同步。对于GNSS可以从卫星获得高精度的时钟信号，而且通常的GNSS都支持PPS脉冲以及GPRMC信号。
（1）使用GNSS作为时钟源，将GNSS的PPS信号提供给LiDAR和一个开发板，开发板将给相机同时提供一个曝光的脉冲信号。CAMVOX采用这种方案。
（2）使用外部时钟源，这种时钟源通常支持PPS信号输入，将GNSS的PPS传给外部时钟源，同时外部时钟源可以使用PTP/NTP/PPS给LiDAR做时间同步，同时触发相机开始曝光。外部时钟源同时也可以使用PTP/NTP对主机进行时间同步。

实际的INS/GNSS组合导航中同步过程主要依赖GNSS模块输出的PPS秒脉冲信号(上述第一种)。

对于其他传感器，比如激光雷达或相机与IMU/GNSS的多传感器融合，

a. 激光雷达：大多数雷达如VLP-16等都提供基于pps脉冲和GPRMC信号的输入接口,PPS和GPRMC信号可以由GNSS或IMU提供，或者由外部时钟源提供。少数激光雷达还支持NTP/PTP同步，PTP的精度一般来说比NTP要高，这两个信号都需要由外部时钟源设备提供。
b. 相机：需要支持外部触发曝光的型号，因为相机帧周期包括曝光时间和readout时间（整帧像素点读出），一般来说readout时间是固定的，可以补偿这个时间，相机的时间戳选择为曝光的中间时间。具体可参考武汉大学多源智能导航实验室讲义。

4.IMU与GNSS延时问题

GNSS信号到来时是有延时的，而IMU给出的加速度和角速度是实时的，这种情况下如何处理延时实现有效融合呢：
可以先通过IMU积分计算实时的轨迹，同时把IMU数据缓存下来，当GNSS信号到来时，再根据GNSS的时间戳去修正历史时刻的数据，然后重新积分该时刻后的IMU。

5.卡尔曼滤波发散控制

状态方程描述的动力学模型不准确，或者噪声的统计模型不准确，这样会使数学模型和量测值不匹配而导致发散。通常来说，模型不准确的情况很少，除非原理上错误。多数由于模型太过粗糙而导致。
递推过程计算机摄入误差累积，使得协方差阵失去正定性或者失去对称性，导致增益计算失去加权效果，从而导致滤波器发散。
先说模型不准确导致的发散处理技术，累积误差导致发散的处理后面有机会再提。
首先需要再次明确的是，卡尔曼滤波器的本质是线性最小方差估计，它虽然是递推形式的，但是每一步递推获得的估计值其实是根据当前时刻之前的所有量测值而获得的最优估计结果。也就是说它实际是有记忆的。
而量测值其实是完全不会发散的东西，因为它会始终在它的数学期望周围分散，我们的思路就是，在滤波器中，我们应该更加相信新鲜的量测值，而减小对陈旧量测值的信任程度，也就是增大新鲜量测值的权重，减少陈旧量测值的权重。

根据上述分析，我们可以重新设计卡尔曼滤波器。提出的发散控制技术包括在KF过程模型中加入虚拟噪声、Schmidt epsilon技术、有限记忆滤波和渐消记忆滤波。

5.1 KF过程模型中添加虚拟噪声

为了使一个真实系统的数学模型易于处理，必须进行许多简化和假设。此外,大多数系统是非线性的，在使用KF之前必须经过线性化，而这种线性化的方法相当于对系统模型又做了进一步的近似。这一系列不确定因素，可能会造成KF性能下降，甚至发散。
针对这种情况，可以通过增加过程噪声协方差Q 或者测量噪声协方差R的方式引入人工噪声。因此，在实际应用中，Q和R的参数设定一般是基于之前经验所得，并通过调整，以优化系统的性能。

5.2 Schmidt Epsilon方法

在KF中控制发散的一种方法是为估计误差的协方差矩阵定义一个阈值，使它不低于某个值。在卡尔曼滤波运行过程中，协方差矩阵Pk(-)不断变小，卡尔曼增益K也不断减小，从公式中可以明显看出，当Pk(-)很小时，K趋近0，新的测量值将无法得到足够的权重，滤波器的输出将主要取决于预测值。这一现象被称为Kalman filter incest 。
在协方差阈值化中，通过给计算得到的误差协方差矩阵Pk(-)加上一个符号为正的微小量，就可以确保卡尔曼增益存在一个下边界。这个小量值是根据经验确定：

5.3 有限记忆滤波

在kalman滤波中，最新的估计值Xk(+)是基于从起始时刻到 k 时刻的所有测量值。为了得到最优估计值，滤波器必须考虑到之前的所有测量值。
但是，旧的数据有时不仅无法提供有效信息，反而会导致滤波器出现问题。
因此，在渐消记忆滤波方法中仅使用最近的测量数据，旧的信息将被丢弃。
例如，仅使用测量的最后100个样本进行滤波，而舍弃更早之前的样本。