Closed-Loop Magnetic Manipulation for Robotic Transesophageal Echocardiography

Abstract: This paper presents a closed-loop magnetic manipulation framework for robotic transesophageal echocardiography (TEE) acquisitions. Different from previous work on intracorporeal robotic ultrasound acquisitions that focus on continuum robot control, we first investigate the use of magnetic control methods for more direct, intuitive, and accurate manipulation of the distal tip of the probe. We modify a standard TEE probe by attaching a permanent magnet and an inertial measurement unit sensor to the probe tip and replacing the flexible gastroscope with a soft tether containing only wires for transmitting ultrasound signals, and show that 6-DOF localization and 5-DOF closed-loop control of the probe can be achieved with an external permanent magnet based on the fusion of internal inertial measurement and external magnetic field sensing data. The proposed method does not require complex structures or motions of the actuator and the probe compared with existing magnetic manipulation methods. We have conducted extensive experiments to validate the effectiveness of the framework in terms of localization accuracy, update rate, workspace size, and tracking accuracy. In addition, our results obtained on a realistic cardiac tissue-mimicking phantom show that the proposed framework is applicable in real conditions and can generally meet the requirements for tele-operated TEE acquisitions.

\section{Introduction}

\begin{figure}[t]
\centering
\includegraphics[scale=1.0,angle=0,width=0.45\textwidth]{fig_app_system.jpg}
\caption{(a) illustrates the procedure of conventional TEE acquisitions, where the echocardiographer manually grips the flexible shaft and adjusts the knobs on the probe handle to advance and steer the probe in the esophagus. (b) shows the proposed application scenario of the MA-TEE system, where the probe tip attached to a permanent magnet and an IMU sensor is manipulated by an external permanent magnet held by a robotic manipulator for tele-operated TEE acquisitions.}
\label{Fig_app_system}
\end{figure}

\IEEEPARstart{U}{ltrasound} imaging is widely used for diagnosis and treatment in many medical disciplines, providing major advantages of safety, portability, low cost, and real-time capabilities compared with other medical imaging modalities such as CT and MRI \cite{chan2011basics}.
Transesophageal echocardiography (TEE) is a test that uses a specialized ultrasound probe placed inside the esophagus to assess the heart structures \cite{khandheria1994transesophageal}. As shown in Fig. \ref{Fig_app_system}(a), in conventional TEE examinations, an experienced echocardiographer manually grips the flexible shaft and adjusts the knobs on the probe handle to advance and steer the distal tip of the probe in the esophagus \cite{hahn2013guidelines}. Since the operator can only manipulate the proximal end of the semi-rigid tube, TEE operation usually requires a long learning curve and extensive training, and the imaging quality is highly dependent on the operator \cite{bloch2017impact}. Moreover, poor image quality and repeated insertion are associated with an increased risk of complications \cite{freitas2020safety}. In addition, the close contact between the echocardiographer and the patient would increase the risk of infection under the epidemic situation \cite{yang2020combating}.

Recently, a large number of robotic systems have been developed for tele-operated or autonomous ultrasound acquisitions \cite{li2021overview}. However, existing studies have mainly focused on extracorporeal applications \cite{mustafa2013robio,chatelain2017confidence,vesnet, li2021autonomous,
ning2021autonomic, li2021image}, which typically use a robotic manipulator to directly maneuver the probe on the skin surface to scan the region of interest. Only a few studies have investigated the robotic ultrasound acquisitions in intracorporeal applications \cite{loschak2016algorithms,wang2016robotic,norton2019intelligent, wang2016design}.
Wang \textit{et al.} \cite{wang2016robotic, wang2016design} developed the first robotic TEE system that can automatically hold the TEE probe handle to manipulate a standard TEE probe with 4 degrees of freedom (DOFs) based on continuum robot control. The internal probe is tracked based on the registration between ultrasound images and preoperative MRI data of the patient \cite{wang2016robotic}. However, since the system still used a standard TEE probe with a flexible gastroscope, it cannot overcome the inherent limitations such as the lack of intuitiveness, indirect and complex control of the probe tip, and the risk of patient injury due to tissue stretching as the semi-rigid endoscope is pushed through the esophagus. Also, the image registration-based localization method was reported to have a limited computational speed and reliability \cite{wang2016robotic}.

Magnetic locomotion has become a promising technology to wirelessly control a medical device in the human body without requiring mechanical linkage between the actuator and the device, which has shown great promise to overcome the limitations of standard endoscopic devices \cite{abbott2020magnetic, sliker2014flexible, xu2022autonomous, xu2022adaptive}.
In this work, we are committed to presenting a more straightforward approach to robotic TEE manipulation based on magnetic methods to allow for more direct, intuitive and accurate control of the TEE probe tip, thereby improving ease of use and patient safety for the targeted application.
The proposed application scenario of the magnetically actuated TEE (MA-TEE) system is shown in Fig. \ref{Fig_app_system}(c), where the internal probe is manipulated by an external permanent magnet held by a robotic manipulator.
By attaching a single permanent magnet and an inertial measurement unit (IMU) sensor to the probe tip and replacing the flexible gastroscope with a soft tether that contains only wires for transmitting ultrasound signals, the modified TEE probe can be viewed as a tethered capsule, and we show that 6-DOF localization and 5-DOF control of the probe can be achieved using an external and internal sensor fusion based approach.
The proposed system can allow the user to focus on the ultrasound images on the screen and directly instruct the desired movement of the probe tip, and the actuator magnet will be automatically adjusted to realize the desired motion of the probe.

To our knowledge, this is the first robotic TEE system based on magnetic control methods. Compared with previous studies on intracorporeal robotic ultrasound acquisitions \cite{loschak2016algorithms, wang2016robotic, wang2016design}, the method proposed in this paper does not require complex modelling of the continuum robot kinematics and can directly control the distal end of the probe, which provides a simpler and more intuitive way for robotic TEE manipulation. Moreover, instead of tracking the probe based on image registration, our method can estimate the 6-DOF pose of the probe tip in real time based on the fusion of internal inertial sensing data and external magnetic field sensing data, which can realize more accurate and efficient localization of the probe tip and provide additional diagnostic information for the echocardiographer. In addition, the modified TEE probe with a string-like soft tether instead of a standard flexible endoscope may help improve patient safety during the manipulation \cite{sliker2014flexible}.

This paper is organized as follows. We first introduce the related work in Section~II. Nomenclature in this paper is presented in Section~III. The details of the proposed closed-loop manipulation framework are introduced in Section~IV. Then, our experimental results are presented in Section~V, before conclusions are drawn in Section~VI.

\section{Related Work}

Several groups have explored the magnetic manipulation of an internal capsule robot using electromagnet coils \cite{son2017magnetically, guitron2017autonomous, 9730063} and permanent magnets \cite{mahoney2016five, popek2017first, xu2020novelsystem, xu2022autonomous, xu2021adaptive} as the magnetic field sources.
Compared with the systems that use electromagnetic coils to actuate the capsule, the permanent-magnet-based systems are generally more compact, cost-effective and energy-efficient, and often provide a larger workspace. Therefore, in this work, we use a single permanent magnet as the external actuator, which is maneuvered by a robotic manipulator, similar to \cite{xu2020novelsystem, xu2021adaptive}.

Localization is essential to achieve closed-loop magnetic control of the internal capsule robot. Some researchers proposed to use magnetic field sensors placed inside the capsule to measure the magnetic field of the external magnet for capsule localization \cite{popek2013localization, popek2016six, taddese2018enhanced}. However, these methods require up to six sensors to be embedded in the capsule with a specific structure to prevent saturation of the sensors, which is challenging in practice and would increase the size of the internal device. Moreover, to address the localization singularity of the internal-sensor-based methods, these systems either use an additional coil in the actuator \cite{taddese2018enhanced} or require the magnets to make specific motions during the capsule localization \cite{popek2016six}, which further increase the complexity of the overall approach.
As an alternative, some researchers proposed to locate the capsule using a magnetic field sensor array placed external to the capsule, so that the capsule can be simplified to contain only a permanent magnet \cite{hu2005efficient, xu2018free, xu2020improved, xu2020novelsystem}. However, these methods can only solve 5-DOF pose of a static capsule and have a limited workspace size.
Recently, we presented a localization method that combines the use of an external magnetic field sensor array and an internal IMU sensor inside the capsule to estimate the full 6-DOF pose of a magnetic capsule robot \cite{li2022external}. The hybrid approach does not require complex structures of the actuator and the capsule, and can avoid the need for the magnets to make specific motions.

