A Robust Tri-Electromagnet-Based 6-DoF Pose Tracking System Using an Error-State Kalman Filter

Abstract

Magnetic pose tracking is a non-contact, accurate, and occlusion-free method that has been increasingly employed to track intra-corporeal medical devices such as endoscopes in computer-assisted medical interventions. In magnetic pose-tracking systems, a nonlinear estimation algorithm is needed to recover the pose information from magnetic measurements. In existing pose estimation algorithms such as the extended Kalman filter (EKF), the 3-DoF orientation in the $S^3$ manifold is normally parametrized as unit quaternions and simply treated as a vector in the Euclidean space, which causes a violation of the unity constraint of quaternions and reduces pose tracking accuracy. In this paper, a pose estimation algorithm based on the error-state Kalman filter (ESKF) is proposed to improve the accuracy and robustness of electromagnetic tracking systems. The proposed system consists of three electromagnetic coils for magnetic field generation and a tri-axial magnetic sensor attached to the target object for field measurement. A strategy of sequential coil excitation is developed to separate the magnetic fields from different coils and reject magnetic disturbances. Simulation and experiments are conducted to evaluate the pose tracking performance of the proposed ESKF algorithm, which is also compared with standard EKF and constrained EKF. It is shown that the ESKF can effectively maintain the quaternion unity and thus achieve a better tracking accuracy, i.e., a Euclidean position error of 2.23 mm and an average orientation angle error of 0.45°. The disturbance rejection performance of the electromagnetic tracking system is also experimentally validated.

1. Introduction

Pose-tracking technology is extensively employed in applications such as rehabilitation [1], virtual/augmented reality [2,3], and computer-assisted medical interventions [4] to track the movement of the human body and various tools and devices. For example, real-time pose tracking of intra-corporeal medical devices enables image registration [5], surgical navigation [6], and closed-loop robotic control [7]. Pose-tracking technology improves accuracy, safety, efficiency, and autonomy in minimally invasive medical interventions. Typical pose-tracking methods include optical tracking [8], image-based tracking [9], radio frequency (RF) positioning [10], inertial measurement unit (IMU)-based tracking [11], and magnetic tracking [4,12,13,14,15,16,17,18,19,20,21]. Optical tracking systems using infrared cameras and optical markers generally provide a high accuracy; however, optical tracking requires line-of-sight access to the target, which is not feasible in scenarios such as localization of intra-body devices in medical procedures. Image-based tracking, such as CT- and MRI-based methods, can provide intuitive guidance; however, it is not suitable for real-time tracking since it takes a long time for image acquisition and processing. RF positioning can only reach a centimeter-level accuracy, which is insufficient for many applications. IMU-based tracking offers a lightweight pose-tracking solution, which is ideal for wearable applications; however, it is susceptible to drift over time, leading to accumulated errors in position and orientation estimates.

Magnetic pose tracking is accurate and safe for the human body. Since magnetic fields can permeate most nonferrous materials, magnetic tracking does not require line-of-sight access to the target. Therefore, magnetic tracking is especially suitable for non-contact localization of intracorporeal medical devices. Magnetic pose-tracking systems are typically categorized into two types, i.e., permanent-magnet-based [14,15,16,17] and electromagnet-based [18,19,20,21] systems. Permanent-magnet-based systems utilize permanent magnets as passive magnetic markers attached to the tracked object, with external magnetic sensor arrays nearby to track these markers. Hu et al. [14] developed a permanent magnet-based 5-DoF pose-tracking system using a cubic sensor array consisting of 64 magnetic sensors. Wang et al. [15] employed the multipoint sampling method to reduce the influence of ambient disturbance on the permanent magnet-based pose-tracking system. Son et al. [16] integrated the permanent magnet-based pose-tracking system into a closed-loop electromagnetic actuation system for manipulation of untethered robots. Popek et al. [17] developed a pose-tracking system for untethered capsule robots, which uses an external rotating permanent magnet and a magnetic sensor attached to the capsule. In this system, the external permanent magnet is used for both actuation and localization of the capsule robot. There are some drawbacks of permanent magnet-based systems. First, they are susceptible to external magnetic disturbance. Furthermore, since the magnetic field of a dipole permanent magnet is rotationally symmetric about its axis of magnetic moment, this degree of freedom of orientation cannot be estimated from magnetic measurements and only 5-DoF pose tracking is feasible. In contrast, electromagnet-based pose-tracking systems are capable of 6-DoF pose tracking and are immune to external magnetic disturbance by using alternating or switched excitation of the electromagnets. Raab et al. [18] proposed the first electromagnetic pose-tracking system, which uses a three-axis transmitting coil to generate a magnetic dipole field to a three-axis receiving coil on the target. This system achieves a millimeter-level tracking accuracy but it has a large size. Yang et al. [19] developed an electromagnetic tracking system using nine transmitting coils to emit sinusoidal magnetic fields at different frequencies, which were received by a small-sized sensing coil for 5-DoF pose tracking. Anser EMT [21] was developed as an open-source electromagnetic tracking system, which integrates multiple electromagnetic coils into a flat printed circuit board. The alternating magnetic signals are received by a sensing coil for pose estimation. Most commercial magnetic tracking systems are electromagnet-based systems, such as NDI Aurora.

In magnetic tracking systems, the magnetic field near the magnetic source is modeled as a nonlinear function of the target pose. Then, the pose can be estimated from the magnetic measurements using a nonlinear estimation algorithm. Pose estimation algorithms can be categorized into analytical solutions [22,23], optimization-based methods [14,15,16,19,24], and Kalman filter-based methods [20,25,26,27,28,29]. The Kalman filter (KF)-based methods [30] are well-known for their simplicity of implementation, high computational efficiency, and high accuracy. The extended Kalman filter (EKF) broadens the application of the linear Kalman filter to nonlinear systems, such as magnetic pose-tracking systems [20]. However, Kalman filters typically treat the state space as a Euclidean space ${\mathbb{R}}^n$, whereas the orientation state in pose-tracking systems evolves on the unit quaternion manifolds $S^3$. If the orientation is simply treated in the Euclidean space, the accuracy and robustness of the pose-tracking system is impaired.

A straightforward approach to addressing this issue is using minimal parameterization [31] of orientation such as Euler angles or axis–angle representation. However, both representations have inherent singularities. Unit quaternion can be used as a valid representation of orientation without the singularity issue although it is redundant. Since the standard EKF typically does not account for the unity constraint of quaternions during pose estimation, the quaternion norm errors accumulate and impair the tracking accuracy. It is noted that not only the orientation accuracy but also the position accuracy is affected because the position and orientation are coupled in the magnetic measurement model. The constrained extended Kalman filter (CEKF) [32,33], as a variant of the standard EKF, enforces algebraic constraints within the standard KF framework to avoid violations of quaternion unity. However, such constraints might cause the singularity issue in state covariance [34]. The error-state Kalman filter (ESKF) [35] operates in the local tangent space of the unit quaternion manifold, where a minimal parameterization of incremental orientation is adopted. The ESKF separates the estimated state into a nominal state and a small error state. The error state is propagated and updated using sensor measurements. After the update, the nominal state is corrected by the error state. Since the correction is conducted on the manifold, the unity of quaternion is automatically satisfied and thus the accuracy of pose estimation is enhanced. The ESKF has been widely used in inertial orientation tracking systems [36,37,38,39]; however, to the best of our knowledge, the ESKF algorithm has never been implemented in a magnetic pose-tracking system. In this paper, the ESKF is employed for pose estimation in electromagnetic tracking systems for the first time to improve the robustness and accuracy of pose tracking.

Another issue considered in this paper is the separation of magnetic fields from different electromagnetic coils and the rejection of ambient magnetic disturbance to the magnetic tracking system. Conventionally, electromagnetic tracking systems use alternating (sinusoidal) excitation of the electromagnetic coils to reject low-frequency disturbance fields, where different frequencies of excitation current are used for multiple electromagnetic coils for field separation. However, complicated demodulation algorithms and circuits are needed to separate the magnetic signals from different coils. To address this problem, a strategy of sequential coil excitation is proposed to separate the fields from different coils, which does not require complicated demodulation. In addition, the ambient disturbance field can be easily canceled by dynamically measuring the difference between the coil magnetic field and the background field.

