Heart motion measurement with three dimensional sonomicrometry and acceleration sensing

Abstract

In robotic assisted beating heart surgery, the goal is to develop a robotic system that can actively cancel heart motion by closely following a point of interest (POI) on the heart surface, a process called Active Relative Motion Canceling (ARMC). In order to track and cancel POI motion precisely, control algorithms require good quality heart motion data. In this paper, a novel method is described which uses a particle filter to estimate the three-dimensional location of POI on heart surface by using measurements obtained from sonomicrometry along with an accelerometer. The new method employs a differential probability approach to increase the accuracy of the particle filter. The performance of the proposed method is evaluated by simulations.

I. INTRODUCTION

In coronary artery bypass graft (CABG) surgery, surgeons require to operate on blood vessels that move very rapidly. This high-bandwidth motion of heart makes it difficult to effectively operate on these arteries by hand [1]. The standard practice is to perform CABG surgery by using heart-lung machine and stopping the heart (on-pump). Offpump (without stopping the heart) is preferable over on-pump CABG surgery because the use of cardio-pulmonary bypass machine can harm patients by hemolysis and cause significant complications that might occur during or after surgery, which includes long-term cognitive loss [2]. On the other hand, off-pump CABG surgery is not effectively applicable to the coronary arteries on the side and the back of the heart, and limited to small number of bypasses due to limitations of passive stabilizers [3].

Robotic-assisted surgery replaces conventional surgical tools with robotic instruments, which are under the direct control of the surgeon through teleoperation. In this system, the surgeon views the surgical site through a camera mounted on a robotic arm that follows the heart motion. The robotic surgical instruments also track the heart motion, canceling the relative motion between the surgical site and the instruments. By this way, it provides surgeon a stabilized view of the surgical site and therefore surgeon operates on the heart as if the heart is stationary [4].

For proper operation of the control algorithms and precise motion tracking, accurate measurement of the POI motion by position tracking sensors is required. Earlier studies in canceling beating heart motion with robotic-assisted tools used vision based and ultrasound based sensor systems to measure heart motion. Nakamura et al. [4] used a 4-DOF master-slave robot along with a high-speed two-camera computer vision system to track heart motion. Ginhoux et al. [5] utilized a high-speed 2D visual servoing scheme to predict heart motion. In another study, Thakor et al. [6] employed a laser range finder system to measure one-dimensional motion of the heart. Even though vision-based systems were preferred in many studies for their excellent accuracy and update rate, their performance significantly degraded by noise and occlusions. Although an algorithm was developed by Ortmaier et al. in [7] to estimate the heart motion, when the view is occluded, it is only applicable to brief occlusions. Therefore, it is desirable to use a sensor system, which is robust to occlusions.

Yuen et al. [8] performed a series of three dimensional ultrasound sonography-guided motion synchronization experiments to measure the mitral annulus position tracking accuracy of a robotic motion compensation system. However. 3D ultrasound sonography is only applicable when working on targets inside the tissue, and cannot be used to measure motions on the surface of the heart, as it is in CABG surgery.

Sonomicrometry is a promising technology for measuring POI motion in this application. A sonomicrometer employs piezoelectric transducers (crystals) and measures the inter-transducer element distances. Sonomicrometer is very accurate and the major source of error is the transducer geometry. Cavusoglu et al. [9] used a sonomicrometry system to collect heart motion data from an adult porcine and showed the feasibility of a robotic system performing off-pump CABG. Although it doesn’t have the occlusion shortcoming of a vision system, sonomicrometer measurements contain noise from ultrasound echoes. Additionally, sonomicrometer is more prone to error in calibration between the sensors and the robotic manipulator coordinate frame.

The primary goal of this study is to accurately estimate 3D POI position on the heart surface. We propose a novel method, which employs particle filter algorithm as a recursive Bayes estimator to clean the noisy sonomicrometer measurements and estimate the 3D POI location using these measurements along with an accelerometer. The presented method uses a differential probability approach to increase the accuracy of the particle filter.

The rest of this paper is organized as follows: The proposed method is explained in Section II. Simulations and results are given in Section III. Finally, the discussion and conclusions are presented in Sections IV and Section V respectively.