In this paper, we move one step further to develop a closed-loop magnetic manipulation framework that can allow 5-DOF control of a capsule robot based on external and internal sensor fusion. Compared with existing methods, we only use a single permanent magnet in the actuator, and the internal control modules in the capsule only include a magnet and an IMU, which can greatly simplify the structures of the actuator and the capsule for easier application in clinical use. Moreover, the proposed method can achieve full 6-DOF localization and 5-DOF control of the capsule without requiring the magnets to perform specific motions during the localization. Finally, we present the first demonstration of magnetically actuated robotic TEE acquisitions in a tissue-mimicking phantom to validate the feasibility of the framework.

\section{Nomenclature}

Throughout this paper, scalars are denoted by normal fonts (e.g., $\mu_0$), vectors are denoted by bold lowercase fonts (e.g., $\textbf{p}_c$), and matrices are denoted by bold uppercase fonts (e.g. $\mathbf{K}$). $\mathbf{R}_x (\alpha)$ denotes the rotation matrix that rotates a vector by an angle $\alpha$ about the $x$-axis in three dimensions. A hat over a symbol (e.g., $\widehat{\mathbf{m}}_c$) indicates the unit vector of the original vector (e.g., $\mathbf{m}_c$).  A tilde over a symbol (e.g., $\widetilde{\mathbf{b}}_{i}$) represents the measurement of that variable. A bar over a symbol (e.g., $\bar{\gamma}$) denotes the \textit{a priori} prediction. $\mathbb{R}$ denotes the real number set. $\mathbf{x} \sim \mathcal{N}(\boldsymbol{\mu}, \mathbf{\Sigma})$ means a random vector $\mathbf{x}$ follows a normal distribution with mean vector $\boldsymbol{\mu}$ and covariance matrix $\mathbf{\Sigma}$.
 $\mathbf{I}_{k}$ indicates a $k \times k$ identity matrix.
\section{Methodology}

\subsection{Problem Formulation}

\begin{figure}[t]
\centering
\includegraphics[scale=1.0,angle=0,width=0.48\textwidth]{fig_dof.jpg}
\caption{The commonly used DOFs of the probe in clinical TEE acquisitions.}
\label{Fig_dofs}
\end{figure}

The commonly used DOFs of the TEE probe in clinical settings are illustrated in Fig. \ref{Fig_dofs}. For simplicity of discussion, we define the probe coordinate frame as follows. The $x$-axis is along the probe's heading direction, the $z$-axis is perpendicular to the transducer array, and the $y$-axis can be obtained by the right-hand rule. Then, the traditional TEE probe manipulation can be described as follows:
\begin{enumerate}
\item ``{Advance / Withdraw}": translation along the $x$-axis;
\item  ``{Turn left / Turn right}": rotation around the $x$-axis;
\item  ``{Retroflex / Anteflex}": rotation around the $y$-axiss;
\item  ``{Flex to the left / Flex to the right}": rotation around the $z$-axis; and
\item  ``{Electronic rotation}": rotation of the imaging plane around the $z$-axis from $0^\circ$ to $180^\circ$
\end{enumerate}

It is known that the the rotation of a single permanent magnet along its axis of magnetization is not achievable by magnetically induced torque. However, since the rotation of the imaging plane around the $z$-axis can be adjusted by means of electronic buttons, we can simplify the TEE probe manipulation task as 3-DOF position control and 2-DOF orientation control of the probe by aligning the internal magnet's axis of magnetization with the $z$-axis of the transducer.

\subsection{System Overview}

\begin{figure}[t]
\setlength{\abovecaptionskip}{-0.2cm}
\centering
\includegraphics[scale=1.0,angle=0,width=0.49\textwidth]{fig_system.jpg}
\caption{Schematic of the MA-TEE system. Combining the measurements from the IMU internal to the probe and the magnetic field sensors external to the probe, the 6-DOF pose of the probe can be estimated in real time and 5-DOF control of the probe can be achieved through interactions between the magnets in the actuator and the capsule.}
\label{Fig_system}
\end{figure}

\begin{figure*}[t]
\centering
\includegraphics[scale=1.0,angle=0,width=0.9\textwidth]{fig_workflow.jpg}
\caption{(a) shows the general workflow of our proposed MA-TEE system. After initialization, closed-loop control of the probe is performed by continuously updating the actuator pose based on the user command and the real-time localization result for TEE image acquisitions. (b) and (c) illustrate the workflow of the real-time localization and magnetic control algorithms, respectively.  }
\label{Fig_overview_workflow}
\end{figure*}

As illustrated in Fig. \ref{Fig_system}, the MA-TEE probe contains a permanent magnet, an IMU sensor, and an ultrasound transducer with a soft tether, which can be viewed as a tethered capsule robot. The We attach a coordinate frame $\{C\}$ to the capsule magnet, and frame $\{A\}$ to the external actuator magnet. We assume that the origins of $\{C\}$ and $\{A\}$ coincide with the center of the permanent magnets $\mathbf{p}_{c}, \mathbf{p}_{a} \in \mathbb{R}^3$, and the $z$-axes are aligned with their magnetic dipole moments $\widehat{\mathbf{m}}_{c}, \widehat{\mathbf{m}}_{a} \in \mathbb{R}^3$, respectively.
It is assumed that $\{C\}$ is coincident with the probe coordinate frame as defined in Section~IV-A given that the overall size of the probe is small.
The world coordinate frame $\{W\}$ is attached to the external magnetic sensor array, which has a known and fixed relationship with the robot base. The orientations of the capsule and the actuator w.r.t. the world frame can be represented by matrices $\mathbf{R}_{c}, \mathbf{R}_{a} \in SO(3)$, respectively. Assume that the external magnetic sensor array contains $N$ three-axis magnetic field sensors, whose positions w.r.t. $\{W\}$ are $\mathbf{p}_{i}$, $i = 1,\cdots, N$.

An overview of the workflow of our proposed MA-TEE system is illustrated in Fig. \ref{Fig_overview_workflow}(a). First, an initial localization of the probe is performed, and the actuator is automatically controlled to move above the probe. Then, the real-time 6-DOF pose of the capsule $\mathbf{p}_{c}, \mathbf{R}_{c}$ is estimated using an Extended Kalman Filter (EKF) based method (see Fig. \ref{Fig_overview_workflow}(b)), and 5-DOF control of the probe is performed for tele-operated ultrasound acquisitions (see Fig. \ref{Fig_overview_workflow}(c)).

In the following, we first describe the localization algorithm based on external and internal sensor fusion, and then introduce the closed-loop actuation algorithm.

\subsection{Sensor Fusion based Localization}
\subsubsection{Localization Model}