The objective of this article is to propose and evaluate a robust tri-electromagnet-based 6-DoF pose-tracking system using an error-state Kalman filter. The system consists of three electromagnetic coils that are sequentially excited to generate dipole fields. A single three-axis magnetic sensor is attached to the target object to measure the magnetic field. A sequential excitation strategy is used to separate the magnetic fields from three coils and eliminate the external magnetic disturbance. The ESKF algorithm is utilized to achieve accurate and robust pose estimation. The major contributions of this article are listed as follows:

• The error-state Kalman filter algorithm is developed, implemented, and evaluated for the electromagnetic pose-tracking system for the first time;
• The error-state Kalman filter algorithm is compared with the conventional EKF and CEKF in terms of pose tracking accuracy and robustness using simulation and experiments;
• A strategy of sequential coil excitation is proposed to separate the magnetic measurements from different coils and reject the external magnetic disturbance.

The article is organized as follows. In Section 2.1, the working principle of the pose-tracking system is presented and the hardware and control strategy of sequential coil excitation are shown. Section 2.2 describes the pose estimation algorithms, including the standard EKF, the constrained EKF, and the proposed ESKF. Section 3 presents the simulation results of pose tracking to compare the performance of the algorithms under highly dynamic conditions. Section 4 provides experimental results of pose tracking along different trajectories to evaluate the proposed algorithm. Section 4 also demonstrates the results of magnetic disturbance rejection. The paper is concluded in Section 5.

2. Methodology

The proposed 6-DoF pose-tracking system consists of three stationary electromagnetic coils and a tri-axial magnetic sensor attached to the moving target, as illustrated in Figure 1a. The configuration of the magnetic sources is designed with coil 1 and coil 2 being orthogonal and coil 2 and coil 3 being parallel. The three coils are excited sequentially and the magnetic sensor is used to measure the magnetic field from each coil for pose estimation. The sequential excitation of electromagnetic coils enables separation of magnetic fields from different coils and makes the pose-tracking system robust to magnetic disturbances. Once the magnetic field measurements from all three coils are collected, the 6-DoF pose of the sensor (target) can be estimated using an error-state Kalman filter, which can maintain quaternion constraints through minimal parameterization and thus enhance the accuracy and stability of pose estimation.

Figure 1b illustrates the proposed pose-tracking system in a medical application, i.e., endoscope tracking. In this example, a magnetic sensor is embedded in the endoscope to measure the magnetic fields from the three coils. The system continuously tracks the position and orientation of the endoscope, which provides accurate and intuitive guidance for the operator in medical procedures.

In this section, the magnetic measurement model is shown first, which describes the relationship between the magnetic measurements and the 6-DoF pose of the sensor. Then, the strategy of sequential coil excitation is shown to explain the mechanism of magnetic field separation and disturbance rejection. Finally, the pose estimation algorithm using an error-state Kalman filter is described.

2.1. Magnetic Measurement Model

A uniaxial electromagnetic coil is commonly modeled as a magnetic dipole, whose magnetic field is given by [40], as detailed below:

$${}^w{\mathbf{B}}_e={\displaystyle \frac{{\mathsf{\mu }}_0}{4\mathsf{\pi }}} {\displaystyle \frac{3\left({}^w{\mathbf{m}}_e\cdot {}^w\mathbf{r}\right){}^w\mathbf{r}-{}^w{\mathbf{m}}_e{‖{}^w\mathbf{r}‖}^2}{{‖{}^w\mathbf{r}‖}^5}}$$

(1)

where ${\mathsf{\mu }}_0=4\pi \times {10}^{-7}\mathrm{T}·\mathrm{m}/\mathrm{A}$ is the magnetic permeability in a vacuum; ${}^w{\mathbf{m}}_e$ denotes the magnetic moment; ${}^w\mathbf{r}$ represents the relative position of the concerned point (e.g., the location of the sensor) with respect to the center of the coil; $‖\cdot ‖$ represents the Euclidean norm of a vector; and ${}^w{\mathbf{B}}_e$ is the magnetic flux intensity at point ${}^w\mathbf{r}$. The left superscript indicates the reference frame in which the vector is represented. Specifically, the superscript $\left\{w\right\}$ represents the world reference frame, and $\left\{s\right\}$ represents the sensor reference frame, as depicted in Figure 1a. The relative position of the sensor to the i-th coil ${}^w{\mathbf{r}}_i$ can be represented by the difference between the absolute position of sensor ${}^w\mathbf{p}$ and the absolute position of the coil ${{}^w\mathbf{r}}_{e,i}$, i.e., ${}^w{\mathbf{r}}_i={}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}$. Thus, for the i-th coil, Equation (1) evolves to the following:

$${}^w{\mathbf{B}}_{e,i}={\displaystyle \frac{{\mathsf{\mu }}_0}{4\mathsf{\pi }}} {\displaystyle \frac{3\left({}^w{\mathbf{m}}_e\cdot \left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)\right)\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)-{}^w{\mathbf{m}}_e{‖\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)‖}^2}{{‖\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)‖}^5}}$$

(2)

The magnetic field measurement by the sensor ${}^s{\mathbf{B}}_{e,i}$ is obtained by rotating the magnetic field ${}^w{\mathbf{B}}_{e,i}$ into the sensor frame $\left\{s\right\}$.

$${}^s{\mathbf{B}}_{e,i}={}_s{}^w{\mathbf{R}}^{\mathrm{T}}{\displaystyle \frac{{\mathsf{\mu }}_0}{4\mathsf{\pi }}} {\displaystyle \frac{3\left({}^w{\mathbf{m}}_e\cdot \left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)\right)\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)-{}^w{\mathbf{m}}_e{‖\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)‖}^2}{{‖\left({}^w\mathbf{p}-{{}^w\mathbf{r}}_{e,i}\right)‖}^5}}$$

(3)

where the ${}_s{}^w\mathbf{R}$ is a 3 × 3 rotation matrix transforming vectors from the sensor frame to the world frame, which can represent the orientation of the sensor. To conveniently include the 3-DoF orientation into the state vector, the unit quaternion ${}_s{}^w\mathbf{q}={\left[\begin{array}{cc}{q}_s& {\mathbf{q}}_v^T\end{array}\right]}^T={\left[\begin{array}{cc}\begin{array}{cc}{q}_s& q_x\end{array}& \begin{array}{cc}{q}_y& q_z\end{array}\end{array}\right]}^T$ is used to represent the orientation in pose estimation. The transformation between the rotation matrix and the unit quaternion is given by the following:

$${}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}=\left[\begin{array}{ccc}{q}_s^2+q_x^2-q_y^2-q_z^2& 2q_x{q}_y-2q_s{q}_z& 2q_x{q}_z+2{q_{s}q}_y\\ 2q_x{q}_y+2q_s{q}_z& q_s^2-q_x^2+q_y^2-q_z^2& 2q_y{q}_z-2q_s{q}_x\\ 2q_x{q}_z-2{q_{s}q}_y& 2q_y{q}_z+2q_s{q}_x& q_s^2-q_x^2-q_y^2+q_z^2\end{array}\right].$$

(4)

It is worth noting that magnetic measurement ${}^s{\mathbf{B}}_{e,i}$ for each coil depends on both the position ${}^w\mathbf{p}$ and the orientation ${}_s{}^w\mathbf{q}$ of the sensor. By stacking the magnetic measurements from all three electromagnetic coils, the magnetic measurement model of the tracking system is given below:

$$\mathbf{y}=\left[\begin{array}{c}{}^s{\mathbf{B}}_{e,1}\\ {}^s{\mathbf{B}}_{e,2}\\ {}^s{\mathbf{B}}_{e,3}\end{array}\right]=h_e\left({}^w\mathbf{p},{}_s{}^w\mathbf{q}\right).$$

(5)

2.2. Sequential Coil Excitation for Field Separation and Disturbance Rejection

2.2.1. Working Principle of Sequential Coil Excitation

It is known that the 6-DoF pose of the sensor cannot be uniquely determined by measuring the magnetic field of a single magnetic dipole source. Therefore, three electromagnetic coils are used in our proposed tracking system. If the three coils are excited simultaneously, their magnetic fields are superposed and mixed. However, to utilize the dipole-based magnetic measurement model (5) for pose estimation, the magnetic measurements of three coils should be separated. There are two methods of field separation, i.e., time-division multiplexing (TDM) and frequency-division multiplexing (FDM). In the TDM method, multiple coils are sequentially excited and their magnetic signals are independently collected. In the FDM method, multiple coils are simultaneously excited but at different frequencies. The mixed magnetic signals are then demodulated using band-pass filters to separate the signals from different magnetic sources. Since the FDM method requires complicated signal processing, the TDM method (sequential excitation) is used to separate the magnetic measurements from different electromagnets.

