6DOF Wireless Tracking Wand Using MARG and Vision Sensor Fusion

Abstract

We present a low cost battery-powered 6-degree-of-freedom wireless wand for 3D modeling in free space by tri-axis Magnetic, Angular Rate, Gravity (MARG) and vision sensor fusion. Our approach has two stages of sensor fusion, each with different algorithms for finding 3D orientation and position. The first stage fusion algorithm, a complementary filter, utilizes MARG sensors to compute 3D orientation relative to the direction of gravity and earth's magnetic field in a quaternion format, which was adjusted with compensations for magnetic distortion. The second stage fusion algorithm, a Kalman filter, utilizes accelerometer data and IR marker velocity to compute 3D position. In order to compute the IR marker linear velocity along the optical axis (the z-axis), we present a simple and efficient image-based technique to find the distance of the object from the camera using blob area pixels in the image. Our fusion (inside-in and outside-in) approach efficiently solves short time occlusion, needs of frequent calibration, and unbounded drift problems involved in numerical integration of inertial sensors data and improves the degrees of freedom at low cost without compromising accuracy. The results are compared with a leading commercial magnetic motion tracking system to demonstrate the performance of the wand.

1. Introduction

There has been increasing research over the last decade in using 6DOF motion tracking devices for 3D spatial sketching and modeling [1–4] in immersive virtual reality (VR) environments. Other efforts include 2D tablet screens used to draw a 2D sketch that is processed into 3D designs [5, 6]. The objective of practical spatial drawing and editing in 3D demands low-cost, precision, small size, and ease of use. Existing professional motion tracking systems that use electromagnetic, ultrasonic, optical, inertial, and multiple-sensor technologies [7] are too expensive for commercial 3D immersive VR and modeling and require a degree of technical knowledge to use them. Outside-in stereo vision has been widely used for 3D modeling, but this system often suffers from occlusion and interference and apparent loss of DOF. Any accidental change in the position of a camera after calibration requires complete recalibration [8].

3D motion-based human computer interaction (HCI) has long been an active research topic in VR, and it has been shown that 3D interfaces can be useful in many consumer-level applications such as home gaming [9] and 3D user input [10–12]. Emerging demands for rich interaction have led to the development of handheld pointing motion interface devices [13, 14]. These commercial devices incorporate micro-electro-mechanical system (MEMS) inertial sensors such as accelerometers and gyroscopes, and their contributions are limited to gesture recognition, rotation, and vision sensing as for 3D position. These devices are aimed to interact with 3D digital media content and motion gaming and are unsuitable for 3D modeling and editing in free space, which requires precise 6DOF motion sensing.

The main technological bottleneck that limits accuracy in computing position and orientation from MEMS inertial sensors is the drift caused by numerical integration of acceleration and angular rate [15–17]. However, inertial sensors are well known for their short term precision, high-frequency data rates, and size. To leverage these advantages, benefits of sensor fusion techniques using additional sensors have been proposed by researchers in the areas of navigation [18, 19] and motion capture [20].

Another recent innovative work, called MEMSEye, uses MEMS-mirror-based optical 3D tracking [21]. Combination of two or more MEMSEye units can track light sources such as IR light-emitting diodes (LED) and corner cube retroreflectors (CCRs). By triangulating the tracked object's relative position from each unit, its 3D position can be computed in relatively large volumes with submillimeter precision at update rates of >20 kHz. However, a fully functional unit using this technology platform costs more than one thousand dollars, making it unaffordable for a wide range of users.

With increasing interest in 3D display devices, a simple and low-cost solution that can provide enough precision and flexibility is not yet available for 3D modeling. Our work aims to advance 6DOF motion sensing by two-stage multisensor (MARG and vision) fusion to make use of their complementary properties, which was inspired by multisensor navigation systems. So it is essential to acquire accurate timestamp information to synchronize MARG measurements and the camera. In order to do that, we use a hardware triggerable IR camera.