As shown in Fig. \ref{Fig_system}, the orientation of the capsule can be represented by the roll, pitch and yaw angles as $\mathbf{R}_c=\mathbf{R}_z (\gamma)\mathbf{R}_y (\beta)\mathbf{R}_x (\alpha)$. The internal IMU can obtain the measurements of the angles $\widetilde{\alpha}, \widetilde{\beta}, \widetilde{\gamma}$. However, the output yaw angle $\widetilde{\gamma}$ will be inaccurate owing to the presence of strong magnetic fields in the environment \cite{li2022external}. Therefore, we directly use the measurements $\widetilde{\alpha}, \widetilde{\beta}$ and assume $\gamma$ to be unknown.
The magnetic field measurement from each sensor in the external magnetic sensor array $\widetilde{\mathbf{b}}_i$ is considered as a superposition of the actuator's magnetic field $\widetilde{\mathbf{b}}_{i,a}$, the capsule's magnetic field $\widetilde{\mathbf{b}}_{i,c}$, and the environmental magnetic field $\widetilde{\mathbf{b}}_{i,e}$, $i=1,\cdots,N$.
The environmental magnetic field can be measured before introducing the magnets in the workspace.
Since the pose of the actuator at each time step $t$ can be measured, the actuator's magnetic field measurement at each sensor $\widetilde{\mathbf{b}}_{i,a}^{(t)}$ can be approximated using the analytical expressions of the actuator's magnetic field model \cite{shi2021optically} or a finite-element method \cite{taddese2018enhanced}. In this work, we estimate the theoretical value of the actuator's magnetic field at each sensor $\mathbf{b}_{i,a}$ based on the generalized complete elliptic integral model \cite{derby2010cylindrical, caciagli2018exact}:

\begin{equation}
C(k_c,p,c,s) = \int_0^{\frac{\pi}{2}} \frac{(c \cos^2 \varphi + s \sin^2 \varphi)d\varphi}{(\cos^2 \varphi + p \sin^2 \varphi)\sqrt{\cos^2 \varphi + k_c^2 \sin^2 \varphi}}
\end{equation}

For a longitudinally magnetized cylinder with length $2L$ and radius $R$ with total magnetization $M$, the closed-form magnetic field components expressed in cylindrical coordinates can be obtained as

\begin{align}
b_\rho &= \frac{\mu_0 M R}{\pi} \left[  \alpha_+ P_1(k_+) - \alpha_- P_1(k_-)  \right] \\
b_z &= \frac{\mu_0 M R}{\pi (\rho + R)} \left[  \beta_+ P_2(k_+) - \beta_- P_2(k_-)  \right] \\
b_\varphi &= 0
\end{align}

\noindent where

\begin{align}
\alpha_\pm &= \sqrt{\frac{1}{\xi_\pm^2 + (\rho +R)^2}} \\
\beta_\pm &= \xi_\pm \alpha_\pm \\
k_\pm &= \sqrt{\frac{\xi_\pm^2 + (\rho - R)^2}{\xi_\pm^2 + (\rho + R)^2}}\\
\xi_\pm &= z \pm L \\
\eta &= \frac{\rho - R}{\rho + R} \\
P_1(k_\pm) &= C(k_\pm,1,1,-1)  \\
P_2(k_\pm) &= C(k_\pm,\eta^2,1,\eta)
\end{align}

Therefore, the magnetic field measurement of the capsule at each time step can be extracted as $\widetilde{\mathbf{b}}_{i,c}^{(t)} = \widetilde{\mathbf{b}}_i^{(t)} - \mathbf{b}_{i,a}^{(t)} - \widetilde{\mathbf{b}}_{i,e}$.

Since the scale of the capsule magnet is much smaller than the scale of the system, we can use the magnetic point-dipole model to estimate the theoretical magnetic field of the capsule at the $i$-th sensor:

\begin{equation}
\label{F_bc}
\begin{aligned}
\mathbf{b}_{i,c}&=\frac{\mu_{0}\|\mathbf{m}_{c}\|}{4\pi\|\mathbf{r}_i\|^5}\left(3\mathbf{r}_i\mathbf{r}_i^{T}-\|\mathbf{r}_i\|^2\mathbf{I}_{3}\right)\widehat{\mathbf{m}}_{c} \\
\end{aligned}
\end{equation}

\noindent where $\mu_0$ is the permeability of free space. $\mathbf{r}_i = \mathbf{p}_i - \mathbf{p}_c$ is the vector pointing from the capsule to the $i$-th sensor. $\widehat{\mathbf{m}}_{c}=\mathbf{R}_z (\gamma)\mathbf{R}_y (\widetilde{\beta})\mathbf{R}_x (\widetilde{\alpha})[0,0,1]^T$ is a function of $\gamma$.

\subsubsection{Initial Localization with Least-Squares Optimization}

For initial localization, we use a non-linear least-squares (LS) optimization method \cite{li2022external} to recover the unknown parameters in the capsule pose $\mathbf{p}_c, \gamma$ by minimizing the cost function $\|\widetilde{\mathbf{b}}_{c}  - \mathbf{b}_{c}\|^2$, where $\mathbf{b}_{c} = [\mathbf{b}_{1,c}^T, \cdots,
\mathbf{b}_{N,c}^T]^T$ and $\widetilde{\mathbf{b}}_{c} = [\widetilde{\mathbf{b}}_{1,c}^T, \cdots, \widetilde{\mathbf{b}}_{N,c}^T]^T$ are the theoretical and measured magnetic fields of the capsule magnet from $N$ magnetic sensors. The method can achieve an average position accuracy of \SI{2.06}{\mm}, \SI{2.86}{\mm}, and \SI{2.81}{\mm} in $x$, $y$, $z$ axes, and an orientation accuracy of \ang{0.19}, \ang{1.02}, and \ang{1.01} in $x$, $y$, $z$ axes, in a \SI[parse-numbers=false]{0.5 \times 0.5 \times 0.2}{\cubic\m} workspace \cite{li2022external}.

\subsubsection{Real-time Tracking with Extended Kalman Filter}
During the real-time localization, in order to better deal with the noisy sensor readings and take advantage of the historical information, in this work, we improve upon our previous work \cite{li2022external} by applying an Extended Kalman Filter (EKF) to combine  IMU-based motion prediction with the magnetic field measurements for real-time tracking of the capsule.
Let $\mathbf{x}_t = \begin{bmatrix}\mathbf{p}_{c}^{(t)} \\ \gamma^{(t)} \end{bmatrix} \in \mathbb{R}^4$ denote the state and $\mathbf{z}_t=\widetilde{\mathbf{b}}_{c}^{(t)} \in \mathbb{R}^{3N}$ denote the magnetic field measurements at time step $t$. Consider the following system model:

\begin{align}
\label{F_system_model}
\, \mathbf{x}_t &= f(\mathbf{x}_{t-1}, \mathbf{u}_{t-1}) + \mathbf{w}_t
\\
\, \mathbf{z}_t &= h(\mathbf{x}_{t}) + \mathbf{n}_t
\end{align}

\noindent where $f(\cdot)$ is the motion model, $\mathbf{u}_{t-1}=\begin{bmatrix} \mathbf{a}^{(t-1)} \\ \boldsymbol{\omega}^{(t-1)} \end{bmatrix} \in \mathbb{R}^6$ is the acceleration and rotational velocity of the capsule measured by the IMU at the previous time step, $\mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}_t)$ is the motion model noise with zero-mean and covariances $\mathbf{Q}_t$. $h(\cdot)$ is the measurement model $\mathbf{b}_{c}$, and $\mathbf{n}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{N}_t)$ is the measurement model noise with zero-mean and covariances $\mathbf{N}_t$.

Given the previous capsule pose $\mathbf{p}_{c}^{(t-1)},\mathbf{R}_{c}^{(t-1)}$, the capsule pose at the time step $t$ can be predicted as

\begin{align}
 \bar{\mathbf{p}}_c^{(t)} &= \mathbf{p}_c^{(t-1)} + \mathbf{v}_c^{(t-1)}\Delta t + \frac{1}{2}(\mathbf{R}_c^{(t-1)}\mathbf{a}^{(t-1)}+\mathbf{g}) \Delta t^2 \label{p_pred}
\\
 \bar{\mathbf{v}}_c^{(t)} &= \mathbf{v}_c^{(t-1)} + (\mathbf{R}_c^{(t-1)}\mathbf{a}^{(t-1)}+\mathbf{g}) \Delta t \label{v_pred}
\\
\bar{\mathbf{R}}_c^{(t)} &= \mathbf{R}_c^{(t-1)}\mathbf{R}\{\boldsymbol{\omega}^{(t-1)}\Delta t\}
\end{align}

