Robust capsule-robot positioning with limited magnetic observations: An inertial-enhanced approach

ABSTRACT

The capsule robot has become an important tool in covering the entire spectrum of digestive tract disease diagnosis. To achieve magnetic capsule-robot localization, the Levenberg-Marquardt (LM) algorithm has become a mainstream approach that provides accurate solutions in the general case. In practice, however, to meet the requirements of wearability, fewer sensors and lower power consumption are required. When the number of sensor observations becomes smaller, local convergences and outliers may occur in positioning results. To mitigate this issue, this paper makes two contributions to enhance the robustness of capsule-endoscope positioning, especially when the quality of magnetic observations is low. First, it proposes a two-step approach that initializes the capsule attitude by using inertial measurements before estimating the position. Second, it presents an improved LM-based positioning algorithm based on vest-type magnetic sensor arrays. Furthermore, to verify the proposed approach, a vest-type wearable device with two low-cost magnetometer arrays is designed. Test results have shown the effectiveness of the proposed LM method in enhancing positioning when there is a lack of observations.

KEYWORDS:

• Capsule-robot
• Levenberg-Marquardt (LM)
• wearable
• positioning

1. Introduction

Since the invention of wireless capsule endoscopy in Israel in 2000 (Iddan et al. 2000), its use in clinical diagnosis has largely compensated for the limited view and poor patient experience of wired endoscopes such as gastroscopy and enteroscopy. The wireless capsule endoscope can diagnose and treat gastrointestinal and digestive disorders without causing discomfort and without affecting the patient’s normal work and life. After years of exploration of clinical and scientific research, the capsule robot has become an important inspection method for the diagnosis of all-digestive roads, which lays a solid foundation for the future (Vasilakakis et al. 2020). At present, the functionality of capsule robot is no more limited to the diagnosis of small intestinal diseases; instead, it has become an increasingly important weapon for the diagnosis of the whole gastrointestinal disease.

To achieve a better clinical application of wireless capsule endoscopes in assisting doctors to diagnose the treatment of gastrointestinal diseases, a key step is the acquisition of the endoscope positions in vivo. Thus, it has important clinical significance to accurately locate the lesion through capsule robots (Dey et al. 2017). Location diagnosis and qualitative diagnosis are two important aspects of disease diagnosis. Because the lesions at various places in the human body may cause different clinical manifestations and diseases, precise knowledge of the lesion location can help doctors to assess the severity of the disease and determine the prognosis of the disease. In addition, accurate lesion positioning can enable a clearer explanation of the condition to patients and their families, greatly facilitating communication between doctors and patients. Moreover, higher lesion-localization accuracy leads to more precise surgical treatment on neoplastic lesions. Furthermore, in the area of capsule-robotic-assisted therapy, drug delivery systems based on magnetically controlled piston release systems and automatic capacitive controllers are currently in clinical trials (Mapara and Patravale 2017; Guo et al. 2019). Targeted drug delivery by a capsule robot maximizes local drug concentrations and reduces systemic adverse effects but also requires precise targeting of the lesion. Therefore, it is a crucial task in the development of wireless capsule endoscopy to configure a positioning system with high precision, easy operation, and portability. Therefore, the localization technology of wireless capsule endoscopes has become a focus of research.

Since 2000, researchers have carried out in-depth research on the trajectory and precise positioning of a wireless capsule endoscope inside the human body by using different sensors and positioning technologies. Similar to indoor positioning technology (Cao et al. 2019; Takayama et al. 2019; Li et al. 2021), at present, the main positioning methods of wireless capsule endoscopy are based on Radio Frequency (RF) signals, electromagnetic positioning (i.e. permanent magnet in the capsule), inertial positioning, visual positioning, multi-sensor fusion positioning, and localization assisted by CT, MRI, ultrasound, and other imaging equipment (Nelson, Kaliakatsos, and Abbott 2010; Ye 2013; Wang et al. 2011; Turan et al. 2018; You 2017). Among these methods, magnetic-based positioning has become a hot research topic because its signal is harmless to the human body and can pass through human tissue without any attenuation (Wang et al. 2019; Popek, Schmid, and Abbott 2016).

To localize the capsule endoscope using magnetic signals, there are active and passive positioning systems. An active system controls the movement of the wireless capsule endoscope through various active actuators to improve the maneuverability of the capsule endoscope inside the human body, reduce the retention time of the wireless endoscope, and improve diagnostic efficiency (Keller et al. 2012; Popek, Schmid, and Abbott 2016; Taddese et al. 2018). This active drive mechanism controls the motion of the in-vivo capsule by placing an external permanent magnet outside the body. The method of determining the position parameters of the wireless capsule endoscope and its trajectory based on a static magnetic field is reliable and simple in principle and generally has a high positioning accuracy. However, this control mainly takes place in the stomach and is difficult to achieve in the intestine. Meanwhile, it restricts the patient’s movement.

By contrast, when using a passive positioning approach, the wireless capsule has relied on the body’s mechanisms (e.g. peristaltic movement and gravitational traction) to power its movement within the body. Hu et al. have been working on passive positioning systems for wireless endoscopic magnetometers since 2004 (Hu, Meng, and Mandal 2005; Hu et al. 2016). Their related research covers various aspects of theoretical research, hardware design, and algorithm research. Afterward, there are subsequent research methods (Xu et al. 2017; Jeong et al. 2017; Turan et al. 2017). The passive drive method has the advantages of simple operation, energy efficiency, and convenience. However, their positioning accuracy is generally lower than the active positioning methods.

The capsule endoscope will stay in the intestine for over seven hours, and the patient usually goes home after swallowing the capsule endoscope and waits for it to be excreted, making it difficult for the active positioning system to achieve full process tracking. While the current passive positioning methods use a large number of sensors and are prototype systems that are still a long way from being practical (Turan et al. 2018; Hu et al. 2016). Therefore, we explored the design of a vest-type wearable capsule endoscope positioning system to achieve the whole-process tracking of the capsule endoscope in the digestive tract.