This paper is organized as follows. Section 2 gives the description of the proposed system design and the details of the wireless wand architecture. Section 3 explains first stage sensor fusion for 3D orientation using MARG sensors and the magnetic distortion correction technique. In order to find the linear velocity of the IR marker along the camera z-axis, we explain our image-based technique in Section 4, followed by a description of the second stage sensor fusion, and performance comparison results with a leading commercial motion tracking system in Sections 5 and 6, respectively. Section 7 concludes our approach. Throughout the paper, a notation system of leading superscripts and subscripts similar to [22] is used to denote the relative frame of orientations and vectors. For example, ${}_b{}^e{q}_t$ describes the quaternion of sensor body frame b relative to the earth frame e at time t, and ${{}^{e}A}_{\text{est}}$ is an estimated vector described in frame e. The ⊗ operator denotes a quaternion product.

2. System Design

2.1. System Overview

The wireless wand is designed to be used in front of a computer monitor with a camera attached on top of it as shown in Figure 1(a). We use a wide field of view camera which has an IR filter and $640\times 480$ resolution to capture the spherical IR marker located at the tip of the wand at 50, 75, and 100 frames per second (FPS). The wand is equipped with triaxis MARG sensors, temperature sensor, and a microcontroller communicates with them to gather a full set of sensor data, from which it computes 3D orientation that is sent in data packets to the computer via wireless Bluetooth link. Data packets contain the first stage sensor fusion output, 3D orientation in quaternion format, raw and calibrated individual sensor data, and buttons and LEDs status information as shown in Figure 2(a). A server program running in the computer communicates with both the wand and the camera as shown in Figure 2(b). In server program, an image-based blob tracking module uses thresholding technique for camera images to find the location and area of the IR marker in image in terms of pixels. The data preprocessing module uses our image-based technique which is explained in Section 4 for finding IR marker velocity along the optical axis (z-axis) of the camera using the area of the marker from the blob tracking result. Also, a technique to calculate gravity vector from the quaternion is incorporated into preprocessing module as MEMS triaxis accelerometer senses gravity plus translational acceleration of wand. The sensor fusion algorithm in Figure 2(b) has a 9-state Kalman Filter (KF) algorithm to compute 3D position. The inputs to the KF are triaxis translational acceleration and velocity vectors from preprocessing module. The server program (API updater) implemented in C++ serves 3D position, orientation, and buttons and LEDs statuses to any application for 3D interaction and modeling. The following section presents the architecture of the wand in detail.

Figure 1

System setup to work with wireless wand; (a) working volume of wand in front of camera and different frame of references involved; (b) wand.

Figure 2

Data packet and API: (a) data packet format sent from wand; (b) API to communicate and process data from the wand and camera.

2.2. Wand Architecture

The wand shown in Figure 1(b) has a triaxis digital 16-bit gyroscope, 12-bit accelerometer, and 12-bit magnetometer, each with its own respective selectable ranges of up to ±2000°/s, ±8 g, and $\pm 8.1$  G. Also, a digital 16-bit thermometer is incorporated into the wand for adaptive compensation of time varying temperature biases in the MEMS sensors. All the sensors, buttons, status LEDs, and Bluetooth transceivers are connected to a microcontroller for collecting, controlling, and processing data as shown in Figure 3. Wand's firmware incorporates MARG sensors calibration routines and data for computing 3D orientation in order to compensate for sensors biases.

Figure 3

Wand architecture.

3. MARG Sensor Fusion for 3D Orientation

To compute drift-free measurement of 3D orientation relative to the direction of gravity and earth's magnetic field, researchers proposed several algorithms using MARG sensors [23–26], also known as an attitude heading reference system (AHRS). A complementary filter using low-cost MEMS inertial measurement unit (IMU) with magnetometer was proposed [23] with deep mathematical basis to compute 3D orientation in a direction cosine matrix (DCM) and quaternion form [24]. Though this algorithm showed how a magnetometer can be used along with an IMU (gyroscope and accelerometer) to compute 3D orientation relative to the earth direction of gravity and magnetic field, it was not able to correct drift as was intended due to lack of a compensation technique for magnetic distortions resulting from nearby sources such as metal structures or power supply buses. Several investigations [25, 27] have shown that substantial errors may be introduced by magnetic distortions in orientation estimated from MARG sensors. By adapting a technique proposed in [22] for compensating for magnetic distortions (termed soft iron errors), a complementary filter algorithm in quaternion form has been implemented on a low-power hardware board in the wand system.