\noindent where $\mathbf{v}_c^{(t)}$ is the velocity of the capsule at time step $t$, $\mathbf{g}$ is the acceleration of gravity, $\mathbf{R}\{\boldsymbol{\omega}^{(t-1)}\Delta t\}$ is the rotation matrix equivalent to the axis-angle representation $\boldsymbol{\omega}^{(t-1)}\Delta t$. By changing the orientation representation as Euler angles, we can obtain the predicted state $\bar{\mathbf{x}}_{t} = \begin{bmatrix}\bar{\mathbf{p}}_{c}^{(t)} \\ \bar{\gamma}^{(t)} \end{bmatrix}$ at time step $t$.
Since the motion of the probe tip in the esophagus is mainly in a stick-slip fashion, we assume zero-velocity at the previous step, i.e.,  $\mathbf{v}_c^{(t-1)}=\mathbf{0}$ in our implementation.

The covariance matrix of the state is predicted as

\begin{equation}
\label{F_covariances}
\bar{\mathbf{P}}_{t} = \mathbf{F} \mathbf{P}_{t-1} \mathbf{F}^T + \mathbf{Q}_t
\end{equation}

\noindent where $\mathbf{F}=\frac{\partial f}{\partial \mathbf{x}_{t-1}}|_{ \mathbf{x}_{t-1}}$ is the Jacobian of $f(\cdot)$. Since the pose change in the period $\Delta t$ is small, we approximate the motion model Jacobian as $\mathbf{F} \doteq \mathbf{I}_4$, and $\mathbf{Q}_t$ is empirically chosen as $\mathbf{Q}_t = \text{diag}(0.01,0.01,0.01,30)$ to indicate an uncertainty of \SI{0.01}{\m} in position and \ang{30} in yaw estimation.

In the correction step, the Kalman gain is computed as

\begin{equation}
\label{F_kalman_gain}
\mathbf{K} = \bar{\mathbf{P}}_{t} \mathbf{H}^T (\mathbf{H} \bar{\mathbf{P}}_{t} \mathbf{H}^T + \mathbf{N}_t)^{-1}
\end{equation}

\noindent where $\mathbf{H}$ is the Jacobian of $h(\mathbf{x}_{t})= [\mathbf{b}_{1,c}^T, \cdots, \mathbf{b}_{N,c}^T]^T$, which can be calculated as

\begin{equation}
\mathbf{H} =\frac{\partial h}{\partial \mathbf{x}_{t}}|_{ \bar{\mathbf{x}}_{t}} = [\mathbf{J}_1^T, \cdots, \mathbf{J}_N^T],
\end{equation}
\begin{equation}
\mathbf{J}_i = \left[ \frac{\partial \mathbf{b}_{i,c}}{\partial \mathbf{p}_c} \ \  \frac{\partial \mathbf{b}_{i,c}}{\partial \gamma} \right]\bigg|_{\bar{\mathbf{p}}_c^{(t)}, \bar{\gamma}^{(t)}}, i \in \{1, \cdots, N\}, \\
\end{equation}

\noindent where

\begin{small}
\begin{align}
&\frac{\partial \mathbf{b}_{i,c}}{\partial \mathbf{p}_c} = -\frac{3 \mu_{0}\|\mathbf{m}_c\|}{4\pi\|\mathbf{r}_i\|^5}
\Big[ \Big( \mathbf{r}_i^T \widehat{\mathbf{m}}_c \Big)
\Big( \mathbf{I}_3-\frac{5  \mathbf{r}_i \mathbf{r}_i^T}{\|\mathbf{r}_i\|^2} \Big)
+ \widehat{\mathbf{m}}_c \mathbf{r}_i^T  + \mathbf{r}_i \widehat{\mathbf{m}}_c ^T \Big] \\
& \frac{\partial \mathbf{b}_{i,c}}{\partial \gamma}  =
\frac{\mu_{0} \|\mathbf{m}_c\|}{4\pi\|\mathbf{r}_i\|^5}\left(3\mathbf{r}_i\mathbf{r}_i^{T}-\|\mathbf{r}_i\|^2\mathbf{I}_{3}\right)\frac{\partial \widehat{\mathbf{m}}_c}{\partial \gamma}, \\
&\frac{\partial \widehat{\mathbf{m}}_c}{\partial \gamma} =
\left[ \begin{matrix} \sin \widetilde{\alpha} \cos {\gamma} - \cos \widetilde{\alpha} \sin \widetilde{\beta} \sin {\gamma} \\
\sin \widetilde{\alpha} \sin {\gamma} + \cos \widetilde{\alpha} \sin \widetilde{\beta} \cos {\gamma} \\ 0 \end{matrix} \right]
\end{align}
\end{small}

Then, the state of the capsule is updated as

\begin{equation}
\label{F_x_update}
\mathbf{x}_{t} = \bar{\mathbf{x}}_{t} + \mathbf{K}\big(\mathbf{z_t}-h(\bar{\mathbf{x}}_{t})\big)
\end{equation}

The covariance matrix is updated using the symmetric and positive \textit{Joseph} form \cite{joseph}

\begin{equation}
\label{F_covariances_update}
\mathbf{P}_{t} = (\mathbf{I}_4 - \mathbf{K}\mathbf{H})\bar{\mathbf{P}}_{t}(\mathbf{I}_4 - \mathbf{K}\mathbf{H})^T + \mathbf{K}\mathbf{N}_t\mathbf{K}^T
\end{equation}
\noindent where the measurement noise covariance matrix is set as $\mathbf{N}_t=\sigma^2 \cdot \mathbf{I}_{3N}$, $\sigma = \SI{1e-5}{\tesla}$ in our experiments.

\begin{algorithm}[t] \smal
\caption{Sensor Fusion based Localization}
\label{Alg_Localization}
\KwIn{{positions of $N$ magnetic field sensors $\mathbf{p}_{1}, \cdots, \mathbf{p}_{N}$,
environmental magnetic fields $\widetilde{\mathbf{b}}_{1,e} \cdots, \widetilde{\mathbf{b}}_{N,e}$,
IMU measurements $\mathbf{a}^{(t)}, \boldsymbol{\omega}^{(t)}, \alpha^{(t)}, \beta^{(t)}$,
actuator's pose $\mathbf{p}_{a}^{(t)},\mathbf{R}_{a}^{(t)}$,
real-time magnetic field measurements $\widetilde{\mathbf{b}}_1^{(t)}, \cdots, \widetilde{\mathbf{b}}_N^{(t)}$,
$t=0, 1, 2,\cdots$}}
\KwOut{6-DOF pose of the capsule $\mathbf{p}_{c}^{(t)},\mathbf{R}_{c}^{(t)}$, $t=0, 1, 2,\cdots$}
\For{ $i=1,\cdots, N$}
{
Calculate the theoretical magnetic field of the actuator and the capsule at each sensor $\mathbf{b}_{i,a}^{(0)}$ and $\mathbf{b}_{i,c}^{(0)}$\;
Extract the magnetic field measurement of the capsule at each sensor $\widetilde{\mathbf{b}}_{i,c}^{(0)} = \widetilde{\mathbf{b}}_{i}^{(0)} - \mathbf{b}_{i,a}^{(0)} - \widetilde{\mathbf{b}}_{i,e}$\;
}
Initialize the capsule pose parameters $\mathbf{p}_{c}^{(0)},\gamma^{(0)}$ with least-squares optimization\;
Initialize the capsule orientation  $\mathbf{R}_{c}^{(0)}$ based on $\alpha^{(0)}, \beta^{(0)},\gamma^{(0)}$\;
\For{$t=1,2,\cdots$}
{
Predict the capsule pose $\mathbf{p}_{c}^{(t)},\mathbf{R}_{c}^{(t)}$ based on the last pose estimate $\mathbf{p}_{c}^{(t-1)},\mathbf{R}_{c}^{(t-1)}$ and IMU measurements $\mathbf{a}^{(t-1)}, \boldsymbol{\omega}^{(t-1)}$ \;
\For{ $i=1,\cdots, N$}
{
Calculate the theoretical magnetic field of the actuator at each sensor $\mathbf{b}_{i,a}^{(t)}$\;
Extract the magnetic field measurement of the capsule at each sensor $\widetilde{\mathbf{b}}_{i,c}^{(t)} = \widetilde{\mathbf{b}}_i^{(t)} - \mathbf{b}_{i,a}^{(t)} - \widetilde{\mathbf{b}}_{i,e}$\;
}
Update the pose estimate $\mathbf{p}_{c}^{(t)},\gamma^{(t)}$ with EKF\;
Calculate the capsule orientation  $\mathbf{R}_{c}^{(t)}$ based on $\alpha^{(t)}, \beta^{(t)},\gamma^{(t)}$\;
}
\Return {6-DOF pose of the capsule} $\mathbf{p}_{c}^{(t)},\mathbf{R}_{c}^{(t)}$, $t=1,2,\cdots$\;
\end{algorithm}