Figure 2a illustrates the circuit diagram for electromagnetic coil control. Each coil has an individual electronic switch (MOSFET, AOD4185 Alpha and Omega Semiconductor Ltd., Sunnyvale, CA, USA). When the control signal $V_{gs}$ is set high, the electronic switch is turned on and the corresponding coil is activated. The electromagnetic coil is modeled as an inductor and resistor in serial connection as shown in Figure 2a. Consequently, as illustrated in Figure 2b, when the coil is activated, the current through the coil exhibits a first-order response to the step voltage input. This dynamic response is described by the transfer function:

$$I\left(s\right)={\displaystyle \frac{K_e}{Ts+1}}V\left(s\right)$$

(6)

where $I\left(s\right)$ and $V\left(s\right)$ are coil current and voltage in the complex frequency domain; $K_e$ is the constant gain; and $T$ is the time constant of the first-order system. Since the magnitude of magnetic moment $‖{}^w{\mathbf{m}}_e‖$ of the coil is proportional to the current $I$, the magnetic field also experiences a first-order process before it settles down. A microcontroller is used to control the excitation of coils, including sending switching signals to activate the coil sequentially and taking stable measurements with an appropriate delay. After activating the coil, the microcontroller delays for a period of time $t_s$ larger than 4$T$ to take measurements of the magnetic field, where 4$T$ is the time required for the current response to settle down (reach approximately 98.2% of the final value). This delay ensures the successful measurement of a steady magnetic field. The time constant for an electromagnetic coil is given by $T={\displaystyle \frac{L}{R}}$, where $L$ is the inductance and $R$ is the resistance of the coil.

Table 1. Electric parameters and time constant of the coils.

	Inductance/mH	Resistance/Ω	Time Constant/ms
Coil 1	4.27	4.35	0.98
Coil 2	8.40	4.74	1.77
Coil 3	8.41	4.67	1.80

shows the electric parameters of coils in the experiment from an LCR meter (UT622A, Uni-Trend Technology Corporation, Ltd., Dongguan, China) and the corresponding theoretical time constant of the three coils. The electromagnetic coils are designed to have a small $T$ (<2 ms) and, thus, a small delay time $t_s$ (<8 ms).

As illustrated in Figure 2b, the three coils are activated sequentially. When the previous coil is switched off and enters its falling response, the next coil is immediately switched on and its current begins to rise. After a delay of $t_s$, the current of the previous coil drops to zero while the current of the activated coil reaches a steady state. The delay time $t_s$ is set to be 8 ms to accommodate the coil with the largest time constant, which ensures a stable sampling of the magnetic field from only the activated coil. At the end of a measurement cycle, all coils are deactivated to allow measurement of the ambient background magnetic field for disturbance cancellation. In each update step of pose estimation, it takes approximately 30 ms in a full measurement cycle to sample the magnetic field from three coils and the background magnetic field. This update time of magnetic field sampling is sufficient for real-time and accurate pose tracking.

2.2.2. Field Separation and Disturbance Rejection

When the i-th electromagnetic coil is excited, the magnetic sensor measures the magnetic field from the i-th electromagnetic coil along with the background field, as given by the following:

$${}^s{\mathbf{B}}_{m,i}={}^s{\mathbf{B}}_{e,i}+{}^s{\mathbf{B}}_b$$

(7)

where ${}^s{\mathbf{B}}_b$ is the background field, primarily consisting of geomagnetic fields and potential disturbance fields. When all three coils are deactivated, the magnetic sensor measures only the background magnetic field.

$${}^s{\mathbf{B}}_{m,0}={}^s{\mathbf{B}}_b.$$

(8)

By taking the difference between the activated measurement ${}^s{\mathbf{B}}_{m,i}$ and the deactivated measurement ${}^s{\mathbf{B}}_{m,0}$ in each measurement cycle, the separated magnetic field from each coil with no background field is acquired as follows:

$${}^s{\mathbf{B}}_{e,i}={}^s{\mathbf{B}}_{m,i}-{}^s{\mathbf{B}}_{m,0}.$$

(9)

The separated magnetic fields of electromagnetic coils are then used as the effective magnetic measurements, shown in Equation (5), for pose estimation.

The sequential coil excitation strategy also contributes to disturbance rejection. The potential disturbance field in the background field is also canceled when taking the difference between the activated and deactivated measurements as shown in Equation (9). It is assumed that the environmental magnetic disturbances remain unchanged in a complete measurement cycle. This assumption holds true as long as the period of the measurement cycle is sufficiently short.

2.3. Error-State Kalman Filter for Pose Estimation

The Kalman filter (KF) is a well-known optimal state estimator widely used in various applications. The extended Kalman filter (EKF) broadens the linear KF by linearizing the system kinematics and measurement models around the current state estimate using a first-order Talyor expansion, enabling accurate estimation in nonlinear systems. However, the standard EKF treats the state space as a Euclidean space ${\mathbb{R}}^n$, which is not appropriate for the pose estimation problem where the orientation is represented using quaternions on the unit quaternions group $S^3$. Specifically, the unit quaternion has only three degrees of freedom, whereas the standard EKF treats it as a vector in ${\mathbb{R}}^4$ with a missing constraint. The constrained EKF (CEKF) was proposed to mitigate this problem by enforcing unity constraints on the quaternion, forcing the redundant parameterized states back to the manifold. However, enforcing constraints might cause singularities on state covariance matrices, which impairs the stability of the system.

As shown in Figure 3, the error-state Kalman Filter (ESKF) operates the system on its equivalent minimally parameterized error space (i.e., the local tangent space) of the manifold, which thereby avoids singularities and redundancy in state variables and enhances the accuracy and stability of the pose-tracking system. The ESKF has been previously applied in inertial pose estimation but has never been used in magnetic pose estimation. In this paper, the ESKF is employed in the proposed 6-DoF electromagnetic pose-tracking system to improve the performance of orientation estimation. The performance of ESKF is also compared with the prevalent pose estimation algorithms of standard EKF and CEKF. In this section, all these estimation algorithms are briefly introduced.

2.3.1. Standard EKF

The state vector $\mathbf{x}$ to be estimated consists of a 3-DoF position, translational velocity ${}^w\mathbf{v}={\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]}^{\mathrm{T}}$, and unit quaternion ${}_s{}^w\mathbf{q}\in S^3$ representing a 3-DoF orientation, which is outlined below:

$$\mathbf{x}=\left[\begin{array}{c}{}^w\mathbf{p}\\ {}^w\mathbf{v}\\ {}_s{}^w\mathbf{q}\end{array}\right].$$

(10)

In pose-tracking applications, the sensor can be put on any freely moving object whose kinematic model might be unknown. Thus, a trivial constant-velocity and constant-orientation kinematic model is used to describe the motion of the object, as given by the following:

$$\begin{array}{l}{}^w\dot{\mathbf{p}}={}^w\mathbf{v}\\ {}^w\dot{\mathbf{v}}={}^w\mathbf{a}\\ {}_s{}^w\dot{\mathbf{q}}={\displaystyle \frac{1}{2}}{}_s{}^w\mathbf{q}\otimes {}^s\mathsf{\omega }\end{array}$$

(11)

where ${}^w\mathbf{a}={\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]}^{\mathrm{T}}$ is assumed to be a zero-mean Gaussian noise that represents the unmodeled acceleration. ${}^s\mathsf{\omega }={\left[\begin{array}{ccc}{\omega }_x& {\omega }_y& {\omega }_z\end{array}\right]}^{\mathrm{T}}$ is also assumed to be a zero-mean Gaussian noise, representing the unmodeled angular velocity. Here, $\otimes$ denotes the quaternion product, which can also be represented by a matrix product as follows:

$${}_s{}^w\dot{\mathbf{q}}={\displaystyle \frac{1}{2}}{}_s{}^w\mathbf{q}\otimes {}^s\mathsf{\omega }={\displaystyle \frac{1}{2}}{\left[{}_s{}^w\mathbf{q}\right]}_L{}^s\mathsf{\omega }$$

(12)

where ${\left[{}_s{}^w\mathbf{q}\right]}_L$ is the left-quaternion-product matrix, detailed below:

$${\left[{}_s{}^w\mathbf{q}\right]}_L={\displaystyle \frac{1}{2}}\left[\begin{array}{c}\begin{array}{ccc}-q_x& -q_y& -q_z\\ q_s& -q_z& q_y\end{array}\\ \begin{array}{ccc}{q}_z& q_s& -q_x\\ -q_y& q_x& q_s\end{array}\end{array}\right]$$