The potential advantage of this algorithm is in correcting drift in orientation computed from gyroscope measurements using an additional reference orientation computed from the accelerometer and magnetometer, by successfully incorporating magnetic distortion compensation without any singularity problems. One more key advantage of this technique is that it eliminates the need for the direction of earth's magnetic field to be predefined, which has been a potential disadvantage of other algorithms [25, 26]. The block diagram shown in Figure 4 represents the first stage sensor fusion AHRS algorithm, in which red color box indicates the magnetic distortion compensation technique. More details of the algorithm are given in the Appendix. The compensation technique is described as follows:

$$\begin{array}{cc}& {}^b{M}_t=\left[\begin{array}{cccc}0& m_x& m_y& m_z\end{array}\right],\hspace{1em}\hspace{1em}{}_e{}^b{q}_t=\left[\begin{array}{cccc}{q}_1& q_2& q_3& q_4\end{array}\right],\end{array}$$

(1)

$$\begin{array}{cc}& {}^e{H}_t=\left[\begin{array}{cccc}0& h_x& h_y& h_z\end{array}\right]={}_e{}^b{q}_{t-1}\otimes {}^b{M}_t\otimes {}_e{}^b{q}_{t-1}^{*},\end{array}$$

(2)

$$\begin{array}{cc}& {}^e{B}_t=\left[\begin{array}{cccc}0& \sqrt{h_x^2+h_y^2}& 0& h_z\end{array}\right],\end{array}$$

(3)

where ${}^b{M}_t$ is the normalized magnetic field vector, computed from the output of magnetometer of the wand body frame at time t. ${}_e{}^b{q}_{t-1}$ and ${}_e{}^b{q}_{t-1}^{*}$ are the normalized quaternion output of the algorithm and its conjugate at previous time step $t-1$ , respectively. In (2), the direction of earth's magnetic field ${}^e{H}_t$ is computed, which may represent erroneous inclination and can be corrected if algorithm's reference direction of earth's magnetic field ${}^e{B}_t$ is of the same inclination. Equation (3) ensures that any magnetic disturbances are limited to only affect the estimated heading component of orientation.

Figure 4

Complementary filter block diagram.

4. IR Marker Velocity Tracking

In order to compensate for the numerical drift that results from double integration of acceleration measurements of triaxis accelerometer, we compute 3D velocity of the IR marker from the output of a blob tracking module, which provides the width, height, area, and 2D position $({}^c{p}_x,{}^c{p}_y)$ of the IR marker in pixels after calibrating the image for distortion correction. Now, finding velocities ${}^c{v}_x$ and ${}^c{v}_y$ using position is straightforward in x- and y-axis directions of the camera frame of reference $O_c$ , as follows:

$$\begin{array}{cc}& {}^c{v}_x=\frac{{}^c{p}_{x\hspace{0.17em}|\hspace{0.17em}t-1}-{}^c{p}_{x\hspace{0.17em}|\hspace{0.17em}t}}{\mathrm{\Delta }t},\hspace{1em}\hspace{1em}{}^c{v}_y=\frac{{}^c{p}_{y\hspace{0.17em}|\hspace{0.17em}t-1}-{}^c{p}_{y\hspace{0.17em}|\hspace{0.17em}t}}{\mathrm{\Delta }t},\end{array}$$

(4)