The localization algorithm is summarized in Algorithm \ref{Alg_Localization}.

\subsection{Closed-Loop Magnetic Actuation}

\subsubsection{Capsule Dynamic Model}

Similar to most existing closed-loop magnetic control systems, our control approach is developed by modelling the magnetic fields of the actuator and the capsule using the magnetic point-dipole model. Given the poses of the capsule magnet and the actuator magnet, the magnetic force and torque applied to the capsule can be calculated as

\begin{align}
\label{F_f}
\boldsymbol{f}& = \nabla \,(\mathbf{m}_c \cdot \mathbf{b})
\\
\boldsymbol{\tau}& =  \mathbf{m}_c \times \mathbf{b}
\end{align}

\noindent where $\nabla$ is the gradient operator, $\mathbf{b}$ is the magnetic field of the actuator at the capsule's position:

\begin{equation}
\label{F_b}
\mathbf{b}=\frac{\mu_{0}\|\mathbf{m}_a\|}{4\pi\|\mathbf{p}\|^5}\left(3\mathbf{p}\mathbf{p}^{T}-\|\mathbf{p}\|^2\mathbf{I}_{3}\right)\widehat{\mathbf{m}}_a \\
\end{equation}

\noindent where $\mathbf{p} = \mathbf{p}_{c} - \mathbf{p}_{a}$ is the vector pointing from $\mathbf{p}_{a}$ to $\mathbf{p}_{c}$.

Different from previous work that focused on the magnetic actuation of a capsule in fluid \cite{mahoney2016five} or in an air-filled colon with little contact with the colon walls \cite{pittiglio2019magnetic}, the esophagus has a small diameter of about \SI{2}{\cm} and is collapsed between swallows \cite{kuo2006esophagus}, and the TEE application requires the probe tip to continuously contact the esophageal wall to ensure acoustic coupling for ultrasound acquisitions. Therefore, the friction between the probe tip and the esophagus should be modeled in our control method.
Based on the experimental data in \cite{lin2017friction}, in this work, we estimate the friction force on the probe-tissue interface as a constant resistance force in the opposite direction of velocity as

\begin{equation}
\label{F_res}
\boldsymbol{f}_{res}= - f_{res} \, \frac{\dot{\mathbf{p}}_c}{\| \dot{\mathbf{p}}_c \|}  \  \\
\end{equation}

Similar to \cite{pittiglio2019magnetic}, we consider the impact of the tether behind the capsule as an unmodelled disturbance, which can actually serve as a stabilizing damper to improve the stability of the system.  Then, the total force applied to the capsule is estimated as the sum of the magnetic force, the capsule's gravity, and the resistance forces, and the total torque is assumed as the magnetic torque only. Therefore, the dynamic model of the capsule can be represented as

\begin{equation}
\label{F_dynamics}
\begin{cases}
\, m_c \ddot{\mathbf{p}}_c= \boldsymbol{f} + \boldsymbol{f}_g + \boldsymbol{f}_{res}\\
\ \mathbf{I}_c \, \boldsymbol{\alpha} = \,\boldsymbol{\tau}
\end{cases}
\end{equation}

\noindent where $m_c$ and $\mathbf{I}_c$ are the the mass and inertia tensor of the capsule, $\ddot{\mathbf{p}}_c$ and $\boldsymbol{\alpha}$ are the position and angular accelerations of the capsule, and $\boldsymbol{f}_g$ is the capsule's gravity.

\subsubsection{Actuation Algorithm}

\begin{algorithm}[t] \small
\label{Alg_actuation}
\caption{Closed-Loop Magnetic Actuation}
\KwIn{Estimated capsule pose $\mathbf{p}_{c}^{(t)}, \widehat{\mathbf{m}}_c^{(t)}$, desired capsule pose $\mathbf{p}_d^{(t)}, \widehat{\mathbf{m}}_d^{(t)}$, $t=1,2,\cdots$}
\KwOut{5-DOF pose of the actuator $\mathbf{p}_{a}^{(t)}$, $\widehat{\pmb{m}}_{a}^{(t)}$, $t=1,2,\cdots$}
\For{$t=1, 2, \cdots$}
{
Estimate the capsule's velocity $\dot{\mathbf{p}}_{c}^{(t)}, \dot{\widehat{\mathbf{m}}}_c^{(t)}$\;
Calculate the pose and velocity errors\;
Obtain the desired magnetic force and torque $\boldsymbol{f}_{d}, \boldsymbol{\tau}_d$ with PD controllers\;
Update the actuator pose $\mathbf{p}_{a}^{(t)}$, $\widehat{\pmb{m}}_{a}^{(t)}$ using (\ref{F_actuator_update})\;
}
\Return{5-DOF pose of the actuator} $\mathbf{p}_{a}^{(t)}$, $\widehat{\pmb{m}}_{a}^{(t)}$, $t=1,2,\cdots$\;
\end{algorithm}

The goal of actuation is to reach the desired 5-DOF pose of the TEE probe tip $\mathbf{p}_d, \widehat{\mathbf{m}}_d$ for ultrasound image acquisition. To this end, two separate PD controllers are developed to ensure the position and orientation errors between the current and desired poses remain small. The control scheme is illustrated in Fig. \ref{Fig_overview_workflow}(c).
Given the pose estimation $\mathbf{p}_{c}, \widehat{\mathbf{m}}_c$ from the localization algorithm, the velocity of the capsule $\dot{\mathbf{p}}_{c}, \dot{\widehat{\mathbf{m}}}_c$ can be estimated by calculating the simple moving average of pose differences between consecutive steps over a short time period.
The position error between the current pose and the desired pose is $\mathbf{e}_p = \mathbf{p}_d - \mathbf{p}_c$, and the velocity error is $\dot{\mathbf{e}}_p = - \dot{\mathbf{p}}_{c}$. The orientation error between the current and desired magnetic dipole moments is calculated as
$\mathbf{e}_o= \widehat{\mathbf{m}}_c \times \widehat{\mathbf{m}}_d$, and the rotational velocity error is calculated as $\dot{\mathbf{e}}_o= \dot{\widehat{\mathbf{m}}}_c \times \widehat{\mathbf{m}}_d$.
Then, the desired force and torque applied to the capsule are given by:

\begin{equation}
\label{F_fd}
\begin{cases}
\boldsymbol{f}_{d} = \mathbf{K}_p \mathbf{e}_p + \mathbf{K}_d \dot{\mathbf{e}_p} - \boldsymbol{f}_g - \boldsymbol{f}_{res} \\
\boldsymbol{\tau}_d =  \mathbf{K}_{po} \mathbf{e}_{o} + \mathbf{K}_{do} \dot{\mathbf{e}_o}  \\
\end{cases}
\end{equation}