Most existing magnetic-based capsule-positioning approaches use a cylindrical magnet as the target and a magnetometer array as the observation device. A magnetic dipole model is used to establish the equations of state for magnetic field strength and position, and the equations are solved to localize the target by either a non-linear or a linear optimization method. The Levenberg-Marquardt (LM) method is not only one of the most widely-used nonlinear algorithms in engineering practice (Kai et al. 2020) but also one of the most accurate and efficient nonlinear algorithms for solving capsule positions (Hu et al. 2016; Hu 2006).

A challenge for the LM method is the requirement of accurate initial position and attitude, which is a condition that is difficult to meet in practical applications. To alleviate this issue, some scholars have incorporated a Micro-Electro-Mechanical System (MEMS) Inertial Measurement unit (IMU) inside the capsule endoscope to determine the capsule attitude when implementing magnetic positioning of active capsule endoscopes (Di Natali, Beccani, and Valdastri 2013; Taddese et al. 2018). This approach has provided a new idea for solving the capsule endoscope attitude.

Compared to the existing IMU-enhanced methods, the proposed approach moves one step further and can provide more accurate capsule endoscope positioning. First, once the attitude parameters are determined and compensated, only the position parameters are estimated in the proposed method, increasing the reliability and efficiency of the positioning system. Second, current research based on magnetometer arrays is still focused on algorithmic exploration and does not consider the practicality of sensor arrays; however, the practical application requires a rational design of the sensor array. Therefore, this paper proposes a vest-type wearable design with two low-cost magnetometer arrays for capsule endoscope positioning. Furthermore, finding that the traditional LM method is not robust enough when using in the improved wearable design, an improved LM positioning algorithm is designed, which improves the robustness of capsule endoscope positioning.

2. Positioning using vest-type wearable magnetic sensor array

2.1. Positioning principle

Current methods for passive positioning of capsule endoscopes based on magnetometer arrays use a magnetic dipole model to describe the magnetic field strength of the magnet (Popek, Schmid, and Abbott 2016):

(1) $\mathrm{B}=B_T\left(\frac{3({\mathit{A}}_0\mathit{P})\mathit{P}}{R^5}-\frac{{\mathit{A}}_0}{R^3}\right)$(1)

where $\mathit{B}=\left[B_x{B}_y{B}_z\right]$ is the magnetic intensity at the target position, $B_T$ is a constant representing the strength of the magnet, ${\mathit{A}}_0$ represents the unit vector of the direction of the magnetic moment, determined by the attitude of the magnet, $\mathit{P}$ is the relative position vector between the target position and the magnet, and $\mathit{R}$ is the Euclidean distance of the target position from the magnet.

If there are enough observations, the relative position vector solution can be performed based on Equation (1)(1) $\mathrm{B}=B_T\left(\frac{3({\mathit{A}}_0\mathit{P})\mathit{P}}{R^5}-\frac{{\mathit{A}}_0}{R^3}\right)$(1) . Therefore, a sensor array design can be employed to provide sufficient observation. This paper designs a vest endoscopic positioning system for capsule endoscopy using two 3 × 3 3-axis sensor arrays. Figure 1 shows the coordinate relationship of the capsule endoscope in this configuration, the green squares in the diagram are the triaxial magnetometers.

Figure 1. A capsule in the sensor arrays system.

The traditionally-used LM algorithm requires accurate initialization of ${\mathit{A}}_0$ and $\mathit{P}$, whereas in practice it is difficult to initialize the position and especially attitude accurately. The attitude of the capsule endoscope is therefore initialized using inertial sensors, the attitude determination method used in this system is described below.

2.2. Attitude determination

For a cylindrical magnet with two cylindrical faces with north and south poles, the direction of the magnetic moment can be determined by its central axis. After attitude transformation, the magnetic moment direction vector ${\mathit{A}}^b=[001{]}^T$ is expressed as ${\mathit{A}}^n$ in the navigation coordinate frame n. After the magnet rotates around the body coordinate frame b z-axis, x-axis, and y-axis with heading angle $\mathit{\psi }$, pitch angle $\mathit{\theta }$, and roll angle $\mathit{\varphi }$, the magnetic moment direction is represented as:

(2) $\begin{array}{c}{\mathit{A}}^n={\mathit{C}}_{\varphi }{\mathit{C}}_{\theta }{\mathit{C}}_{\psi }{\mathit{A}}^b\\ =\left(\left[\begin{array}{ccc}cos\mathit{\varphi }& 0& -sin\mathit{\varphi }\\ 0& 1& 0\\ sin\mathit{\varphi }& 0& cos\mathit{\varphi }\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\mathit{\theta }& sin\mathit{\theta }\\ 0& -sin\mathit{\theta }& cos\mathit{\theta }\end{array}\right]\right)\left(\left[\begin{array}{ccc}cos\mathit{\psi }& -sin\mathit{\psi }& 0\\ sin\mathit{\psi }& cos\mathit{\psi }& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\right)\\ =\left[\begin{array}{ccc}cos\mathit{\varphi }& sin\mathit{\varphi }sin\mathit{\theta }& -sin\mathit{\varphi }cos\mathit{\theta }\\ 0& cos\mathit{\theta }& sin\mathit{\theta }\\ sin\mathit{\varphi }& -cos\mathit{\varphi }sin\mathit{\theta }& cos\mathit{\varphi }cos\mathit{\theta }\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]\end{array}$(2)

(3) $C_b^n=\left[\begin{array}{ccc}cos\mathit{\varphi }& sin\mathit{\varphi }sin\mathit{\theta }& -sin\mathit{\varphi }cos\mathit{\theta }\\ 0& cos\mathit{\theta }& sin\mathit{\theta }\\ sin\mathit{\varphi }& -cos\mathit{\varphi }sin\mathit{\theta }& cos\mathit{\varphi }cos\mathit{\theta }\end{array}\right]$(3)

where ${\mathit{C}}_{\psi }$, ${\mathit{C}}_{\theta }$, and ${\mathit{C}}_{\varphi }$ are the attitude transfer matrices rotating around z, y, and x, respectively. From Equation (1)(1) $\mathrm{B}=B_T\left(\frac{3({\mathit{A}}_0\mathit{P})\mathit{P}}{R^5}-\frac{{\mathit{A}}_0}{R^3}\right)$(1) , the rotation matrix ${\mathit{C}}_{\psi }$ around the z-axis has no effect on ${\mathit{A}}^b$ during the rotation transformation, which means the process of changing the magnetic moment direction vector from the b-frame to the n-frame does not require the heading angle. That is, if the horizontal attitude angle of the cylindrical magnet is known, then the direction cosine matrix ${\mathrm{C}}_b^n$ of the magnet’s b-frame to n-frame can be found.

Unlike the IMU attitude study of an external magnet-driven capsule, in this paper, in the absence of an external magnet, the IMU is attached to the cylindrical magnet of the capsule, with the z-axis of the IMU coinciding with the central axis of the cylindrical magnet. When the capsule is in a state where the external forces are low, the acceleration information can be used to directly calculate its horizontal attitude angles (i.e. pitch and roll) as:

(4) $\mathit{\theta }=\mathit{arcsin}\left(\frac{a_x^b}{\mathit{g}}\right)$(4)

(5) $\mathit{\varphi }=arctan\left(\frac{a_y^b}{a_z^b}\right)$(5)

such that $a_x^b$, $a_y^b$, and $a_z^b$ are the three-axis output of the accelerometer under the b-frame.

However, the capsule may move inside the body with a pitch angle close to 90°, when using Equation (5)(5) $\mathit{\varphi }=arctan\left(\frac{a_y^b}{a_z^b}\right)$(5) to solve for the roll angle will result in a large error, which is known as the attitude singularity problem.

2.3. Nonlinear least-squares-based localization of magnets based on triaxial magnetometer observations

2.3.1. Nonlinear least-squares algorithm design

The system designed in this paper only estimates the magnet position as it does not need to estimate the magnetic field direction. Therefore, this paper designs a non-linear least-squares algorithm for the position parameters only.

Small-sized cylindrical magnets can be treated as magnetic dipoles in capsule endoscope positioning. Let the coordinates of the magnet in the n-frame be $(\begin{array}{ccc}\mathit{x}& \mathit{y}& \mathit{z}\end{array})$, and the magnetic moment direction vector be $\mathrm{A}=[\mathit{l}\mathit{m}\mathit{n}{]}^{\mathrm{T}}$. Then, according to the magnetic dipole model in Equation (1), the magnetic induction intensity ${\mathrm{B}}_i=[B_{\mathit{ix}}{B}_{\mathit{iy}}{B}_{\mathit{iz}}{]}^{\mathrm{T}}$ of the triaxial magnetometer $S_i$ at coordinate $(\begin{array}{ccc}{x}_i& y_i& z_i\end{array})$ can be written as:

(6) $B_{\mathit{ix}}=B_T\left\{\frac{3[l(x_i-\mathit{x})+\mathit{m}(y_i-\mathit{y})+\mathit{n}(z_i-\mathit{z})](x_i-\mathit{x})}{L_i^5}-\frac{l}{L_i^3}\right\}$(6)

(7) $B_{\mathit{iy}}=B_T\left\{\frac{3[l(x_i-\mathit{x})+\mathit{m}(y_i-\mathit{y})+\mathit{n}(z_i-\mathit{z})](y_i-\mathit{y})}{L_i^5}-\frac{m}{L_i^3}\right\}$(7)

(8) $B_{\mathit{iz}}=B_T\left\{\frac{3[l(x_i-\mathit{x})+\mathit{m}(y_i-\mathit{y})+\mathit{n}(z_i-\mathit{z})](z_i-\mathit{z})}{L_i^5}-\frac{n}{L_i^3}\right\}$(8)

where $L_i$ is the Euclidean distance from the magnet to the sensor $S_i$ and is calculated by:

(9) $L_i=\sqrt{{(x_i-\mathit{x})}^2+{(y_i-\mathit{y})}^2+{(z_i-\mathit{z})}^2}$(9)

The previous formula is the state equation of the magnetic induction intensity at the magnetometer, and the typical nonlinear least-squares observation model of the magnetometer is:

(10) $\mathit{s}=\mathit{f}(\mathit{x})+\mathit{v}$(10)

where $\mathit{s}$ is the magnetic sensor observation, $\mathit{f}(\mathit{x})$ is the estimated magnetic field strength of the magnetic dipole model, and $\mathit{v}$ is the error in the observation. The Taylor series expansion of the above equation at the current estimate $\hat{x}$ is

(11) $\mathit{s}=\mathit{f}\left(\hat{x}\right)+\frac{\mathit{df}\left(\mathit{x}\right)}{\mathit{d}\mathit{x}}{|}_{\mathit{x}=\hat{x}}\left(\mathit{x}-\hat{x}\right)+\mathrm{L}+\mathit{v}$(11)

When the higher-order terms are omitted, and only the first-order term is used in the linearization, the expression is truncated to:

(12) $\mathit{s}=\mathit{f}\left(\hat{x}\right)+\mathit{J\delta }\mathit{x}+\mathit{v}$(12)

where $\mathit{\delta }\mathit{x}=\mathit{x}-\hat{x}={\left[\mathit{\delta x}\mathit{\delta y}\mathit{\delta z}\right]}^T$ represents the coordinate error in the state vector and Jacobi matrix $\mathit{J}=\mathit{dh}(\mathit{x})/\mathit{d}\mathit{x}$ is the design matrix. A linear observation model of the observation misclosure vector $\mathit{\delta s}$ can be obtained by transforming the above formula:

(13) $\begin{array}{c}\mathit{s}-\mathit{f}\left(\hat{x}\right)=\mathit{J}\mathit{\delta }\mathit{x}+\mathit{v}\\ \mathit{\delta s}=\mathit{J}\mathit{\delta }\mathit{x}+\mathit{v}\end{array}$(13)

where $\mathit{\delta }\mathit{s}$ is the difference between the measured value of the magnetometer and the estimated magnetic intensity. The linear observation model of the nonlinear least-squares algorithm based on the observed values of the magnetometer array can then be designed as:

(14) ${\left[\mathit{\delta }{\mathit{B}}_{1\mathit{x}}\mathit{\delta }{B}_{1\mathit{y}}\mathit{\delta }{B}_{1\mathit{z}}\cdots \mathit{\delta }{B}_{\mathit{nx}}\mathit{\delta }{B}_{\mathit{ny}}\mathit{\delta }{B}_{\mathit{nz}}\right]}^{\mathrm{T}}=\mathit{J}\left[\begin{array}{c}\mathit{\delta x}\\ \mathit{\delta y}\\ \mathit{\delta z}\end{array}\right]$(14)

where

(15) $\mathit{\delta }{B}_{\mathit{ix}}={\widehat{B}}_{\mathit{ix}}-\mathit{Bix}\mathit{\delta }{B}_{\mathit{iy}}={\widehat{B}}_{\mathit{iy}}-\mathit{Biy}\mathit{\delta }{B}_{\mathit{iz}}={\widehat{B}}_{\mathit{iz}}-\mathit{Biz}$(15)

$\hat{B}$ is the magnetometer observation, $\bar{B}$ is the estimated magnetic field strength, $\mathit{i}=1\cdots \mathit{n}$, n is the total number of magnetometers, and the design matrix $\mathit{J}$ can be calculated by:

(16) $\mathit{J}={\left[\begin{array}{ccccc}\frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{x}}}{\mathrm{\partial x}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{y}}}{\mathrm{\partial x}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{z}}}{\mathrm{\partial x}}& \mathit{\hspace{0.17em}}& \frac{\mathrm{\partial }{\mathit{B}}_{\mathit{nz}}}{\mathrm{\partial x}}\\ \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{x}}}{\mathrm{\partial y}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{y}}}{\mathrm{\partial y}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{z}}}{\mathrm{\partial y}}& \cdots & \frac{\mathrm{\partial }{\mathit{B}}_{\mathit{nz}}}{\mathrm{\partial y}}\\ \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{x}}}{\mathrm{\partial z}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{y}}}{\mathrm{\partial z}}& \frac{\mathrm{\partial }{\mathit{B}}_{1\mathit{z}}}{\mathrm{\partial z}}& \mathit{\hspace{0.17em}}& \frac{\mathrm{\partial }{\mathit{B}}_{\mathit{nz}}}{\mathrm{\partial z}}\end{array}\right]}^{\mathrm{T}}$(16)