where $({}^c{p}_{x\hspace{0.17em}|\hspace{0.17em}t-1},{}^c{p}_{y\hspace{0.17em}|\hspace{0.17em}t-1})$ and $({}^c{p}_{x\hspace{0.17em}|\hspace{0.17em}t},{}^c{p}_{y\hspace{0.17em}|\hspace{0.17em}t})$ are the 2D positions in camera frame of reference at the previous and present time step, respectively, and $\mathrm{\Delta }t=1/\text{FPS}$ . There are two methods to find object position in the z-axis using images: stereovision and monovision. We propose a simple and efficient experimental method to find object distance using an object's area in a single image, and using this distance we find the velocity of a marker along z-axis.

Finding the distance of a specific-shaped object using a single image has been proposed in [28, 29]. Object's height in a thresholded binary image has been used to determine its distance from the camera using rectangular, triangular, cylindrical, and spherical shaped objects. However, object's size was relatively large, with minimum diameter of 0.65 m for a spherical object. For our experiment, we used an industry-standard IR camera with uniform radiation capability to illuminate a retroreflective spherical marker with 0.01 m diameter. Figure 5 shows the experimental setup used to determine how the IR marker object height and area pixels in the image change with varying distance from the optical center of the camera in the z- and x- axes. Initially, the marker is positioned exactly at the optical center of the camera and is moved away from the camera on a 2-axis linear rail system.

Figure 5

Experimental setup to observe object height and area.

It is found that the object height and area decrease exponentially with increasing distance (0.36 m to 1.5 m) from the camera along the z-axis, as shown in Figures 6(a) and 6(b), respectively. But, from Figure 6(a), it is clear that the object height does not change continuously with increasing distance when compared to the area of the object; this is the main reason for not using methods presented in other previous schemes [29]. It is also observed that the area measurements are repeatable at any particular intensity, exposure, and threshold settings of the camera. However, in order to calculate linear velocity, it is essential to linearize

$$\begin{array}{cc}& d=-\mathrm{0.0232577929}\log \left(a\right)+\mathrm{6.7653875804},\end{array}$$

(5)

$$\begin{array}{cc}& {}^c{v}_z=\frac{\left(d_{t-1}-d_t\right)k}{\mathrm{\Delta }t}.\end{array}$$

(6)

Figure 6(b) is done by taking the logarithm. Analysis of Figure 6(c) leads to the fact that the object depth has a direct relationship with its logarithmic pixel area. Now, we can find the best fitting linear polynomial by linear regression to find the object distance to the camera, which is given in (5), where d is object distance and a is the object area in pixels. Having object distance, we can find the velocity along z-axis using (6), where k is a constant (scaling factor) determined by observation to get velocity in m/s units.

Figure 6

Object height and area with increasing distance from camera.

However, at a particular distance from the camera, when the marker moves laterally to the camera (perpendicular to optical axis) from camera center in either x- or y-axis direction, the object shape in image loses circularity, and the object area is not constant; it varies as shown in Figure 7. This significantly affects the velocity ${}^c{v}_z$ even if the marker moves only in the x- and y-axis. For this, instead of finding an analytical method based on blob tracking results to solve this problem, we exploit sensor fusion to compensate for the error introduced in the measured z-axis velocity.

Figure 7

Object area at different depth when object moved laterally to camera from its center.

The inertial navigation module shown in Figure 10 computes position and velocity estimates using translational accelerations and corrections obtained from a KF algorithm. The estimated z-axis velocity from this module, $v_{z\text{est}}$ , can be taken as a corrective reference for two reasons: estimated velocity does not have significant integral drift because correction from the KF is used to compute it; and next, the frame rate of camera is less than the update rate of the sensor fusion algorithm (equal to the sampling rate of inertial sensors). Based on $v_{zest}$ , lower and upper thresholds $t_l$ and $t_u$ , a discriminating window is applied for ${}^c{v}_z$ to compute valid z-axis velocity ${}^c{v}_z^{\prime }$ using (7). Where thresholds $t_l$ and $t_u$ are determined by