Since the theoretical magnetic force and torque can be calculated using (\ref{F_f}) as a function of the actuator's pose $\mathbf{p}_{a}, \widehat{\mathbf{m}}_a$, we can update the desired actuator pose by solving the constrained non-linear least-squares problem as follows:

\begin{equation} \small
\label{F_actuator_update}
\begin{aligned}
\mathbf{p}_{a}^*, \mathbf{m}_a^* = \mathop{\arg\min}_{\mathbf{p}_{a}, \mathbf{m}_a} \quad & \bigg\| \left[ \begin{matrix}\boldsymbol{f}_{d}\\\boldsymbol{\tau}_d\end{matrix} \right] - \left[ \begin{matrix}\boldsymbol{f}(\mathbf{p}_{a}, {\mathbf{m}}_a)\\\boldsymbol{\tau}(\mathbf{p}_{a}, {\mathbf{m}}_a) \end{matrix} \right] \bigg\|^2\\
\textrm{subject to} \quad
& \mathbf{p}_{a} \in \mathbb{W}, \\
 & \mathbf{m}_a \in \mathbb{R}^3 \\
\end{aligned}
\end{equation}

\noindent where $\mathbb{W} \subseteq \mathbb{R}^3$ is used to limit the movement of the actuator for safety reasons. The unit magnetic dipole moment of the actuator $\widehat{\mathbf{m}}_a$ is then updated by normalizing $\mathbf{m}_a^*$.

The closed-loop magnetic actuation algorithm is summarized in Algorithm \ref{Alg_actuation}.

\begin{figure}[tb]
\centering
\includegraphics[scale=1.0,angle=0,width=0.48\textwidth]{fig_system_setup.jpg}
\caption{(a) shows the experimental setup of the proposed MA-TEE system. The actuator magnet is rigidly mounted at the end-effector of a UR5 robotic arm for closed-loop control of the probe in the simulated esophagus in a cardiac phantom. The workspace of the system is above the external sensor array placed on a horizontal platform.
(b) takes  a closer look at the MA-TEE probe prototype, and (c) illustrates its components, including a TEE transducer, a cuboid permanent magnet and an IMU. The black dashed box shows a dummy transducer fabricated for use in our quantitative experiments.}
\label{Fig_system_setup}
\end{figure}

\section{Experiments}
\subsection{System Setup} \label{section_system_setup}

An overview of our real-world system setup is shown in Fig. \ref{Fig_system_setup}(a). The actuator is a cylindrical NdFeB magnet (N45 grade, axially magnetized, diameter and length: \SI{90}{\mm}) with a 3D-printed shell, which is attached to the end-effector of a 6-DOF robotic arm (UR5, Universal Robots). The distance from the center of the magnet to the robot flange is set to \SI{100}{\mm} to ensure normal operation of the motors in the robotic arm.
The external sensor array is composed of $6 \times 6$ three-axis high-precision magnetic field sensors (IIS2MDC, STMicroelectronics) and is placed on a horizontal platform with a spacing of \SI{120}{\mm}. The magnetic sensors are connected to a desktop via cables, and the output rate is \SI{100}{\hertz}. The transformation between the robot base and the external sensor array was measured before the experiments and fixed during the experiments. The localization and closed-loop control algorithms were implemented on the desktop using Python.

The MA-TEE probe prototype was modified from a standard adult TEE probe (X7-2t, Philips Healthcare) as follows. First, the flexible gastroscope in the traditional TEE probe was replaced with a soft tether that contains only wires to transmit ultrasound signals. Then, the probe tip was attached to a cuboid NdFeB magnet (N52 grade, $20 \times 15 \times \SI{11}{\cubic \mm}$) and an IMU sensor (ICM-20948, InvenSense, TDK) using 3D-printed connectors, and wrapped with thermoplastic elastomers (TPE), as shown in Fig. \ref{Fig_system_setup}(b-c). The overall size of the MA-TEE probe is $58 \times 16 \times \SI{15}{\cubic \mm}$, and the soft tether has a diameter of \SI{6}{\mm}. The MA-TEE probe was connected to a medical ultrasound machine (Sparq, Philips Healthcare) and the ultrasound images were streamed to the desktop via a data acquisition card. The data from the IMU is transmitted to the desktop through wires at \SI{100}{\hertz}.
We also fabricated a mockup MA-TEE probe to quantitatively evaluate the localization and closed-loop control methods as described in Section~\ref{section_exp_localization} and \ref{section_exp_actuation}, in order to avoid damaging the real ultrasound transducer. The dummy probe contains the same internal magnet and IMU as the MA-TEE probe prototype, and a 3D-printed dummy transducer with the same shape and mass as the real transducer, as shown in the lower right corner of Fig. \ref{Fig_system_setup}(c).

\begin{figure*}[tbh]
\setlength{\abovecaptionskip}{-0.1cm}
\centering
\includegraphics[scale=1.0,angle=0,width=0.75\textwidth]{fig_experiment_localization_spiral.jpg}
\caption{(a) shows the moving trajectory of the probe estimated by the LS and  EKF methods compared with the ground truth (GT) when the probe is moving along a 3-D spiral trajectory. (b) and (c) show the position and orientation errors during the movement. The red dashed circles highlight some locations where the large errors of the LS method are smoothed by the EKF method.}
\label{Fig_exp_localization_spiral}
\end{figure*}

