An IMU/ODM/UWB Joint Localization System Based on Modified Cubature Kalman Filtering

Tang, Chao; He, Chengyang; Dou, Lihua

doi:10.3390/s21144823

开放式访问第条

基于改进Cubature Kalman滤波的IMU/ODM/UWB联合定位系统

通过

赵堂

^1,2,

何成阳

^1,2和

李华斗

^1,2,*

¹

北京理工大学自动化学院，北京100081

²

北京理工大学重庆创新中心，中国重庆401135

^*

信件应寄给的作者。

传感器 2021,21(14), 4823;https://doi.org/10.3390/s21144823

收到日期：2021年5月25日/修订日期：2021年7月7日/接受日期：2021年7月12日/发布日期：2021年7月15日

（本条属于本节物理传感器)

下载

浏览地物

版本注释

摘要

:

本文提出了一种基于改进容积卡尔曼滤波的多传感器联合定位系统，目的是在高过程噪声条件下，在适度的计算负担下提高状态估计的精度。具体来说，首先，基于自适应滤波匹配过程噪声的协方差。然后利用相关传感器获取的惯性测量单元（IMU）、里程表（ODM）和超宽带（UWB）信息来增强系统状态，并进行融合以降低过程噪声的影响。在所提出的定位设置中，所有传感器（IMU/ODM/UWB）在联邦卡尔曼滤波器（FKF）框架下并行工作，这可以纠正内部传感器的累积误差，并可以提高计算效率。通过两组数值模拟表明，该方法可以在略微增加计算量的情况下获得准确的状态估计。

关键词：

联合定位;多传感器;CKF公司;FKF公司;信息融合

1.简介

随着社会和经济的发展，随着智能产品的使用，对精确定位的需求不断增加。全球定位系统（GPS）作为目前应用最广泛的方法，因其技术成熟、精度高、鲁棒性强而备受青睐[1]它已广泛应用于导航、智能驾驶等领域。然而，它无法在大多数室内场景中提供有效定位，例如家庭服务、工厂物流、洞穴探险和室内反恐活动。在这种背景下，开发高精度的室内定位技术具有迫切的重要性。

作为GPS的替代方案，遥感器已被广泛用于室内定位，近年来计算设备和网络的快速发展有力地支持了遥感器。有多种基于遥感器的室内定位技术，包括WiFi[2,三]，淄博[4]，红外[5,6]、VICON[7]和超宽带（UWB）[8,9,10]. 具体来说，WiFi定位部署简单，成本低，但由于易受干扰，无法实现高精度定位，因此备受批评。相比之下，ZIGBEE具有能耗低的优点，同时也存在定位精度不足的缺点。红外定位具有提供高精度定位信息的能力，由于其可靠性，通常无法定位隐藏目标。相比之下，VICON技术具有较高的定位精度和抗干扰性能，除成本高、复杂度高外，可视为理想的室内定位解决方案。在这种情况下，超宽带定位由于其成本低、体积小、定位精度高、抗多径效应强等优点，似乎比其他同类定位更具优势，所有这些都有助于其在多智能体系统中的广泛应用。然而，在需要姿态控制的多机器人协作系统中，外部传感器通常无法提供与位置信息同等重要的准确姿态信息。在这种情况下，内部传感器，如惯性导航系统[11]，用于提供系统中机器人的姿态信息。具体来说，由于其技术成熟、可靠性高，惯性导航系统能够在不依赖外部信息的情况下测量机器人的姿态。因此，它在导航过程中不会受到任何外部干扰。然而，该系统容易受到误差累积的影响，并且无法对累积误差进行自我校准。作为一种对策，集成了内部和外部传感器来估计机器人的姿态和位置[12,13,14]. 在这些系统中，准确的姿态和位置信息通常由安装在物体内部的传感器（内部传感器）获得，环境中的外部传感器用于进一步校准此信息。

传感器之间的信息融合成为多传感器系统中不可避免的问题。目前信息融合主要有两种方法，其算法分别基于图优化[15]和过滤[16,17]. 具体来说，基于图优化的信息融合具有较高的准确性，但其计算的高度复杂阻碍了其在实时性要求较高的环境中的应用。相比之下，基于联邦卡尔曼滤波器（FKF）的信息融合以其结构灵活、鲁棒性强、精度好、易于工程实现等优点在多传感器定位系统中得到了广泛应用[18,19].

近年来，多传感器联合定位领域取得了巨大进展。例如，Li等人[20]结合超宽带提供的位置信息和惯性测量单元（IMU）提供的方向定位信息，对煤矿机器人（CMR）进行井下定位，并讨论了超宽带基站的优化部署方案。最终的定位精度接近最先进的激光雷达。Giarre等人[21]使用UWB校正行人的航位推算结果。基于此联合定位方法，提高了真实环境中行人的定位精度。Hu等人[22]通过使用三个系统（即GPS、IMU和天体导航系统（CNS））导航航天器，将研究中的姿态和位置信息解耦，成功降低了联合定位系统的计算负荷。Han等人[23]通过使用UWB定位校准和补偿GPS信号，以及使用基于蚁群优化算法的改进粒子滤波融合GPS和UWB位置信息，成功地将UWB节点数从三个减少到一个。使用IMU传感器补充UWB信息，Li等人[24]通过使用加速度计测量系统速度中的高斯噪声，利用状态增强成功地提高了超宽带定位系统处理过程噪声的能力。He等人[25]通过在联合卡尔曼滤波（FKF）框架中融合UWB和IMU传感器获取的数据，将漂移误差的影响降至最低。

尽管上述研究取得了突破性进展，但也存在固有的局限性。例如，He等人提出的算法[25]可以有效抑制IMU数据的累积误差，具有较高的定位精度。然而，在高过程噪声的情况下，它很容易出现滤波发散，从而会恶化跟踪性能。相反，Li等人提出的算法[24]能够有效地处理系统的过程噪声，从而具有较高的跟踪性能。然而，位置信息是由外部传感器独立提供的，并且该算法基于具有较大线性化误差的扩展卡尔曼滤波（EKF）。因此，需要提高其定位精度。

作为我们之前工作的后续研究[25]发现该算法未能有效抑制过程噪声。因此，本文从以下四个方面进行了相关研究，以追求定位过程中的鲁棒跟踪、高精度和高实时性能：

为了提高He等人提出的算法的性能，引入了自适应滤波[25]处理过程噪声。
Li等人提出的扩展方法[24]通过对模型的扩展，提高了算法的跟踪性能，进一步增强了算法处理过程噪声的能力。
鉴于扩展方法导致系统维数增加[22]用于分解系统状态，从而尽可能减少扩展方法的副作用。

经过这些改进，获得了一种高鲁棒性和高精度的状态估计算法，尽管它略微增加了计算负担。这是这项工作的主要贡献。

算法的总体结构如所示图1.

本文的其余部分组织如下。在第2节建立了系统的动态模型和测量模型，并分析了上述算法对过程噪声的滤波效果较差[25].第3节引入自适应卡尔曼滤波（AKF）并改进了[25]基于这种滤波方法，通过详细的步骤说明了算法的改进。在第4节，中提出的状态向量增强方法[24]介绍了该方法，并基于该方法描述了增强状态向量。此外，基于Hu等人的方法对状态向量进行了分解[22]保持系统矩阵的维数，从而避免过多的计算负载增加，同时提高系统处理过程噪声的能力。在第5节通过对运动系统的数值仿真验证了该算法的有效性。最终得出结论第6节.

2.系统建立和问题描述

2.1. 系统模型

本文考虑的非线性离散时间系统是