$$\begin{array}{cc}& {}^c{v}_z^{\prime }=\{\begin{array}{cc}0\hfill & \text{if}\mathrm{\hspace{0.17em}\hspace{0.17em}}{v}_{z\text{est}}>t_l,v_{z\text{est}}<t_u,\hfill \\ \frac{v_{z\text{est}}+{}^c{v}_z}{2}\hfill & \text{otherwise},\hfill \end{array}\end{array}$$

(7)

$$\begin{array}{cc}& {}^{c}V=\left[\begin{array}{ccc}{}^c{v}_x& {}^c{v}_y& {}^c{v}_z^{\prime }\end{array}\right]\end{array}$$

(8)

observing $v_{z\text{est}}$ , which implies that below these thresholds $v_{z\text{est}}$ has only integral drift resulting from noise in the translational acceleration. Figure 8 shows the velocity graphs for the marker moving back and forth in the x, y, and z directions, before and after applying (7). If $v_{z\text{est}}$ falls below both thresholds, ${}^c{v}_z$ is treated as error and is nullified; otherwise we take the mean of both, which is better than trusting noisy ${}^c{v}_z$ . Figures 8(a) and 8(b) show the affected ${}^c{v}_z$ and estimated velocity $v_{z\text{est}}$ when the marker is moved back and forth along the x- and y-axes, and their corrected counterparts in Figures 8(d) and 8(e). Similarly, Figure 8(c) shows velocities when the marker is moved along z-axis and its corrected counterpart in Figure 8(f), which shows effectiveness of (7) in reducing noise.

Figure 8

Before and after correction of ^c v _z : (a) error introduced in ^c v _z when marker moved along x-axis; (d) after correcting error in ^c v _z ; (c) error introduced in ^c v _z when marker moved along y-axis; (e) after correcting error in ^c v _z .

5. Sensor Fusion for 3D Position

The MEMS triaxis accelerometer measures acceleration of the wand in the body (moving) frame of reference $O_b$ and has two components, translational acceleration (actual acceleration), and gravity, which is a function of the 3D orientation of the sensor. There are different ways to remove gravity from acceleration [14, 30]. In this section we first explain gravity removal from acceleration which is a part of the data preprocessing module of Figure 2. The next step is a KF sensor fusion algorithm to find 3D position from the translational acceleration vector and velocity vector ${}^{c}V$ .

5.1. Gravity Removal in Acceleration

A conditional offset filter [14] to remove gravity from acceleration may be sufficient when its output is used only for gesture recognition, but is not optimal in terms of responsiveness (depends on past data) and accuracy (gravity still persists in transition regions). This affects accuracy when our interest is precise position computation from translational acceleration. The following method, using a quaternion ${}_b{}^e{q}_t$ , computes gravity according to (9) and (10). Now, using (11), we can remove gravity from the acceleration vector ${}^{b}A$ to find the translational acceleration vector ${}^b{A}_r$ of moving $O_b$ . Figure 9(a) shows the acceleration of the wand in a stationary state A and arbitrary complex rotation regions B and C. Figure 9(b) indicates how close the computed gravity is to the accelerations. The result after gravity removal in the respective regions is shown in Figure 9(c), where region A contains only noise, and other parts with magnitude greater than region A contain translational accelerations occurring during rotation of wand. Now, body frame translational accelerations have to be transformed to $O_e$ according to strap-down kinematics theory using (12). Equation (13) converts ${}^{e}A$ to units of $\text{m}/{\text{s}}^{\mathrm{2}}$ .

Figure 9

Gravity removal in acceleration: (a) acceleration; (b) gravity vector computed; (c) resulting translational acceleration after gravity removed.

Figure 10

Combined tracking.

Consider the following:

$$\begin{array}{cc}& {}_e{}^b{q}_t^{-1}=\left[\begin{array}{cccc}{q}_{\mathrm{1}}^{\prime }& q_{\mathrm{2}}^{\prime }& q_{\mathrm{3}}^{\prime }& q_{\mathrm{4}}^{\prime }\end{array}\right]=\frac{{}_b{}^e{q}_t^{*}}{{\left|{}_b{}^e{q}_t\right|}^{\mathrm{2}}},\end{array}$$

(9)

$$\begin{array}{cc}& G=\left[\begin{array}{c}\mathrm{2}\left(q_{\mathrm{2}}^{\prime }{q}_{\mathrm{4}}^{\prime }-q_{\mathrm{1}}^{\prime }{q}_{\mathrm{3}}^{\prime }\right)\\ \mathrm{2}\left(q_{\mathrm{1}}^{\prime }{q}_{\mathrm{2}}^{\prime }+q_{\mathrm{3}}^{\prime }{q}_{\mathrm{4}}^{\prime }\right)\\ {\left(q_{\mathrm{1}}^{\prime }\right)}^{\mathrm{2}}-{\left(q_{\mathrm{2}}^{\prime }\right)}^{\mathrm{2}}-{\left(q_{\mathrm{3}}^{\prime }\right)}^{\mathrm{2}}+{\left(q_{\mathrm{4}}^{\prime }\right)}^{\mathrm{2}}\end{array}\right],\end{array}$$

(10)

$$\begin{array}{cc}& \left[\begin{array}{ccc}{a}_{tx}& a_{ty}& a_{tz}\end{array}\right]={}^{b}A-G^T,\hspace{1em}\hspace{1em}{}^{b}A=\left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right],\end{array}$$