\begin{figure*}[t]
\setlength{\abovecaptionskip}{-0.05cm}
\centering
\includegraphics[scale=1.0,angle=0,width=0.75\textwidth]{fig_experiment_localization_quantitative.jpg}
\caption{(a) illustrates the localization experiments where the probe is rigidly mounted at five points on each horizontal plane (red), and the localization results at each test point were recorded when the actuator was moving along an ``S"-shaped trajectory over the probe (blue). The box-plots in (b) and (c) show the overall localization errors v.s. the height of the test points in position and orientation, respectively.}
\label{Fig_exp_localization_quantitative}
\end{figure*}

\begin{table*}[tb] \renewcommand\arraystretch{1.25}
\centering
\caption{Localization Accuracy (Mean $\pm$ Standard Deviation) of the MA-TEE Probe Rigidly Mounted at Five Points on Four Horizontal Planes at Different Heights When the Actuator is Moving Over It}
\resizebox{0.99\textwidth}{15 mm}{
\begin{tabular}{ccccc}
\toprule
Test Positions & $h=\SI{50}{\mm}$ & $h=\SI{100}{\mm}$ & $h=\SI{150}{\mm}$ & $h=\SI{200}{\mm}$ \\
\midrule
(\SI{-0.18}{\m}, \SI{-0.18}{\m}) & 	$8.73 \pm \SI{2.85}{\mm }$ / $3.34 \pm \ang{2.58}$	 & $2.45 \pm \SI{1.51}{\mm}$ / $0.71 \pm \ang{0.42}$	 & $5.58 \pm \SI{3.03}{\mm}$ / $2.66 \pm \ang{1.66}$ & $6.13 \pm \SI{3.52}{\mm}$ / $2.54 \pm \ang{1.69}$\\
(\SI{-0.18}{\m}, \SI{0.18}{\m})	 & $4.36 \pm \SI{1.90}{mm}$ / $1.92 \pm \ang{1.51}$ & 	$1.24 \pm \SI{0.61}{mm}$ / $1.09 \pm \ang{0.57}$	 & $3.21 \pm \SI{2.66}{mm}$ / $2.23 \pm \ang{2.23}$	 & $5.26 \pm \SI{2.66}{mm}$ / $4.16 \pm \ang{2.75}$ \\
(\SI{0}{\m}, \SI{0}{\m}) & 	$1.29 \pm \SI{0.40}{mm}$ / $0.94 \pm \ang{0.50}	$ & $1.50 \pm \SI{0.50}{mm}$ / $1.58 \pm \ang{0.85}$ & 	$4.43 \pm \SI{1.76}{mm}$ / $3.39 \pm \ang{1.46}$	 & $7.01 \pm \SI{2.16}{mm}$ / $4.82 \pm \ang{2.19}$\\
(\SI{0.18}{\m}, \SI{-0.18}{\m}) & 	$11.28 \pm \SI{2.28}{mm}$ / $11.36 \pm \ang{11.26}$	 & $5.08 \pm \SI{2.61}{mm}$ / $1.34 \pm \ang{0.71}$	 & $5.96 \pm \SI{2.35}{mm}$ / $3.44 \pm \ang{1.87}$	 & $8.03 \pm \SI{2.39}{mm}$ / $3.08 \pm \ang{1.99}$ \\
(\SI{0.18}{\m}, \SI{0.18}{\m}) & 	$11.03 \pm \SI{2.24}{mm}$ / $10.13 \pm \ang{7.97}$	 & $2.55 \pm \SI{1.91}{mm}$ / $0.95 \pm \ang{0.93}$	 & $6.39 \pm \SI{1.62}{mm}$ / $2.50 \pm \ang{1.69}$ & 	$10.69 \pm \SI{1.71}{mm}$ / $4.43 \pm \ang{2.79}$ \\
Total	 & $6.80 \pm \SI{4.36}{mm}$ / $4.36 \pm \ang{6.33}$ & 	$2.57 \pm \SI{2.13}{mm}$ / $1.13 \pm \ang{0.78}$ & 	$5.00 \pm \SI{2.57}{mm}$ / $2.80 \pm \ang{1.83}$	 & $7.24 \pm \SI{3.11}{mm}$ / $3.73 \pm \ang{2.42}$ \\
\bottomrule
\end{tabular}}
\label{T_localization}
\end{table*}

\begin{figure*}[t]
\setlength{\abovecaptionskip}{-0.1cm}
\centering
\includegraphics[scale=1.0,angle=0,width=0.99\textwidth]{fig_experiment_actuation_quantitative.jpg}
\caption{(a) shows the experimental setup where the mockup MA-TEE probe was actuated to follow a straight-line trajectory between two acrylic planes placed above the sensor array. (b-d) and (e-g) show the estimated trajectories and tracking errors in the two sets of 5 trials with different initial positions. The red dashed lines in (b) and (e) illustrate the desired trajectory, and the black dashed lines in (c)(d)(f)(g) highlight the 40\% progress of the movement, which is used to calculate the steady-state tracking errors.}
\label{Fig_exp_actuation_quantitative}
\end{figure*}

\begin{figure*}[t]
\setlength{\abovecaptionskip}{-0.2cm}
\centering
\includegraphics[scale=1.0,angle=0,width=0.85\textwidth]{fig_experiment_actuation_disturb.jpg}
\caption{(a-d) show the snapshots of the closed-loop control experiments on a straight-line trajectory with two manual disturbances. The red dotted circles highlight the positions of the probe tip. (e) and (h) show the trajectories of the probe in two trials, respectively, where the red dashed lines show the desired trajectory. (f)(g)(i)(j) show the position and orientation errors in the two trials, respectively. The red shadowed areas highlight the duration of the disturbances.}
\label{Fig_exp_actuation_disturbance}
\end{figure*}

\subsection{Evaluation  of the Localization Method} \label{section_exp_localization}

First, we experimentally evaluated the performance of the proposed EKF-based method and the LS-based method in \cite{li2022external} for real-time localization of the probe. In this set of experiments, the mockup MA-TEE probe was rigidly connected to the flange of the robotic arm and was controlled to move along a 3-D spiral trajectory in a \SI[parse-numbers=false]{0.5 \times 0.5 \times 0.2}{\cubic\m} workspace, as shown in Fig. \ref{Fig_exp_localization_spiral}(a). The moving speed of the probe was set to \SI{10}{mm/s}. The ground-truth pose of the capsule was provided by the robotic arm. The pose estimation errors were calculated as the Euclidean distance between the positions and the minimum angle required to rotate from the estimated orientation to the true orientation. As shown in Fig. \ref{Fig_exp_localization_spiral}, the EKF method achieves a slightly better localization accuracy of $3.06 \pm \SI{1.33}{mm}$ and $0.69 \pm \ang{0.29}$ compared with the LS method ($3.21 \pm \SI{1.36}{mm}$ and $0.83 \pm \ang{0.57}$). It can be seen from Fig. \ref{Fig_exp_localization_spiral}(b-c) that large errors occasionally occur for the LS method as it only uses the current sensor measurements for localization. In contrast, the EKF method can take advantage of the previous pose estimation by fusing the IMU and magnetic field sensing data, which demonstrated a better capability to deal with noisy sensor measurements and provides a smoother trajectory. In addition, the EKF method provides a higher localization update frequency of \SI[parse-numbers=false]{80 \sim 90}{\hertz}, which is faster than the LS method (\SI[parse-numbers=false]{50\sim 60}{\hertz}).

Furthermore, in order to validate the compatibility of the localization method with magnetic actuation, we conducted a set of localization experiments at a total of 20 positions in the 3-D workspace above the sensor array in the presence of a dynamic actuator. 4 horizontal planes were selected with an increasing height from \SI{50}{mm} to \SI{200}{mm}, and 5 test points were selected on each plane, as shown in Fig. \ref{Fig_exp_localization_quantitative}(a). At each test position, the probe was rigidly mounted, and the actuator was commanded to move along an ``S"-shaped trajectory on a horizontal plane at a height of \SI{250}{mm} above the probe. The real-time localization results at each test point were recorded during the movement of the actuator, and reported in Table \ref{T_localization} and Fig. \ref{Fig_exp_localization_quantitative}(b-c). It was found that the best localization performance was achieved when the probe was placed at medium heights (e.g., between \SI{100}{mm} and \SI{150}{mm}) above the sensor array. When the probe is farther away from the sensor array, the signal-to-ratio at the magnetic field sensors will be lower, which would result in a decreased localization accuracy. On the other hand, when the probe was too close to the sensor array, the actuator would also be very close to the sensor array to realize effective actuation of the probe, and the inaccuracy of the magnetic field model of the actuator would have a greater impact on the localization performance. It was also found that locations closer to the center of the workspace generally had a higher localization accuracy, which may be because more precise sensor measurements were used for localization. The average localization accuracy over the entire workspace in the presence of a moving actuator was \SI{5.4}{\mm} and \ang{3.0} in position and orientation, respectively.

\subsection{Evaluation of the Closed-Loop Control Method}  \label{section_exp_actuation}

\begin{figure*}[tb]
\centering
\includegraphics[scale=1.0,angle=0,width=0.99\textwidth]{fig_phantom_advance.jpg}
\caption{Snapshots of the movement trajectories of the MA-TEE probe and the actuator and the corresponding ultrasound images during the propulsion experiment in the cardiac phantom.}
\label{Fig_exp_phantom_advance}
\end{figure*}

\begin{figure*}[tb]
\centering
\includegraphics[scale=1.0,angle=0,width=0.75\textwidth]{fig_phantom_rotation.jpg}
\caption{(a) and (b) illustrate the in-plane rotation and out-of-plane rotation of the MA-TEE probe in the cardiac phantom. The red and blue arrows show the rotation axes and directions in the two experiments, respectively. The yellow dotted lines and arrows in the ultrasound images are used to highlight the differences between the images acquired before and after the rotations.}
\label{Fig_exp_phantom_rotation}
\end{figure*}

In order to validate the effectiveness of the proposed closed-loop control method, we inserted the mockup TEE probe between two acrylic planes placed horizontally above the sensor array, as shown in Fig. \ref{Fig_exp_actuation_quantitative}(a). The vertical position of the probe was limited by the two planes, which was approximately \SI{100}{\mm} above the sensor array. The system was commanded to actuate the probe to follow a straight-line trajectory on the horizontal plane (i.e., $y=0$). The desired magnetic moment of the probe $\widehat{\mathbf{m}}_{d}$ was set to be aligned with the $-z$ direction in the world frame. The acrylic planes were rigidly attached to the sensor array during the experiments.

First, two sets of 5 trials were conducted to assess the control performance of the system when the initial position of the capsule deviates from the desired trajectory. The initial deviation of the probe from the $y$-axis was set to \SI{100}{mm} and \SI{-100}{mm} in the two sets of experiments, respectively. The tracking error in position was computed as the lateral offset from the desired trajectory, and the orientation error was computed as the angle between the desired and estimated magnetic moments of the probe $\widehat{\mathbf{m}}_{d}$ and $\widehat{\mathbf{m}}_{c}$.
Fig. \ref{Fig_exp_actuation_quantitative}(b-g) show the estimated trajectories of the capsule compared with the desired trajectory and the tracking errors in position and orientation in the two sets of experiments. For the first set of experiments, the steady-state position error after the capsule has traveled 40\% of the trajectory was $1.97 \pm 2.97$ \si{mm}, and the steady-state orientation error was $3.63 \pm 1.75 ^\circ$ over the 5 trials. For the second set of experiments, the steady-state tracking errors were $-0.14 \pm 4.71$ \si{mm} and $5.46 \pm 2.04 ^\circ$ in position and orientation, respectively.

We additionally conducted two experiments to demonstrate the capability of the system to overcome undesired disturbances and maintain a small steady-state tracking error. As shown in Fig. \ref{Fig_exp_actuation_disturbance}, the probe was initialized on the desired trajectory, and two manual disturbances were implemented during the voyage of the probe to push the probe tip laterally by approximately \SI{100}{\mm} when it has travelled about \SI{120}{\mm} and \SI{240}{\mm}. It can be seen from Fig. \ref{Fig_exp_actuation_disturbance} (b)(d)(e)(h) that the system quickly responded to the position errors and controlled the probe to recover from the disturbances. Fig. \ref{Fig_exp_actuation_disturbance} (f)(g)(i)(j) show the tracking errors in the two trials. The tracking errors calculated over the trajectory before and after the two disturbances were $3.23 \pm \SI{4.80}{mm}$  and $3.86 \pm \ang{2.40}$ for the first experiment, and $2.59 \pm \SI{7.93}{mm}$ and
$4.80  \pm \ang{3.36}$ for the second experiment, respectively.

\subsection{Demonstration of Tele-operated MA-TEE in a Cardiac Phantom}

Finally, we carried out a set of qualitative tests using a realistic echocardiography training phantom with simulated cardiac tissues (Blue Phantom Cardiac Echo Model BPH700, CAE Healthcare) to demonstrate the tele-operated TEE acquisitions using our proposed MA-TEE system, as shown in Fig. \ref{Fig_system_setup}(a). The simulated esophagus was lubricated with ultrasound coupling gel to ensure acoustic coupling and improve the lubricity of the probe-tissue interface. The MA-TEE probe prototype as described in Section~\ref{section_system_setup} was manually placed at the entry of the simulated esophagus. After initial localization, the actuator was initialized above the estimated probe position with $\widehat{\mathbf{m}}_a$ pointing downward. Then, the desired motion of the probe was specified by the user via a graphical user interface developed on the desktop, and the movement of the actuator was automatically computed and executed by the system.
The goal of this experiment is to preliminarily demonstrate the capability of the system to conduct remote manipulation of the probe for ultrasound image acquisitions. In all the tests, the electronic steering angle was set as 0, which means the imaging plane coincided with the $y$-$z$ plane of the probe.

In the first test, the system was commanded to advance the probe along its heading direction (i.e., $x$-axis of the probe frame). Snapshots of the motions of the actuator and the probe as well as the corresponding ultrasound images are shown in Fig. \ref{Fig_exp_phantom_advance}. The total length of the moving trajectory of the probe was about \SI{200}{\mm}, and the propulsion time was about \SI{20}{\s}. It can be seen that the probe was able to be smoothly advanced in the simulated esophagus and acquired clear ultrasound images of the heart tissues.

In the second test, we qualitatively demonstrate the in-plane and out-of-plane rotations of the probe performed by the MA-TEE system under the tele-operation in the cardiac phantom, as shown in Fig. \ref{Fig_exp_phantom_rotation}. First, the probe was required to rotate around its $x$-axis by \ang{15}. The images acquired before and after applying the rotation are shown in Fig. \ref{Fig_exp_phantom_rotation}(a).
Then, the probe was commanded to perform an out-of-plane rotation around its $y$-axis by \ang{-15}, which corresponds to the ``anteflex” operation in traditional TEE manipulation. As illustrated in Fig. \ref{Fig_exp_phantom_rotation}(b), the heart cavity was not visualized with the original probe orientation, and was clearly visualized after the out-of-plane rotation executed by the MA-TEE system.
These results have demonstrated the capability of the proposed MA-TEE system to adjust the position and orientation of the probe under remote control for TEE image acquisitions.

\subsection{Video Demonstration}
A detailed video demonstration of our experiments in Section~V-B to V-D is provided in the supplementary multimedia attachment. Specifically, an illustration of the localization experiments is given in Supplementary Video 1\footnote{Demonstration in Section~V-B: available at \url{https://youtu.be/3ykfcZ0i7Sw}}. An illustration of the closed-loop control experiments is provided in Supplementary Video 2\footnote{Demonstration in Section~V-C: available at \url{https://youtu.be/4o6V57p78hk}}. Finally, a visualization of the qualitative experiments in the cardiac phantom can be found in Supplementary Video 3\footnote{Demonstration in Section~V-D: available at \url{https://youtu.be/yEbEgG_c5cg}}.

\section{Conclusion}
In this paper, we have presented the first closed-loop magnetic manipulation framework to perform 6-DOF pose estimation and 5-DOF control of a robotic TEE probe. We have shown that by modifying a standard TEE probe to forgo the flexible gastroscope and attach the probe tip with a magnet and an IMU sensor, the probe can be viewed as a magnetic capsule robot, and direct manipulation of the distal tip of the probe in the esophagus can be achieved based on an external and internal sensor fusion approach. Our method can achieve accurate and efficient control of the magnetic capsule robot without requiring complex structures of the capsule or the external actuator, and can circumvent the need for applying specific motions of the capsule.
Our results show that
\begin{enumerate}
\item The proposed EKF method can effectively fuse the external and internal sensor data for probe tip localization, which can better deal with the noisy sensor measurements and obtain a smoother trajectory compared with the LS method \cite{li2022external}.
\item The real-time localization method can achieve an average localization accuracy of \SI{5}{\mm} and \ang{3} in a large workspace of  \SI[parse-numbers=false]{0.5 \times 0.5 \times 0.2}{\cubic\m} workspace at an update rate of $80 \sim \SI{90}{\hertz}$, and is compatible with magnetic actuation.
\item The proposed localization and actuation methods can effectively allow closed-loop control of the probe to follow a manually specified trajectory. The results on a tissue-mimicking phantom demonstrate that the proposed framework is applicable in real conditions and generally meet the requirements for tele-operated TEE acquisitions.
\end{enumerate}

The MA-TEE probe prototype used in this work was manually assembled and fabricated to preliminarily validate the feasibility of the proposed approach. In view of a clinical translation, the design of the probe will need further refinements, e.g., to use a more spherical shape to reduce friction, and coat the probe with soft and biocompatible materials \cite{norton2019intelligent}.
Moreover, the size of the MA-TEE probe and the soft tether can be further reduced by integrating the TEE transducer and the magnetic control modules in a more compact design. The size of the permanent magnet in the TEE probe can be reduced by increasing the volume of the actuator magnet to keep the same actuation distance \cite{mahoney2016five}.
In addition, it was observed in our experiments that the ultrasound signal was occasionally lost due to insufficient contact between the probe and the tissue, and adding more coupling gel on the probe could temporarily solve this issue. In view of this, a visual servoing method that can control the pose of the probe based on the ultrasound image quality will be helpful to maintain good contact between the probe and the esophageal tissue for better image quality \cite{chatelain2017confidence}.

\bibliographystyle{IEEEtran}