and the partial derivative of $B_{\mathit{ix}}(\mathit{i}=1\cdots \mathit{n})$ is given directly by:

(17) $\begin{array}{r}\frac{\mathrm{\partial }{\mathit{B}}_{\mathit{ix}}}{\mathrm{\partial }\mathit{x}}=\frac{(-P_i-3\mathit{l}(x_i-\mathit{x}))L_i^5-15P_i(x_i-\mathit{x})L_i^4(\mathrm{\partial }{L}_i/\mathrm{\partial }\mathit{x})}{L_i^{10}}\\ +\frac{3l}{L_i^4}\frac{\mathrm{\partial }{L}_i}{{\mathrm{\partial }}_x}\\ \frac{\mathrm{\partial }{\mathit{B}}_{\mathit{ix}}}{\mathrm{\partial }\mathit{y}}=\frac{(-3m(x_i-\mathit{x}))L_i^5-15P_i(x_i-\mathit{x})L_i^4(\mathrm{\partial }{L}_i/\mathrm{\partial }\mathit{y})}{L_i^{10}}\\ +\frac{3m}{L_i^4}\frac{\mathrm{\partial }{L}_i}{{\mathrm{\partial }}_y}\\ \frac{\mathrm{\partial }{\mathit{B}}_{\mathit{ix}}}{\mathrm{\partial }\mathit{z}}=\frac{(-3n(x_i-\mathit{x}))L_i^5-15P_i(x_i-\mathit{x})L_i^4(\mathrm{\partial }{L}_i/\mathrm{\partial }\mathit{z})}{L_i^{10}}\\ +\frac{3n}{L_i^4}\frac{\mathrm{\partial }{L}_i}{{\mathrm{\partial }}_z}\end{array}$(17)

where

(18) $P_i=3[\mathit{l}(x_i-\mathit{x})+\mathit{m}(y_i-\mathit{y})+\mathit{n}(z_i-\mathit{z})]$(18)

(19) $\frac{\mathrm{\partial }{L}_i}{\mathrm{\partial x}}=\frac{x-x_i}{L_i}\frac{\mathrm{\partial }{L}_i}{\mathrm{\partial y}}=\frac{y-y_i}{L_i}\frac{\mathrm{\partial }{L}_i}{\mathrm{\partial y}}=\frac{z-z_i}{L_i}$(19)

Similarly, the partial derivatives of $B_{\mathit{iy}}$ and $B_{\mathit{iz}}$ can be obtained, which are then used to determine the design matrix $\mathit{J}$.

After the above process, this nonlinear least-squares problem is completely converted into a linear least-squares problem, which can significantly improve computational efficiency. The least-squares solution can be written as:

(20) $\mathit{\delta }\mathit{x}=({\mathit{J}}^{\mathrm{T}}\mathit{J}{)}^{-1}{\mathit{J}}^T\mathit{\delta }\mathit{s}$(20)

2.3.2. Conventional LM algorithm for capsule endoscope positioning