II. METHOD

In this study, we propose a method to accurately estimate 3D POI position on heart surface from noisy sonomicrometer and accelerometer measurements. In the proposed scheme, one crystal of the sonomicrometry system would be sutured on the heart (POI location) and four or more other crystals would be mounted on a rigid base forming a reference coordinate frame. The distances between each base crystal and the moving crystal would be measured. These measurements would be used to estimate the 3D position information of the crystal attached next to the POI relative to the reference frame [10]. A three-axis solid state micro-accelerometer would be mounted on the sonomicrometery crystal, which is sutured at the POI (target crystal).

The sonomicrometric sensor is susceptible to a peculiar form of error caused by the obstruction of ultrasound transmission paths and echo effects. These effects result in a non-Gaussian noise component to appear as instantaneous jumps in the 3D coordinates calculated using the sonomicrometer system. In the proposed scheme, the inertial sensor would be employed in combination with the sonomicrometer to eliminate these effects. Fig. 1 depicts the configuration of the various components and associated coordinate frame involved.

Fig. 1.

The proposed scheme: One crystal of the sonomicrometry system is sutured on the heart, four or more other crystals are mounted on a rigid base forming a reference coordinate frame. A three-axis solid state micro-accelerometer is mounted on the target sonomicrometry crystal.

The particle filter algorithm is employed to estimate the three-dimensional POI position coordinates from the noisy sonomicrometer and accelerometer measurements. Finally, a differential probability approach is utilized to increase the accuracy of the particle filter. In this section, these four elements, which comprise the developed method are explained.

A. Sonomicrometer

A sonomicrometer (Sonometrics Inc., Ontario, Canada) accurately measures the distances within the moving soft tissue via ultrasound signals by a set of small piezoelectric crystals. For this study, these crystals are attached to the heart as described above while it is still beating and they are used to transmit and receive short pulses of ultrasound signal. The distance between the transmitting and receiving crystals are computed by measuring the time of flight of the sound wave. The 3D configuration of all the crystals can be calculated from these distance data [11].

B. Accelerometer

In the proposed scheme, the estimation of POI location from sonomicrometer measurements can be improved by using an accelerometer, which is mounted on the target crystal and measures the acceleration of the target crystal.

The output of the accelerometer is the summation of the true acceleration of target crystal and gravitational acceleration since gravity of Earth also acts on the sensing axis of the accelerometer. This leads to a significant challenge, as the orientation of the accelerometer is not known and it doesn’t change during the motion. The effect of the gravitational component on the accelerometer output at time t is given by:

$$R_{\mathrm{wa}}\left[\begin{array}{c}\hfill s_{x,t}\hfill \\ \hfill s_{y,t}\hfill \\ \hfill s_{z,t}\hfill \end{array}\right]=R_{\mathrm{ws}}\left[\begin{array}{c}\hfill a_{x,t}\hfill \\ \hfill a_{y,t}\hfill \\ \hfill a_{z,t}\hfill \end{array}\right]+\left[\begin{array}{c}\hfill 0\hfill \\ \hfill 0\hfill \\ \hfill -g\hfill \end{array}\right],$$ (1)

where g is the gravitational acceleration, and a_t = [a_x,t, a_y,t, a_z,t]^T and s_t = [s_x,t, s_y,t, s_z,t]^T are respectively the true acceleration of target crystal and accelerometer output. Note that acceleration “a_t” of the crystal is expressed in the sonomicrometer frame (S), and the gravity vector [0, 0, − g]^T is in the world coordinate frame (W), and the acceleration sensor output “s_t” is in the body frame (A). R_ws is the rotation matrix from sonomicrometer to world coordinates and is assumed to be known and constant. Finally, R_wa is the rotation matrix from accelerometer to world coordinate frame, which is unknown and time varying.

By rearranging (1), the true acceleration of the target crystal can be represented as follows:

$$\left[\begin{array}{c}\hfill a_{x,t}\hfill \\ \hfill a_{y,t}\hfill \\ \hfill a_{z,t}\hfill \end{array}\right]=R_{\mathrm{sw}}\left(\left[\begin{array}{c}\hfill r_{x,t}\hfill \\ \hfill r_{y,t}\hfill \\ \hfill r_{z,t}\hfill \end{array}\right]-\left[\begin{array}{c}\hfill 0\hfill \\ \hfill 0\hfill \\ \hfill -g\hfill \end{array}\right]\right),$$ (2)

where r_t = [r_x,t, r_y,t, r_z,t]^T is the output of accelerometer in world frame, as given by:

$$\left[\begin{array}{c}\hfill r_{x,t}\hfill \\ \hfill r_{y,t}\hfill \\ \hfill r_{z,t}\hfill \end{array}\right]=R_{\mathrm{wa}}\left[\begin{array}{c}\hfill s_{x,t}\hfill \\ \hfill s_{y,t}\hfill \\ \hfill s_{z,t}\hfill \end{array}\right].$$ (3)

The right hand side (RHS) of (3) can be represented by multiplication of a unit vector and a scalar:

$$R_{\mathrm{wa}}\left[\begin{array}{c}\hfill s_{x,t}\hfill \\ \hfill s_{y,t}\hfill \\ \hfill s_{z,t}\hfill \end{array}\right]=|S|\left[\begin{array}{c}{u}_{x,t}\\ u_{y,t}\\ -\sqrt{1-u_{x,t}^2-u_{y,t}^2}\end{array}\right],$$ (4)

$$|S|\text{=}\sqrt{s_{x,t}^2+s_{y,t}^2+s_{z,t}^2}$$ (5)

The new representation in (4) and (5) decreases the computational load of computing a_t by avoiding the matrix multiplication of R_wa and R_sw. As the gravitational acceleration would be much larger than acceleration of the POI, the alternate sign version of the square root in (4) would not cause any ambiguity.

Since heart is beating, the POI, the crystal attached to POI, and the accelerometer move with time. This causes R_wa to be time-varying which prevents the straightforward computation of a_t by using Equation 2. Therefore, in order to utilize acceleration information in estimating POI location, R_wa, or more specifically, r_t = R_was_t needs to be estimated. In order to improve the robustness of the estimation, instead of estimation the orientation of the accelerometer, R_wa, here we will instead estimate the unit direction vector $u_t={[u_{x,t},u_{y,t},-\sqrt{1-u_{x,t}^2-u_{y,t}^2}]}^T.$.

C. Particle Filter

The idea of estimating POI location on the heart surface from sonomicrometer and accelerometer measurements is the core of this study. By using state estimation, the POI location (state of the system) can be recovered from gathered sensor data. However as mentioned in Section II, sonomicrometer measurements are corrupted by noise. So for precise POI position estimation, the noise in the sonomicrometer has to be characterized and filtered.

One approach is to use a Bayes estimator, which recursively estimates the current state of the system from the state of the system at the previous time step. Modeling the sonomicrometer measurement error by a Gaussian noise distribution is a simple and direct way to model system noise. Since the underlying dynamics of the system is nonlinear, Bayes estimator can be easily implemented by an Extended Kalman Filter (EKF) when the system noise assumed to be Gaussian.

Although the Gaussian noise distribution will be sufficient enough to model system noise for regular heart motion, a more detailed noise model is needed to capture the uncertainties when the heart statistics are likely to change as in heart rate variations and arrhythmias. Particle filter is a Monte-Carlo implementation of iterative Bayes’ filter used for state estimation of non-Gaussian non-linear state space models [12]. A major advantage of a particle filter over an EKF is that a particle filter can incorporate non-Gaussian noise models. Additionally, if the posterior distribution of the state is represented by sufficiently large number of particles, particle filter approaches to the optimal Bayesian estimate, which makes it more accurate than EKF.

In this study, two different process models will be considered for the cases with and without accelerometer measurements.

When there is no accelerometer measurement available, the system dynamics are then modeled as

$$p_{t+1}=p_t+v_t\Delta +\frac{1}{2}{a}_t{\Delta }^2+{∊}_p$$ (6)

$$v_{t+1}=v_t+a_t\Delta +{∊}_v$$ (7)

