用於物體追蹤的加速度計星群

國

立

交

通

大

學

資訊學院資訊科技（IT）產業研發碩士班

碩

士

論

文

用於物體追蹤的加速度計星群

G-Sensor Constellations for Object Tracking

研 究 生：蔡文添

指導教授：黃俊龍 教授

易志偉 教授

用於物體追蹤的加速度計星群

G-Sensor Constellations for Object Tracking

研 究 生：蔡文添 Student：Wen-Tien Tsai

指導教授：黃俊龍 Advisor：Jiun-Long Huang

易志偉 Chih-Wei Yi

國 立 交 通 大 學

資訊學院資訊科技（IT）產業研發碩士班

碩 士 論 文

A Thesis

Submitted to College of Computer Science National Chiao Tung University in partial Fulfillment of the Requirements

for the Degree of Master

in

Industrial Technology R & D Master Program on Computer Science and Engineering

June 2009

Hsinchu, Taiwan, Republic of China

用於物體追蹤的加速度計星群

學生：

蔡文添

指導教授

：黃俊龍 易志偉

國立交通大學資訊學院產業研發碩士班

摘

要

在多數的慣性追蹤系統中，物體旋轉角度通常都是由陀螺儀或是磁力計所測量得 來，在這篇論文中，我們提出一個新的物體移動軌跡追蹤系統，稱作加速度計星群，主要 是純用多顆加速度計來做物體的追蹤，並不需要使用陀螺儀或是磁力計來做角度的測量。 我們所提出的加速度星群技術除了可以用在導航的輔助外、車禍的分析或是新的人機介面 也都是我們可以應用的範圍。

G-Sensor Constellations for Object Tracking

Student：

Wen-Tien Tsai

Advisors：Dr.

Jiun-Long Huang

Dr.

Chih-Wei Yi

Industrial Technology R & D Master Program of

Computer Science College

National Chiao Tung University

ABSTRACT

In most inertial tracking systems, motion directions are measured by gyroscopes and/or

magnetometers. In this work, we propose a new tracking system, called g-sensor

constellations, that is composed of only accelerometers. Gyroscopes and magnetometers

are not necessary to detect the change of motion directions. The proposed technique can be

used to improve the accuracy of dead reckoning systems, help the analyzing of traffic

accidents, and develop new human-computer interfaces. A g-sensor constellation that is

composed of three accelerometers is built to verify the proposed technique.

誌

謝

首先我要感謝我的家人跟朋友對我的支持與照顧，由於他們在我背後的支持， 才能讓我在研究及學習上無後顧之憂，希望他們也能跟我一同分享完成這篇論文 的喜悅。再來我要感謝的是我的指導教授黃俊龍教授以及共同指導易志偉教授， 在研究學習過程中，給予我的指導和協助並且分享他們的經驗，使我能夠順利的 完成論文，還有我要感謝實驗室學長姐、同學及學弟，經過與他們不斷的討論以 及適時的鼓勵及兩年來的陪伴，讓我在研究學習之餘，能得到壓力的紓解及不斷 往前的動力，最後我要再一次感謝所有人，讓我在研究所的這兩年得到許多美好 的回憶。

Contents

1 Introduction 1

2 Preliminaries-Rotation Matrix 5

2.1 Unit Quaternion Representation of Rotation . . . 5

2.2 Rotation Matrix by a Unit Vector . . . 8

2.3 Rotation Matrix by Yaw Pitch Roll(ZYX) . . . 9

3 Related Works 11 3.1 Accelerometer + Gyroscope . . . 11

3.2 Accelerometer + Magnetometer . . . 12

3.3 Accelerometer + Gyroscope + Magnetometer . . . 13

3.4 IMU + Kalman filter . . . 14

3.5 Six Linear Accelerometers . . . 15

4 Sensor Calibration 17 4.1 Single Sensor Calibration . . . 17

4.1.1 Linear Calibration . . . 17

4.1.2 Least Square Calibration . . . 18

4.2 Multiple Sensors Calibration . . . 19

5 G-Sensor Constellations 20 5.1 Framework . . . 20

5.2 The Object Tracking Algorithm . . . 23

5.2.1 Extrapolation . . . 24

5.2.2 Least Square Method . . . 24

5.2.4 Object Rotation . . . 27

6 Experiments 29 6.1 Static Experiment . . . 29

6.2 Static Rotation Experiment . . . 32

6.3 Dynamic Experiment in a Straight Line . . . 35

6.4 Dynamic Experiment in a S-curve . . . 36

List of Figures

2.1 Rotation about an arbitrary unit vector u(ux, uy, uz) by an angle θ. . . . . 8

2.2 Yaw-Pitch-Roll. . . 9

2.3 Rotation by X, Y, Z axis. . . 10

3.1 A simplified strapped down system [From ”Simplified strapped down iner-tial navigation utilizing bang-bang gyro torquing”]. . . 12

3.2 Six linear accelerometer system [From ”Gyroscope free Strapdown inertial measurement unit by six linear accelerometers”]. . . 15

4.1 Alignment between sensors. . . 19

5.1 Constellations with four g-sensors. . . 20

5.2 Constellations with five g-sensors. . . 21

5.3 A g-sensor constellation. . . 21

5.4 System framework of the g-sensor constellation. . . 22

5.5 Object tracking by extrapolation and least square method. . . 23

5.6 Flowchart of the object tracking algorithm. . . 28

6.1 The vibratility of sensor 1 in a static state. . . 30

6.2 The vibratility of sensor 2 in a static state. . . 30

6.3 The vibratility of sensor 3 in a static state. . . 31

6.4 Displacement and angle with different thresholds. . . 32

6.5 Rotation along time. . . 33

6.6 Rotate 30◦by Z-axis with average method. . . 33

6.7 Rotate 30◦ by Z-axis with threshold method. . . 33

6.8 Rotate 60◦ by Z-axis with average method. . . 34

6.10 Rotate 90◦ by Z-axis with average method. . . 34

6.11 Rotate 90◦ by Z-axis with threshold method. . . 34

6.12 The track of a striaght line movement by average method. . . 35

6.13 The track of a striaght line movement by threshold method. . . 35

6.14 The track of a S-curve movement by average method. . . 36

List of Tables

6.1 The drifting distance of a static constellation after 30 seconds. . . 31 6.2 The Yaw-Pitch-Roll angle by rotating 30◦, 60◦ and 90◦. . . 32 6.3 The distance measured after the constellation moves 45m in a straigt line. 35

Chapter 1

Introduction

The Micro-Electro-Mechanical System (MEMS) is a popular technique which allows both electronic circuits and mechanical devices to be manufactured on a silicon chip. MEMS promises to revolutionize nearly every product category by bringing together silicon-based microelectronics and makes possible the realization of complete systems-on-a-chip. Due to MEMS technique is not only more and more practice but also it has advantages of low cost and small size. New applications are emerging as the existing technology is applied to the miniaturization and integration of conventional devices. Examples of MEMS de-vice applications include gyroscopes, accelerometers, press sensors, DLP and wireless RF sensors. In recently years it is even applied on bioengineering, technique of memory and game platforms.

MEMS-based inertial measurement units (IMUs) sensors are now becoming pervasive. Inertial measurement units are usually composed of an accelerometer, a gyroscope, and a magnetometer. The accelerometer can measure the acceleration, the gyroscope can measure the rotation speed, and the magnetometer can give the orientation. Through accelerometers the distance of motion can be got by integrating acceleration, the rotation and orientation of motion also can get through gyroscopes or magnetometers.

