A Tightly-Integrated Magnetic-Field aided Inertial Navigation System
紧密集成的磁场辅助惯性导航系统

Index Terms-inertial navigation, magnetic field, polynomial model, error state Kalman filter

The magnetic-field is omnipresent and stable vector field, which can be a highly informative and reliable source for localization if measured accurately [1]; an example of the magneticfield variations inside a building is shown in Fig. 1. The distorted earth magnetic-field and magnetized materials in the environment provide fingerprints highly correlated to position. Hence, the magnetic-field is a viable and robust information source for localization in Global Navigation Satellite System (GNSS) denied environments, such as indoors and underwater [2], [3].
磁场是无处不在的稳定矢量场，如果测量准确，它可以成为一个信息量大且可靠的定位源[1]；图 1 显示了建筑物内磁场变化的一个例子。扭曲的地球磁场和环境中的磁化材料提供了与位置高度相关的指纹。因此，在室内和水下等拒绝全球导航卫星系统（GNSS）的环境中，磁场是一种可行且稳健的定位信息源[2]，[3]。

Indeed, magnetic-field based simultaneous localization and mapping (SLAM), where magnetic-field measurements are fused with the navigation solution from a inertial navigation system (INS), has turned out be one of the most promising techniques for scalable indoor localization [4]. However, when using low-cost inertial sensors the error growth rate of the inertial navigation system is typically in the order of 10 meters per minute [5]. Therefore, the allowable length of the exploration phases, where new areas are mapped, is extremely limited when using low-cost inertial sensors. Hence, to increase the usability of current magnetic-field based SLAM solutions we need robust magnetic-field odometry techniques that reduce navigation error drift rate.
事实上，基于磁场的同步定位和测绘（SLAM），即磁场测量与惯性导航系统（INS）的导航解决方案相融合，已成为最有前途的可扩展室内定位技术之一[4]。然而，在使用低成本惯性传感器时，惯性导航系统的误差增长率通常在每分钟 10 米左右 [5]。因此，在使用低成本惯性传感器时，绘制新区域地图的探索阶段的允许长度极为有限。因此，为了提高当前基于磁场的 SLAM 解决方案的可用性，我们需要能够降低导航误差漂移率的稳健磁场里程测量技术。

Fig. 1. Illustration of the magnetic-field magnitude variations inside a building. The field was measured with an magnetometer array, whose location was tracked by camera-based tracking systems. The field measurement was then interpolated and the field magnitude was projected on the floor.
图 1.建筑物内磁场大小变化示意图。磁场由磁力计阵列测量，其位置由摄像跟踪系统跟踪。然后对磁场测量结果进行内插，并将磁场大小投射到地板上。

Thanks to the recent sensor technology development, highperforming and affordable magnetometer vector-sensor arrays can be constructed. These arrays allows for snap-shot "images" of the magnetic-field to be collected, which in turns enables faster and richer feature learning in the SLAM process. Further, magnetometer array measurements must comply with easy to model physical laws. This allows us to model them in such a way that position translation and orientation change are encoded, making it perfect for complementing or correcting an INS. To that end, in this paper, we present a method for tightly integrated magnetic-field aided inertial navigation. The resulting navigation system has, compared to a pure inertial navigation system, an significantly reduced error growth rate. Hence, the proposed navigation method has the potential to greatly extend the allowable length of the exploration phases in magnetic-field based SLAM system.
得益于近年来传感器技术的发展，高性能、价格合理的磁强计矢量传感器阵列得以构建。这些阵列可以收集磁场的快照 "图像"，从而在 SLAM 过程中实现更快、更丰富的特征学习。此外，磁强计阵列测量必须符合易于建模的物理定律。这样，我们就可以对其进行建模，从而对位置平移和方向变化进行编码，使其成为补充或修正 INS 的完美工具。为此，我们在本文中提出了一种紧密集成磁场辅助惯性导航的方法。与纯惯性导航系统相比，该导航系统的误差增长率显著降低。因此，所提出的导航方法有可能大大延长基于磁场的 SLAM 系统中探索阶段的允许长度。

The idea behind magnetic-field based odometry is that the velocity of a magnetometer vector sensor array can be
基于磁场的里程测量背后的理念是，磁强计矢量传感器阵列的速度可以
estimated via the differential equation
通过微分方程估计

