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
, whereas the orientation state in pose-tracking systems evolves on the unit quaternion manifolds
. 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:
where
is the magnetic permeability in a vacuum;
denotes the magnetic moment;
represents the relative position of the concerned point (e.g., the location of the sensor) with respect to the center of the coil;
represents the Euclidean norm of a vector; and
is the magnetic flux intensity at point
. The left superscript indicates the reference frame in which the vector is represented. Specifically, the superscript
represents the world reference frame, and
represents the sensor reference frame, as depicted in
Figure 1a. The relative position of the sensor to the
i-th coil
can be represented by the difference between the absolute position of sensor
and the absolute position of the coil
, i.e.,
. Thus, for the
i-th coil, Equation (1) evolves to the following:
The magnetic field measurement by the sensor
is obtained by rotating the magnetic field
into the sensor frame
.
where the
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
is used to represent the orientation in pose estimation. The transformation between the rotation matrix and the unit quaternion is given by the following:
It is worth noting that magnetic measurement
for each coil depends on both the position
and the orientation
of the sensor. By stacking the magnetic measurements from all three electromagnetic coils, the magnetic measurement model of the tracking system is given below:
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
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:
where
and
are coil current and voltage in the complex frequency domain;
is the constant gain; and
is the time constant of the first-order system. Since the magnitude of magnetic moment
of the coil is proportional to the current
, 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
larger than 4
to take measurements of the magnetic field, where 4
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
, where
is the inductance and
is the resistance of the coil.
Table 1 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
(<2 ms) and, thus, a small delay time
(<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
, the current of the previous coil drops to zero while the current of the activated coil reaches a steady state. The delay time
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:
where
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.
By taking the difference between the activated measurement
and the deactivated measurement
in each measurement cycle, the separated magnetic field from each coil with no background field is acquired as follows:
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 , which is not appropriate for the pose estimation problem where the orientation is represented using quaternions on the unit quaternions group . Specifically, the unit quaternion has only three degrees of freedom, whereas the standard EKF treats it as a vector in 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
to be estimated consists of a 3-DoF position, translational velocity
, and unit quaternion
representing a 3-DoF orientation, which is outlined below:
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:
where
is assumed to be a zero-mean Gaussian noise that represents the unmodeled acceleration.
is also assumed to be a zero-mean Gaussian noise, representing the unmodeled angular velocity. Here,
denotes the quaternion product, which can also be represented by a matrix product as follows:
where
is the left-quaternion-product matrix, detailed below:
Discretizing the system kinematic model yields the following:
where
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:
where
represents
identity matrices; and
denotes null matrices with the dimension of
.
is the concatenation of the process noises
and
, which is modeled as a zero-mean Gaussian noise, outlined below:
where the notion
represents a Gaussian distribution with its mean
and covariance
. The covariance matrix of the process noise
is defined by the following:
where
is the expectation of the random variable. The process noise covariance
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.
where
is the measurement noise vector, which is also assumed to be zero-mean and Gaussian.
where the covariance matrix of measurement noise
is defined as follows:
which is set according to the sensor noise in the experiment. The Jacobian of the measurement model
is given by the following:
where
,
, and
are sub-Jacobian matrices corresponding to position, velocity, and quaternion components, respectively; and
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]:
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:
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.
where
is the constraint measurement function. The corresponding Jacobian of the constraint measurement model is given by the following:
Similar to the measurement update step in the EKF framework (22), the constraint “measurement” is included in the algorithm as an additional correction step.
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 in parallel with the nominal state 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
captures small deviations from the nominal state, including position error
, velocity error
, and the orientation error represented by angle vector
.
where the quaternion error is related with the angle error by
. It is noted that the dimension of
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).
The process noise is considered in the kinematic model of the error state.
Discretizing the error-state kinematics yields the following:
The discrete error-state kinematic model can be reformulated into a compact form.
The measurement model in ESKF is the same as Equation (18), while the Jacobian of the measurement model with respect to the error state
is defined as detailed below:
where
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 , state estimate , and error-state covariance |
at the previous iteration step |
Output: state estimate and state covariance at the current step |
Nominal-state propagation: |
|
Error-state propagation: |
|
Error-state measurement update: |
|
Nominal-state correction: |
|
Error-state reset: |
|
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
, 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.
where
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
, 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.
The sensor noise in the simulation is set to be zero-mean, white, and Gaussian with a standard deviation of
. As shown in
Figure 5, the sensor moves along a helical trajectory. For translation, the trajectory started from position
, the velocity in
z-axis is
m/s, the projection of the trajectory on the XOY-plane is a circular trajectory with a radius of
m and an angular rate of
. The rotational speed of the sensor is
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:
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, 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
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
and that of ESKF is only 5
, which is close to the magnitude of machine precision in MATLAB (
). 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.
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.