{X（X）}_{k + 1} = A类 ({X（X）}_{k}) + ω_{k}

(1)

{X（X）}_{k} = {[\begin{matrix} x个 & 年 & v（v） & θ & φ \end{matrix}]}^{吨}

(2)

哪里x个和年是移动对象的坐标，v（v）是移动物体的速度，

θ

是移动对象的偏航角，

φ

是横摆角随时间的变化，

ω_{k}

是系统的过程噪声，以及

A类 (.)

是一个非线性时间更新。作为一个恒定转弯率和速度（CTRV）模型，本文研究的运动模型可以用以下公式表示：

A类 ({X（X）}_{k}) = A类 ([\begin{matrix} x个 \\ 年 \\ v（v） \\ θ \\ φ \end{matrix}]) = [\begin{matrix} x个 \\ 年 \\ v（v） \\ θ \\ φ \end{matrix}] + [\begin{matrix} \frac{v（v）}{φ} (秒 我 n个 (θ + φ 吨) - 秒 我 n个 (θ) \\ \frac{v（v）}{φ} (- c（c） o个 秒 (θ + φ 吨) + c（c） o个 秒 (θ) \\ 0 \\ φ 吨 \\ 0 \end{matrix}]

(3)

哪里吨是采样时间。系统的测量模型如下：

{Z轴}_{k + 1}^{U型 W公司 B} = {H（H）}_{U型 W公司 B} {X（X）}_{k + 1} + υ_{k + 1}^{u个 w个 b条} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{u个 w个 b条}

(4)

{Z轴}_{k + 1}^{我 M（M） U型} = {H（H）}_{我 M（M） U型} {X（X）}_{k + 1} + υ_{k + 1}^{我 米 u个} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{我 米 u个}

(5)

哪里

{Z轴}^{我 M（M） U型}

和

{Z轴}^{U型 W公司 B}

分别是IMU和UWB系统的测量矢量；

{H（H）}_{我 M（M） U型}

和

{H（H）}_{U型 W公司 B}

分别是它们的状态转移矩阵；和

ν^{我 米 u个}

和

ν^{u个 w个 b条}

分别是高斯分布的独立测量噪声。测量噪声和过程噪声的协方差为

\begin{matrix} E类 [{w个}_{k} {w个}_{k}^{吨}] = 问_{k} \\ E类 [υ_{k}^{我 米 u个} υ {_{k}^{我 米 u个}}^{吨}] = {R（右）}_{k}^{我 米 u个} \\ E类 [υ_{k}^{u个 w个 b条} υ {_{k}^{u个 w个 b条}}^{吨}] = {R（右）}_{k}^{u个 w个 b条} \end{matrix}

(6)

哪里

问_{k}

是过程噪声的协方差，以及

{R（右）}_{k}^{我 米 u个}

和

{R（右）}_{k}^{u个 w个 b条}

分别是IMU和UWB传感器测量噪声的协方差。

2.2. 工艺噪声分析

为了将算法移植到物理平台上，我们使用不同的参数进行了几次数值模拟实验，以收集先前提出的奇异值分解-联邦导出立方卡尔曼滤波器（SVD-FDCKF）算法的数据[25]. 当过程噪声较大时，仿真中会出现滤波发散现象。此外，现有的滤波算法通常会因过程噪声超过一定值而失效。基于这一现象提出了一些假设。

首先，过程噪声的协方差(问)与过程噪声的大小成比例，并且很难确定问在实际应用中。因此问测量噪声的协方差(R（右）)在某些情况下，如果问无法根据过程噪声的大小自适应调整。因此，在滤波结果中，在运动过程中可能存在较大的过程噪声，从而导致滤波发散。

其次，由于过程噪声对系统速度的影响，系统可能不再处于匀速运动状态。当实验环境中的过程噪声较小时，由于测量的残差较小，迭代过程中的测量协方差可能在合理的范围内。在这种情况下，滤波算法可以有效地抑制噪声。然而，当实验环境中的过程噪声设置为较大时，测量的残余误差会增加，因此测量的协方差会随着迭代而增加，从而使滤波器倾向于为状态估计分配更高的权重。通过这种方式，基于CTRV模型的状态估计不再准确，因为由于噪声的影响，系统不再以恒定速度移动。因此，滤波发散变得不可避免。

鉴于上述问题，有必要改进现有的滤波算法，从而提高跟踪性能。

2.3. 问题公式化

幸运的是，这两个问题已有相关研究[24,26,27]. 圣人与胡萨[26]在研究噪声的不确定性协方差时，提出了基于卡尔曼滤波的自适应滤波。它们成功地抑制了不准确的噪声方差导致的滤波器发散。谈到实际应用，Kownacki等人[27]采用自适应滤波对红外测距仪采集到的受未知外界干扰影响的数据进行滤波；因此，很难确定R（右）通过这种方式，他们设法提高了测距和定位的精度。也可以使用类似的方法来抑制问和R（右）由于工艺噪声大，提高了滤波器的精度，从而避免了工艺噪声过大导致的滤波发散。

为了提高滤波算法对高动态运动目标的跟踪性能，Li等人[28]使用状态向量增强方法将CTRV模型更改为CTRA模型。他们将状态矢量中速度的过程噪声视为加速度，并使用IMU的加速度计对其进行测量和滤波。实验结果表明，改进的算法能够有效抑制过程噪声，从而在跟踪移动机器人方面表现出更好的性能。

然而，在Li等人的方法中，位置信息的测量值[24]完全由超宽带传感器获得，只能提供分米级定位精度。因此，虽然状态矢量增强方法具有优异的跟踪性能，但由于状态矢量维数的增加，状态矢量增强算法也会对系统的稳定性和计算负担产生一定的负面影响。

总之，需要解决某些科学问题：首先，应该通过抑制过程噪声引起的滤波发散来提高算法的跟踪性能。其次，在实现上述改进的同时，应保持状态向量的维数。

具体解决方案可分为以下步骤：

我们以前的工作[25]应基于自适应滤波的思想进行改进，使问和R（右）仍然可以在高过程噪声环境中进行协调，以避免滤波发散。
应使用状态向量增强方法扩展模型以增强系统的可追溯性[24].
增强状态矢量应根据每个传感器的测量矢量进行分解[28]以减小系统的尺寸，从而提高系统的测量精度。

3.自适应SVD-DCKF滤波

3.1. 前期工作

如所示第2.1节系统具有非线性系统状态方程和线性测量方程。在这种情况下，如果使用无迹卡尔曼滤波（UKF）和立方卡尔曼滤波法（CKF）以及其他非线性卡尔曼滤波算法进行滤波，则会出现冗余计算，并且由于线性化误差，精度会降低[29]，我们提出了导出立方卡尔曼滤波器（DCKF），它减少了系统的计算负担，并消除了一些不必要的线性化误差。

此外，受文献中随机稳定性分析的启发[30]，我们进一步开发了SVD-DCKF算法[25]基于DCKF算法。该SVD-DCKF算法可以在分解系统估计协方差时用SVD分解代替传统的Cholesky分解(P（P）). 这样，约束条件从正定方差扩展到正定半定，使得算法不再需要正定辅助矩阵(

δ P（P）

)从而消除了由辅助矩阵引起的微小误差。文献中详细描述了SVD-DCKF算法的具体步骤[25]因此这里不再重复。

此外，为了抑制IMU的累积误差，我们采用UWB在FKF框架下连续校准IMU[25]通过数值模拟进行了验证。

然而，SVD-DCKF算法在不同过程和测量噪声的特定条件下，滤波性能恶化甚至发散。因此，本研究旨在改进基于自适应滤波方法的SVD-DCKF算法。

3.2. 自适应滤波

这个问和R（右）对卡尔曼滤波算法的性能有直接影响[27]. 根据卡尔曼增益的计算公式(K（K）),K（K）与…成比例问与…成反比R（右）因此，当问则过程噪声对状态预测具有很大的干扰，因此，K（K）这将增加滤波器中测量方程的权重，从而用更多的测量值来修改状态估计。在这种情况下问可能是在应用中提出的算法时出现的滤波发散的原因[25]在高工艺噪音环境中。然而，过大的问会降低过滤精度[26]. 此外，通常很难准确模拟系统噪声，以确定问在实际应用中。因此，通过设置问根据先验值直接在初始时刻。因此，通常使用自适应滤波技术来自适应修改问.

考虑到中所示的系统第2.1节，剩余误差(

{e（电子）}_{k}

)应在应用自适应滤波期间首先计算：

{e（电子）}_{k} = {X（X）}_{k} - {X（X）}_{k | k - 1}

(7)

哪里

{X（X）}_{k}

是系统在某个时间的状态估计k，同时

{X（X）}_{k | k - 1}

是对系统状态的一步预测k基于当时的系统状态

k - 1

.

过程噪声协方差在时间上的校正矩阵k(

问_{k}^{*}

)然后应根据系统的剩余误差进行计算：

问_{k}^{*} = {e（电子）}_{k} {e（电子）}_{k}^{吨} + {P（P）}_{k | k - 1} - {P（P）}_{k} - 问_{k - 1}

(8)

哪里

{P（P）}_{k | k - 1}

是对系统误差方差的一步预测k基于系统误差方差

k - 1

，同时

问_{k - 1}

是当时的过程噪声协方差估计

k - 1

.

随后，应使用低通滤波器修改矩阵

问_{k}^{*}

和噪声协方差

问_{k - 1}

在前一时刻，生成时间的过程噪声协方差估计k(

问_{k}

):

问_{k} = 问_{k - 1} + \frac{(问_{k}^{*} - 问_{k - 1})}{M（M）}

(9)

哪里M（M）是窗口宽度，其值可通过以下公式获得：

\{\begin{matrix} M（M） = 1, & d日 \geq 1 \\ M（M） = k, & d日 \leq 0 \\ M（M） = k d日, & 0 < d日 < 1 \end{matrix}

(10)

哪里d日可通过残值公式得出：

d日 = {e（电子）}_{k}^{吨} E类 [{e（电子）}_{k} {e（电子）}_{k}^{吨}] {e（电子）}_{k}

(11)

什么时候？

d日 > 1

过程噪声的统计特征不明确，系统无法得到正确的状态预测，应采用最小值。什么时候？

d日 < 0

，过程噪声的统计特征是明确的，因此系统状态方程的预测是相对准确的，应采用最大值。什么时候？

0 < d日 < 1

，应根据比率采用中间值，以确保问处于正确的状态。

3.3. 基于自适应滤波的SVD-ADCKF算法

中提出的SVD-DCKF算法[25]基于上一节中描述的自适应滤波方法进行了改进。它能够自适应调整问在不同的过程噪声环境下，该改进算法被称为奇异值分解自适应导数立方卡尔曼滤波（SVD-ADCKF）。算法的具体步骤如算法1所示。从算法1可以看出，所提出的算法基本上与原始CKF相似，但增加了过程噪声协方差的时间更新步骤，这使其能够随着迭代次数的增加自适应地接近真实值。

算法1。SVD-ADCKF的具体步骤。

输入：

{X（X）}_{k}

,

{P（P）}_{k}

,

问_{k}

输出：

{X（X）}_{k + 1}

,

{P（P）}_{k + 1}

,

问_{k + 1}

非线性时间更新

1.初始化状态估计

{X（X）}_{k}

和协方差估计

{P（P）}_{k}

;

2.对执行SVD分解

{P（P）}_{k}

以获得

{S公司}_{k}

;

3.计算容积点

{X（X）}_{k}^{j个}

根据方程式：

\{\begin{matrix} {X（X）}_{k}^{j个} = {X（X）}_{k} + {S公司}_{k} 我_{j个} \sqrt{n个} & , j个 = 1, \dots, n个 \\ {X（X）}_{k}^{j个} = {X（X）}_{k} - {S公司}_{k} 我_{j个} \sqrt{n个} & , j个 = n个 + 1, \dots, 2 n个 \end{matrix}

;

4.计算每个容积点的一步预测(

{X（X）}_{k + 1 | k}^{j个}

)基于状态方程和

过程噪声协方差(

问_{k}

);

5.合并每个体积点以获得线性近似时间更新

{X（X）}_{k + 1 | k}

:

{X（X）}_{k + 1 | k} = \sum_{j个 = 1}^{2 n个} ω_{j个} {X（X）}_{k + 1 | k}^{j个}

四边形，其中n个是系统顺序；

ω_{j个}

是每个容积点的重量；

6.计算线性近似误差协方差的时间更新

{P（P）}_{k + 1 | k}

:

{P（P）}_{k + 1 | k} = \sum_{j个 = 1}^{2 n个} ω_{j个} ({X（X）}_{k + 1 | k}^{} - {X（X）}_{k + 1 | k}^{}) {({X（X）}_{k + 1 | k}^{} - {X（X）}_{k + 1 | k}^{})}^{吨};

线性测量更新

7.获得测量的线性预测值(

{Z轴}_{k + 1 | k}

)基于的时间更新

线性近似的测量方程；

8.计算卡尔曼增益K（K）根据公式：

K（K） = {P（P）}_{k + 1 | k} {H（H）}^{吨} {(H（H） {P（P）}_{k + 1 | k} {H（H）}^{吨} + {R（右）}_{k})}^{- 1}

;

9.根据卡尔曼增益更新系统状态：

{X（X）}_{k + 1} = {X（X）}_{k + 1 | k} + K（K） ({Z轴}_{k + 1} - {Z轴}_{k + 1 | k})

;

10.根据卡尔曼增益更新系统误差协方差：

{P（P）}_{k + 1} = {P（P）}_{k + 1 | k} - K（K） H（H） {P（P）}_{k + 1 | k}

;

过程噪声协方差更新

11.计算剩余误差：

{e（电子）}_{k + 1} = {X（X）}_{k + 1} - {X（X）}_{k + 1 | k}

;

12.计算校正矩阵：

问_{k + 1}^{*} = {e（电子）}_{k + 1} {e（电子）}_{k + 1}^{吨} + {P（P）}_{k + 1 | k} - {P（P）}_{k + 1} - 问_{k}

;

13.根据公式（10）和（11）计算窗宽M；

14.校正过程噪声协方差：

问_{k + 1} = 问_{k} + \frac{(问_{k + 1}^{*} - 问_{k})}{M（M）}

;

15.输出

{X（X）}_{k + 1}

,

{P（P）}_{k + 1}

,

问_{k + 1}

作为下一时刻的初始值，并重复步骤1-14。

结束.

4.跟踪性能的改进

4.1. 状态向量增强后的系统模型

在李等人的工作中[24]，将系统速度的噪声视为系统的加速度，通过状态向量增强将X轴和Y轴方向的加速度引入系统的状态向量，然后由IMU传感器测量。随后，他们根据系统状态的估计和传感器的测量值对增强的系统状态进行了滤波，结果证明了跟踪性能的提高。

由方程式定义的系统状态向量(2)可以相应地重写为

{X（X）}_{k} = {[\begin{matrix} x个 & 年 & v（v） & 一 & θ & φ \end{matrix}]}^{吨}

(12)

哪里一是系统的加速度。

同时，非线性离散系统的时间更新方程如方程所示(三)可以定义为

A类 ({X（X）}_{k}) = A类 ([\begin{matrix} x个 \\ 年 \\ v（v） \\ 一 \\ θ \\ φ \end{matrix}]) = [\begin{matrix} x个 \\ 年 \\ v（v） \\ 一 \\ θ \\ φ \end{matrix}] + [\begin{matrix} Δ x个 \\ Δ 年 \\ 一 吨 \\ 0 \\ φ 吨 \\ 0 \end{matrix}]

(13)

哪里吨是采样时间，而

Δ x个

和

Δ 年

是x个和年方向，分别表示为

Δ x个 = \frac{(v（v） + 一 吨) 罪 (θ + φ 吨) - v（v） 罪 (θ)}{φ} + \frac{一}{φ^{2}} [余弦 (θ + φ 吨) - 余弦 (θ)]

(14)

Δ 年 = \frac{(v（v） + 一 吨) 余弦 (θ + φ 吨) - v（v） 余弦 (θ)}{φ} + \frac{一}{φ^{2}} [罪 (θ + φ 吨) - 罪 (θ)]

(15)

新的六阶CTRA系统中使用的测量模型定义为

{Z轴}_{k + 1}^{U型 W公司 B} = {H（H）}_{U型 W公司 B} {X（X）}_{k + 1} + υ_{k + 1}^{u个 w个 b条} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{u个 w个 b条}

(16)

{Z轴}_{k + 1}^{我 M（M） U型} = {H（H）}_{我 M（M） U型} {X（X）}_{k + 1} + υ_{k + 1}^{我 米 u个} = [\begin{matrix} 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{我 米 u个}

(17)

此外，在改进后的系统中引入里程表（ODM）作为辅助内部传感器，其测量模型可以表示为

{Z轴}_{k + 1}^{ODM公司} = {H（H）}_{O（运行） D类 M（M）} {X（X）}_{k + 1} + υ_{k + 1}^{o个 d日 米} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{o个 d日 米}

(18)

4.2. 增强系统模型的准确性

由于IMU传感器的航位推算是基于加速度计记录的数据的二次积分，因此在增强系统模型中不可避免地存在较大的累积误差。作为一种对策，改进系统中的IMU传感器现在仅用于测量移动机器人的速度、加速度、偏航角和偏航角运动变化，而系统的位置信息由里程表提供，里程表根据车轮的转数直接计算航位推算。在此背景下，系统的加速度、偏航角和偏航角运动变化分别由加速度计、磁强计和陀螺仪直接测量，减少了累积误差。IMU中的加速计测量物体接收到的重力以外的比力，将其计算为加速度，最后输出加速度。加速度计对系统速度的积分计算和里程表对位置信息的积分计算仍存在一定的累积误差。然而，这些累积误差相对较小第2.1节，这需要二次积分。此外，外部超宽带传感器提供的系统位置和速度信息用于校准内部传感器的累积误差。

4.3. 状态向量的简化

虽然状态向量增强方法在使用时可以有效地提高系统的跟踪性能并抑制过程噪声，但状态向量的维数会相应增加。在这种情况下，状态向量维数的增加伴随着UKF和CKF算法计算负担的增加，尽管计算精度和稳定性很少受到影响[28,31]. 在本研究中，状态向量分解方法[22]和状态矢量增强方法[24]集成以最小化状态向量增强方法所造成的副作用。

在一些集成IMU/CNS系统中，CNS可以输出准确的姿态信息和相对准确的位置信息，但不能进行高精度的速度信息计算[22]. 因此，与估计速度误差相对应的系统误差协方差矩阵中的误差不断增大。因此，可以推断，在集成IMU/CNS系统的子系统输出的局部状态估计中，姿态、速度和位置误差信息之间的相关性极弱[22]. 鉴于此，Hu等人[22]在IMU/GPS/CNS组合导航系统中，将状态向量分解为三个子向量，从而成功地降低了状态向量的阶数，从而降低了子滤波器的计算负担。

此外，我们发现在CTRA系统中，位置信息、速度信息和姿态信息之间只有单向的强相关性。更具体地说，位置信息的准确估计依赖于速度信息和姿态信息，而速度信息和姿势信息的准确估算很少依赖于位置信息。因此，本研究基于IMU传感器在本研究中仅提供速度信息和姿态信息的事实，简化了IMU子滤波器的状态向量，简化后的子向量如下：

{X（X）}_{k}^{我 M（M） U型} = {[\begin{matrix} v（v） & 一 & θ & ϕ \end{matrix}]}^{吨}

(19)

新的时间更新方程和非线性时间更新函数定义如下：

{X（X）}_{k}^{我 M（M） U型} = {A类}^{我 M（M） U型} ({X（X）}_{k}^{我 M（M） U型}) + {w个}_{k}^{我 M（M） U型}

(20)

{A类}^{我 M（M） U型} ({X（X）}_{k}^{我 M（M） U型}) = A类 ([\begin{matrix} v（v） \\ 一 \\ θ \\ ϕ \end{matrix}]) = [\begin{matrix} v（v） \\ 一 \\ θ \\ ϕ \end{matrix}] + [\begin{matrix} 一 吨 \\ 0 \\ ϕ 吨 \\ 0 \end{matrix}]

(21)

可以看出，简化IMU子滤波器的状态向量是一个四阶系统，新的四阶系统的测量模型为

{Z轴}_{k + 1}^{我 M（M） U型} = {H（H）}_{我 M（M） U型} {X（X）}_{k + 1}^{我 M（M） U型} + υ_{k + 1}^{我 米 u个} = [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] {X（X）}_{k + 1} + υ_{k + 1}^{我 米 u个}

(22)

经过简化，新系统内部传感器的状态向量由六阶ODM传感器和四阶IMU传感器组成。相比之下，增强之前的原始系统中的内部传感器的状态向量由五阶ODM传感器和五阶IMU传感器组成，而增强之后的CTRA系统中的内部传感器的状态向量由六阶ODM传感器和六阶IMU传感器组成。这三个系统的计算复杂性将在下一小节中进行分析。

4.4. 计算复杂性分析

本研究旨在分析基于“奇异值分解衍生立方卡尔曼滤波器”（SVD-DCKF）算法的内部传感器滤波操作的计算复杂性[25]. 我们将每个乘法或加法操作称为一个触发器，触发器的总数表示算法的计算复杂性(表1).

如所示表2在每次迭代中，增广系统比原系统多需要3054个触发器（43.42%），而简化增广系统只需要比原系统多一550个触发器（12.11%）。因此，可以得出结论，简化的增强系统成功地减少了原始增强系统的计算负担，同时保持了强大的跟踪性能。

4.5. 次国家的融合与重组

内部传感器不顾其提供准确位置、速度和姿态信息的能力，无法校准计算过程中出现的累积误差。相比之下，外部传感器通常精度较低，但不会产生累积误差。因此，基于FKF框架的融合可以使外部传感器校准内部传感器，提高定位精度。

如所示图2通过子滤波器对三个不同的传感器进行滤波，可以得到三个子状态向量及其协方差矩阵。具体来说，三个子状态向量表示为

\begin{matrix} {X（X）}_{k}^{U型 W公司 B} = {[\begin{matrix} {x个}_{u个 w个 b条} & 年_{u个 w个 b条} & {v（v）}_{u个 w个 b条} & 一_{u个 w个 b条} & θ_{u个 w个 b条} & ϕ_{u个 w个 b条} \end{matrix}]}^{吨} \\ {X（X）}_{k}^{我 M（M） U型} = {[\begin{matrix} {v（v）}_{我 米 u个} & 一_{我 米 u个} & θ_{我 米 u个} & ϕ_{我 米 u个} \end{matrix}]}^{吨} \\ {X（X）}_{k}^{O（运行） D类 M（M）} = {[\begin{matrix} {x个}_{o个 d日 米} & 年_{o个 d日 米} & {v（v）}_{o个 d日 米} & 一_{o个 d日 米} & θ_{o个 d日 米} & ϕ_{o个 d日 米} \end{matrix}]}^{吨} \end{matrix}

(23)

如中所述第3.1节UWB传感器仅测量位置信息和速度信息，ODM传感器仅观察位置信息，IMU传感器仅记录加速度信息用于信息融合。因此，子向量中的冗余信息可以被消除，三个子状态向量可以重写为

\begin{matrix} {X（X）}_{k | k}^{U型 W公司 B} = {[\begin{matrix} {x个}_{u个 w个 b条} & 年_{u个 w个 b条} & {v（v）}_{u个 w个 b条} \end{matrix}]}^{吨} \\ {X（X）}_{k | k}^{我 M（M） U型} = [{v（v）}_{我 米 u个}] \\ {X（X）}_{k | k}^{O（运行） D类 M（M）} = {[\begin{matrix} {x个}_{o个 d日 米} & 年_{o个 d日 米} \end{matrix}]}^{吨} \end{matrix}

(24)

UWB和ODM传感器的协方差矩阵均为六阶，IMU传感器的协变矩阵为四阶。因此，每个子向量的协方差矩阵也应根据简化的子状态向量进行简化。简化的协方差矩阵如下：

{P（P）}_{k | k}^{U型 W公司 B} = [\begin{matrix} {P（P）}_{k, 11}^{U型 W公司 B} & {P（P）}_{k, 12}^{U型 W公司 B} & {P（P）}_{k, 13}^{U型 W公司 B} \\ {P（P）}_{k, 21}^{U型 W公司 B} & {P（P）}_{k, 22}^{U型 W公司 B} & {P（P）}_{k, 23}^{U型 W公司 B} \\ {P（P）}_{k, 33}^{U型 W公司 B} & {P（P）}_{k, 33}^{U型 W公司 B} & {P（P）}_{k, 33}^{U型 W公司 B} \end{matrix}]

(25)

{P（P）}_{k | k}^{我 M（M） U型} = [{P（P）}_{k, 33}^{我 M（M） U型}]

(26)

{P（P）}_{k | k}^{ODM公司} = [\begin{matrix} {P（P）}_{k, 11}^{ODM公司} & {P（P）}_{k, 12}^{ODM公司} \\ {P（P）}_{k, 21}^{ODM公司} & {P（P）}_{k, 22}^{ODM公司} \end{matrix}]

(27)

哪里

{P（P）}_{k}^{U型 W公司 B}, {P（P）}_{k}^{我 M（M） U型}

、和

{P（P）}_{k}^{O（运行） D类 M（M）}

是相应子状态向量的协方差矩阵，而

{P（P）}_{k | k}^{U型 W公司 B}, {P（P）}_{k | k}^{我 M（M） U型}

、和

{P（P）}_{k | k}^{O（运行） D类 M（M）}

是通过消除冗余信息而简化的子状态向量的协方差矩阵。然后根据FKF框架融合这些协方差矩阵：

{P（P）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个} = {({[\begin{matrix} {P（P）}_{k, 11}^{U型 W公司 B} & {P（P）}_{k, 12}^{U型 W公司 B} \\ {P（P）}_{k, 21}^{U型 W公司 B} & {P（P）}_{k, 22}^{U型 W公司 B} \end{matrix}]}^{- 1} + {P（P）}_{k | k}^{O（运行） D类 M（M） - 1})}^{- 1}

(28)

{P（P）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年} = {({P（P）}_{k, 33}^{U型 W公司 B - 1} + {P（P）}_{k | k}^{我 M（M） U型 - 1})}^{- 1}

(29)

哪里

{P（P）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个}

是融合后位置信息的协方差k，同时

{P（P）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

是融合后速度信息的协方差k随后，基于融合协方差计算融合位置信息和速度信息的子状态向量：

{X（X）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个} = {P（P）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个} ({[\begin{matrix} {P（P）}_{k, 11}^{U型 W公司 B} & {P（P）}_{k, 12}^{U型 W公司 B} \\ {P（P）}_{k, 21}^{U型 W公司 B} & {P（P）}_{k, 22}^{U型 W公司 B} \end{matrix}]}^{- 1} [\begin{matrix} {X（X）}_{k | k, 11}^{U型 W公司 B} \\ {X（X）}_{k | k, 21}^{U型 W公司 B} \end{matrix}] + {P（P）}_{k | k}^{O（运行） D类 M（M） - 1} {X（X）}_{k | k}^{O（运行） D类 M（M）})

(30)

{X（X）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年} = {P（P）}_{k}^{v（v） e（电子） 我 o个 c（c） 我 t吨 年} ({P（P）}_{k | k}^{我 M（M） U型 - 1} {X（X）}_{k | k}^{我 M（M） U型} + {P（P）}_{k, 33}^{超宽带 - 1} {X（X）}_{k, 31}^{超宽带})

(31)

哪里

{X（X）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个}

和

{X（X）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

分别是融合位置信息和速度信息的子状态向量。此时，我们可以通过组合IMU传感器子状态矢量中的剩余信息来获得姿态的子状态矢量，表示为

{X（X）}_{k}^{A类 t吨 t吨 我 t吨 u个 t吨 e（电子）} = {[\begin{matrix} {X（X）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年} & 一_{我 米 u个} & θ_{我 米 u个} & ϕ_{我 米 u个} \end{matrix}]}^{吨}

(32)

最后，如所示图2，将位置信息和姿态信息的子向量重新组合以形成新的状态向量：

{X（X）}_{k} = [\begin{matrix} {X（X）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个} \\ {X（X）}_{k}^{A类 t吨 t吨 我 t吨 u个 t吨 e（电子）} \end{matrix}]

(33)

类似地，通过提取对应于每个状态分量的协方差分量并重新组合，新协方差可以定义为

{P（P）}_{k} = [\begin{matrix} {P（P）}_{k}^{P（P） o个 秒 我 t吨 我 o个 n个} \\ {P（P）}_{k}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年} \\ {P（P）}_{k, 22}^{我 米 u个} \\ {P（P）}_{k, 33}^{我 米 u个} \\ {P（P）}_{k, 44}^{我 米 u个} \end{matrix}]

(34)

外部传感器已被证明在基于FKF框架的惯性导航系统的累积误差校准中是有效的[25]. 同样，里程表理论上应能使用相同的方法进行校准。此外，本研究中标定的数据仅通过一次积分运算获得，因此其精度理论上应高于He等人的位置信息[25]（通过二次积分运算获得）。因此，有理由相信IMU/UWB/ODM协同定位系统应该比IMU/UWB系统具有更好的精度，相关的数值模拟将在第5节.

最后，本文提出的算法，即改进的联邦立方卡尔曼滤波器（MFCKF），能够在分解和状态重组后准确估计系统的姿态信息。此外，它具有很高的实时跟踪性能。算法2中详细介绍了该算法的具体步骤。

算法2。MFCKF的具体步骤。

输入：

{X（X）}_{k}

,

{P（P）}_{k}

,

问_{k}

输出：

{X（X）}_{k} + 1

,

{P（P）}_{k} + 1

,

问_{k + 1}

1.分解

{X（X）}_{k}

和

{P（P）}_{k}

并获得子状态（即。，

{X（X）}_{k}^{我 米 u个}

,

{X（X）}_{k}^{u个 w个 b条}

、和

{X（X）}_{k}^{o个 d日 米}

)

及其协方差（即。，

{P（P）}_{k}^{我 米 u个}

,

{P（P）}_{k}^{u个 w个 b条}

、和

{P（P）}_{k}^{o个 d日 米}

);

2.根据子状态和子协方差以及测量值过滤子滤波器

获得时间上的子状态和子协变

k + 1

(

{X（X）}_{k + 1}^{我 米 u个}

,

{X（X）}_{k + 1}^{o个 d日 米}

,

{X（X）}_{k + 1}^{u个 w个 b条}

和

{P（P）}_{k + 1}^{我 米 u个}

,

{P（P）}_{k + 1}^{o个 d日 米}

,

{P（P）}_{k + 1}^{u个 w个 b条}

,

问_{k + 1}

)

（基于算法1中显示的过滤器）；

3.消除冗余信息并获得融合的子状态（即。，

{X（X）}_{k + 1}^{我 米 u个}

,

{X（X）}_{k + 1}^{u个 w个 b条}

、和

{X（X）}_{k + 1}^{o个 d日 米}

)

及其方差（即。，

{P（P）}_{k + 1}^{我 米 u个}

,

{P（P）}_{k + 1}^{u个 w个 b条}

、和

{P（P）}_{k + 1}^{o个 d日 米}

);

4.基于FKF框架融合子状态和协方差，从而获得

{X（X）}_{k + 1}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

和

{X（X）}_{k + 1}^{P（P） o个 秒 我 t吨 我 o个 n个}

以及它们的协方差

{P（P）}_{k + 1}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

和

{P（P）}_{k + 1}^{P（P） o个 秒 我 t吨 我 o个 n个}

;

5.重组

{X（X）}_{k + 1}^{我 米 u个}

,

{X（X）}_{k + 1}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

和

{X（X）}_{k + 1}^{P（P） o个 秒 我 t吨 我 o个 n个}

获取最终状态更新

{X（X）}_{k + 1}

;

6.重组

{P（P）}_{k + 1}^{我 米 u个}

,

{P（P）}_{k + 1}^{V（V） e（电子） 我 o个 c（c） 我 t吨 年}

和

{P（P）}_{k + 1}^{P（P） o个 秒 我 t吨 我 o个 n个}

获取最终状态更新

{P（P）}_{k + 1}

;

7.输出

{X（X）}_{k + 1}

,

{P（P）}_{k + 1}

,

问_{k + 1}

作为下一时刻的初始值，并重复步骤1-6。

结束.

5.仿真

使用MATLAB 2019a在x86 PC上进行仿真，使用Intel Core i5 7500 CPU和16GB内存，以全面评估所提算法的性能。使用的仿真程序是MATLAB 2019a。

进行了两组模拟。第一组主要测试改进的SVD-ADCKF算法与SVD-DCKF算法的相对性能。这两种方法都是基于FKF框架融合多传感器信息，因此在仿真结果中被称为SVD-FDCKF和SVD-FADCKF。具体来说，这组仿真评估了这两种算法在不同过程噪声环境中的性能，以验证改进算法的自适应功能，主要关注算法的状态估计精度、标准偏差、误差协方差和收敛代数。第二组仿真介绍了中提出的MFCKF算法第4节并旨在验证状态增强方法在更有效地抑制过程噪声方面的能力。在这组仿真中，还在第一组仿真的基础上增加了计算时间，以定性分析状态分解后的算法是否会带来巨大的额外计算负担。

数值模拟模型如所述第2.2节和第4.1节.加入不相关高斯白噪声作为系统的过程噪声，位置信息的噪声服从正态分布。仿真的具体参数如所示表2。估计协方差将在滤波器中迭代收敛，因此估计协方差的迭代初始值是根据经验给出的非最优值。为了初始化1500个蒙特卡罗模拟，每个模拟中状态向量的迭代初始值等于真实初始状态加上从高斯分布中提取的随机分量，零均值和方差等于滤波器的初始协方差。为了便于将算法移植到物理平台上进行未来测试，仿真的测量噪声接近预期使用的物理组件的参数（参见图3; 模拟的实际参数取决于表2). 在这种情况下，数值模拟的结果可以尽可能接近实际情况。（由于技术原因，实验目前仍处于模拟阶段。）

图4显示了不同算法的仿真路径。更详细的分析见第5.1节和第5.2节.

5.1. SVD-FADCKF仿真

这组仿真的目的是验证SVD-ADCKF算法与原始算法（SVD-DCKF）相比的性能改进。基于中提出的算法第3节，分别在IMU和UWB系统中实现滤波，并在FKF框架（SVD-FADCKF和SVD-FDCKF）下实现融合。同时，记录并评估了状态向量的估计误差(图5和图6).

如所示图5引入自适应因子的改进算法（SVD-ADCKF）的平均定位误差明显低于原始算法（SVD-DCKF）。表3结果表明，SVD-ADCKF的平均定位误差比SVD-DCKF低约7.89%。

如所示图5和图6在大多数情况下，这两种算法具有相似的定位误差，但当误差波动较大时，SVD-ADCKF性能更好。这可能是由于当系统的时间更新不准确时，SVD-ADCKF对过程噪声协方差进行了自适应调整。这样，状态估计中的观测值权重就增加了，从而减少了总体误差。

5.2. MFCKF的仿真

这组仿真主要是为了验证MFCKF算法相对于原始算法（SVD-FDCKF）和自适应算法（SVD-FADCKF）的性能改进。主要实验环境与第5.1节，而不同之处在于添加了第4节作为实验对象，可以记录和评估状态向量的估计误差和标准偏差。仿真结果如所示图7,图8和图9.

如所示图7，MFCKF不仅比原始算法具有更好的性能，而且比中提出的自适应算法具有更低的定位误差第3节（SVD-FADCKF）。如图所示表4状态增强后，改进算法的平均定位误差比原算法（SVD-FDCKF）低约32.76%。

如所示图7和图8在状态矢量增强和添加新的状态信息后，误差曲线变得更加平滑。因此，可以推断，状态增强方法可以在跟踪运动物体的状态方面改进算法。然而，我们也注意到，改进的算法在初始超调和动态响应性能方面恶化，这可能是由于状态向量的阶数增加所致。需要作出更多努力来进一步解决这个问题。

图9显示了1500蒙特卡罗实验中每个迭代的定位标准偏差。从图中可以明显看出，MFCKF的标准偏差优于其他两种算法。

图10显示了1500个蒙特卡罗实验中每个迭代的平均估计误差协方差。可以看出，在大多数仿真中，所提出的算法（MFCKF）具有更好的性能。然而，与状态向量相同，改进算法在初始超调量和动态响应性能方面恶化，这可能是由于状态向量的阶数增加所致。需要作出更多努力来进一步解决这个问题。

图11显示了协方差在迭代中收敛的过程。这是从1500个蒙特卡洛实验中随机选择的一组。我们可以得出类似的结论图10该算法收敛后估计误差协方差性能较好，但初始超调量和动态响应性能较差。

此外，在1500次蒙特卡罗实验中，平均每150秒101.95秒，定位误差包含在误差协方差曲线中，占总仿真时间的67.97%。

图12结果表明，所提出的算法（SVD-MFDCKF）的计算时间略高于对照组（SVD-FDCKF），约为11%(表5). 这与中所示的理论计算复杂性分析基本一致表2(~12.11%). 尽管如此，考虑到定位精度提高了32.76%，增加的计算负担被认为是值得的。

6.结论

本研究设计了一套解决方案来解决应用He等人提出的算法时的滤波发散问题[25]在高工艺噪声环境中。首先，引入自适应滤波来及时调整过程噪声协方差，这有助于设置问从而提高滤波精度，抑制滤波发散。随后，采用状态向量增强方法来增强系统状态向量，从而提高算法的可追溯性。此外，将状态向量分解方法与状态向量增强方法相结合，简化了增强后的状态向量，从而减轻了状态增强方法带来的额外计算负担。综合计算复杂性分析表明，简化后的系统几乎没有增加计算负担。此外，该算法的整体定位精度也得到了提高。为了验证该算法的有效性，进行了两组数值仿真，结果表明，与原始算法（SVD-FDCKF）相比，本文提出的MFCKF定位精度显著提高了32.76%，但增加了较小的额外计算负担（11%）。此外，它可以在高过程噪声环境中有效地进行准确的系统状态估计。因此，本文提出的算法达到了预期的目标。

我们未来的研究将集中在以下两个方面：

将该算法移植到物理平台上，在实际条件下验证了该算法的滤波性能。
研究该算法的动态响应性能，以进一步提高其在高动态环境中的性能。

作者贡献

概念化，C.T。；数据管理，C.T。；写作、C.T.和C.H。；校对，C.H。；监督，L.D。；所有作者都阅读并同意手稿的出版版本。

基金

本研究得到了国家自然科学基金61991414和61873301两项资助。

机构审查委员会声明

这项研究是根据北京理工大学的指导方针进行的。

知情同意书

所有参与研究的受试者均获得知情同意。

数据可用性声明

本研究中使用的数据可向相应作者索取。

利益冲突

作者声明没有利益冲突。

工具书类

Dong，X。；周，Y。；任，Z。；Zhong，Y.具有切换交互拓扑的无人机时变编队控制。控制工程实践。 2016,46, 26–36. [谷歌学者] [交叉参考]
Golden，S.A.公司。；Bateman，S.S.Wi-Fi位置的传感器测量，重点是到达时间测距。IEEE传输。暴徒。计算。 2007,6, 1185–1198. [谷歌学者] [交叉参考] [绿色版本]
Wang，Y。；Ye，Q。；Cheng，J。；Wang，L.基于RSSI的蓝牙室内定位。2015年12月16日至18日，中国深圳，第十一届移动自组织和传感器网络国际会议（MSN）会议记录；第165-171页。[谷歌学者]
Sugano先生。；卡瓦泽，T。；Ohta，Y。；Murata，M.基于zigbee标准的无线传感器网络RSSI测量室内定位系统。2006年7月3日至5日，加拿大AB省班夫市，IASTED第六届无线和光通信国际会议记录；第503-508页。[谷歌学者]
Lee，S。；Song，J.B.移动机器人利用红外光反射地标进行定位。2007年10月17日至20日，韩国首尔，《利用红外线反射地标进行移动机器人定位的会议记录》；第674–677页。[谷歌学者]
Nageli，T。；孔戴，C。；Domahidi，A。；莫拉里，M。；Hilliges，O.《智能机器人和系统》（IROS 2014）中微型飞行器的环境相关编队飞行。2014年IEEE/RSJ智能机器人和系统国际会议论文集，美国伊利诺伊州芝加哥，2014年9月14日至18日；第1141-1146页。[谷歌学者]
Kushleyev，A。；梅林格，D。；权力，C。；库马尔，V.朝向一群敏捷的微型四驱车。自动。机器人。 2013,35, 287–300. [谷歌学者] [交叉参考]
李，Z。；德哈内，W。；Gielen，G.基于超低功率UWB的室内定位系统设计。2007年IEEE超宽带国际会议（ICUWB 2007）会议记录，新加坡，2007年9月24日至26日；第159-164页。[谷歌学者]
H.B.李。；Wang，J。；Wang，C.Y.关于超宽带室内动态定位影响因素的讨论。J.纳维格。位置。 2018,6, 45–48. [谷歌学者]
丹尼斯，B。；Pierrot，J.B。；Abou-Rjeily，C.使用TOA在UWB自组网中联合分布式同步和定位。IEEE传输。微型。理论与技术。 2006,54, 1896–1911. [谷歌学者] [交叉参考]
Ruotsalainen，L。；Kirkko-Jaakkola，M。；Rantanen，J。；Maija，M.，无基础设施室内导航中多传感器测量的误差建模。传感器 2018,18, 590. [谷歌学者] [交叉参考] [绿色版本]
冯，X。；张，T。；林，T。；Tang，H。；Niu，X.车辆城市导航用低成本MEMS惯性传感器深耦合GNSS接收机的实现和性能。传感器 2020,20, 3397. [谷歌学者] [交叉参考]
Kim，J。；帕克，M。；Bae，Y。；O.-J.金。；Kim，D。；Kim，B。；Kee，C.使用TDCP测量的用于深城市多径环境的低成本、高精度车辆导航系统。传感器 2020,20, 3254. [谷歌学者] [交叉参考] [公共医学]
艾哈迈德·H。；乌拉，I。；Khan，U。；库雷希，医学学士。；Manzoor，S。；穆罕默德，N。；沙希德·汗，M.U。；Nawaz，R.基于GPS辅助MEMS-IMU的自适应滤波，用于地面车辆轨迹的最优估计。传感器 2019,19, 5357. [谷歌学者] [交叉参考] [绿色版本]
库科，A。；Kaijaluoto，R。；Kaartinen，H。；V.V.Lehtola。；Jaakkola，A。；Hyypp，J.北方森林冠层下单扫描仪MLS森林数据的图形SLAM校正。ISPRS J.摄影。远程传感器。 2017,132, 199–209. [谷歌学者]
高，Y。；刘，S。；医学硕士Atia。；Noureldin，A.INS/GPS/LiDAR组合导航系统，用于使用混合扫描匹配算法的城市和室内环境。传感器 2015,15, 23286–23302. [谷歌学者] [交叉参考] [绿色版本]
蒋介石，K.-W。；蔡国杰。；李彦宏。；李毅。；El-Sheimy，N.使用INS/GNSS/3D LiDAR-SLAM和完整性评估的自动驾驶导航引擎设计。远程传感器。 2020,12, 1564. [谷歌学者] [交叉参考]
卡尔森，N.A。；Berarducci，M.P.联合卡尔曼滤波器仿真结果。导航 1994,41, 297–322. [谷歌学者] [交叉参考]
邱海忠。；张，H.Y。；Jin，H.相关局部估计的融合算法。Aerosp.航空公司。科学。Technol公司。 2004,8, 619–626. [谷歌学者] [交叉参考]
李，M。；朱，H。；你，S。；Tang，C.用于地下煤矿应用的基于UWB的惯性传感器辅助定位系统。IEEE传感器J。 2020,20, 6652–6669. [谷歌学者] [交叉参考]
贾雷，L。；Pascucci，F。；莫罗西，S。；Martinelli，A.通过基于UWB锚的在线校准改进PDR定位。2018年9月10日至13日在意大利巴勒莫举行的2018 IEEE第四届社会与工业研究与技术国际论坛（RTSI）会议记录；第1-5页。[谷歌学者]
胡，G。；高，S。；钟，Y。；高，B。；Subic，A.改进的强跟踪无迹卡尔曼滤波器用于具有过程模型不确定性的非线性状态估计。国际期刊改编。控制。信号处理。 2015,29, 1561–1577. [谷歌学者] [交叉参考]
韩，Y。；魏，C。；李，R。；Wang，J。；Yu，H.一种新的基于IMU和UWB的协同定位方法。传感器 2020,20, 467. [谷歌学者] [交叉参考] [公共医学] [绿色版本]
李，J。；毕，Y。；李凯。；王凯。；林，F。；Chen，B.M.通过UWB和IMU融合实现MAV群的精确三维定位。2018年6月12日至15日，美国阿拉斯加州安克雷奇，IEEE第14届控制与自动化国际会议（ICCA）会议记录。[谷歌学者]
他，C。；唐，C。；Yu，C.用于IMU-UWB室内定位的联合导数Cubature Kalman滤波器。传感器 2020,20, 3514. [谷歌学者] [交叉参考] [公共医学]
沈毅。；朱，H。；莫，J。；Song，Y.简化Sage-Husa自适应滤波器在组合导航系统中的应用与仿真。青岛大学工程技术学院。预计起飞时间。 2001。在线提供：https://www.researchgate.net/publication/291150654_Application_and_simulation_of_simplified_Sage-Husa_adaptive_filter_in_integrated_navigation_system（2021年6月30日访问）。
Kownacki，C.设计自适应卡尔曼滤波器，以消除无人机系统中使用的激光测距仪的测量故障。Aerosp.航空公司。科学。Technol公司。 2015,41, 81–89. [谷歌学者] [交叉参考]
Sabatini，A.M.变状态维卡尔曼滤波器，用于使用惯性和磁性传感器确定方向。传感器 2012,12, 8491–8506. [谷歌学者] [交叉参考]
胡，G。；高，S。；Zhong，Y.紧耦合INS/GPS组合导航的衍生UKF。ISA事务。 2015,56, 135–144. [谷歌学者] [交叉参考]
高戈，H。；She-Sheng，G。；永民，Z。；Bing-Bing，G.导数无迹卡尔曼滤波器的随机稳定性。下巴。物理学。B 2015,24, 070202. [谷歌学者]
Julier，S.J.《无心滤波和非线性估计》。IEEE程序。 2004,92, 401–422. [谷歌学者] [交叉参考] [绿色版本]

图1。算法的总体结构。

图2。系统的总体结构。

图3。本文中涉及的传感器和移动机器人：(一)超宽带（UWB）传感器（YCHIOT，中国浙江温州），由YCHOOT公司提供的商用产品，带有Mini3s模块；(b条)空间运动传感器芯片MPU9150作为IMU传感器（Digi-Key Electronics，美国明尼苏达州Thief River Falls）；(c（c）)里程表84由带编码器的直流齿轮电机MG513构成（中国江苏昆山奉化变速器）；和(d日)通过携带上述传感器实现精确定位的轮式移动机器人平台（中国广东佛山瑞曲科技）。

图4。不同算法的模拟路径。

图5。两种算法的定位误差。

图6。 X（X）-轴和Y（Y）-选定仿真中两种算法的轴误差。

图7。三种算法的定位误差。

图8。 X（X）-轴和Y（Y）-选定仿真中三种算法的轴误差。

图9。1500个蒙特卡罗实验中定位误差的平均标准偏差（3种算法）。

图10。1500个蒙特卡罗实验中的平均估计误差协方差（3种算法）。

图11。选定模拟中的估计误差协方差（3种算法）。

图12。两种算法的仿真时间比较。

表1。三个系统的计算复杂性分析。

步骤	原件系统（5+5）	增强型系统（6+6）	简化加强系统（6+4）
计算体积点的	450次触发器 460次失败 200次失败	792次失败 844次失败 552次失败	508次失败 538次失败 300次失败
时间更新	50次失败	72次失败	52次失败
预测协方差更新的	260次失败	372次失败	270次失败
计量预测	90次失败	132次失败	94次失败
计算卡尔曼增益的	1504次失败	2596次失败	1684次失败
状态更新	30次失败	36次失败	30次失败
协方差更新	950次失败	1656次失败	1068个触发器
总计	3994次失败	7052次失败	4544次失败

表2。模拟参数。

项目	参数
初始状态	X坐标：4（cm） Y坐标：4（cm）速度：5（cm/s）加速度：0（cm/s $^{2}$ ) 偏航角： $π$ /3 ( $^{\circ}$ ) 偏航角变化： $π$ /90 ( $^{\circ}$ )
模拟时间	150（/秒）
采样时间	$吨 = 1$
初始协方差	${P（P）}_{1} = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0.5 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0.1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0.15 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0.001 \end{matrix}]$
初步估算	${X（X）}_{1} = [\begin{matrix} 4 \\ 4 \\ 5 \\ 0 \\ π / 三 \\ π / 90 \end{matrix}]$ + $[\begin{matrix} N个 (0, 1) \\ N个 (0, 1) \\ N个 (0, 0.5) \\ N个 (0, 0.1) \\ N个 (0, 0.15) \\ N个 (0, 0.001) \end{matrix}]$
过程噪声驱动矩阵	${G公司}_{k} = [\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 吨 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & π 吨 / 三 & 0 \\ 0 & 0 & 0 & 0 & 0 & π / 三 \end{matrix}]$
工艺噪声分布 ${w个}_{k}$	${w个}_{k} = [\begin{matrix} 0 \\ 0 \\ N个 (0, 0.5) \\ 0 \\ N个 (0, 0.1) \\ N个 (0, 0.01) \end{matrix}]$
过程噪声的初始协方差 $问_{1}$	六阶对角恒等矩阵
超宽带测量噪声	$N个 (0, 16)$
陀螺白噪声	$N个 (0, 0.05)$
陀螺恒定漂移	0.1°/小时
加速度计白噪声	$N个 (0, 0.001)$
加速度计恒定偏置	$10^{- 三}$ 克
磁强计白噪声	$N个 (0, 0.1)$
里程表白噪声	$N个 (0, 0.1)$