The equation relates the rate of change of the magnetic field to the rotation rate of the array , the Jacobian of the magnetic field with respect to position , and the velocity . With a magnetometer array, such as that in Fig. 2, the Jacobian can be estimated from spatially distributed measurements and the velocity can be determined.
该方程将磁场 的变化率与阵列 的旋转速率、磁场相对于位置的雅各布系数 以及速度 联系起来。利用磁强计阵列（如图 2 所示），可以根据空间分布的测量结果估算出雅各比，并确定速度。

In [6]-[10], the differential equation (1) was used to develop magenetic-field aided INS solutions. The resulting implementations achieve much lower error growth rate compared to stand-alone inertial navigation systems. In a more recent work [11], a model-based approach to magnetic-field odometry was proposed, in which a polynomial model describing the local magnetic-field was developed. In the proposed modelbased odometry method the velocity was viewed as a model parameter to be estimated, which allowed estimation theory to be used to analyse the problem. Presented experiential results showed that the model-based odometry approach achieved a higher accuracy compared to approaches based upon directly solving (1). The model-based odometry approach was further explored in [12], where it was used to estimate both the translation and orientation change of the array.
在[6]-[10]中，微分方程（1）被用于开发磁场辅助 INS 解决方案。与独立的惯性导航系统相比，由此实现的误差增长率要低得多。最近的一项研究[11]提出了一种基于模型的磁场里程测量方法，其中开发了一个描述本地磁场的多项式模型。在所提出的基于模型的测距方法中，速度被视为一个需要估算的模型参数，因此可以使用估算理论来分析问题。实验结果表明，与直接求解(1)的方法相比，基于模型的里程测量法获得了更高的精度。文献[12]进一步探讨了基于模型的里程测量方法，并将其用于估计阵列的平移和方位变化。

We will in this paper, encouraged by the promising results on model-based magnetic-field odometry shown in [12], present a method for model-based magnetic-field aided inertial navigation. More precisely, we will: (a) derive a tightly integrated magnetic-field aided inertial navigation system using a recursively updated polynomial model; and (b) evaluate its performance using Monte Carlo simulations. All the data and code used to produce the presented results are made available at https://github.com/Huang-Chuan/magnetic-field-odometry.
受 [12] 中基于模型的磁场里程测量法所取得的可喜成果的鼓舞，我们将在本文中介绍一种基于模型的磁场辅助惯性导航方法。更准确地说，我们将(a) 利用递归更新的多项式模型推导出一个紧密集成的磁场辅助惯性导航系统；(b) 利用蒙特卡罗模拟评估其性能。用于得出上述结果的所有数据和代码均可在 https://github.com/Huang-Chuan/magnetic-field-odometry 网站上查阅。

A moving platform with an inertial measurement unit (IMU) and a magnetometer sensor array consisting of sensors is considered; see Fig. 2. Our focus is on estimating the position, velocity, and orientation of the platform from the data generated by the sensors. To that end, in this section a state-space model that can be used to fuse the inertial and magnetic-field measurement will be derived.
我们考虑了一个带有惯性测量单元（IMU）和由 传感器组成的磁力计传感器阵列的移动平台；见图 2。我们的重点是从传感器生成的数据中估计平台的位置、速度和方向。为此，我们将在本节中推导出一个可用于融合惯性和磁场测量的状态空间模型。

The navigation equations for an inertial navigation system using low-cost sensor and moving at moderate velocities are given by [13]
使用低成本传感器并以中等速度移动的惯性导航系统的导航方程为 [13]

Fig. 2. Example of magnetic-field quiver plot, that is, a magnetic-field image, overlaid on the magnetic-field sensor array used to capture the field. Two arrows are missing due to broken sensors.
图 2.磁场颤动图示例，即磁场图像与用于捕捉磁场的磁场传感器阵列的叠加图。由于传感器损坏，缺少两个箭头。

where 其中