$$a_{t+1}=a_t+{∊}_a$$ (8)

where $p_t=(p_{x_t},p_{y_t},p_{z_t})$ and $v_t=(v_{x_t},v_{y_t},v_{z_t})$ are, respectively, the three-dimensional position and velocity of POI at time t, a_t is the POI acceleration as defined in (2), Δ is the time step, and ε(·) are zero mean Gaussian noise terms. Then the system state at time t is specified by

$$q_t={[p_t,v_t,a_t]}^T.$$ (9)

When accelerometer measurements are available, the process model changes to

$$p_{t+1}=p_t+v_t\Delta +\frac{1}{2}{a}_t{\Delta }^2+{∊}_p$$ (10)

$$v_{t+1}=v_t+a_t\Delta +{∊}_v$$ (11)

$$v_{x,t+1}=v_{x,t}+{∊}_{\mathrm{ux}}$$ (12)

$$v_{y,t+1}=v_{y,t}+{∊}_{\mathrm{ux}}$$ (13)

where a_t is no longer a state, but, an input to the system as calculated by (2)–(5). Then, the states of the system become

$$q_t={[p_t,v_t,u_{x,t},u_{y,t}]}^T,$$ (14)

where, (u_x,t, u_y,t) are from Equation 4.

D. Measurement Model: Differential Probability Method

At this point, the only measurement that can be used in measurement updates of the Bayes filter is the position measurement obtained from the sonomicrometer¹. Assuming zero mean Gaussian noise in the sonomicrometer measurements, the probability density function of the measurement model would be of the form:

$$\begin{array}{l}P(p_{\text{POI},t}|p_{\text{particle},t})=P_{p,t}=\\ \frac{1}{\sqrt{2\pi {\sigma }_p^2}}\exp (-\frac{1}{2}{\left(\frac{p_{\text{POI},t}-p_{\text{Particle},t}}{{\sigma }_p}\right)}^2)\end{array}$$ (15)

where, p_POI,_t is the measured position of the POI (as provided by sonomicrometer), p_particle,_t is the position of the particle, and ${\sigma }_p^2$ is the covariance of the Gaussian.

Recall that, the equation of motion given by the process model for the POI position is of the form:

$$p_{t+1}=p_t+v_t\Delta +\frac{1}{2}{a}_t{\Delta }^2.$$ (16)

for small time steps Δ, the changes in the estimated position of the POI is not very sensitive to the values of the velocity and acceleration estimates, which are multiplied by Δ and Δ², respectively. As a result, the probability density function (15), which is used as the weighting function for measurement updates, is not very sensitive to velocity and acceleration errors. This exhibits itself as delays in acceleration and velocity corrections, as the influence of these errors need to accumulate over multiple time steps, leading to poor performance.

In order to remedy this problem, we propose the “differential probability” method, which modifies the probability density function of the measurement model to be more sensitive to velocity and acceleration errors, as described below.

We start by defining the following derived velocity and acceleration variables:

$$v_{\text{POI,}t}=\frac{v_{\text{POI,}t}-p_{\text{belief,}t-1}}{\Delta },$$ (17)

$$a_{\text{POI,}t}=\frac{v_{\text{POI,}t}-v_{\text{belife,}t-1}}{\Delta },$$ (18)

where p_belief,_t−1 and v_belief,_t−1 are respectively the expected values of the position and velocity beliefs at time t − 1, as given by the particle distributions. It is then possible to define the new probability density functions over these new derived variables as:

$$P_{v,t}=\frac{1}{\sqrt{2\text{π}{\sigma }_v^2}}\exp \left(-\frac{1}{2}{\left(\frac{v_{\text{POI,}t-}{v}_{\text{particle,}t}}{{\sigma }_v}\right)}^2\right)$$ (19)

$$P_{a,t}=\frac{1}{\sqrt{2\text{π}{\sigma }_a^2}}\exp \left(-\frac{1}{2}{\left(\frac{a_{\text{POI,}t}-a_{\text{particle,}t}}{{\sigma }_a}\right)}^2\right).$$ (20)

Consequently, alternate forms for the probability density function for the measurement model P(p_POI,t|p_particle,t) can be defined as:

$$P_{p,t}{P}_{v,t},$$ (21)

$$P_{p,t}{P}_{a,t},$$ (22)

$$P_{p,t}{P}_{v,t}{P}_{a,t},$$ (23)

in addition to its original form

$$P_{p,t}.$$ (24)

The advantage of these alternate forms are that they are more sensitive to errors in velocity and acceleration estimates.

If they are used by themselves, the covariances for P_v,t and P_a,t would, respectively, be

$${\sigma }_v^2={\left(\sqrt{{\sigma }_p^2+{\sigma }_{p,\text{belife}}^2}/\Delta \right)}^2,$$ (25)

$${\sigma }_a^2={\left(\sqrt{{\sigma }_p^2+{\sigma }_{p,\text{belife}}^2}/{\Delta }^2\right)}^2.$$ (26)

When these probability density functions are used in combination, with P_p,t, and with each other, it is necessary to perform a correction to the covariance values. Specifically, when two of the P_p,t, P_v,t and P_a,t are used in combination, the covariance terms in (15), (19), and (20) would need to be modified as

$${\sigma }_{\text{corrected,(}\cdot \text{)}}=\sqrt{2}{\sigma }_{(\cdot )}$$ (27)

and when all three of the P_p,t, P_v,t and P_a,t are used in combination, the covariance terms would need to be modified as

$${\sigma }_{\text{corrected,(}\cdot \text{)}}=\sqrt{3}{\sigma }_{\text{(}\cdot \text{)}}.$$ (28)

These corrections are required due to the fact that the new quantities v_POI,_t and a_POI,_t are not actually independent from each other or p_POI,_t, as they are quantities derived from p_POI,_t.

III. SIMULATIONS

A. Accuracy

The performance of the proposed method is evaluated by simulations. Two distinct set of simulations are performed. First set employs only sonomicrometer measurements in estimating POI position whereas the second set includes measurements gathered by accelerometer as well as sonomicrometer. In each set, accuracy of the estimations of four different methods are tested. Each method employs respectively P_p,t, P_p,tP_v,t, P_p,tP_a,t, P_p,tP_v,tP_a,t as its error distribution model.

16 seconds long artificial data was generated for the simulations, in a way to resemble the actual heart motion data collected in in vivo studies [10], [13]. Fig. 2 shows the generated artificial data superimposed on raw data and Fig. 3 shows the Fourier analysis the artificial data.

Fig. 2.

Artificial heart motion data generated for simulations superimposed on raw data collected by sonomicrometer

Fig. 3.

Fourier spectrum of artificial heart motion data. Tall, narrow peaks indicate that it resembles the quasiperiodic motion of the heart. First peak at 0.3 Hz corresponds to breathing motion. Peaks at 2 Hz and 4 Hz correspond respectively to the fundamental component and first harmonic of the heartbeat motion.

The motion of the POI on the heart is primarily the superposition of two effects: motion due to the heart beating and motion due to breathing. Each of these signals closely resemble periodic signals.

In this manner, artificial heart motion is generated by using the principal component of breathing motion and the principle component and first harmonic of the heartbeat motion with a constant offset. This approximation for a single axis is given as:

$$\begin{array}{l}{m}_t=c_0+k_{b_1}\sin (w_{b_1}t-{\theta }_{b_1})+k_{h_1}\sin (w_{h_1}t-{\theta }_{h_1})\\ +k_{h_2}\sin (w_{h_2}t-{\theta }_{h_2}),\end{array}$$ (29)

where $w_{b_1}=2\text{π}{f}_{b_1}$, $w_{h_1}=2\text{π}{f}_{h_1}$ and $w_{h_2}=2\text{π}{f}_{h_2}$. $f_{b_1}=0.3\text{Hz}$ is the fundamental frequency of breathing motion, $f_{h_1}=1.95\text{Hz}$ and $f_{h_2}=3.90\text{Hz}$ are respectively frequencies of the fundamental component and first harmonic of the heartbeat motion. Table I shows the parameters $c_0,k_{b_1},k_{h_1},k_{h_2},{\theta }_{b_1},{\theta }_{h_1},{\theta }_{h_2}$ used to generate motion in each x/y/z axes.