(11)

$$\begin{array}{cc}& \left[\begin{array}{cccc}\mathrm{0}& a_{tx}^{\prime }& a_{ty}^{\prime }& a_{tz}^{\prime }\end{array}\right]={}_b{}^e{q}^{*}\otimes \left[\begin{array}{cccc}\mathrm{0}& a_{tx}& a_{ty}& a_{tz}\end{array}\right]\otimes {}_b{}^{e}q,\end{array}$$

(12)

$$\begin{array}{cc}& {}^{e}A=\left[{}^{e}A\right]9.81,\hspace{1em}\hspace{1em}{}^{e}A=\left[\begin{array}{ccc}{a}_{tx}^{\prime }& a_{ty}^{\prime }& a_{tz}^{\prime }\end{array}\right].\end{array}$$

(13)

5.2. Combined Tracking

A block diagram of combined tracking is shown in Figure 10 which serves as second stage sensor fusion algorithm. A 9-state Kalman filter incorporated is the heart of this algorithm. The KF is an efficient recursive filter algorithm that provides optimal estimates of system states from noisy observation data given the underlying model of the system and assumes all the errors and measurements have zero mean white Gaussian noise. It is also well known that to compensate for errors of inertial navigation systems, inertial sensors have to be assisted by other sensors, and use of a KF is common for fusing data from different sensors. Also KF is used in different applications, for example, moving target tracking in a video [31]. We propose fusion of high sampling rate MARG sensors and low frame rate vision sensor in second stage sensor fusion for 3D position tracking, which benefits from their complementary characteristics. The state variables of the KF are position, velocity, and translational acceleration.

The inertial navigation computing module estimates position and velocity from translational acceleration vector ${}^{e}A$ and corrections obtained from the filter at a previous time step. The KF takes these estimates and velocity vector ${}^{c}V$ to find the optimal estimates for position, velocity, and acceleration. The filter architecture has the following system dynamics and measurement model:

$$\begin{array}{cc}& X_k=F_{k-\mathrm{1}}{X}_{k-\mathrm{1}}+w_{k-\mathrm{1}},\hspace{1em}{w}_k~N\left(\mathrm{0},Q_k\right),\\ Z_k=H_k{X}_k+v_k,\hspace{1em}{v}_k~N\left(\mathrm{0},R_k\right).\end{array}$$

(14)

Time Update. The state estimate and error covariance are propagated based on the optimal estimation at previous time step $k-1$ .

Consider the following:

$$\begin{array}{cc}& {\widehat{x}}_k=F{x}_{k-1},\end{array}$$