The Equation (20)(20) $\mathit{\delta }\mathit{x}=({\mathit{J}}^{\mathrm{T}}\mathit{J}{)}^{-1}{\mathit{J}}^T\mathit{\delta }\mathit{s}$(20) , i.e. the Gauss-Newton solution, requires an accurate initial value to estimate the position, and it also requires ${\mathit{J}}^{\mathrm{T}}\mathit{J}$ to be positive definite, otherwise, the algorithm does not converge. Therefore, the LM method is mostly used in the literature to localize magnetic sensor arrays. According to the LM algorithm Equation (20)(20) $\mathit{\delta }\mathit{x}=({\mathit{J}}^{\mathrm{T}}\mathit{J}{)}^{-1}{\mathit{J}}^T\mathit{\delta }\mathit{s}$(20) can be written as

(21) $\mathit{\delta }\mathit{x}=({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}{\mathit{J}}^{\mathrm{T}}\mathit{\delta }\mathit{s}$(21)

where $\mathit{\lambda }$ is a non-negative damping factor. That is, the LM method keeps ${\mathit{J}}^T\mathit{J}$ positive by adding a non-negative diagonal array to it. Each iteration requires a re-estimate of $\mathit{\lambda }$. If $\mathit{\lambda }$ decreases to close to 0, the above equation converts to a Gauss-Newton method. In contrast, if $\mathit{\lambda }$ is sufficiently large, the solution approaches the direction of steepest descent.

The most commonly-used LM adaptive strategy is to adjust the damping factor by increasing the damping factor $\mathit{\lambda }$ if the current step size increases the target function, i.e. the sum of squared magnetic field strength errors $\mathit{S}=\sum _i^n(\mathit{\delta }{s}_i{)}^2$ and $\mathit{\delta }{s}_i=\mathit{\delta }{B}_{\mathit{ix}}+\mathit{\delta }{B}_{\mathit{iy}}+\mathit{\delta }{B}_{\mathit{iz}}$. If the current step size decreases the target function $\mathit{S}$, the damping factor $\mathit{\lambda }$ needs to be reduced.

As Figure 2 indicates, the current LM algorithm used for capsule endoscope positioning follows the steps below (Kai et al. 2020; Hu et al. 2016; Zhang and Geng 2009):

1. Initialize the coordinates ${\mathit{x}}_0$, the damping factor ${\lambda }_0$, the scaling factor $\mathit{v}=2$ (empirical values are generally constants of 2 to 10), and calculate the initial objective function value.

2. Solve to obtain the Jacobi matrix $\mathit{J}$, calculate $({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}$ and solve for $\mathit{\delta }{x}_1$ according to Equation (21)(21) $\mathit{\delta }\mathit{x}=({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}{\mathit{J}}^{\mathrm{T}}\mathit{\delta }\mathit{s}$(21) .

3. Calculate the current objective function value $S_1$, and get the objective function difference value $\mathrm{\Delta S}=S_1-S_0$.

4. If $\mathrm{\Delta S}<0$, make $\mathit{\lambda }=\mathit{\lambda }/\mathit{v}$, get the current position ${\mathit{x}}_1={\mathit{x}}_0+\mathit{\delta }{\mathit{x}}_1$, make ${\mathit{x}}_0={\mathit{x}}_1$, $S_0=S_1$and go to step 2, if $\mathit{\delta }{\mathit{x}}_1<\mathit{\epsilon }$ then stop the iteration

5. If $\mathrm{\Delta S}>0$, then set $\mathit{\lambda }=\mathit{v\lambda }$ and go to step (2).

Figure 2. Flowchart of the typical LM method used for capsule endoscope positioning.

2.3.3. Conventional LM algorithm for capsule endoscope positioning

The algorithm presented in the previous section is widely used in engineering practice. However, the damping coefficient update algorithm needs to be improved to make it more stable in the application of this system when the amount of data observations is not abundant and the data quality is not good. To this end, this paper proposes a trust-region-based LM method for vest-type wearable magnetometer arrays. The model based on the trust region approach is:

(22) $\left\{\begin{array}{c}min\mathit{s}\left(\mathit{\delta }\mathit{x}\right)=\mathit{f}\left(\hat{x}\right)+\mathit{J}\mathit{\delta }\mathit{x}\\ \mathit{s}.\mathrm{t}.\mathit{\delta }\mathit{x}{ }_2\le \mathit{h}\end{array}\right.$(22)

The core idea of the trust region is to first determine the upper limit of the step size, from which a neighborhood $\mathit{\delta }\mathit{x}{ }_2\le \mathit{h}$ of $\mathit{x}$ is defined and the observed magnetic sensor intensity $\mathit{s}(\mathit{\delta }\mathit{x})$ in this neighborhood is assumed to be consistent with the objective function, i.e. the estimated magnetic sensor intensity $\mathit{f}(\mathit{x})$. This method has both fast local convergence and overall convergence. Where $\mathit{h}$ is the radius of the trust region and ${∥\mathit{\hspace{0.17em}}∥}_2$ represents the L2 norm. The Lagrangian function is introduced as:

(23) $\mathit{L}(\mathit{\delta }\mathit{x},\mathit{\lambda })=\mathit{s}(\mathit{\delta }\mathit{x})+\frac{1}{2}\mathit{\lambda }(\mathit{\delta }{\mathit{x}}^T\mathit{\delta }\mathit{x}-h^2)$(23)

Letting ${\mathrm{\nabla }}_{\mathit{\delta }\mathit{x}}\mathit{L}=0$, Equation (21)(21) $\mathit{\delta }\mathit{x}=({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}{\mathit{J}}^{\mathrm{T}}\mathit{\delta }\mathit{s}$(21) can be obtained.

The choice of the radius $\mathit{h}$ of the trust region should be guided by the fact that $\mathit{h}$ should be as large as possible when $\mathit{s}(\mathit{\delta }\mathit{x})$ and $\mathit{f}(\hat{x})$ satisfy a certain consistency, defining the ratio:

(24) $\mathit{R}=\frac{\mathrm{\Delta S}}{||\delta {\mathit{s}}_1||{ }_2-||\mathit{\delta }{\mathit{s}}_0||{ }_2}$(24)

where $\mathrm{\Delta S}$ is the difference between the objective function value obtained from the currently estimated coordinates and that obtained from the previous estimation step, i.e. the actual reduction in the objective function, and $\mathit{\delta }{\mathit{s}}_1{ }_2-\mathit{\delta }{\mathit{s}}_0{ }_2$ is the reduction in the objective function value estimated from Equation (13)(13) $\begin{array}{c}\mathit{s}-\mathit{f}\left(\hat{x}\right)=\mathit{J}\mathit{\delta }\mathit{x}+\mathit{v}\\ \mathit{\delta s}=\mathit{J}\mathit{\delta }\mathit{x}+\mathit{v}\end{array}$(13) . The closer $\mathit{R}$ is to 1, the more similar $\mathit{s}(\mathit{\delta }\mathit{x})$ and $\mathit{f}(\hat{x})$ are, at which point you should decrease $\mathit{\lambda }$ while increasing $\mathit{\lambda }$ as $\mathit{R}$ approaches 0.

Figure 3 shows the flow chart of the proposed LM method, in accordance with this principle, the following improved LM algorithm is proposed in this paper:

1. Initialize the coordinates ${\mathit{x}}_0$, the damping factor ${\lambda }_0$, the scaling factor $\mathit{v}=2$ (empirical value is a constant from 2 to 10), and calculate the initial objective function value $S_0$ and the initial magnetic field strength estimation error $\mathit{\delta }{\mathit{s}}_0$.

2. Solve for the Jacobi matrix $\mathit{J}$, then calculate $({\mathit{J}}^{\mathrm{T}}\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}$ and solve for $\mathit{\delta }{\mathit{x}}_1$ according to Equation (21)(21) $\mathit{\delta }\mathit{x}=({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}{\mathit{J}}^{\mathrm{T}}\mathit{\delta }\mathit{s}$(21) .

3. Calculate the current objective function value $S_1$ and the estimated error in the current magnetic field strength $\mathit{\delta }{\mathit{s}}_1$, and obtain the objective function difference $\mathrm{\Delta S}$ and the estimated reduction in the objective function value $||\mathit{\delta }{\mathit{s}}_1||{ }_2-||\mathit{\delta }{\mathit{s}}_0||{ }_2$.

4. The value of $\mathit{R}$ is obtained from Equation (24)(24) $\mathit{R}=\frac{\mathrm{\Delta S}}{||\delta {\mathit{s}}_1||{ }_2-||\mathit{\delta }{\mathit{s}}_0||{ }_2}$(24) . ${\lambda }_1=\mathit{v}{\lambda }_0$ if $\mathit{R}<0.25$; ${\lambda }_1={\lambda }_0/\mathit{v}$ if $\mathit{R}>0.75$; otherwise ${\lambda }_1={\lambda }_0$.

5. If $\mathrm{\Delta S}<0$ then ${\mathit{x}}_1={\mathit{x}}_0+\mathit{\delta }{\mathit{x}}_1$ so that ${\mathit{x}}_0={\mathit{x}}_1$, $S_0=S_1$ and set $\mathit{i}=0$ go to step (2), otherwise $\mathit{i}=\mathit{i}+1$, go directly to step 2, if $\mathit{i}>10$ then use least squares to get $\mathit{\delta }{\mathit{x}}_1$ and update ${\mathit{x}}_1={\mathit{x}}_0+\mathit{\delta }{\mathit{x}}_1$， ${\mathit{x}}_0={\mathit{x}}_1$， $S_0=S_1$. If $||\mathit{\delta }{\mathit{x}}_1||<\mathit{\epsilon }$ then stop the iteration.

Figure 3. Flow chart of the modified LM method used for capsule endoscope positioning.

3. Experimental verification

3.1. Experimental platform

The capsule endoscope magnetic positioning system consists of two magnetometer arrays that can be worn on the front and back of the body in order to fit the actual scenario. As shown in the diagram, each sensor array of the prototype system consists of 3 × 3 HMC58831 3-axis magnetometers with a sampling rate of 20 Hz and data stored on an SD card. The resolution of HMC58831 is 5 mGs. According to our research, the accuracy of Honeywell HMC58831 is a high level among the current low-power magnetic sensors, and the tests show that the accuracy can meet the requirements of this system.

The object of this experiment is a small cylindrical NdFeB magnet with a diameter of 12 mm and a thickness of 5 mm, which is fixed to an IMU module. The module has a built-in consumer grade MEMS inertial sensor. The module was fixed to a set of three-axis mechanical motion stages with a positioning accuracy of 0.05 mm (as shown in Figure 4), which provided the trajectory reference in the experiments.

Figure 4. The sensor array and experimental platform.

3.2. Experimental platform

3.2.1. Tests with abundant observations

In order to fully compare the localization performance of the two algorithms, the performance of the two LM algorithms was first examined in the presence of a rich set of observations. Equation (1)(1) $\mathrm{B}=B_T\left(\frac{3({\mathit{A}}_0\mathit{P})\mathit{P}}{R^5}-\frac{{\mathit{A}}_0}{R^3}\right)$(1) indicates that when the magnetic moment of the magnet is perpendicular to the sensor array plane, the magnetic sensor array receives the strongest signal strength. Figure 5 shows the number of magnetometers that can observe the magnet target during the experiment in this situation. The average number of magnetometers available for observation is 3.1.

Figure 5. Number of sensors capable of providing effective observation.

Figure 6 gives the positioning results of both algorithms in this case and Figure 7 offers the corresponding error curve. In Figure 6, the red trajectories in the three upper graphs are the trajectories of the magnet motion calculated by the typical LM method for three repeated experiments. The blue trajectories in the three lower graphs are the corresponding trajectories obtained by the modified LM method. The black line represents the ground truth, the green circle in the figure indicates the starting point, the red circle indicates the endpoint, and the black arrow indicates the direction of motion, with the magnet moving at 5 mm/s. From Figure 6, it can be seen that both LM methods have strong repeatability for three tests, and Figure 7 gives the average positioning error for the three repeated tests.

Figure 6. Localization results of two LM methods under observation-abundant condition.

Figure 7. Position errors of two LM methods under observation-abundant condition.

It can be seen from Figure 7 and Table 1 that both methods give accurate positioning results in the presence of abundant observations, the RMS value of the modified LM method is 1 mm smaller than that of the traditional LM method and the accuracy is at the same level. However, the maximum error of the modified LM method is 58.1% smaller than that of the conventional LM method. This result shows that under the condition of sufficient observations, the improved method is similar to the existing method in positioning accuracy, but has higher reliability.

Table 1. Positioning error statistics.

	Typical LM method		Modified LM method
Method	RMS (mm)	Maximum error (mm)	RMS (mm)	Maximum error (mm)
Round 1	6.0	21.5	4.1	7.4
Round 2	6.3	19.8	5.3	9.8
Round 3	6.5	21.6	5.4	9.1
Mean	6.3	21.0	4.9	8.8

3.2.2. Tests with poor observations

Equation (1)(1) $\mathrm{B}=B_T\left(\frac{3({\mathit{A}}_0\mathit{P})\mathit{P}}{R^5}-\frac{{\mathit{A}}_0}{R^3}\right)$(1) shows that the closer the position vector is to the magnetic moment, the greater the magnetic field strength. If the magnetic moment is pointing in a direction parallel to the sensor array then the magnetic strength received by the sensor array will be weak resulting in a lack of abundance of observations. Figure 8 shows the number of magnetometers that were able to observe the magnet target during the experiment under observation-poor conditions. The average number of magnetometers available for observation is 2.5. Only 1 magnetometer can observe the target in the time period 79 s to 86 s.

Figure 8. Number of sensors capable of providing effective observation.

The following figure will show the trajectory when the magnetic moment of the magnet is pointing to the parallelity of the two sensor array plates, Figure 9 shows the results of three localizations of the same trajectory in this case. The experimental trajectory in this section is the same as that in Section 3.2.1. Also, the label and the legend of Figure 9 are consistent with Figure 6. Figure 10 shows the average positioning error for three repetitions of the test. It can be seen that in cases where the magnet is far away from the magnetometer and the observations are not abundant, the traditional LM algorithm can fall into local convergence causing large errors.

Figure 9. Performance of modified LM method under observation-poor condition.

Figure 10. Position errors of two LM methods under observation-poor condition.

It can be seen that the results of both methods are close to the reference values at the initial stage. When the magnets are far away from the sensor array (two periods from 70 s to 94 s and from 132 s to 156 s), the solutions of the typical LM method diverge from the reference, while the results from the modified LM positioning method are still close to the reference.

Table 2 gives the statistical results of the two LM algorithms in the three experiments. Compared to the traditional LM approach, the proposed LM method has reduced the position error RMS value from around 35.6 mm to 6.5 mm, with a performance improvement of 81.7%. Meanwhile, with the proposed method, the maximum position error drops from over 122.9 mm to 16.5 mm, with a performance improvement of 86.6%. These results have shown the effectiveness of the proposed LM method in enhancing positioning when there is a lack of observations.

Table 2. Positioning error statistics.

	Typical LM method		Modified LM method
Method	RMS (mm)	Maximum error (mm)	RMS (mm)	Maximum error (mm)
Round 1	34.7	111.1	6.4	15.0
Round 2	35.5	123.9	6.5	16.3
Round 3	36.6	133.7	6.5	18.2
Mean	35.6	122.9	6.5	16.5

3.3. Discussion

In this paper, the update of the damping coefficients is optimized for our proposed vest-type wearable capsule localization system, which provides better robustness than the traditionally-used LM algorithm. The modified LM algorithm proposed in this paper still yields accurate results even in the harsh case where observations are not abundant.

The experimental results show that when the magnetic moment is oriented in the parallel x-z plane, the signal strength received by the magnetometer array is limited and the accuracy of the typical LM method decreases significantly, while the LM method proposed in this paper still shows good robustness. When the magnetic moment is pointing to the x-z plane, the magnetometer array receives a richer signal at this point, and both methods are at the same level of accuracy.

From Figures 7 and 10, it can be seen that the error of the typical LM method in both situations increases abruptly during the three periods of 75-80s, 95-101s, and 137-140s, especially in the case of poor observations there is no convergence at the end. And these three time periods correspond to the scenarios in Figures 5 and 8 where the observations become fewer. At this time, the $\mathit{\lambda }$ of the typical LM method rapidly becomes larger, leading to an increase in error. And since the modified LM method adds a constraint mechanism based on the trust domain, the LM method is degraded to the least-squares method when the $\mathit{\lambda }$ is accumulated too large, thus avoiding divergence

One possible reason why the conventional LM method performed less well than expected in this paper is that previous studies have used more than 36 magnetometers and have used an annular magnet structure, which picked up a richer signal. In contrast, this paper uses 18 sensors with a limited number of observations, which in some cases makes the damping factor $\mathit{\lambda }$ too large during the iterative process. When $\mathit{\lambda }$ is too large, it invalidates the $\mathit{H}={\mathit{J}}^{\mathrm{T}}\mathit{J}\mathit{+}\mathit{\lambda }\mathit{I}$ matrix in Equation (21)(21) $\mathit{\delta }\mathit{x}=({\mathit{J}}^T\mathit{J}+\mathit{\lambda }\mathit{I}{)}^{-1}{\mathit{J}}^{\mathrm{T}}\mathit{\delta }\mathit{s}$(21) , causing the direction of the iteration to deviate from the globally optimal result (Fan, and Yuan 2014).

4. Conclusions

From a practical point of view, wearable devices need to meet the requirements of low power consumption and ease of wear. Thus, this paper explores a vest-type magnetic positioning system based on two sensor arrays with 18 magnetometers. The current permanent magnet-based capsule endoscope positioning system requires accurate initialization of the magnet attitude; thus, in the case of fewer magnetometers, the magnet position can only be accurately estimated if the magnet attitude is determined. For this purpose, we propose a magnet axial determination method based on the IMU fixed to the magnet. Noticed that the traditional LM algorithm suffers from local convergence under conditions with few magnetometers, an improved LM algorithm is proposed for the magnetometer array positioning system. In tests with harsh conditions of low observation volume and poor observation quality, the modified LM algorithm can achieve an accuracy of 6.5 mm with a maximum error of 18.2 mm, with performance improvements of 81.9% and 86.4%, respectively. The results have shown the effectiveness of the proposed LM method in enhancing positioning when there is a lack of observations. Since this paper focuses on the performance of the improved LM algorithm in the system designed in this paper with poor observations, the trajectories of the experiments are relatively simple. In future experiments, more complex trajectories will be added to verify the effectiveness of the attitude algorithm and the improved LM algorithm under complex conditions.

Disclosure statement

No potential conflict of interest was reported by the author(s).

Data availability statement

The data that support the findings of this study are available from the corresponding author upon reasonable request.

Additional information

Funding

This work was supported in part by the National Natural Science Foundation of China [grant number 42104038].

Notes on contributors

Peng Zhang

Peng Zhang received the PhD degree from the Wuhan University in 2016 and is now an associate researcher at the State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing, Wuhan University. He is mainly engaged in indoor positioning, wearable devices and mobile health research. He presided over and participated in 6 projects including the National Natural Science Foundation of China, the Natural Foundation of Hubei Province, the National Key Research and Development Program, and the Canadian Association for Scientific and Engineering Research Foundation projects. He has co-published over 20 academic papers and has 3 patents pending and received over 4 academic awards.

Ruizhi Chen

Ruizhi Chen received the PhD degree in Geophysics from University of Helsinki, Finland, in 1991. He is currently a Professor and Director of the State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing at Wuhan University. He used to work as an Endowed Chair Professor at Texas A&M University Corpus Christi, US; Head and Professor of the Department of Navigation and Positioning at Finnish Geodetic Institute, Finland, and Engineering Manager in Nokia, Finland. He has published two books and more than 260 scientific papers. His current research interests include indoor positioning, satellite navigation and location-based services.

Weiguo Dong

Weiguo Dong is a professor, archiater and doctoral tutor of the Department of Gastroenterology, Renmin Hospital of Wuhan University. Recently, he is a member of Chinese Society of Gastroenterology, a member of the General Practitioner Branch of Chinese Medical Doctor Association and a Chairman-designate of Digestive Disease Branch of Hubei Medical Association. His research interests include gastrointestinal tumor and inflammatory bowel disease. He has been awarded 6 items for the National Natural Science Fund and 16 scientific research items at the provincial and ministerial levels. Besides, he has won one second prize for scientific and technological progress in Hubei province, two third prize for natural science (all ranked first).

You Li

You Li is a Professor at the State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing (LIESMARS), Wuhan University, China. He received PhD degrees from Wuhan University and University of Calgary in 2015 and 2016, respectively, and a BEng degree from China University of Geoscience (Beijing) in 2009. His research focuses on positioning and motion-tracking techniques and applications. He has co-published over 80 academic papers and has over 20 patents pending. He serves as an Associate Editor for the IEEE Sensors Journal, a committee member at the IAG unmanned navigation system and ISPRS mobile mapping sessions.

Yan Xu

Yan Xu received her Bachelor’s degree in Surveying and Mapping Engineering from Liaoning Technical University in 2015 and her Master’s degree in Photogrammetry and Remote Sensing from Liaoning Technical University in 2018. She is currently studying for a doctorate degree in Geodesy and Surveying Engineering in the State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing in Wuhan University. She is mainly engaged in the research of remote sensing image processing algorithm, medical image interpretation and visual positioning technology of medical application engineering.

Jian Kuang

Jian Kuang received the PhD degree in Geodesy and Survey Engineering from Wuhan University in 2019. He is currently an Associate Researcher with the GNSS Research Center in Wuhan University, China. His research interests focus on inertial navigation, pedestrian navigation and indoor positioning.

Yuan Zhuang

Yuan Zhuang is a professor and the founder of the Sensing, Navigation & Artificial Intelligence Lab (SNAIL) at the State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing, Wuhan University, China. He received the PhD degree in geomatics engineering from the University of Calgary, Canada in 2015. His current research interests include multi-sensors integration, realtime location system, wireless localization, Internet of Things (IoT), and machine learning for navigation applications. To date, he has co-authored over 100 academic papers and over 20 patents and has received over 10 academic awards. He is on the editorial board of Satellite Navigation and IEEE Access, the guest editor of the IEEE Internet of Things Journal, and a reviewer of over 20 IEEE journals.

Rong Yu

Rong Yu received the Bachelor of Medicine in 2019. she is currently pursuing the master’s degree Gastroenterology at Renmin Hospital of Wuhan University. Her research interests include the Basic and clinical research of gastrointestinal tumors and diagnosis and treatment of digestive endoscope.

Mingyue Dong

Mingyue Dong received the BS degree in School of Remote Sensing and Information Engineering from Wuhan University, China, in 2021. She is currently pursuing the PhD degree at State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing (LIESMARS), Wuhan University. Her research interests include 3D reconstruction, indoor and outdoor scene parsing.

Xiaoji Niu

Xiaoji Niu is a Professor of GNSS Research Center at Wuhan University in China. He got his PhD and bachelor degrees (with honor) from the Department of Precision Instruments at Tsinghua University in 2002 and 1997 respectively. He did post-doctoral research at the University of Calgary and worked as a senior scientist in SiRF Technology Inc.He is currently leading the integrated & intelligent navigation group (i2Nav). His research focuses on GNSS/INS integrations, low-cost navigation sensor fusion, and the relevant new applications. He has published 100+ academic papers and own 30+ patents.