表3。1500次蒙特卡罗实验中的平均定位误差。

项目	SVD-FDCKF公司	SVD-FADCK公司
平均定位误差（cm）	1.2937	1.1915

表4。1500次蒙特卡罗实验中的平均定位误差。

项目	SVD-FDCKF公司	SVD-FADCK公司	MFCKF公司
平均定位误差（cm）	1.2937	1.1915	0.8698

表5。两种算法的平均模拟时间。

项目	SVD-FDCKF公司	MFDCK公司
平均计算时间（s）	0.0445	0.0398

出版商备注：MDPI对公布的地图和机构关联中的管辖权主张保持中立。

分享和引用

MDPI和ACS样式

唐，C。；他，C。；杜，L。基于改进Cubature Kalman滤波的IMU/ODM/UWB联合定位系统。传感器 2021,21, 4823.https://doi.org/10.3390/s21144823

AMA风格

唐C，何C，窦L。基于改进Cubature Kalman滤波的IMU/ODM/UWB联合定位系统。传感器. 2021; 21(14):4823.https://doi.org/10.3390/s21144823

芝加哥/图拉宾风格

唐、赵、何成阳和窦丽华。2021.“基于改进Cubature Kalman滤波的IMU/ODM/UWB联合定位系统”传感器21，第14期：4823。https://doi.org/10.3390/s21144823

请注意，从2016年第一期开始，该杂志使用文章编号而不是页码。请参阅更多详细信息在这里.

文章菜单