(15)

$$\begin{array}{cc}& {\widehat{P}}_k=F{P}_{k-1}{F}^T+Q.\end{array}$$

(16)

For (15) we use estimates of inertial navigation computing task and optimal states at the previous time step $k-1$ . The symbol (^∧) represents predictions, $x_k$ is state vector, and F and Q are state transition and process noise covariance:

$$\begin{array}{cc}& x_k={\left[\begin{array}{ccccccccc}{p}_x& v_x& a_x& p_y& v_y& a_y& p_z& v_z& a_z\end{array}\right]}^T,\\ F=\left[\begin{array}{ccc}{F}_i& {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& F_i& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}& F_i\end{array}\right],\hspace{1em}{F}_i=\left[\begin{array}{ccc}\mathrm{1}& \mathrm{\Delta }t& \frac{\mathrm{1}}{\mathrm{2}}\mathrm{\Delta }{t}^{\mathrm{2}}\\ \mathrm{0}& \mathrm{1}& \mathrm{\Delta }t\\ \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right],\hspace{1em}i=x,y,z,\\ Q=\left[\begin{array}{ccc}{Q}_i& {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& Q_i& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}& Q_i\end{array}\right],\hspace{1em}\hspace{1em}{Q}_i=\left[\begin{array}{ccc}\frac{\mathrm{\Delta }{t}^{\mathrm{5}}}{\mathrm{20}}& \frac{\mathrm{\Delta }{t}^{\mathrm{4}}}{\mathrm{8}}& \frac{\mathrm{\Delta }{t}^{\mathrm{3}}}{\mathrm{6}}\\ \frac{\mathrm{\Delta }{t}^{\mathrm{4}}}{\mathrm{8}}& \frac{\mathrm{\Delta }{t}^{\mathrm{3}}}{\mathrm{6}}& \frac{\mathrm{\Delta }{t}^{\mathrm{2}}}{\mathrm{2}}\\ \frac{\mathrm{\Delta }{t}^{\mathrm{3}}}{\mathrm{6}}& \frac{\mathrm{\Delta }{t}^{\mathrm{2}}}{\mathrm{2}}& \mathrm{\Delta }t\end{array}\right]q_c.\end{array}$$

(17)

Matrices of size $9\times 9$ , $0_3$ represent a $3\times 3$ zero matrix, $\mathrm{\Delta }t$ is the sampling period of the MARG sensors, and $q_c$ is the process noise covariance in continuous time.

Measurement Update. If measurements are available, this step incorporates those measurements in vector $z_k$ , by adjusting the state variables, generating an optimal estimate and uncertainty $P_k$ , using (18)–(20). $K_k$ is the Kalman gain ( $9\times 9$ matrix), often called acceptability vector of $z_k$

$$\begin{array}{cc}& K_k={\widehat{P}}_k{H}_k{\left(H_k{\widehat{P}}_k{H}_k^T+R_k\right)}^{-\mathrm{1}},\end{array}$$

(18)

$$\begin{array}{cc}& x_k={\widehat{x}}_k+K_k\left(z_k-{\widehat{x}}_k\right),\end{array}$$

(19)

$$\begin{array}{cc}& P_k=\left(I-K_k{H}_k\right){\widehat{P}}_k.\end{array}$$

(20)

Measures of $z_k$ , $H_k$ , and R are $9\times 9$ measurement sensitivity and noise covariance matrices, respectively. Covariance calculated using (20) will be used for the next iteration. Matrices $z_k$ , $H_k$ , and $R_k$ are given below. Consider the following:

$$\begin{array}{cc}& z_k={\left[\begin{array}{ccccccc}\mathrm{0}& \mathrm{0}& \mathrm{0}& {}^{c}V& \mathrm{0}& \mathrm{0}& \mathrm{0}\end{array}\right]}^T,\\ R_k=\left[\begin{array}{ccc}{R}_x& {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& R_y& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}& R_z\end{array}\right],\hspace{1em}\hspace{1em}{R}_i=I_{\mathrm{3}\times \mathrm{3}}{\sigma }_i^{\mathrm{2}},\hspace{1em}{\sigma }_i^{\mathrm{2}}=\text{variance},\\ H_k=\left[\begin{array}{ccc}{H}_i& {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& H_i& {\mathrm{0}}_{\mathrm{3}}\\ {\mathrm{0}}_{\mathrm{3}}& {\mathrm{0}}_{\mathrm{3}}& H_i\end{array}\right],\hspace{1em}\hspace{1em}{H}_i=\left[\begin{array}{ccc}\mathrm{0}& \mathrm{0}& \mathrm{0}\\ \mathrm{0}& \mathrm{1}& \mathrm{0}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}\end{array}\right].\end{array}$$

(21)

Since MARG sensors are sampled at 120 Hz and the camera is sampled at 75 FPS (<MARG sensors data rate), during each time step, we check the inertial navigation computing module for estimates and marker velocity vector ${}^{c}V$ from the camera if they are available. If both are available, both time and measurement update steps are updated to get the current optimal estimate. If both are not available, a previous optimal estimate becomes our current optimal estimate. If estimates are available and ${}^{c}V$ is not ready, only predictions are updated and these become the current optimal estimates. If estimates are not available but ${}^{c}V$ is ready, predictions and measurement updates are both calculated, but the prediction equations use previous optimal estimates since new estimates are not available from the inertial navigation module.

6. Results

The proposed tracking system was tested against a leading commercial DC magnetic tracking system at 120 Hz. To do so, we set the update rate of the proposed tracking system at 120 Hz and the camera FPS at 75 and fixed them to a rigid platform to be moved by hand. The trajectories of translational motion obtained from both the systems along x-, y-, and z-axes are plotted in Figures 11(a), 11(b), and 11(c), respectively. Comparison shows the potential of our system in both static and dynamic arbitrary movements. A close inspection of Figure 11(c) also reveals the linearity and accuracy of measurements obtained from (5), (6), and (7). This shows that our simple and efficient idea presented in Section 4 can find the marker velocity along the camera z-axis using area pixels of IR marker in thresholded image, without the need for another camera or additional markers to track the 3D position of the object.

Figure 11

Comparison of trajectories of translational motion obtained from commercial tracker with our wand.

In order to compare the quaternion orientation data of the two systems, orientation with respect to their fixed, steady state quaternions were measured and then decoupled to Euler parameters describing the pitch φ, roll θ, and heading ψ corresponding to rotations around the body frame x-, y-, and z-axes, respectively. Figure 12 shows plots of 3D orientation obtained from the wands complementary filter for which magnetic distortion compensation incorporated. To show the performance of our wand, comparison of the 3D trajectories for helical motion in the earth frame of reference is presented in Figure 13.

Figure 12

Comparison of Euler angles computed from quaternion orientations obtained from commercial tracker with our wand.

Figure 13

Comparison of 3D trajectories obtained from wand with commercial tracker.

7. Conclusion

Motion tracking using MARG sensors with additional sensors is a mature field of research. Modern techniques [14–16] have focused on simpler fusion approaches on low power hardware to reach a wide range of users. We presented a simple and accurate approach for a wand system with two stage sensor fusion: the first stage of fusion offloads the 3D orientation computation from the computer, allowing focus on only 3D position computation in the computer as the camera is connected to it. The basic idea is to utilize low cost and wide field of view USB camera with IR filter to prevent numerical drift in the position computed from the acceleration of a MEMS accelerometer. Thus the overall system benefits from the complementary properties of inertial and vision sensing. Key advantages of the proposed system are (1) the working area of device, which allows user to interact with a computer or 3D TV at a comfortable distance by changing size of IR marker; (2) the small size and higher update rate; (3) the magnetic distortion compensation that helps to use the wand in challenging environments; and (4) another potential application of this device that includes air digital writing and signature verification.