(13)

Discretizing the system kinematic model yields the following:

$$\begin{array}{l}{}^w{\mathbf{p}}_{k+1}={}^w{\mathbf{p}}_k+\mathsf{\Delta }t{}^w{\mathbf{v}}_k+{\displaystyle \frac{1}{2}}\mathsf{\Delta }{t}^2{{}^w\mathbf{a}}_k\\ {}^w{\mathbf{v}}_{k+1}={}^w{\mathbf{v}}_k+\mathsf{\Delta }t{{}^w\mathbf{a}}_k\\ {}_s{}^w{\mathbf{q}}_{k+1}={\displaystyle \frac{1}{2}}\mathsf{\Delta }t{\left[{}_s{}^w{\mathbf{q}}_k\right]}_L{}^s{\mathsf{\omega }}_k\end{array}$$

(14)

where $\mathsf{\Delta }t$ is the time step, which is equal to the sampling time of a complete measurement cycle. The discrete system process model (14) can be reformulated into a compact matrix form as follows:

$$\begin{array}{cc}\hfill {\mathbf{x}}_{k+1}& =\mathbf{A}{\mathbf{x}}_k+{\mathbf{G}}_k{\mathbf{w}}_k\hfill \\ \hfill \mathbf{A}& =\left[\begin{array}{ccc}{\mathbf{I}}_3& \mathsf{\Delta }t{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_3\end{array}\right]\hfill \\ \hfill {\mathbf{G}}_k& =\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}{\mathsf{\Delta }t}^2{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ \mathsf{\Delta }t{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{4\times 3}& {\displaystyle \frac{1}{2}}\mathsf{\Delta }t{\left[{}_s{}^w{\mathbf{q}}_k\right]}_L\end{array}\right]\hfill \end{array}$$

(15)

where ${\mathbf{I}}_n$ represents $n\times n$ identity matrices; and ${\mathbf{0}}_{m\times n}$ denotes null matrices with the dimension of $m\times n$. ${\mathbf{w}}_k={\left[\begin{array}{cc}{{}^w\mathbf{a}}_k^{\mathrm{T}}& {}^s{\mathsf{\omega }}_k^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ is the concatenation of the process noises ${{}^w\mathbf{a}}_k$ and ${}^s{\mathsf{\omega }}_k$, which is modeled as a zero-mean Gaussian noise, outlined below:

$${\mathbf{w}}_k~N\left(\mathbf{0},\mathbf{Q}\right)$$

(16)

where the notion $N\left(\mathsf{\mu },\mathsf{\Sigma }\right)$ represents a Gaussian distribution with its mean $\mathsf{\mu }$ and covariance $\mathsf{\Sigma }$. The covariance matrix of the process noise ${\mathbf{Q}}_k$ is defined by the following:

$${\mathbf{Q}}_k=E\left({\mathbf{w}}_k{\mathbf{w}}_k^{\mathrm{T}}\right)$$

(17)

where $E\left(\cdot \right)$ is the expectation of the random variable. The process noise covariance $\mathbf{Q}$ is a tuning parameter that can be adjusted to achieve the desired performance of estimation.

The measurement model is simply given by Equation (5) with additive measurement noise.

$${\mathbf{y}}_k=h_e\left({\mathbf{x}}_k\right)+{\mathbf{B}}_{n,k}$$

(18)

where ${\mathbf{B}}_{n,k}$ is the measurement noise vector, which is also assumed to be zero-mean and Gaussian.

$${\mathbf{B}}_{n,k}~N\left(\mathbf{0},{\mathbf{R}}_k\right)$$

(19)

where the covariance matrix of measurement noise ${\mathbf{R}}_k$ is defined as follows:

$${\mathbf{R}}_k=E\left({\mathbf{B}}_{n,k}{\mathbf{B}}_{n,k}^{\mathrm{T}}\right)$$

(20)

which is set according to the sensor noise in the experiment. The Jacobian of the measurement model ${\mathbf{H}}_k$ is given by the following:

$$\begin{array}{cc}\hfill {\mathbf{H}}_k& ={\left.{\displaystyle \frac{\partial h_e\left({\mathbf{x}}_k\right)}{\partial \mathbf{x}}}\right|}_{\mathbf{x}={\hat{\mathbf{x}}}_k^{-}}=\left[\begin{array}{ccc}{\mathbf{H}}_p& {\mathbf{H}}_v& {\mathbf{H}}_q\end{array}\right]\hfill \\ \hfill {\mathbf{H}}_p& =\left[\begin{array}{c}{}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}{\displaystyle \frac{\partial {}^w{\mathbf{B}}_{e,1}}{\partial {}^w\mathbf{p}}}\\ {}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}{\displaystyle \frac{\partial {}^w{\mathbf{B}}_{e,2}}{\partial {}^w\mathbf{p}}}\\ {}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}{\displaystyle \frac{\partial {}^w{\mathbf{B}}_{e,3}}{\partial {}^w\mathbf{p}}}\end{array}\right]\hfill \\ \hfill {\mathbf{H}}_v& ={\mathbf{0}}_{9\times 3}\hfill \\ \hfill {\mathbf{H}}_q& =2\left[\begin{array}{cc}{q}_s{}^w{\mathbf{B}}_{e,1}-{\mathbf{q}}_v\times {}^w{\mathbf{B}}_{e,1}& {\mathbf{q}}_{\mathit{v}}^{\mathrm{T}}{}^w{\mathbf{B}}_{e,1}{\mathbf{I}}_3+{\mathbf{q}}_v{}^w{\mathbf{B}}_{e,1}^T-{}^w{\mathbf{B}}_{e,1}{\mathbf{q}}_v^{\mathrm{T}}+q_s{\left[{}^w{\mathbf{B}}_{e,1}\right]}_{\times }\\ q_s{}^w{\mathbf{B}}_{e,2}-{\mathbf{q}}_v\times {}^w{\mathbf{B}}_{e,2}& {\mathbf{q}}_{\mathit{v}}^{\mathrm{T}}{}^w{\mathbf{B}}_{e,2}{\mathbf{I}}_3+{\mathbf{q}}_v{}^w{\mathbf{B}}_{e,2}^T-{}^w{\mathbf{B}}_{e,2}{\mathbf{q}}_v^{\mathrm{T}}+q_s{\left[{}^w{\mathbf{B}}_{e,2}\right]}_{\times }\\ q_s{}^w{\mathbf{B}}_{e,3}-{\mathbf{q}}_v\times {}^w{\mathbf{B}}_{e,3}& {\mathbf{q}}_{\mathit{v}}^{\mathrm{T}}{}^w{\mathbf{B}}_{e,3}{\mathbf{I}}_3+{\mathbf{q}}_v{}^w{\mathbf{B}}_{e,3}^T-{}^w{\mathbf{B}}_{e,3}{\mathbf{q}}_v^{\mathrm{T}}+q_s{\left[{}^w{\mathbf{B}}_{e,3}\right]}_{\times }\end{array}\right]\hfill \end{array}$$

(21)

where ${\mathbf{H}}_p$, ${\mathbf{H}}_v$, and ${\mathbf{H}}_q$ are sub-Jacobian matrices corresponding to position, velocity, and quaternion components, respectively; and ${\left[\cdot \right]}_{\times }$ represents the skew-symmetric matrix.

With the kinematic model, measurement model, and stochastic noise defined above, the standard EKF can be implemented for pose estimation [30]:

$$\begin{array}{cc}\hfill {\hat{\mathbf{x}}}_{k+1}^{-}& =\mathbf{A}{\hat{\mathbf{x}}}_k^{+}\hfill \\ \hfill {\mathbf{P}}_{k+1}^{-}& =\mathbf{A}{\mathbf{P}}_k^{+}{\mathbf{A}}^{\mathrm{T}}+{\mathbf{G}}_k{\mathbf{Q}}_k{\mathbf{G}}_k^{\mathrm{T}}\hfill \\ \hfill {\mathbf{K}}_{k+1}& ={\mathbf{P}}_{k+1}^{-}{\mathbf{H}}_{k+1}^{\mathrm{T}}{\left({\mathbf{H}}_{k+1}{\mathbf{P}}_{k+1}^{-}{\mathbf{H}}_{k+1}^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\hfill \\ \hfill {\hat{\mathbf{x}}}_{k+1}^{+}& ={\hat{\mathbf{x}}}_{k+1}^{-}+{\mathbf{K}}_{k+1}\left[{\mathbf{y}}_{k+1}-h_e\left({\hat{\mathbf{x}}}_{k+1}^{-}\right)\right]\hfill \\ \hfill {\mathbf{P}}_{k+1}^{+}& =\left({\mathbf{I}}_{10}-{\mathbf{K}}_{\mathrm{k}+1}{\mathbf{H}}_{k+1}\right){\mathbf{P}}_{k+1}^{-}\hfill \end{array}$$

(22)

The standard EKF provides a basic framework for electromagnetic pose estimation; however, the problem of quaternion constraint violation caused by redundant parameterization remains unresolved for the standard EKF.

2.3.2. Constrained EKF

The EKF-based estimation algorithms that enforce the unity constraint of quaternion are called constrained EKF [30]. The unity constraint of quaternion is given as follows:

$$q_s^2+q_x^2+q_y^2+q_z^2=1.$$

(23)

There are many ways of enforcing the constraint while we employ the perfect measurement method [32] to enforce the unity constraint of quaternion on the basis of the standard EKF framework. This method treats the unity constraint as a measurement with zero noise.

$${1=h}_c\left(\mathbf{x}\right)={‖{}_s{}^w\mathbf{q}‖}^2$$

(24)

where $h_c(·)$ is the constraint measurement function. The corresponding Jacobian of the constraint measurement model is given by the following:

$${\mathbf{D}}_k={\left.{\displaystyle \frac{\partial h_c\left({\mathbf{x}}_k\right)}{\partial \mathbf{x}}}\right|}_{\mathbf{x}={\hat{\mathbf{x}}}_k^{-}}={\left[\begin{array}{cc}{\mathbf{0}}_{1\times 6}& \begin{array}{cc}\begin{array}{cc}2q_s& 2q_x\end{array}& \begin{array}{cc}2q_y& 2q_z\end{array}\end{array}\end{array}\right]}^T$$

(25)

Similar to the measurement update step in the EKF framework (22), the constraint “measurement” is included in the algorithm as an additional correction step.

$$\begin{array}{cc}\hfill {\tilde{\mathbf{K}}}_{k+1}& ={\mathbf{P}}_{k+1}^{+}{\mathbf{D}}_{k+1}^{\mathrm{T}}{\left({\mathbf{D}}_{k+1}{\mathbf{P}}_{k+1}^{+}{\mathbf{D}}_{k+1}^{\mathrm{T}}\right)}^{-1}\hfill \\ \hfill {\tilde{\mathbf{x}}}_{k+1}^{+}& ={\hat{\mathbf{x}}}_{k+1}^{+}+{\tilde{\mathbf{K}}}_{k+1}\left[1-h_c\left({\hat{\mathbf{x}}}_{k+1}^{+}\right)\right]\hfill \\ \hfill {\tilde{\mathbf{P}}}_{k+1}^{+}& =\left({\mathbf{I}}_{10}-{\tilde{\mathbf{K}}}_{k+1}{\mathbf{D}}_{k+1}\right){\mathbf{P}}_{k+1}^{+}\hfill \end{array}$$

(26)

where the variables with ”$~$” represent the constrained variables. This approach adds constraint measurement to project the quaternion onto the unit sphere, which maintains the validity of the orientation representation throughout the estimation process. However, this method cannot ensure that the constraint is satisfied perfectly. Additionally, a singular state covariance might appear and cause numerical problems such as ill-conditioning in the state covariance estimation [35].

2.3.3. ESKF

The Error-State Kalman Filter addresses the limitations of the standard EKF and constrained EKF by utilizing an error-state formulation. This approach ensures minimal parameterization of the incremental orientation update, thereby eliminating the need for extra constraints to maintain a valid quaternion orientation representation. The core idea of the ESKF is to operate with the minimal parameterized error state $\delta \mathbf{x}$ in parallel with the nominal state $\mathbf{x}$ as defined in (10). The nominal state is corrected by the error state to obtain an accurate estimate of the true state.

The error-state vector $\delta \mathbf{x}$ captures small deviations from the nominal state, including position error $\delta \mathbf{p}$, velocity error $\delta \mathbf{v}$, and the orientation error represented by angle vector $\delta \mathsf{\theta }\in {\mathbb{R}}^3$.

$$\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathsf{\theta }\end{array}\right]$$

(27)

where the quaternion error is related with the angle error by $\delta \mathbf{q}=\exp \left(\delta \mathsf{\theta }/2\right)$. It is noted that the dimension of $\delta \mathsf{\theta }$ is three so it is the minimal parametrization of the incremental orientation with no redundancy.

In the ESKF, the kinematic model of the nominal state is assumed to have zero noise, as given by (28).

$$\begin{array}{l}{}^w\dot{\mathbf{p}}={}^w\mathbf{v}\\ {}^w\dot{\mathbf{v}}=\mathbf{0}\\ {}_s{}^w\dot{\mathbf{q}}=\mathbf{0}\end{array}$$

(28)

The process noise is considered in the kinematic model of the error state.

$$\begin{array}{l}\delta \dot{\mathbf{p}}=\delta \mathbf{v}\\ \delta \dot{\mathbf{v}}={}^w\mathbf{a} \\ \delta \dot{\mathsf{\theta }}={}_s{}^w\mathbf{R}{}^s\mathsf{\omega }+{\displaystyle \frac{1}{2}}{\left[\delta \mathsf{\theta }\right]}_{\times }{}_s{}^w\mathbf{R}{}^s\mathsf{\omega }\end{array}$$

(29)

Discretizing the error-state kinematics yields the following:

$$\begin{array}{l}\delta {\mathbf{p}}_{k+1}=\delta {\mathbf{p}}_k+\mathsf{\Delta }t\delta {\mathbf{v}}_k+{\displaystyle \frac{1}{2}}{\mathsf{\Delta }t}^2{}^w{\mathbf{a}}_k\\ \delta {\mathbf{v}}_{k+1}=\delta {\mathbf{v}}_k+\mathsf{\Delta }t{}^w{\mathbf{a}}_k\\ \delta {\mathsf{\theta }}_{k+1}=\delta {\mathsf{\theta }}_k+\mathsf{\Delta }t{}_s{}^w\mathbf{R}\left\{{{}_s{}^w\mathbf{q}}_k\right\}{}^s{\mathsf{\omega }}_k+{\displaystyle \frac{1}{2}}\mathsf{\Delta }t{\left[\delta {\mathsf{\theta }}_k\right]}_{\times }{}_s{}^w\mathbf{R}\left\{{{}_s{}^w\mathbf{q}}_k\right\}{}^s{\mathsf{\omega }}_k\end{array}.$$

(30)

The discrete error-state kinematic model can be reformulated into a compact form.

$$\begin{array}{cc}\hfill {\delta \mathbf{x}}_{k+1}& =\mathbf{A}{\delta \mathbf{x}}_k+{\mathbf{G}}_{\delta x,k}{\mathbf{w}}_k\hfill \\ \hfill {\mathbf{G}}_{\delta x,k}& =\left[\begin{array}{cc}{\displaystyle \frac{1}{2}}{\mathsf{\Delta }t}^2{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ \mathsf{\Delta }t{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& \mathsf{\Delta }t{}_s{}^w\mathbf{R}\left\{{{}_s{}^w\mathbf{q}}_k\right\}+{\displaystyle \frac{1}{2}}\mathsf{\Delta }t{\left[\delta {\mathsf{\theta }}_k\right]}_{\times }{}_s{}^w\mathbf{R}\left\{{{}_s{}^w\mathbf{q}}_k\right\}\end{array}\right]\hfill \end{array}$$

(31)

The measurement model in ESKF is the same as Equation (18), while the Jacobian of the measurement model with respect to the error state ${\mathbf{H}}_{\delta x,k}$ is defined as detailed below:

$$\begin{array}{cc}\hfill {\mathbf{H}}_{\delta x,k}& ={\left.{\displaystyle \frac{\partial h_e\left(\mathbf{x}\right)}{\partial \mathbf{x}}}\right|}_{\mathbf{x}={\hat{\mathbf{x}}}_k^{-}}\cdot {\left.{\displaystyle \frac{\partial \mathbf{x}}{\partial \delta \mathbf{x}}}\right|}_{\mathbf{x}={\hat{\mathbf{x}}}_k^{-}}\hfill \\ & ={\mathbf{H}}_k{\mathbf{C}}_k\hfill \end{array}$$

(32)

where

$$\begin{array}{cc}\hfill {\mathbf{C}}_k& =\left[\begin{array}{ccc}{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{4\times 3}& {\mathbf{0}}_{4\times 3}& {\mathbf{Q}}_{\delta \mathit{x}}\end{array}\right]\hfill \\ \hfill {\mathbf{Q}}_{\delta \mathit{x}}& ={\displaystyle \frac{1}{2}}\left[\begin{array}{ccc}\begin{array}{c}\begin{array}{c}-q_x\\ q_s\end{array}\\ \begin{array}{c}-q_z\\ q_y\end{array}\end{array}& \begin{array}{c}\begin{array}{c}-q_y\\ q_z\end{array}\\ \begin{array}{c}{q}_s\\ -q_x\end{array}\end{array}& \begin{array}{c}\begin{array}{c}-q_z\\ -q_y\end{array}\\ \begin{array}{c}{q}_x\\ q_s\end{array}\end{array}\end{array}\right]\hfill \end{array}$$

(33)

With the kinematic model and measurement model defined above, the ESKF algorithm can be implemented for pose estimation, as shown in Algorithm 1.

Algorithm 1: ESKF Algorithm for Electromagnetic Pose Tracking

Input: Magnetic measurement ${\mathbf{y}}_{k+1}$, state estimate ${\hat{\mathbf{x}}}_k^{+}$, and error-state covariance

${\tilde{\mathbf{P}}}_{\delta x,k}^{+}$ at the previous iteration step $k$

Output: state estimate ${\hat{\mathbf{x}}}_{k+1}^{+}$ and state covariance ${\tilde{\mathbf{P}}}_{\delta x,k+1}^{+}$ at the current step $k+1$

Nominal-state propagation:

${\hat{\mathbf{x}}}_{k+1}^{-}=\mathbf{A}{\hat{\mathbf{x}}}_k^{+}$

Error-state propagation:

$\begin{array}{cc}\hfill {\delta \hat{\mathbf{x}}}_{k+1}^{-}& =\mathbf{A}\delta {\hat{\mathbf{x}}}_k^{+}\hfill \\ \hfill {\mathbf{P}}_{\delta x,k+1}^{-}& =\mathbf{A}{\tilde{\mathbf{P}}}_{\delta x,k}^{+}{\mathbf{A}}^{\mathrm{T}}+{\mathbf{G}}_{\delta x,k}{\mathbf{Q}}_k{\mathbf{G}}_{\delta x,k}^{\mathrm{T}}\hfill \end{array}$

Error-state measurement update:

$\begin{array}{cc}\hfill {\mathbf{K}}_{\delta x,k+1}& ={\mathbf{P}}_{\delta x,k+1}^{-}{\mathbf{H}}_{\delta x,k+1}^{\mathrm{T}}{\left({\mathbf{H}}_{\delta x,k+1}{\mathbf{P}}_{\delta x,k+1}^{-}{\mathbf{H}}_{\delta x,k+1}^{\mathrm{T}}+{\mathbf{R}}_k\right)}^{-1}\hfill \\ \hfill {\delta \hat{\mathbf{x}}}_{k+1}^{+}& ={\delta \hat{\mathbf{x}}}_{k+1}^{-}+{\mathbf{K}}_{\delta x,k+1}\left[{\mathbf{y}}_{k+1}-h_e\left({\hat{\mathbf{x}}}_{k+1}^{-}\right)\right]\hfill \\ \hfill {\mathbf{P}}_{\delta x,k+1}^{+}& =\left({\mathbf{I}}_9-{\mathbf{K}}_{\delta x,k+1}{\mathbf{H}}_{\delta x,k+1}\right){\mathbf{P}}_{\delta x,k+1}^{-}\hfill \end{array}$

Nominal-state correction:

$\begin{array}{l}{{}^w\hat{\mathbf{p}}}_{k+1}^{+}={{}^w\hat{\mathbf{p}}}_{k+1}^{-}+\delta {\hat{\mathbf{p}}}_{k+1}^{+}\\ {{}^w\hat{\mathbf{v}}}_{k+1}^{+}={{}^w\hat{\mathbf{v}}}_{k+1}^{-}+\delta {\hat{\mathbf{v}}}_{k+1}^{+}\\ {{}_{\mathit{s}}{}^w\hat{\mathbf{q}}}_{k+1}^{+}=\mathrm{e}\mathrm{x}\mathrm{p}\left(\delta {\hat{\mathsf{\theta }}}_{k+1}^{+}/2\right)\otimes {{}_{\mathit{s}}{}^w\hat{\mathbf{q}}}_{k+1}^{-}\end{array}$

Error-state reset:

$\begin{array}{cc}\hfill {\delta \hat{\mathbf{x}}}_{k+1}^{+}& =0\hfill \\ \hfill {\tilde{\mathbf{P}}}_{\delta x,k+1}^{+}& ={\mathbf{U}}_{k+1}{\mathbf{P}}_{\delta x,k+1}^{+}{\mathbf{U}}_{k+1}^T\hfill \end{array}$

Figure 4 provides a flowchart of major steps in the ESKF algorithm. It is seen that the nominal state is propagated according to the kinematic model (28). The propagation of the error state is trivial since the error-state estimate is reset to zero after correcting the nominal state and it remains zero in the propagation step. The error state, instead of the nominal state, is updated using the magnetic measurements. Once the error state is obtained, it is added to the nominal state to obtain the final pose estimate.

It is noted that the addition of orientation is implemented in the unit quaternions group $S^3$, which obviously preserves the unity of the corrected quaternion. In addition, the covariance of the error state is propagated and updated. There is no need to propagate and update the covariance of the nominal state. It is noted that the error-state covariance needs to be reset by (34) at the end of each estimation step.

$${\tilde{\mathbf{P}}}_{\delta x,k+1}^{+}={\mathbf{U}}_{k+1}{\mathbf{P}}_{\delta x,k+1}^{+}{\mathbf{U}}_{k+1}^{\mathrm{T}}$$

(34)

where

$${\mathbf{U}}_{k+1}=\left[\begin{array}{ccc}{\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_3& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_3+{\left[\delta {\hat{\mathsf{\theta }}}_{k+1}^{+}/2\right]}_{\times }\end{array}\right].$$

(35)

In summary, the ESKF offers an elegant solution to the challenges encountered in quaternion-based pose estimation. Figure 3 demonstrates the process of quaternion estimation using the ESKF, where the measurement update steps are conducted on the tangent space and subsequently mapped back to the manifold. By utilizing a minimal parameterization of the incremental orientation in measurement update steps, i.e., small angle vector $\delta \mathsf{\theta }$, the ESKF maintains the quaternion unity without the need to enforce additional constraints. This ensures accurate orientation estimation in the electromagnetic pose-tracking system. Furthermore, ESKF reduces the risk of singularity in the state covariance caused by redundant parameterization and, thus, enhances the stability of the pose estimation system.

3. Simulation Results

A simulation is conducted to evaluate the performance of the proposed ESKF algorithm in electromagnetic pose estimation, particularly under the conditions where the rotation of the sensor is rapid. The ESKF is also compared with the standard EKF and CEKF. The simulation of the fast-rotation scenario is used to highlight the difference between the three estimation algorithms and reveal the advantage of the proposed ESKF algorithm.

The magnetic moments and center positions of three electromagnetic coils used in the simulation are given below.

$$\begin{array}{cc}\hfill {}^w{\mathbf{m}}_1& ={\left[\begin{array}{ccc}0& 2.5& 0\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,1}& ={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \\ \hfill {}^w{\mathbf{m}}_2& ={\left[\begin{array}{ccc}0& 0& 2.5\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,2}& ={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \\ \hfill {}^w{\mathbf{m}}_3& ={\left[\begin{array}{ccc}0& 0& 2.5\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,3}& ={\left[\begin{array}{ccc}-0.05& 0& 0\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \end{array}$$

The sensor noise in the simulation is set to be zero-mean, white, and Gaussian with a standard deviation of ${10}^{-7}\mathrm{T}$. As shown in Figure 5, the sensor moves along a helical trajectory. For translation, the trajectory started from position $\left[\begin{array}{ccc}100& 0& 2\end{array}00\right]\mathrm{m}\mathrm{m}$, the velocity in z-axis is $0.001$ m/s, the projection of the trajectory on the XOY-plane is a circular trajectory with a radius of $0.1$ m and an angular rate of $0.0018$ $\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s}$. The rotational speed of the sensor is ${{}^s\mathsf{\omega }}_t={\left[\begin{array}{ccc}0.105& 0.105& 0.105\end{array}\right]}^T\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s}$ throughout the trajectory. The initial point in the algorithms is set close to the ground truth. The entire simulation lasts for 60 s with an update rate of 100 Hz. The covariance for process and measurement noise of all three estimation algorithms are set as follows:

$$\begin{array}{cc}\hfill {\mathbf{Q}}_k& =\left[\begin{array}{cc}164 {\mathbf{I}}_3{\left(\mathrm{m}/{\mathrm{s}}^2\right)}^2& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& 92 {\mathbf{I}}_3{\left(\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s}\right)}^2\end{array}\right]\hfill \\ \hfill {\mathbf{R}}_k& =10^{-14}{\mathbf{I}}_9{\left(\mathrm{T}\right)}^2\hfill \end{array}$$

(36)

Figure 6 illustrates the pose tracking error over time in the simulation. In Figure 6a, the standard EKF exhibits the largest position tracking error and shows a drift beyond 5 mm in the z-axis. As shown in

Table 2. RMS pose tracking errors in simulation.

		Standard EKF	CEKF	ESKF
Position Error (mm)	x	2.12	1.75	1.33
	y	1.31	1.36	0.96
	z	3.14	0.84	0.57
	Euclidean	4.02	2.37	1.74
Orientation error (°)	$\mathrm{Yaw} (\psi$)	0.46	0.61	0.42
	$\mathrm{Pitch} (\theta$)	0.45	0.45	0.39
	$\mathrm{Roll} (\varphi$)	0.49	0.60	0.45
	Average	0.47	0.55	0.42

, the ESKF has the smallest position and orientation error. Specifically, the Euclidean position error of ESKF is 1.74 mm and the average Euler angle error of the ESKF is 0.42°. It is noted that the proposed ESKF shows a significantly smaller position error while it has a limited advantage in orientation tracking.

To explain the advantage of the ESKF in pose tracking, the norm of the estimated quaternion using the standard EKF, CEKF, and the ESKF is analyzed. As illustrated in Figure 7, the norm of the estimated quaternion using standard EKF keeps increasing, deviating from unity continuously and reaching up to 1.036. It is known that the position and orientation are coupled in the electromagnetic pose-tracking system (see the measurement models (3) and (5)). Therefore, the violation of the quaternion constraint means that the determinant of rotation matrix ${}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}$ also deviates from unity, thereby distorting the translation part in measurement model (3) and causing large position errors. The EKF shows an increasing position error in Figure 6a because of the growing violation of the quaternion constraint. The subfigure of Figure 7 shows the norm of the estimated quaternion using the CEKF and ESKF. The maximum norm error of the CEKF is $7.32\times {10}^{-5}$ and that of ESKF is only 5$.32\times {10}^{-15}$, which is close to the magnitude of machine precision in MATLAB ($ϵ=2.22\times {10}^{-16}$). Hence, the ESKF has the smallest position tracking error.

In summary, the proposed electromagnetic pose-tracking system using ESKF demonstrated a significantly superior position accuracy and a slightly better orientation accuracy by maintaining the quaternion unity.

4. Experimental Results

4.1. Experimental Setup

Figure 8 shows the experimental setup for evaluation of the 6-DoF tri-electromagnet pose-tracking system. The center positions and magnetic moments of the three electromagnetic coils used in the experiment are given below.

$$\begin{array}{cc}\hfill {}^w{\mathbf{m}}_1& ={\left[\begin{array}{ccc}0& 2.08& 0.02\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,1}& ={\left[\begin{array}{ccc}0& 0& 0.10\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \\ \hfill {}^w{\mathbf{m}}_2& ={\left[\begin{array}{ccc}0.42& 0.22& -6.09\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,2}& ={\left[\begin{array}{ccc}0& 0& 0.10\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \\ \hfill {}^w{\mathbf{m}}_3& ={\left[\begin{array}{ccc}0.45& -0.16& -4.68\end{array}\right]}^{\mathrm{T}}\mathrm{A}\cdot {\mathrm{m}}^2\hfill \\ \hfill {{}^w\mathbf{r}}_{e,3}& ={\left[\begin{array}{ccc}-0.19& -0.01& 0.10\end{array}\right]}^{\mathrm{T}}\mathrm{m}\hfill \end{array}$$

Coil 1 and coil 2 are collocated and orthogonal and coil 2 and coil 3 are parallel. Coil 2 and coil 3 have the same size of $\varnothing 100 \mathrm{m}\mathrm{m}\times 87 \mathrm{m}\mathrm{m}$, while the size of coil 1 is $\varnothing 64 \mathrm{m}\mathrm{m}\times 50 \mathrm{m}\mathrm{m}$. The electric parameters of the coils can be found in Table 1. The electromagnetic coils are controlled by the driving circuits outlined in Section 2.2, running at a sampling rate of 30 Hz for a complete measurement cycle.

As illustrated in the subfigure of Figure 8, a tri-axial anisotropic magnetoresistive (AMR) magnetic sensor (MMC5983MA, MEMSIC Semiconductor Co., Ltd., Tianjin, China) was used with on-chip signal processing and integrated I2C bus. The sensor has a scale range of ±8 Gauss with an integrated 18-bit ADC and a noise level of 0.4 mGauss. A microcontroller (ATmega2560, Microchip Technology Inc., Chandler, AZ, USA) was employed to collect data from the sensor through the I2C bus and to control the coils. The sensor data were transmitted to a personal computer for implementing the pose estimation algorithm.

A robotic arm (CR5, DOBOT Robotics Co., Ltd., Shenzhen, China) was used to move the sensor along the testing trajectories. An optical tracking system (PrimeX 22, NaturalPoint Inc., Corvallis, OR, USA) was used to provide the reference pose of the sensor with a sub-millimeter and sub-degree accuracy.

4.2. Pose Estimation Results

In this section, the pose estimation experiments are conducted along two testing trajectories to evaluate the performance of the electromagnetic pose-tracking system. The accuracy and robustness of the proposed ESKF pose estimation algorithm are evaluated for different dynamics of motion. The covariance of process and measurement noise are kept the same for all three algorithms, which are given by the following:

$$\begin{array}{cc}\hfill {\mathbf{Q}}_k& =\left[\begin{array}{cc}1 {\mathbf{I}}_3{\left(\mathrm{m}/{\mathrm{s}}^2\right)}^2& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& 0.64 {\mathbf{I}}_3{\left(\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s}\right)}^2\end{array}\right]\hfill \\ \hfill {\mathbf{R}}_k& =10^{-14}{\mathbf{I}}_9{\left(\mathrm{T}\right)}^2\hfill \end{array}$$

(37)

The first testing trajectory, as illustrated in Figure 9, consists of four arcs and a linear segment. The waypoints of the first testing trajectory are pointed in the subfigure of Figure 9, with their coordinates and orientations listed in

Table 3. Waypoints for the first testing trajectory.

	Position (mm)	Orientation 1 (°)
Start Point P0	(−132, −35, 226)	(−168, 12, 0)
P1	(−130, 55, 239)	(−160, −2, 40)
P2	(−24, 34, 256)	(−150, 57, 40)
P3	(−44, −97, 221)	(125, 26, 2)
P4	(−54, −82, 238)	(112, 6, 2)
P5	(−131, −64, 259)	(−171, 15, −16)

¹ The orientations ${}_s{}^w\mathbf{R}\left\{{}_s{}^w\mathbf{q}\right\}$ are represented by Z-Y-X (yaw, pitch, roll) Euler angles.

. As the sensor moves along the trajectory, it rapidly rotates as shown in the subfigure of Figure 9, where the tri-axis frames represent the sensor reference frame along the trajectory. The initial point in the algorithms is set close to the start point P0. It is shown that both the standard EKF and ESKF track the true pose trajectory well. Nevertheless, CEKF diverges shortly after the test begins, which indicates that the CEKF is potentially unstable for certain trajectories with fast rotations.

The instability of the CEKF results in enormous pose tracking errors. Therefore, the tracking errors of the CEKF are excluded from Figure 10 and Table 3. As shown in Figure 10a, the standard EKF exhibits a larger position tracking error than the ESKF, particularly in the z-axis.

Table 4. RMS pose tracking errors for the first testing trajectory.

		Standard EKF	ESKF
Position Error (mm)	x	2.97	2.71
	y	1.55	1.74
	z	1.38	0.71
	Euclidean	3.63	3.29
Orientation Error (°)	Yaw ($\psi$)	0.70	0.71
	Pitch ($\theta$)	1.42	1.45
	Roll ($\varphi$)	2.04	2.12
	Average	1.39	1.43

presents the RMS pose tracking error in the experiment. It is shown that the z-axis position error of the standard EKF is approximately two times larger than that of ESKF. The overall Euclidean error for standard EKF is 3.63 mm, which is 0.34 mm larger than ESKF. However, similar to the simulation results, the orientation errors of the two algorithms are quite close, which indicates that orientation errors caused by incorrect quaternion normalization might be overshadowed by other influential factors such as magnetic field model errors.

The norm of the estimated quaternion is illustrated in Figure 11. The norm of the quaternion estimated by the standard EKF deviates from unity significantly. Moreover, the two peaks of the deviation pattern correspond to the two peaks of the z-axis position error of the standard EKF in Figure 10a. This observation reinforces the argument in Section 3 that the large position error of the EKF is caused by the unnormalized quaternions. The norm of the quaternion estimated by the ESKF has the largest deviation of $3.55\times {10}^{-15}$, ensuring accurate and robust pose estimation.

The second testing trajectory, as illustrated in Figure 12, is composed of a closed triangular segment followed by two repeating arcs with varying linear velocity.

Table 5. Waypoints for the second testing trajectory.

	Position (mm)	Orientation (°)
Start Point P0	(70, 57, 330)	(−50, −4, −35)
P1	(−70, 77, 385)	(−28, −4, −17)
P2	(−195, 57, 300)	(−60, −22, −2)
P3	(−54, 57, 400)	(−50, −12, −20)

lists the critical waypoints. The closed triangular trajectory consists of three segments, which are linear routes from P0 to P1, P1 to P2, and an arc from P2 back to P0. The next segment is an arc from P0 to P2 with midpoint P3, followed by an arc from P2 back to P0. The final part repeats the arc from P0 to P2 with midpoint P3 and the arc from P2 back to P0 but with a slower velocity. The rotation of the sensor is moderate compared with the first trajectory. The initial point in the algorithms is set to the same point and close to the start point P0. All three algorithms can achieve successful 6-DoF pose tracking in this test scenario with a slow and small-range rotation.

The pose tracking errors for the second testing trajectory are shown in

Table 6. RMS pose tracking errors for the second testing trajectory.

		Standard EKF	CEKF	ESKF
Position Error (mm)	X	1.52	1.45	1.43
	Y	1.67	1.57	1.53
	Z	0.97	0.70	0.75
	Euclidean	2.46	2.25	2.23
Orientation Error (°)	Yaw ($\psi$)	0.39	0.39	0.38
	Pitch ($\theta$)	0.52	0.41	0.52
	Roll ($\varphi$)	0.49	0.47	0.46
	Average	0.47	0.42	0.45

. The orientation errors of the three algorithms range from 0.42$°$ to 0.47$°$, which are approximately the same. However, the position errors of the ESKF and CEKF are smaller than that of the standard EKF. Specifically, the Euclidean position errors of the standard EKF, CEKF, and ESKF are 2.46 mm, 2.25 mm, and 2.23 mm, respectively. The experiment for the second testing trajectory demonstrates that the ESKF can consistently achieve robust and accurate pose estimation. The CEKF can achieve acceptable tracking performance under less challenging conditions, with slow and small-range rotations.

In summary, experimental results for both testing trajectories confirm that the ESKF is the most robust and accurate estimation algorithm for electromagnetic pose tracking. The standard EKF struggles with significant position errors due to the violation of the quaternion unity constraint. Although the CEKF works well in some scenarios, it shows instability and potentially diverges for certain challenging trajectories with fast and large-range rotations. These experimental findings conclude that the proposed 6-DoF tri-electromagnet pose-tracking system using the ESKF algorithm is reliable and accurate.

4.3. Disturbance Rejection Performance

A fundamental problem with magnetic pose-tracking systems is their susceptibility to environmental magnetic disturbances. It is claimed that the proposed system can reject magnetic disturbances by using the strategy of sequential coil excitation and background field cancellation. In this section, the disturbance rejection capability of the proposed electromagnetic pose-tracking system is experimentally validated.

The experimental setup for disturbance rejection validation is illustrated in Figure 13. During this experiment, the sensor follows a trajectory similar to the second trajectory in Section 4.2. Meanwhile, a NdFeB permanent magnet ($\varnothing 20\times 10 \mathrm{m}\mathrm{m}$, $m=1.8 \mathrm{A}\cdot {\mathrm{m}}^2$) is manually moved toward the workspace of the tracking system and then kept near the setup, as depicted in Figure 13. The permanent magnet can exert a strong disturbance field larger than 500 mGauss.

Pose estimation experiments using the ESKF were repeated with and without the disturbance of the permanent magnet.

Table 7. RMS pose tracking errors with and without disturbance using ESKF.

		No Disturbance	Disturbance Added
Position Error (mm)	X	1.43	1.47
	Y	1.53	1.51
	Z	0.75	0.66
	Euclidean	2.23	2.21
Orientation Error (°)	Yaw ($\psi$)	0.38	0.44
	Pitch ($\theta$)	0.52	0.49
	Roll ($\varphi$)	0.46	0.43
	Average	0.45	0.45

illustrates the RMS pose tracking errors with and without the disturbance. The Euclidean position error with disturbance is 2.21 mm, which is approximately the same as that without the disturbance (2.23 mm). The average orientation error also remains the same when the disturbance is added. Therefore, it is validated that the proposed electromagnetic pose-tracking system is robust to magnetic disturbances.

To reveal the underlying mechanism of disturbance rejection, the effective magnetic measurement of coil 2 ${}^s{\mathbf{B}}_{e,2}$ and the background magnetic field measurement ${}^s{\mathbf{B}}_{m,0}$ are plotted in Figure 14. In this test, the sensor is held stationary at the center of the workspace and then the permanent magnet (PM) is introduced. When the PM approaches the tracking system, the background field measurement is influenced by the disturbance significantly while the effective magnetic measurement used for pose estimation remains unaffected due to the cancellation of the background field with disturbance, i.e., ${}^s{\mathbf{B}}_{e,2}={}^s{\mathbf{B}}_{m,2}-{}^s{\mathbf{B}}_{m,0}$.

5. Conclusions

In this article, a robust tri-electromagnet-based 6-DoF pose-tracking system is proposed using an error-state Kalman filter algorithm. The pose-tracking system consists of three stationary electromagnetic coils and a magnetic sensor attached to the moving target. By employing a sequential excitation strategy, the system can effectively separate the magnetic fields of three coils and cancel the environmental magnetic disturbances. The magnetic measurements from three coils are fused using an error-state Kalman filter, which fundamentally avoids the violation of quaternion unity and achieves accurate and robust pose estimation.

A simulation is designed to evaluate the proposed system under a challenging condition with rapid and large-range rotations of the sensor. Simulation results show that the RMS Euclidean position tracking error is 1.74 mm and the average orientation error is 0.42°. Although the orientation tracking error of the proposed ESKF is only slightly smaller than those using the standard EKF and CEKF, the proposed system using the ESKF shows a significant improvement in position tracking accuracy.

Experiments were conducted to validate the pose-tracking system for two trajectories with different dynamics. The experimental results demonstrate that position tracking performance is significantly improved by the ESKF compared to the traditional standard EKF, while the orientation tracking errors are almost identical. It is found that the violation of quaternion unity can significantly distort position estimation due to the coupling effect of position and orientation in the measurement model, which leads to large position tracking errors in the standard EKF. Nevertheless, the orientation estimation is only slightly affected by the quaternion normalization error, resulting in the slight advantage of ESKF in orientation estimation. While the CEKF can preserve the unity of quaternion and achieve a high accuracy close to ESKF, it shows instability in pose estimation in certain challenging dynamics of motion. Overall, the proposed ESKF stands out for its enhanced accuracy and robustness.

An experiment was conducted to validate the disturbance rejection performance of the proposed pose-tracking system. It is shown that the strategy of sequential coil excitation and background field cancellation can effectively remove magnetic disturbances. The Euclidean position tracking error is 2.21 mm and the average orientation angle error is 0.45° in presence of the disturbance from a permanent magnet.

Future works include optimizing the electromagnetic tracking system to increase the update rate and accuracy and evaluating the tracking system in applications of medical robot navigation.