TABLE I.

Values of the parameters used in Equation 29 generating artificial heart motion data for each principle axes.

	Parameters
Axis	c0
x	−10.4	2.2	−1.6	1.5
y	−75.3	−3.4	2.4	1.8
z	65	3.2	−1.7	2.2	−π

The generated data is corrupted by white Gaussian noise with σ_s = 0.315, approximately equal to the absolute accuracy of the Sonomicrometry system, which is 250 μm ≈ 1/4 wavelength of the ultrasound [14].

For the set of simulations, in which accelerometer was employed, the simulated accelerometer output was computed as

$$s_t=R_{\mathrm{wa}}^T\left(R_{\mathrm{ws}}{a}_t+{[\begin{array}{ccc}0& 0& -g\end{array}]}^T\right)$$ (30)

and then corrupting it by a white noise with σ_a = 85. Here, a_t was computed by taking the second derivative of (29). The orientation of the accelerometer R_wa was simulated to change during the trajectory as R_wa = R_x(θ_x,t)R_y(θ_y,t), with

$$\begin{array}{c}{\theta }_{x,t}=\frac{\text{π}}{180}\left(10+10\sin \frac{\text{π}}{180}\frac{t}{250}\right)\\ {\theta }_{y,t}=\frac{\text{π}}{180}\left(10+5\sin \frac{\text{π}}{180}\frac{t}{250}\right).\end{array}$$ (31)

The σ_a value was chosen to mimic the noise characteristics of the acceleration sensor “MMA7260QT” (Freescale Semiconductor, Inc. TX, USA).

The results of the simulation studies are given in Table II. Simulations were run 10 times for each model and the average results are reported in the table. 3000 particles were used in the particle filter. The root-mean-square (RMS) errors (both for individual x/y/z axes and for the 3 dimensional error) are calculated from the difference between the generated artificial motion of the POI and POI positions estimated by the employed models. The error values are reported as a percentage of the reference value, i.e., for the case without any acceleration sensor and the error probability model with P_p,t only. The estimated position and velocity of POI for x/z axes are shown in Fig. 4. The result of position show our method decrease error.

TABLE II.

The estimation errors and computation times (in parentheses) for each of the algorithms. The error values are reported as a percentage of the reference value, namely, the case without any acceleration sensor and the error probability model with P_p,t only.

RMS Error (as % reference) (Estimation Time for 10 sec of data (in sec))		Acceleration Sensor
		Used		Not Used
		3D	x/y/z	3D	x/y/z
Error distribution model used	Pp,tPv,tPa,t	72.8% (173s)	83%	85.2% (158s)	87%
			80%		83%
			55%		86%
	Pp,tPv,t	74.2% (163s)	84%	80.3% (154s)	81%
			81%		78%
			55%		81%
	Pp,tPa,t	74.2% (168s)	84%	85.6% (152s)	87%
			81%		84%
			57%		86%
	Pp,t	77.5% (155s)	88%	–(147s)	–
			85%		–
			59%		–

Fig. 4.

Position and velocity estimates for x and z axes. Estimations with acceleration sensor employs $P_{p_t}{P}_{v_t}{P}_{a_t}$ error model. Estimations without acceleration sensor corresponds to the Reference method which uses only $P_{p_t}$ error model. Actual motion and velocity data are computed respectively by Equation 29 and taking its first derivative.

B. Effect of number of particles

In the simulations reported in Section III-A, the number of particles used in the particle filters was set to 3000. As the computational complexity of the particle filtering algorithms depend O(n) in the number of particles used, analyzing the effect of the number of particles used on the estimation accuracy is important. In this section, simulations are performed only for the setup with accelerometer, as this is the primary focus of the paper. The same artificial motion and acceleration sensor data is used. As in the previous section four methods, P_p,t, P_p,tP_v,t, P_p,tP_a,t, P_p,tP_v,tP_a,t, are compared. The reported results are average values obtained from 5 repetitions of the simulations.