Inertial measurement units (IMUs) are widely used in various applications. In [1], a triaxial accelerometer was used to monitor human movements and postures. The tech-nique can be used for tracking patients in hospitals or detecting the falls of elders. In [2], IMUs were used for detecting personal activity, and then small-scale location information was revealed by comparing with a pre-built knowledge database. That is useful to improve the accuracy of GPSs. In [3], accelerometer-based estimators for pedestrian step lengths

were developed. However, positions and motion trajectory were not studied. In [4], [5], and [6], authors used IMUs to implement pedometers to calculate step lengths, walking distances and velocities, but there was a gyroscope in their IMUs to detect motion di-rections. In [7], a position tracing system for human movements are developed. In the system, accelerometers and magnetometers were used, but the accuracy was dropping as metals or electronic devices around. Besides these, IMUs or inertial sensors still have a lot of other applications, including

• Electronic products: On game controllers it can get the player’s movement or motion of hand for controlling operation of games. It also can apply for pedometers by gathering information from IMUs.

• Vehicular applications: Car alarms can use it to detect open of a car door and turn on an alarm when a door is opened. Besides on rollover protection or airbag activation it can be detected by IMUs and adopt some protections when a car occurs an accident.

• Geographic applications: On motion tracking the accelerometer can be used to calculate the distance of an object and the gyroscope or magnetometer can be used to calculate the rotation or orientation of an object. On earthquake monitoring is also useful for using IMUs.

• Others: some notebooks will integrate 3D g-sensor for HDD protection, it can au-tomatic lock the HDD for protecting the data loss when g-sensor detects the fall of notebooks.

Although IMUs can be used on a lot scopes, most and popular applications are used on navigation system, called Inertial Navigation Systems. These INSs compose of ac-celerometers and gyroscopes and/or magnetometers. Inertial Navigation Systems com-bine with GPS for position and attitude determination are widely. They provide many complimentary characteristics that overcome the limitations experienced when using them individually. The integration of GPS and INS is always achieved using a Kalman filter for example [8] [9] [10]. An inertial navigation system includes at least a computer and a platform or module containing accelerometers, gyroscopes, or other motion-sensing de-vices. They are in general very expensive because they demand precision requirements,

high computational efforts, and last but not least sufficient reliability under harsh envi-ronmental conditions.

Most IMUs are composed of accelerometers, gyroscopes, and magnetometers. In the past, IMU-based object tracking techniques need gyroscopes and/or magnetometers to detect the change in motion directions or simply the motion directions. However, the price of IMUs is relatively high for some daily applications. In addition, magnetometers are interfered by other electronic devices.

In this work, we propose a pure accelerometer-based solution, called g-sensor constella-tions. A g-sensor constellation is a collection of accelerometers that has a rigid geometric topology and can be used for object tracking in the 3D space. Due to no need for the help of magnetometers, the interference caused by electronic devices is avoided and gyro-free which is cheaper than gyro-based IMUs. We apply extrapolation methods and least square methods (LSM) to track the locations of sensors. First, we apply the extrapolation method to get an estimation of sensor positions, and then the topology information of the constellation is utilized to calibrate positions by the LSM. Later, the LSM outcomes are applied to correct accumulation error of velocities and used in the next iteration. Besides the displacement, the rotation information is also obtained.

Whether accelerometers have high accuracy, the acceleration value of three axes still has slight vibration even sensors at static state. This small and slight value will increase error of displacement or velocity and become bigger over the time when we use integra-tion to get this informaintegra-tion. In this work a method can reduce the error by using the characteristic of fixed graph and LSM. The g-sensor constellation system can use the rigid geometric topology to calculate the distance and the rotation of an object with extrapo-lation, LSM and transformation matrix based on coordination and vector methods. The error can be corrected when an object is static or moving dynamic. The motion of straight line or curved line even 3D also can be tracked.

The rest of this thesis is organized as follows. Chapter 2 will describe the related work in recently years including how they to do, what differences with our work. In chapter 3 some preliminary proofs will also describe here. Chapter 4 the main approach will be described including the g-sensor constellation architecture, sensors alignment and the object tracking algorithm. The extrapolation and LSM will be described in this chapter. The experimental condition and devices will be introduced in chapter 5. The result of

experiment also will be described. Chapter 6 is our conclusion for g-sensor constellations system and what performance we achieve what problems we observe.

Chapter 2

Preliminaries-Rotation Matrix

In this chapter three rotation matrixes will be described after we use them. In first section we will show how a unit quaternion is used to represent a rotation of a vector in 3D space. In second section rotation in 3 dimensional space by a unit vector will be introduced. Finally we will describe a matrix expression for any frame given its Euler angles. Using the Z − Y − X convention, a matrix can be constructed that transforms every vector of the given reference frame in the corresponding vector of the referred frame. Before we start to introduce those rotation methods, some rules will be describes. The lowercase letters (ex. u) are marked bold to represent a vector and the uppercase letters (ex. M, T ) are marked italic to represent the matrix. Other lowercase letters represent a variable or an element.

2.1

Unit Quaternion Representation of Rotation

Quaternions were first defined by W.R. Hamilton in the 1800’s. Quaternions can be used in describing rotations in 3 dimensional real vector space. First we will define properties and operations of quaternions, and then show the relationships with the rotation matrix. Using complex number notation, a quaternion q can be represented by the following :

b

q = q0+ qx+ qy+ qz

k have the following properties :

i2 _{= −1, j}2 _{= −1, k}2 _{= −1} ij = k, jk = i, ki = j ji = −k, kj = −i, ik = −j. If br = r0+ irx+ jry + krz, quaternion multiplication becomes

b

rbq = (r0q0 − rxqx− ryqy− rzqz) + i(r0qx+ rxq0+ ryqz− rzqy)

+ j(r0qy− rxqz+ ryq0+ rzqx) + k(r0qz+ rxqy − ryqx+ rzq0). It can be easily verified that the multiplication operation is not commutative.

The conjugate of q has the same real part with negated imaginary part , bq∗ _{= q}

0 − iqx − jqy − kqz. The conjugate of a product of two quaternions (bqbr)∗ is br∗bq∗. The dot

product of two quaternions is the sum of products of corresponding components. b

p · bq = bp0bq0 + bpxqbx+ bpybqy + bpzqbz =