Here the superscript and subscript and denote the navigation frame and body frame at timestep , respectively. , and (3D rotation group) denote the position, velocity, and orientation (parameterized as a unit quaternion), respectively. The specific force and gravity are denoted by and , respectively and the sampling interval is denoted by . The body angular velocity is denoted by . Moreover, is the rotation matrix that transforms a vector in the body frame to the navigation frame . Furthermore, denotes quaternion multiplication, is the operator that maps a axis-angle to a quaternion, and is the operator that maps a quaternion to a rotation matrix (see [14] for details on quaternion algebra).
这里的上标和下标 和 分别表示时间步 时的导航帧和身体帧。 , 和 （三维旋转组）分别表示位置、速度和方向（参数为单位四元数）。比力和重力分别用 和 表示，采样间隔用 表示。机体角速度用 表示。此外， 是旋转矩阵，用于将身体框架 中的矢量转换到导航框架 中。此外， 表示四元数乘法， 是将轴角映射到四元数的算子， 是将四元数映射到旋转矩阵的算子（有关四元数代数的详情，请参阅 [14]）。

Acceleration and gyroscope measurements, denoted by and , are modeled as the true values affected by sensor bias and noise, i.e.,
加速度和陀螺仪测量值（分别用 和 表示）被模拟为受传感器偏差和噪声影响的真实值，即

Here and denote accelerometer and gyroscope bias, respectively. Further, and denote accelerometer and gyroscope noise, respectively. The accelerometer and gyroscope measurement noises are modeled as white Gaussian noises with covariance and , respectively. Here, denote an identity matrix of dimension .
和 分别表示加速度计和陀螺仪偏差。此外， 和 分别表示加速度计和陀螺仪噪声。加速度计和陀螺仪测量噪声分别建模为协方差为 和 的白高斯噪声。这里， 表示维数为 的标识矩阵。

The IMU sensor biases are modeled as random walk processes, i.e.,
IMU 传感器的偏差被模拟为随机漫步过程，即

where and are Gaussian white noise with covariance matrix and , respectively.
其中 和 分别为高斯白噪声，其协方差矩阵分别为 和 。

The magnetometer sensor array measurement is denoted as
磁强计传感器阵列测量值表示为

where denotes the measurement from the magnetometer in the array. Further, locally the magnetic-field at location in the body coordinate frame at time is modeled as
其中 表示阵列中 磁强计的测量值。此外，在人体坐标系中， 位置在时间 的局部磁场 建模为

where is a regression matrix defined in [11] and is the coefficients of the polynomial model; for a order polynomial the model has 3 unknown parameters [12]. Thus, the magnetometer sensor array measurement is modeled as
其中 是[11]中定义的回归矩阵， 是多项式模型的系数；对于 阶多项式，模型有 3 个未知参数[12]。因此，磁强计传感器阵列测量的模型为

where denotes the location of the magnetometer in the array expressed in the body coordinate frame at time . Further, denotes the measurement noise which is assumed to be white and Gaussian distributed with covariance matrix .
其中 表示 磁强计在阵列中的位置，以时间 的身体坐标系表示。此外， 表示测量噪声，假定为白色高斯分布，协方差矩阵为 。

The polynomial model in (6) describes the local magneticfield in the body frame at time . Given the change in pose
(6) 中的多项式模型描述了时间 时身体框架中的局部磁场。鉴于姿势的变化

where 其中

a way to update the polynomial model is needed. To that end, assume that is valid within the volume , centered at the origin of the array at time . Then if there exists a location such that , the magnetic field at location can be expressed as
需要一种更新多项式模型的方法。为此，假设 在体积 内有效，以时间 的阵列原点为中心。那么，如果存在一个位置 ，使得 ，则位置 的磁场可表示为

where 其中

Here is the operator that maps a vector in to a skewsymmetric matrix such that .
这里， 是将 中的向量映射到偏对称矩阵的算子，这样， 。

Combined with (6), it leads to
结合 (6)，可以得出

where 其中

Note for a given , and , (10a) represents three equations. Since is of dimension , different location vectors are need to solve the equation system with respect to . For a polynomial model of order 2 or is divisible by 3. In these cases, vectors can be used to construct the equation system
请注意，对于给定的 , 和 , (10a) 表示三个方程。由于 的维数为 ，因此需要不同的位置向量来求解与 有关的方程组。对于阶数为 2 或 可被 3 整除的多项式模型，可使用 向量来构建方程组。

Finally, given that is invertible, it holds that
最后，考虑到 是可逆的，因此可以认为