Fig. 5 presents how the RMS error varies with the number of particles used in estimation, and, Table III reports the execution time of each method. In this table, reference method corresponds to the case, in which accelerometer measurements weren’t used and P_p,t was employed as the error distribution model. Reference method is considered as baseline while comparing the speed of estimation algorithms.

Fig. 5.

Variation of the RMS error with the number of particles used in estimation

TABLE III.

Comparison of executions times of each of the methods that employ accelerometer data. The algorithm with the error probability model P_p,t AND WITHOUT ANY ACCELERATION SENSOR MEASUREMENTS IS USED AS THE REFERENCE.

Method	Estimation time for 10s data per particle (in msec)	Computation time as a % of the reference
Reference	4.89	100
Pp,t	5.18	94
Pp,tPv,t	5.41	90
Pp,tPa,t	5.58	88
Pp,tPv,tPa,t	5.78	85

From Table III, it can be observed that the speed of differential probability method is slower than other methods. However, the speed of differential probability method can be increased by reducing particle number used.

Fig. 5 shows that the method which uses P_p,tP_v,tP_a,t as the error distribution model yields the best estimation results, and exhibits little degradation of performance with as little as 500 particles. This algorithm reduces the RMS error as much as 27% when compared with the reference method.

IV. DISCUSSION

A. Estimation Accuracy

Results presented in Table II indicates that if accelerometer is employed, accuracy of POI position estimates improves considerably. This improvement is compounded with the use of the proposed “differential probability methods,” i.e., by employing the P_v,t and P_at terms in the error distribution model, with marginal computational cost. P_v,t term in the model improves the accuracy for both sets of simulations. However, incorporating P_a,t in the error model doesn’t has significant effect on the accuracy of estimations as much as P_v,t when the accelerometer is not used. It should be noted that, for the simulation set with accelerometer, the accuracy of the methods, P_p,tP_v,t and P_p,tP_a,t are equal as the effects of velocity and acceleration appear to be matched.

One crucial observation is that P_p,tP_v,tP_a,t method can still accurately estimate POI position when the number of particles are reduced to 500 when accelerometer is used. (See Fig. 5). However, P_p,t method requires at least 3000 particle to decrease RMS estimation error considerably form the Reference method. Therefore, the proposed method significantly improves the efficacy of particle filter.

B. Accuracy of Individual Axes

When the accelerometer is used, accuracy of estimations in z-axis was higher than x/y axes. This is simply because of the parametrization used for calculating true acceleration of target crystal (See Equations 4 and 5). According to the parametrization in (4), acceleration errors in x/y axes are respectively ε_x and ε_y. The error in z-axis is given by,

$${∊}_z=\sqrt{1-u_{x,t}^2-u_{y,t}^2}-\sqrt{1-{(u_{x,t}+{∊}_x)}^2-{(u_{y,t}+{∊}_y)}^2}$$ (32)

If both u_x,t and u_y,t are smaller than 0.4, then by (32) ε_z is always less than ε_x and ε_y. Since the acceleration of POI motion is always less than gravitational acceleration, g, u_x,t and u_y,t are never larger than 0.4. Therefore accuracy of the estimations in z-axis is better than x/y axes which can be also observed from the results presented in Fig. 4.

C. Computation Time

In the simulations, POI estimations are performed for 10 s of data. The method P_p,tP_v,tP_a,t, when used with 500 particles, yield the best compromise between the computation time and accuracy. For the specific implementation and the computer system used, this case resulted in a computation time of 28.8 s which is approximately 3 times slower than the real time. The necessary speedup can potentially be achieved with parallel processing, as particle filtering algorithm can be trivially parallelized. Even further speedups can potentially be achieved by a multi-threaded implementation.

V. CONCLUSION

In this study, a novel motion estimation method for heart motion using sonomicrometry and acceleration sensing is presented. The proposed algorithm employs particle filters as a recursive Bayes estimator to clean the noisy sonomicrometer measurements and estimate the 3D POI location on the heart. The proposed method is shown to reduce the estimation accuracy by 27% in simulation studies, compared to the case without any acceleration sensing when particle number is 500.

Future work includes the hardware validation of the proposed scheme. Parallel processing will be also incorporated to improve the computational performance of the proposed method.