(bpbq∗_{+ bqbp}∗₎

2

It is obvious that the dot product is a real number. The square of magnitude of a quaternion is the dot product of the quaternion with itself.

|bq|2 = bq · bq = bqbq∗ = bq∗qb A multiplicative inverse of a nonzero quaternion is

b

q−1 = ( 1 b q · bq)bq

∗

A vector in 3D space t = (tx ty tz)T can be represented by a purely imaginary

quater-nion

b_{t = 0 + itx+ jty+ ktz.}

We will denote t = bt, if the corresponding components are the same between a 3D vector and a pure imaginary quaternion. Let s be vector in 3D space, and bs be its pure imaginary representation; then we can verify the following properties, which will be used in the following proofs.

s · t = bs · bt = (bsbt

∗_{+ btbs}∗₎

2 s × t = (bsbt − btbs)

We will the claim that bt0 _{= brbtbr}∗ _{is a rotation operation when bt is a pure imaginary}

quaternion equivalent to a 3D vector t, and br is a unit quaternion. The inverse mapping is br∗b_t0_{br and the mapping is bijective. First it will be shown that bt}0 _{is pure imaginary by}

checking if bt0_{+ bt}0∗ _{= 0.}

b

t0_{+ bt}0∗ _{= brbtbr}∗_{+ (brbtbr}∗₎∗ _{= brbtbr}∗_{+ brbt}∗_br∗

= br(bt + bt∗_)br∗ _{= 0}

from the fact that t is pure imaginary.

The rotation operation must preserve the dot product. s0_{· t}0 ₌ (bs0tb0 ∗ + bt0_sb0∗₎ 2 = £ b rbsbr∗_(brbtbr∗₎∗ _{+ brbtbr}∗_(brbsbr∗₎∗¤ 2 = £ b rbs(br∗_r)bbtbr∗_{+ brbt(br}∗_br)bsbr∗¤ 2 = (brbsbt∗br∗+ brbtbs∗br∗ 2 ) = br( b sbt∗ _{+ btbs}∗ 2 )br ∗ = br(bs · bt)br∗ _{= (bs · bt)(brbr}∗_{) = (bs · bt) = s · t}

Here we use the fact the dot product bs · bt is a real number, and the product of a real number with quaternion is commutative. It is a special case of the above that bt · bt is the same as t · t, which means the length of a vector is invariant after the rotation.

If t is taken from t rotated by an angle θ around the u = (ux, uy, uz) axis is a unit

vector, then we represent a unit quaternion br = r0+ irx+ jry+ krz in the following form

cos(θ 2) + sin( θ 2)u ,where r0 = cos(θ₂), h rx ry rz i_T = sin(θ 2) h ux uy uz i_T

then brbtbr∗ _{reduce to}

follow-ing equation. A component by component equation for bt0 _{= brbtbr}∗ _{with matrix notation}

becomes      t0 x t0 y t0 z     =      (r2 0+ r2x− r2y− r2z) (−2r0rz + 2rxry) (2r0ry+ 2rxrz) (2r0rx+ 2rxry) (r02− rx2+ r2y− rz2) (−2r0rx+ 2ryrz) (−2r0ry + 2rxrz) (2r0rx+ 2ryrz) (r02− r2x− r2y+ r2z)           tx ty tz      (2.1)

θ

u

Figure 2.1: Rotation about an arbitrary unit vector u(ux, uy, uz) by an angle θ.

2.2

Rotation Matrix by a Unit Vector

Let a unit vector u = (ux, uy, uz), where u2x+ u2y + u2z = 1, the transformation matrix M

for a counterclockwise rotation by an angle of θ about an axis the direction of u showed as Figure 2.1. We can get r0 = cos(θ₂),

h rx ry rz i_T = sin(θ 2) h ux uy uz i_T and substitute to Eq. 2.1, which will use following proof :

sin2 θ 2 = 1 − cos θ 2 , cos 2 θ 2 = 1 + cos θ 2 sin θ = 2 sinθ 2cos θ 2      (r2 0+ r2x− r2y− r2z) (−2r0rz+ 2rxry) (2r0ry+ 2rxrz) (2r0rx+ 2rxry) (r02− r2x+ r2y − rz2) (−2r0rx+ 2ryrz) (−2r0ry + 2rxrz) (2r0rx+ 2ryrz) (r02− r2x− r2y+ r2z)      =      1+cos θ

2 + (1−cos θ2 )(2u2x− 1) (− sin θ)uz+ 2(1−cos θ₂ )uxuy sin θuy + 2(1−cos θ₂ )uxuz

sin θux+ 2(1−cos θ₂ )uxuy 1+cos θ₂ + (1−cos θ₂ )(2u2y − 1) (− sin θ)ux+ 2(1−cos θ₂ )uyuz

(− sin θ)uy + 2(1−cos θ₂ )uxuz sin θux+ 2(1−cos θ₂ )uyuz 1+cos θ₂ + (1−cos θ₂ )(2u2z− 1)

     =     

cos θ + (1 − cos θ)u2

x (− sin θ)uz+ (1 − cos θ)uxuy sin θuy+ (1 − cos θ)uxuz

sin θux+ (1 − cos θ)uxuy cos θ + (1 − cos θ)u2y (− sin θ)ux+ (1 − cos θ)uyuz

(− sin θ)uy + (1 − cos θ)uxuz sin θux+ (1 − cos θ)uyuz cos θ + (1 − cos θ)u2z

    

It can also be proofed from [12].

Theorem 1 Rotation about an arbitrary unit vector u (ux, uy, uz) by an angle θ is given

M(u, θ) =      c + (1 − c)u2

x (1 − c)uxuy − (s)uz (1 − c)uxuz+ (s)uy

(1 − c)uyyx+ (s)uz c + (1 − c)u2y (1 − c)uyuz− (s)ux

(1 − c)uzux− (s)uy (1 − c)uzuy+ (s)ux c + (1 − c)u2z

   

 (2.2)

,where c = cos θ, s = sin θ

2.3

Rotation Matrix by Yaw Pitch Roll(ZYX)

Roll (α) Yaw (γ) Pitch (β) X Z Y Figure 2.2: Yaw-Pitch-Roll.

As Figure 2.2 we define the Euler angles for our system. Yaw, pitch and roll are counterclockwise by Z, Y and X axis.

Considering three basic rotation matrices are in three dimensions. These matrices represent counterclockwise rotations of an object relative to fixed coordinate axes, by an angle of α, β, γ around the X, Y and Z axes, respectively. The direction of the rotation is determined by the right-hand rule: Rx rotates the Y -axis towards the Z-axis, Ry rotates the Z-axis towards the X-axis, and Rz rotates the X-axis towards the Y -axis. The transpose of the above matrices represents positive (right-hand sense) rotation of the coordinate axes relative to a fixed object. As Figure 2.3 (a) (b) (c) we can get the Yaw-Pitch-Roll (Z − Y − Z) transform matrix. From Eq. 2.2, the matrix of Rx(α)

describes rotation about an arbitrary unit vector (1, 0, 0) by an angle α. The matrix of Ry(β) describes rotation about an arbitrary unit vector (0, 1, 0) by an angle β and Rz(γ)

( c ) ( b ) ( a ) γ X Y β X Z α Y Z V V _V V Rz(γ)‧ Ry(β)‧V Rx(α)‧_V

Figure 2.3: Rotation by X, Y, Z axis. the following equations (those equations can also be proofed in [13]):

Rx(α) =      1 0 0 0 cos α − sin α 0 sin α cos α     , Ry(β) =      cos β 0 sin β 0 1 0 − sin β 0 cos β     , Rz(γ) =      cos γ − sin γ 0 sin γ cos γ 0 0 0 1     

The product T = Rx(α) · Ry(β) · Rz(γ) represents a rotation matrix whose Yaw, Pitch,

and Roll are γ, β, and α (using the Z − Y − X convention for Euler angles). Theorem 2 Rotation by Yaw-Pitch-Roll(Z − Y − X axes)

T =      c1c2 −c2s1 s2 c3s1+ c1s2s3 c1c3− s1s2s3 −c2s3 s1s3− c1c3s2 c3s1s2+ c1s3 c2c3     

Chapter 3

Related Works

In this chapter, we will introduce some tracking systems with IMUs. Related works are about tracking or motion by accelerometers, gyroscopes and magnetometer. Finally the six linear accelerometer system will be introduced. It describes a tracking system which composes of six linear accelerometers without any gyroscope or magnetometer. It designs one accelerometer at the center of face of a cube and the sensing axis of each accelerometer is along the respective cube face diagonal.

The research for tracking, motion and navigation system is currently carried out in many laboratories with using of accelerometers, gyroscopes and magnetometers. This is the basic idea for tracking and navigation by using accelerometers, gyroscopes and mag-netometers. They usually use accelerometers to get acceleration of objects and integrate the acceleration to calculate the moving distance. Gyroscopes and magnetometers are usually used to calculate the rotation or change of orientation.

3.1

Accelerometer + Gyroscope

In [14], it represented a simplified strapped down system for navigation. A strapped down navigation system comprised of gyroscopes and accelerometers without magnetometers. They could be mounted on body or vehicle. Gyroscopes provided attitude angle and an-gular rate signals along three axes. Accelerometers meant provide signals representative of the acceleration along three independent axes. First transformation matrix connected to the attitude angle output of the gyroscopes to accelerometers transforms the gyroscope and the accelerometer signals from body coordinates to gyroscope coordinates. Second

transformation matrix connected to the output of the gyros, transforms the gyro coor-dinates into navigation coorcoor-dinates. The algorithm flow is shown as Figure 3.1. It is a simplified and basic system for tracking or motion. Because of gyroscopes had expensive price and error increased with double integration by the time. Attitude determination systems that used inexpensive sensors had been discussed.

Gyroscopes (Captured)

Accelerometers

Transformation

to gyro coor. Transformation to inertial coor. Torquing Rates(ω)

Gravitational

acceleration Inertial velocity Inertial position

Figure 3.1: A simplified strapped down system [From ”Simplified strapped down inertial navigation utilizing bang-bang gyro torquing”].

3.2

Accelerometer + Magnetometer

In [15], it replaced expensive gyroscopes with magnetometers. This IMU composed of accelerometers and magnetometer. It used the earth’s magnetic field and gravity as the two measured quantities, a low-cost attitude determination system is proposed. It used the magnetic field vector and the acceleration, a unique plane containing the two vectors can be defined, and if the components of those two vectors can be measured in two non-aligned coordinate frames, then the rotation needed to align the two coordinate frames can be determined. In [16], the author also used a accelerometer and a tri-magnetometer to determine the orientation of a static or slow-moving rigid body. It presented a geometrically intuitive 3-degree-of-freedom (3-DOF) orientation estimation algorithm with physical meaning which is called the factored quaternion algorithm (FQA). It is defined orientation by rotating it about its z-axis by an angle ψ (azimuth or yaw rotation), then about its y-axis by angle θ (elevation or pitch rotation), and finally about

its x-axis by angle ϕ (bank or roll rotation). The elevation and roll rotation are determined from accelerometers. The azimuth rotation is determined from magnetometers. The value of ax, ay, az is from a tri-accelerometer. And the value of bmx, bmy, bmz is from a

tri-magnetometer in body coordinate system. The e_m

x, emy, emz are magnetic vector in

Earth coordinate system. The initial magnetic vector is h nx ny nz i_T . Elevation Quaternion : qe= cos θ 2(1 0 0 0) + sin θ 2(0 0 1 0) sin θ = ax g , cos θ = p 1 − sin2_θ Roll Quaternion : qr = cos ϕ 2(1 0 0 0) + sin ϕ 2(0 1 0 0) sin ϕ = −ay g cos θ, cos ϕ = −az g cos θ Azimuth Quaternion : qa = cos ψ 2(1 0 0 0) + sin ψ 2(0 0 0 1) e_{m = q} e qr bm qr−1 q−1e ,   nx ny   =   cos ψ − sin ψ sin ψ cos ψ     emx e_m y  

Having obtained all three rotation quaternion, the quaternion estimates representing the orientation of rigid body is finally given by

b

q = qa qe qr

Although they are cheaper than the gyroscope, the magnetometer is easily interfered by other electronic device. When a ferrous object is close to those systems, they will get bigger noise and lost their accuracy.

3.3

Accelerometer + Gyroscope + Magnetometer

In order to improve the accuracy or reduce the error of orientation, much research had focused on using IMUs system that composed of accelerometers, gyroscopes and magne-tometer. They not only used gyroscopes to calculate orientation but also magnetometers had been used to enhance accuracy and reduce error. In [7], it described a self-contained

method for relative position tracking of a human engaged in various types of motion in-volving discrete steps. This method is based on the use of the inertial and magnetometer sensor module attached to the foot. This modules contained three orthogonally mounted angular rate sensors, three orthogonal linear accelerometers and three orthogonal magne-tometers. In generally, the output of an accelerometer will be integrated twice to obtain displacement information. However it is susceptible to drift errors. A drift correction can be applied to the accelerometer with double integration method so that the final estimated velocity should be zero when a foot instant contacts with the ground. These inertial and magnetic sensor modules are primarily designed for tracking orientation. Although this system is high accuracy, it is not cheap and also is easily interfered by electronic devices.

3.4

IMU + Kalman filter

Not only using multi kinds of sensors but also applying Kalman filter to improve accuracy or reduce drift error on IMUs system. The Kalman filter is the most widely used state estimator for tracking applications. The filter is the general solution to the recursive linear minimum mean square estimation problem. In [17], it used two different adaptive Kalman filter. Each individual process noise covariance value will provide optimum estimates of the target’s states only when underlying model, which is represented by that specific process noise covariance, is correct. For example a low level process noise covariance will match the target dynamics during an almost straight line motion period but fail to track when the target moves into a turn manoeuvre. The first filter is adjusted at each time step according to the estimated turn rate. The turning rate is estimated from acceleration divided by the estimated speed of target. Second uses a scale filter which is estimated from the available data of sensors reading. The first algorithm exploits the idea ”the choice of the process covariance level must be made according to the expected turn rate” and utilizes an empirical turn rate-process noise covariance level curve. And the second algorithm introduces a scale factor which represents the current magnitude of process noise, i.e., target unpredictability, at time t as estimated from the available data. But a proper Kalman filter is complex and hard to design.

2ρ zb yb xb Ob (a) OI P ωωωω Ob R r ρ -> -> -> (b)

Figure 3.2: Six linear accelerometer system [From ”Gyroscope free Strapdown inertial measurement unit by six linear accelerometers”].

3.5

Six Linear Accelerometers

Gyroscopes are not inexpensive and magnetometers are easily interfered by other elec-tronic devices. In 1994 Jeng-Heng [18] presented a method to determine the kinematics of a rigid body by using only linear accelerometers. In their work, six linear accelerometers are used for a complete description of a rigid body motion. They design one accelerometer at the center of each face of a cube and the sensing axis of each accelerometer is along the diagonal of respective cube face, shows as Figure 3.2 (a). The line of cube side is 2ρ and xb, yb, zb are body frame of cube. Ob is the center of this cube. Figure 3.2 (b) shows

relation of the inertial frame(OI) and a rotating moving frame (Ob). The

− →

R = −→r + −→ρ then the accelerometer of point P can be calculated by double integration −→R . S and T are 3 × 6 matrices After calculating, the formula will be produced :

  ∂−→ω ∂−→v   = 1 2   S/ρ T   A + ρ               0 0 0 ωyωz ωxωz ωxωy              

,where A is the acceleration from accelerometers, ∂−→ω is the angular acceleration, the ω is the angular velocity by integrating ∂−→ω , ∂−→v is inertial acceleration.

S = √1 2      1 −1 0 0 1 −1 −1 0 1 −1 0 −1 0 1 −1 −1 1 0     , T = 1 √ 2      1 1 0 0 −1 −1 1 0 1 −1 0 1 0 1 1 1 1 0     

Although this system only uses accelerometer, it needs six accelerometer, fixed graph and fixed direction when installs the accelerometer. Because of the high accurate at the installing position is difficult, it needs to lathe to reach this request If the direction of accelerometer does not aligned, it will cause error. The major request is high accuracy for those accelerometers.

Some related works usually use gyroscopes or magnetometers to calculate change of orientation and accelerometer to get distance information. But they have the problem of expensive price and easily are influenced by electric/ferrous devices. Although the system of six linear accelerometers can also use pure accelerometers to tracking objects, it is hard to design and lathe is needed. In this work the g-sensor constellation system not only considers the price and is influenced by other device but also considers the sensor error itself. The pure accelerometers can reduce the cost and does not be influence by electric devices. And the LSM is a method to correct the error itself in this rigid graph system.

Chapter 4

Sensor Calibration

After each sensor leaves the factory, although the hardware specification is the same, the hardware itself has the trifle error to cause the reading to have the offset. In this chapter we will introduce some calibrated methods to correct offset of the sensor, including, single sensor calibration and multiple sensors calibration.

4.1

Single Sensor Calibration

Before using the inertial sensor to implement on all kinds of applications, it is basic and the most important first step to calibrate the sensor itself. In this section two single calibration methods will be introduced.

4.1.1

Linear Calibration

The linear calibration uses the linear function y = a(x − b) to correct offset of the sensor. It means the reading of the sensor maybe have translation or scaling. Using linear function can efficient reduce this offset. The major steps are to measure the maximal and minimal reading of a sensor with X, Y, Z axes, respectively. Here we define the fy(i) to present

the reading of the sensor which it should be. The fx(i) presents the measured reading

of the sensor from the axis, where i presents the X, Y or Z axis. In order to measure the maximal and minimal reading of a sensor, each axis (X, Y, Z) must put toward to down and up, respectively which the reading should only be effect from gravity. When the axis puts toward to down, it means we can obtain the maximal reading (fx−max(i))

minimal reading (fx−min(i)) and the fy−min(i) should be −1g. The simultaneous equation

is following :

fy−max(i) = 1 = a(fx−max(i) − b) (4.1)

fy−min(i) = −1 = a(fx−min(i) − b) (4.2)

from Eq. 4.1 and Eq. 4.2, we can solve variables a and b.

4.1.2

Least Square Calibration

Assuming the sensor has an offset at each X, Y and Z axis which are c1, c2 and c3. The measured reading of a sensor at three axes is define to xi, yi, zi, where i is the i-th sample.

It can be presented to least square methods to find the minimum of (xi − c1)2 + (yi −

c2)2_{+ (z}

i− c3)2. Assuming the minimum is r2, the formula can be written as :

(xi− c1)2 + (yi− c2)2+ (zi− c3)2 = r2. (4.3) This problem is similar to determine how close those points (you can think that xi, yi,

zi is a point in 3D space) are to being circular, we can fit a least squares circle to the

data and check to see how close the measured points are to the circle. To fit a circle to n sample pairs of measured readings (x1, y1, z1), (x2, y2, z2), ..., (xn, yn, zn) in a static state,

we must determine the center (c1, c2, c3) and the radius r. Rewriting Eq. 4.3, we get 2xic1+ 2yic2+ 2zic3+ r2 − c12− c22− c23 = x2i + yi2+ zi2.

If we set c4 = r2− c21− c22− c23, then the equation takes the form 2xic1+ 2yic2+ 2zic3+ c4 = x2i + yi2+ zi2.

Substituting each of the measured reading with n sample into this equation, we obtain the overdetermined system

        2x1 2x1 2x1 1 2x2 2x2 2x2 1 ... ... ... ... 2xn 2xn 2xn 1                 c1 c2 c3 c4         =         x2 1+ y21 + z12 x2 2+ y22 + z22 ... x2 n+ yn2+ zn2         . (4.4)

4.2

Multiple Sensors Calibration

In our work we use multiple sensors to implement our system, so the multiple sensors calibration is need. As Figure 4.1, three axes of each sensor has deviation with other sensor. That means the X/Y /Z axis of sensor 1 maybe be not aim at the X/Y /Z axis of sensor 2 or sensor 3. So we must do the alignment between every sensor. The multiple sensors calibration (this also is called the sensors alignment method) is a method to correct this deviation. In our work the 1st sensor is denoted by S1, is called as the body frame, and the 2nd and 3rd sensors, respectively denoted by S2 and S3, are aligned to the body frame. The transformation matrix TSi→S1 from the i-th sensor to the body frame, we need

to sample the reading (in a static state) in three independent orientations. If Gi,j, a 3 × 1

vector, is the j-th reading of the i-th sensor, we have G1,j = TSi→S1Gi,j for i, j = 1, 2, 3.

The transformation matrices TSi→S1 can be obtained by solving linear systems. Note that

TS1→S1 = I. Sensor 1 Sensor 2 Sensor 3 Y Y Y X X X Z Z Z

Chapter 5

G-Sensor Constellations

In this chapter we will introduce our g-sensor constellations architecture. In the beginning aligning between g-sensors is the first task in our work. The transformation matrix will also be described from initial coordinate to present coordinate. Finally the object tracking algorithm will be described, including extrapolation, LSM, adjust velocity and the object rotation.

5.1

Framework

A sensor constellation (abbreviated as constellation) is composed of at least three g-sensors with fixed topology. In a general case, a constellation may be composed of more than three sensors and the topology may not be a regular polygon. For example it can also compose of four/five or more g-sensors, as Figure 5.1, 5.2.

S1 S2 S3 S4 S1 S2 S3 S4

Figure 5.1: Constellations with four g-sensors.

In this work, a g-sensor constellation consisting of three Witilt V3 sensors are built to verify our tracking technique. The three sensors are stuck at the vertices of an equilateral triangle of 0.3m edge lengths. So, the constellation is depicted as in Figure 5.3. The

S1 S2 S3 S4 S5 S1 S2 S3 S4 S5

Figure 5.2: Constellations with five g-sensors.

constellation is output via Bluetooth interface. The receiver maybe is a computer or a phone which receive the data from the constellation. After receiving the data, the object tracing algorithm will be executed then the outcome will be input to the draw programming. The system framework is depicted as in Figure 5.4.

S₁(x₁, y₁, z₁) l_1,3 l_1,2 l_2,3 S₃(x₃, y₃, z₃) S2(x2, y2, z2)

Figure 5.3: A g-sensor constellation.

Since the coordinates of three sensors may be not perfectly aligned, linear transforma-tions are needed to align the coordinate systems of three sensors. The coordinate system of the 1st sensor, denoted by S1, is called as the body frame, and the coordinate systems of the 2nd and 3rd sensors, respectively denoted by S2 and S3, are aligned to the body frame. To compute the transformation matrix TSi→S1 (or sometimes written as TSi→B)

from the i-th sensor to the body frame, we need to sample the reading (in a static state) in three independent orientations. If Gi,j, a 3 × 1 vector, is the j-th reading of the i-th

sensor, we have G1,j = TSi→S1Gi,j for i, j = 1, 2, 3. The transformation matrices TSi→S1

can be obtained by solving linear systems. Note that TS1→B = TS1→S1 = I. Let eG i,j

denote the raw reading of the i-th sensor, i.e., measured in the coordinate system of Si,

Communicate by Bluetooth

G-Sensor Constellations

Receiver

Data from G-Sensor Constellations

The Object Tracking Algorithm

Draw trajectory from outcome

Figure 5.4: System framework of the g-sensor constellation. Then,

GB

i,j = TS1→BGei,j.

Besides displacement, the constellation may rotate during the movement. A reference coordinate system is needed to record the movement of the constellation. We choose the body frame at time t0 as the reference coordinate system and call it the earth frame. Let TB→E

j be the linear transformation from the body frame to the earth frame at time tj,

and GE

i,j denote the reading of the i-th sensor measured in the earth frame at time tj.

Then,

GE

i,j = TjB→EGBi,j.

Let Si,j denote the position of the i-th sensor at time tj

mea-sured in the earth frame. Then, the linear transformation matrix from the body frame to the earth frame at time tj can be determined by

the mapping from {S2,j− S1,j, S3,j − S1,j, (S2,j − S1,j) × (S3,j − S1,j)} to {S2,0− S1,0, S3,0− S1,0, (S2,0− S1,0) × (S3,0− S1,0)}. Let Nj = (S2,j − S1,j)×(S3,j− S1,j). Then, the transformation matrix TB→E

j can be obtained by solving the following linear

equation ³ S2,0− S1,0 S3,0− S1,0 N0 ´ = TB→E j ³ S2,j − S1,j S3,j− S1,j Nj ´ . (5.1)

Let Ai,j be the acceleration of the i-th sensor at time tj measured in the earth frame.

Ai,j, the gravity must be deducted from GEi,j. If the constellation is in a static state at

time t0, then the gravity can be given by GE

1,0, GE2,0, and GE3,0. So, for i = 1, 2, 3, we have Ai,j = GEi,j− GEi,0

= TB→E

j TSi→BGei,j− T0B→ETSi→BGei,0

= TB→E

j TSi→BGei,j− TSi→BGei,0. (5.2)

5.2

The Object Tracking Algorithm

We iteratively keep tracking the positions and velocities of sensors by extrapolation based on the accelerations reported by g-sensors. However, accumulated error is a major con-cern and rotations of sensors can’t be detected by sensors themselves. So, we develop a heuristic by utilizing the relative positions between sensors to adjust the displacements and velocities of sensors by least square methods (abbreviated as LSM). In the mean while, rotations can also be revealed.

In addition to position Si,j and acceleration Ai,j, let Vi,j denote the velocity of the i-th

sensor at time tj measured in the earth frame, and eSi,j+1 denote the estimated position

of the i-th sensor at time tj+1 obtained from Si,j and Vi,j by extrapolation. Based on the

topology information of the constellation, Si,j+1 is obtained and Vi,j+1 is also calculated.

The process of object tracking from time tj to time tj+1 is depicted in Figure 5.5. The

S_1,j S S S 2,j 3,j ~ S~ S~ S_{2,j +1} S_{1,j +1} S_{3,j +1} LSM extrapolation 3,j +1 2,j +1 1,j +1

Figure 5.5: Object tracking by extrapolation and least square method. process will repeat again and again.

5.2.1

Extrapolation

Let 4tj = tj+1− tj, i.e., the time span between the j-th and (j + 1)-th samples. Assume

the motion from tj to tj+1 has a constant acceleration. Then, eSi,j+1 can be obtained by

applying the constant acceleration motion formula e

Si,j+1= Si,j+ Vi,j4tj+

1

2Ai,j(4tj)

2_{. (5.3)}

This also is called the extrapolation method.

5.2.2

Least Square Method

Since the constellation is rigid, the topology metrics between sensors is fixed, especially the distance. However, these geometric properties may be broken by the extrapolation process. Therefore, the predicted positions can be adjusted based on topology information. We apply the least square method (abbrev. as LSM) given in Alg. 1 to do adjustment. For convenience, if it is not necessary to emphasize the time index j, we frequently suppress the index j and simply use Si to denote the position of the i-th sensor.

Let Si = (xi, yi, zi) for i = 1, 2, 3 be the position of the i-th sensor; and eSi = (exi, eyi, ezi)

for i = 1, 2, 3 be the predicted position of the i-th sensor that is obtained by the extrapo-lation method. Due to the measurement error of acceleration and the accumuextrapo-lation error of velocity, the predicted position may not precise. Since the system is rigid, the topology metrics between S1, S2, S3 is fixed, especially the distance. We have kS2− S1k = l1,2, kS3− S2k = l2,3, and kS1− S3k = l1,3. So, we can calibrate the predicted position under the topology constraints by the least square method.

Algorithm 1 [Least Square Problem (LSP)]

for all eSi, i = 1, 2, 3, and li,k, 1 ≤ i < k ≤ 3 such that kSi− Skk = li,k do

Measurement: MinimizeP3_i=1 ° ° °Si− eSi ° ° °2 end for

To solve the LSM, we apply Lagrange multipliers λi,j for 1 ≤ i < j ≤ 3. Let

f (S1, S2, S3) = 3 X i=1 ° ° °Si− eSi ° ° °2 = 3 X i=1 (xi− exi)2 + (yi− eyi)2+ (zi− ezi)2,

, for 1 ≤ i < k ≤ 3 gi,k(S1, S2, S3) = kSi− Skk2− (li,k)2 = (xi− xk)2+ (yi− yk)2+ (zi− zk)2− (li,k)2, and Λ (P1, P2, P3, λ1, λ2, λ3) = f (S1, S2, S3) − X 1≤i<k≤3 λi,kgi,k(S1, S2, S3) . To obtain S1, S2, S3, we solve the system

   ∇xi,yi,ziΛ = 0 for i = 1, 2, 3; ∇λ1,2,λ1,3,λ2,3Λ = 0. For i = 1, 2, 3, from ∂ ∂xiΛ = 0, ∂ ∂yiΛ = 0, and ∂ ∂ziΛ = 0, we have Si − eSi = P

k6=iλi,k(Sk− Si), or, more explicitly,

     xi− exi yi− eyi zi− ezi     =      xj− xi xk− xi yj− yi yk− yi zj− zi zk− zi        λj,i λk,i   . For 1 ≤ i < j ≤ 3, from ∂

∂λi,jΛ = 0, we have kSi− Sjk = li,j, or, more explicitly,

(xi− xj)2+ (yi− yj)2+ (yi− yj)2 = (li,j)2.

Put all together and then we have

x1+ x2+ x3 = x01+ x02+ x03, y1+ y2+ y3 = y01+ y20 + y30, z1+ z2+ z3 = z10 + z20 + z30.

Therefore, the barycenter of S1, S2, S3 and eS1, eS2, eS3 are overlapping, and we have the following theorem.

Put a constellation at the barycenter of eS1Se2Se3, then based on Theorem 3, the LSP can be solved by finding a best rotation. In this work, we further assume that S1S2S3 and

e

S1Se2Se3 are in the same plane, and therefore, the solution can be found by a 2D rotation. A numerical method is used to find the best rotation. The numerical method is used to solve the LSM which described by following :

In the beginning, whose vertices are S1, S2 and S3 and G is the center of mass of the triangle. Let ∠S1GS2 = θ3, ∠S2GS3 = θ1 and ∠S3GS1 = θ2, then we can use law of cosines to get θ1, θ2 and θ3. After time 4t, the vertices of triangle become eS1, eS2 and

e

S3, which are derived using the extrapolation method. So we can get eG the center of mass whose vertices are eS1, eS2 and eS3, which equals G by theorem 3. We also can decide the initial position of S1. Assume the unit normal vector becomes u = (ux, uy, uz) whose

vertices are eS1, eS2 and eS3. Now, we need to find the coordinates of S2 and S3. Then find the best possible coordinates of S1, S2 and S3. Then rotation about an arbitrary unit vector (ux, uy, uz) by an angle θ is given by a matrix by Theorem 1

M(u, θ) =      c + (1 − c)u2

x (1 − c)uxuy − (s)uz (1 − c)uxuz+ (s)uy

(1 − c)uyux+ (s)uz c + (1 − c)u2y (1 − c)uyuz− (s)ux

(1 − c)uzux− (s)uy (1 − c)uzuy+ (s)ux c + (1 − c)u2z

    

,where c = cos θ, s = sin θ

Using the theorem above, we have −→GS2 = M(u, θ3) −→ GS1k −→ GS2k k−→GS1k and −→ GS3 = M(u, θ2) −→ GS1k −→ GS3k

k−→GS1k. Now, to find the best possible coordinates of S1, S2 and S3,

cal-culate −−→GS_i0 = M(u, θ)−−→GSi for i = 1, 2, 3 and −π ≤ θ ≤ π. So, our goal is to find bθ such

that P_i ° ° ° ° −−→ G eSi− −−→ GS_i0 ° ° ° ° 2

has the minimum. This means that

b θ = arg min à X i ° ° ° ° −−→ G eSi− −−→ GS_i0 ° ° ° ° 2 , θ ! . Having bθ, the best possible coordinates can be found.

5.2.3

Adjust Velocity

After applying the LSM, we get the positions Si,j+1. We can further to adjust the velocities

of sensors according to the displacement. Let Di,j = Si,j+1− Si,j be the displacement of

In addition, we also have Di,j = Vi,j∆tj + 1 2Ai,j(∆tj) 2 = 1 2∆tj(Vi,j + Vi,j+1) . So, the velocity Vi,j+1 can be given by

Vi,j+1 =

2 (Si,j+1− Si,j)

∆tj

− Vi,j. (5.4)

5.2.4

Object Rotation

From Eq. 5.1 we can get the transformation matrix TB→E

j and by Theorem 2 the Z−Y −X

rotation matrix is known.

TB→E j =      T00 T01 T02 T10 T11 T12 T20 T21 T22     =      c1c2 −c2s1 s2 c3s1+ c1s2s3 c1c3− s1s2s3 −c2s3 s1s3− c1c3s2 c3s1s2+ c1s3 c2c3     

,where c1 = cos γ, c2 = cos β, c3 = cos α and s1 = sin γ, s2 = sin β, s3 = sin α.

Then s2 = sin β = T02, β can be obtained, α, γ can also be obtained from β and T01, T12.

Finally the Figure 5.6 shows the procedure of this algorithm. In the beginning the coordinate, velocity and transformation matrix will be initiated. The alignment between g sensors will also be calculated in this section. After initial and alignment have done, the new coordinates of g sensors will be calculated by extrapolation. Then the corrected coordinates will be calculated from new coordinates by LSM. Finally the velocity and transformation matrix can be determined by initial coordinates and corrected coordinates. Rotation information can also be got from the transformation matrix.

Initiate

j = 0, Vi,0 = 0, Si,0, T , G

Calculate T and Ai,j by Eq. (1), (2)

Extrapolate Si,j+1

by Eq. (3)

Use LSM to obtain Si,j+1

from Si,j+1

Calculate velocity Vi,j+1

by Eq. (4) j = j + 1 Si -> S1 B i,0 j B -> E ~ ~ (5.1), (5.2) (5.3) (5.4)

Chapter 6

Experiments

In our experiments, the constellation is composed of three Witilt V3 sensors which has a triaxial accelerometer and a single axis gyroscope. But only the accelerometer is used. The sampling rate is set up 90Hz. The sampling range is around ±6g. Three sensors are fixed on a plastic board at the vertices of an equilateral triangle. The edge of the equilateral triangle is 0.3m. The data of Witilt V3 sensors is output via a Bluetooth interface.

6.1

Static Experiment

The g-sensor is also with a certain level of inaccuracy after signal calibration (here, we use the linear calibration method). Even sensors are in a static state, the reading of sensors slightly vibrate. Figure 6.1, Figure 6.2 and Figure 6.3 show the vibratility of three sensors. The inaccuracy will accumulate and cause significant error on velocities and displacements. In the first experiment, the constellation keeps static for 30s, and the drifting that caused by accumulation error is calculated for three individual sensors and the constellation. The constellation is represented by its barycenter. The field with LSM by average in Table 6.1 describes a method for determining the output value of the sensor when the sensor is static. It is meaning a sensor is static for 30 seconds, we gather all output values in this interval and calculate the average of those values. When we do calculation by our object tracking algorithm, every reading value from sensors will be subtract this average value. For example if the average value is x when the objects keeps for an interval. When we get the reading value from a sensor is y. The really output value

Figure 6.1: The vibratility of sensor 1 in a static state.

Figure 6.3: The vibratility of sensor 3 in a static state.

is y − x. This goal is to reduce the vibratility in sensor itself. The field with LSM by threshold. In Table 6.1 is another method to do this. This method is to set a threshold to reduce the vibratility in sensor itself. For example, if threshold sets as a value, the reading value from a sensor is b and the average is x. The really output value is |b − x| if |b − x| = a else 0 if |b − x| < a. Figure 6.4 shows the displacement and angle what threshold is better. The threshold is set from between 0.01 and 0.1. Finally, the threshold is set to 0.03 in our experiments. Table 6.1 records the displacement of each sensors and the constellation with LSM by average and LSM by threshold methods.We can see the

Table 6.1: The drifting distance of a static constellation after 30 seconds.

S1 S2 S3 LSM by average LSM by threshold

X 12.3 m 3.6 m 3.9 m 6.8 m 3.2 m

Y 5.2 m 3.1 m -0.8 m 2.6 m 2.5 m

Z 0.9 m 3.0 m 4.4 m 2.7 m 1.6 m

Figure 6.4: Displacement and angle with different thresholds.

6.2

Static Rotation Experiment

We also do the rotation tests. In a rotation test, rotate the constellation counterclockwise about the Z-axis by 30◦_{, 60}◦ _{and 90}◦ _{at static state. Thus the z-component of each A}

i,j

should be zero. Figure 6.6, Figure 6.8 and Figure 6.10 show the result of the rotation test with LSM by average We also do same tests with LSM by threshold Figure 6.7, Figure 6.9 and Figure 6.11 show the this result. Table 6.2 shows the rotational angle by Z − Y − X axis (yaw,pitch,roll) for all methods. Figure 6.5 shows the relation between time and angle. We can see that, after being calibrated by the LSM, the angle between the orientation of the initial and the final constellation is better than without LSM.

Table 6.2: The Yaw-Pitch-Roll angle by rotating 30◦, 60◦ and 90◦. Yaw Pitch Roll

30◦ with average 22.2 0 0.09 60◦ with average 50.6 0 0.09 90◦with average 79.6 0 0.09 30◦ with threshold 22.3 0 0.08 60◦ with threshold 51.7 0 0.08 90◦with threshold 79.8 0 0.08

Figure 6.5: Rotation along time. −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.1 0 0.1 3+ 3v 2* x 1+ 1* 2+ 2v 3* 1v y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.6: Rotate 30◦by Z-axis with average method. −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.1 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.1 0 0.1 3+ 3v 2* x 1+ 1* 2+ 3* 2v 1v y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.7: Rotate 30◦ by Z-axis with thresh-old method.

0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.1 −0.05 0 0.05 0.1 3+ 2* 3v x 2v 3* 1+ 2+ 1* 1v y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.8: Rotate 60◦by Z-axis with average method. 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 −0.02 0 0.02 0.04 0.06 2* 3+ 3v 3* 2v x 1+ 2+ 1v 1* y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.9: Rotate 60◦ by Z-axis with thresh-old method. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 −0.05 0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0 0.02 0.04 2* 2v 1* 3v x 3+ 1v 3* 1+ 2+ y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.10: Rotate 90◦ by Z-axis with aver-age method. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 −0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0 0.005 0.01 2* 2v 1* 3v x 3+ 1v 3* 1+ 2+ y z

cyan + : initial position red * : without LSM green v : with LSM

Figure 6.11: Rotate 90◦ by Z-axis with threshold method.

6.3

Dynamic Experiment in a Straight Line

Besides the static experiment, we also do some dynamic experiments. For example, the constellation moves about 45m in a straight line. The outcome of the straight line test is depicted in Figure 6.12 and Figure 6.13. Figure 6.12 shows the result of straight line with LSM by average method. Figure 6.13 shows the result of straight line with LSM by threshold method. From the reconstructed tracks, the constellation marked by the red curve moves almost in a straight line. However, the tracks of three individual sensors are far from a straight line. We also calculate how far sensors move. The moving distances of the sensors and the constellation are is calculated bypx2 _{+ y}2_{+ z}2_{, and the outcomes} are listed in Table 6.3.

0 20 40 60 80 100 120 140 0 10 20 30 40 50 60 −100 0 100 x y z blue + : sensor 1 cyan * : sensor 2 green x : sensor 3 red − : LSM

Figure 6.12: The track of a striaght line movement by average method.

0 20 40 60 80 100 120 140 0 10 20 30 40 50 60 −100 0 100 x y z blue + : sensor 1 cyan * : sensor 2 green x : sensor 3 red − : LSM

Figure 6.13: The track of a striaght line movement by threshold method.

Table 6.3: The distance measured after the constellation moves 45m in a straigt line.

S1 S2 S3 LSM

Distance by average 150m 57m 74m 49.5m Distance by threshold 148m 56m 72m 43.3m

6.4

Dynamic Experiment in a S-curve

Finally we do a dynamic moving and rotation test. For example, the constellation moves about a S-curve. The outcome of the S-curve test is depicted in Figure 6.14 and Figure 6.15. Figure 6.14 shows the result of S-curve with LSM by average method. Figure 6.15 shows the result of S-curve with LSM by threshold method. From the reconstructed tracks, the constellation marked by the red curve moves almost in a S-curve. Senor1, senor 2 and sensor 3 are far from a S-curve. The constellation is smooth better than three individual sensors. −40 −30 −20 −10 0 10 20 30 40 −20 0 20 40 60 80 100 120 140 160 −100 −50 0 50 x y z blue + : sensor 1 cyan * : sensor 2 green x : sensor 3 red − : LSM

Figure 6.14: The track of a S-curve move-ment by average method.

−30 −20 −10 0 10 20 30 −20 0 20 40 60 80 100 120 140 160 −100 −50 0 50 x y z blue + : sensor 1 cyan * : sensor 2 green x : sensor 3 red − : LSM

Figure 6.15: The track of a S-curve move-ment by threshold method.

Chapter 7

Conclusions

In this work, we proposed an inexpensive IMU solution called g-sensor constellations in which only accelerometers are used. It does not need gyroscopes and magnetometers to detect motion directions and the rotations of objects. The proposed technique can be applied to many applications, e.g. vehicle accident reconstruction, dead reckoning for PNSs, or human-computer interfaces. In the future, we will extend this work for constellations composed of more than three sensors and give analysis of accuracy. From our experiments, we also found that inaccurate calibration of sensors may cause significant accumulation error. Therefore, it is important to integrate calibration algorithm into the system.

Bibliography

[1] D. M. Karantonis, M. R. Narayanan, M. Mathie, N. H. Lovell, and B. G. Celler, “Im-plementation of a real-time human movement classifier using a triaxial accelerome-ter for ambulatory monitoring,” IEEE Transactions on Information Technology in Biomedicine, vol. 10, no. 1, pp. 156–167, January 2006.

[2] A. Ofstad, E. Nicholas, R. Szcodronski, and R. R. Choudhury, “AAMPL: accelerom-eter augmented mobile phone localization,” in Proceedings of the first ACM interna-tional workshop on Mobile entity localization and tracking in GPS-less environments (MELT 2008), 19 September 2008, pp. 13–18.

[3] D. Alvarez, R. C. Gonz´alez, A. M. L´opez, and J. C. Alvarez, “Comparison of step length estimators from weareable accelerometer devices,” in Proceedings of the 2006 28th Annual International Conference of the IEEE Engineering in Medicine and Biology Society (EMBS 2006), 30 August - 3 September 2006, pp. 5964–5967. [4] J. C. Alvarez, R. C. Gonz´alez, D. Alvarez, A. M. L´opez, and J. Rodr´ıguez-Ur´ıa,

“Multisensor approach to walking distance estimation with foot inertial sensing,” in Proceedings of the 2007 29th Annual International Conference of the IEEE En-gineering in Medicine and Biology Society (EMBS 2007), 22-26 August 2007, pp. 5719–5722.

[5] A. M. Sabatini, C. Martelloni, S. Scapellato, and F. Cavallo, “Assessment of walking features from foot inertial sensing,” IEEE Transactions on Biomedical Engineering, vol. 52, no. 3, pp. 486–494, March 2005.

[6] K. Sagawa, H. Inooka, and Y. Satoh, “Non-restricted measurement of walking dis-tance,” in Proceedings of the 2000 IEEE International Conference on Systems, Man, and Cybernetics, 8-11 October 2000, pp. 1847–1852 vol.3.

[7] X. Yun, E. R. Bachmann, H. Moore, and J. Calusdian, “Self-contained position tracking of human movement using small inertial/magnetic sensor modules,” in Pro-ceedings of the 2007 IEEE International Conference on Robotics and Automation, 10-14 April 2007, pp. 2526–2533.

[8] T. S. M. Hide, C. Moore, “Adaptive kalman filtering algorithms for integrating gps and low cost ins,” in Position Location and Navigation Symposium, 2004. PLANS 2004, 26-29 April 2004, pp. 227 – 233.

[9] B. A. B. A. A. Rezaie, J. Moshiri, “Gps/ins integration using nonlinear blending filters,” in SICE, 2007. Annual Conference, 17-20 Sept. 2007, pp. 1674 – 1680. [10] C.-M. H. Dah-Jing Jwo, “An adaptive fuzzy strong tracking kalman filter for gps/ins

navigation,” in Industrial Electronics Society, 2007. IECON 2007. 33rd Annual Con-ference of the IEEE, 5-8 Nov. 2007, pp. 2266 – 2271.

[11] S. U. Byung-Uk Lee, “Unit quaternion representation of rotation,” 1991.

[12] C. S. o. M. U. Murray, G., “Rotation about an arbitrary axis in 3 dimensions,” 2005, pp. 1–5.

[13] D. Steven J. Leon, University of Massachusetts, “Linear algebra,” 2003, p. chapter 4.

[14] E. G. S. W. Peter Michael Brodie, Oakland, “Simplified strapped down inertial nav-igation utilizing bnag-bang gyro torquing,” in US patent, 26 July 1997.

[15] J. D. P. Demoz Gebre-Egziabher, Gabriel H. Elkaim and B. W. Parkinson, “A gyro-free quaternion-based attitude determination system suitable for implementation us-ing low cost sensors,” in Position Location and Navigation Symposium, IEEE 2000, 13-16 March 2000, pp. 185–192.

[16] E. R. B. Xiaoping Yun and R. B. McGhee, “A simplified quaternion-based algorithm for orientation estimation from earth gravity and magnetic field measurements,” in IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, VOL. 57, NO. 3, March 2008, pp. 638 – 650.

[17] B. Sch. of Eng., Sussex Univ., “Adaptive kalman filters for manoeuvring target track-ing,” in Target Tracking and Data Fusion (Digest No. 1998/282), IEE Colloquium on, 9 June 1998, pp. 4/1 – 4/7.

[18] L. S. C. Chen J. H. and D. D. B., “Gyroscope free strapdown inertial measurement unit by six linear accelerometers,” in AIAA Vol. 17, No.2, March-April 1994, pp. 286–290.