Since the polynomial model only describes the magneticfield locally, the change in origin of the model from the body coordinate frame at time to will introduce additional model errors. To describe these errors a white noise is added to the recursions in (12). That is, with a slight abuse of notation, the update of the polynomial coefficients given a displacement is modeled as
由于多项式模型只能描述局部磁场，因此模型原点从 时的身体坐标系到 时的身体坐标系的变化会带来额外的模型误差。为了描述这些误差， 。也就是说，略微滥用一下符号，给定位移 的多项式系数更新模型为

where is assumed to be white and Gaussian distributed with covariance matrix .
其中 假设为白色高斯分布，协方差矩阵为 。

By combining the models in (2), (3), (4), (7) and (13) a state-space model relating the IMU and magnetometer array measurements can be found. Let the state vector , input vector , and process noise be defined as
将 (2)、(3)、(4)、(7) 和 (13) 中的模型结合起来，就能找到一个与 IMU 和磁强计阵列测量相关的状态空间模型。假设状态矢量 、输入矢量 和过程噪声 定义为

respectively. Then the state-space model becomes
分别为然后状态空间模型变为

Fig. 3. The flowchart of the ESKF algorithm. The filter first propagates the nominal state and predicts measurement which is then compared with the actual measurement. The error in prediction goes into a Kalman filter with error state model and corrects the propagated nominal state to output the final state estimate.
图 3 ESKF 算法流程图ESKF 算法流程图。滤波器首先传播标称状态并预测测量值，然后与实际测量值进行比较。预测误差进入带有误差状态模型的卡尔曼滤波器，修正传播的标称状态，输出最终的状态估计值。

where 其中

and

The quaternion in the state-vector in (14) does not belong to Euclidean space. Therefore, standard nonlinear filter algorithms, such as the extended Kalman filter and unscented Kalman filter, cannot be applied to the state-space model in (15) without appropriate modifications. The error state Kalman filter (ESKF) presented in [14] includes such modifications and is a long-time established workhorse for creating various sensor aided INS solutions [15]. With reference to Fig. 3, the algorithm works by propagating a nominal state via the state-transition model in (15a) and using a complimentary Kalman filter to estimate the errors in the nominal states. By assuming the errors in the orientation, i.e., the quaternion vector, to be small the fact that orientations do not belong to Euclidean space can be neglected and a standard Kalman filter is used. The estimated errors are then used to correct the nominal states. Since ESKF is a standard algorithm, only the error propagation model that is unique to the statespace model in (15), will here be presented. For details about the full ESKF the reader is referred to [14].
(14) 中状态矢量 中的四元数不属于欧几里得空间。因此，标准的非线性滤波算法，如扩展卡尔曼滤波器和无符号卡尔曼滤波器，在没有适当修改的情况下，不能应用于 (15) 中的状态空间模型。文献[14]中介绍的误差状态卡尔曼滤波器（ESKF）就包含了这种修改，并且是长期以来用于创建各种传感器辅助 INS 解决方案的主要工具[15]。参考图 3，该算法通过 (15a) 中的状态转换模型传播标称状态 ，并使用卡尔曼滤波器估计标称状态中的误差 。假设方向（即四元数矢量）误差较小，就可以忽略方向不属于欧几里得空间这一事实，并使用标准卡尔曼滤波器。然后利用估计误差修正标称状态。由于 ESKF 是一种标准算法，这里只介绍 (15) 中状态空间模型所独有的误差传播模型。有关完整 ESKF 的详细信息，读者可参阅 [14]。

The nominal state , error state are defined as
标称状态 ，误差状态 定义如下

respectively. Here, denotes the perturbation between the nominal and true quaternion. The true state and nominal state relate to each other via
分别为 表示标称四元数与真实四元数之间的扰动。真实状态和标称状态之间的关系通过

The dynamics of all error states except has been derived in [14] and are given by
除 之外的所有误差状态的动力学参数已在文献 [14] 中得到，其值为

where 其中

To the first order the errors in (13) propagates according to
在一阶，(13) 中的误差根据以下公式传播

where 其中

However, instead of expressing the error development in terms of we would like to express it in terms of the orientation error , velocity error , accelerometer bias estimation error , and gyroscope bias estimation error . To do so, note that
不过，我们希望用方向误差 、速度误差 、加速度计偏差估计误差 和陀螺仪偏差估计误差 来表示误差的发展，而不是用 来表示。为此，请注意

Thus, it holds that
因此，可以认为

where 其中

which gives that 得出