Development and Testing of Performance Assessment Equipment for Elite Fencers

Development and Testing of Performance Assessment Equipment for Elite Fencers

YU GE

KTH

SKOLAN FÖR ELEKTROTEKNIK OCH DATAVETENSKAP

Performance Assessment Equipment for Elite Fencers

YU GE

Master in Information and Network Engineering Date: September 23, 2019

Supervisor: Magnus Jansson, Håkan Carlsson, Johan Harmenberg Examiner: Magnus Jansson

School of Electrical Engineering and Computer Science

Abstract

The main purpose of this thesis is to use a low-quality inertial sensor, which only provides 3-D acceleration and 3-D angular velocity, to have good pose estimation results, which is to track the orientation, the velocity and the dis- placement of some fencing relevant movements. This thesis focuses on two main parts, algorithms for pose estimation and algorithms for re-calibration of both the accelerometer and the gyroscope.

Three algorithms for pose estimation are proposed, including dead reck- oning, extended Kalman filter with zero-velocity updates and marginalized particle filter with zero-velocity updates. The dead reckoning is a very ba- sic algorithm, which just integrates the measurements to get the result. The other two algorithms use measurements and the zero-velocity as observations and implement the extended Kalman filter or the marginalized particle filter to get the result. The algorithms are evaluated and compared with each other via experiments. The results demonstrate that the extended Kalman filter with zero-velocity updates algorithm performs best among the three proposed al- gorithms.

Two algorithms for re-calibrating the sensor are proposed, including the ellipsoid fitting algorithm for the accelerometer re-calibration and an algo- rithm for the gyroscope re-calibration. They estimate the biases, scales and orthogonal errors for the accelerometer, and these parameters together with misalignment errors for the gyroscope. These algorithms do not require any external equipment, only require the sensor to be moved into a set of differ- ent positions. The performances of these two algorithms are also successfully evaluated.

Sammanfattning

Huvudsyftet med denna avhandling är att använda en tröghetssensor av låg kvalitet, som endast ger 3D-acceleration och 3D-vinkelhastighet, för att erhål- la en bra skattning av pose, det vill säga att skatta orienteringen, hastigheten och förflyttningen vid vissa rörelser. Denna avhandling fokuserar på två hu- vuddelar, algoritmer för pose-uppskattning och algoritmer för omkalibrering av både accelerometern och gyroskopet.

Tre algoritmer för pose-uppskattning föreslås, inklusive dödräkning, utö- kat Kalmanfilter med nollhastighetsuppdateringar och marginaliserat partikel- filter med nollhastighetsuppdateringar. Dödräkning är en mycket grundläggan- de algoritm som bara integrerar mätningarna för att få resultatet. De andra två algoritmerna använder mätningar och nollhastigheten som observationer och implementerar det utökade Kalmanfiltret eller det marginaliserade partikelfilt- ret för att få resultatet. Algoritmerna utvärderas och jämförs med varandra via experiment. Resultaten visar att det utökade Kalmanfiltret med nollhastighets- uppdateringsalgoritmen fungerar bäst bland de tre föreslagna algoritmerna.

Två algoritmer för omkalibrering av sensorerna föreslås, inklusive en el- lipsoidanpassningsalgoritm för acceleratoromkalibrering och en algoritm för kalibrering av gyroskopet. De uppskattar medelvärdesfel, skalfel och ortogo- nala fel för accelerometrar, och motsvarande parametrar för gyroskopet till- sammans med parametrar för fel mellan accelerometer- och gyroskopkoordi- nater. Dessa algoritmer kräver ingen extern utrustning, de kräver endast att sensorn flyttas mellan en uppsättning av olika positioner. Resultaten från des- sa två algoritmer är också framgångsrikt utvärderat.

Acknowledgements

I would first like to express my special thanks of gratitude to my supervi- sors at KTH Prof. Magnus Jansson and Håkan Carlsson. Their feedback and continuous support helped me with the research and writing of this thesis. I am also very grateful to my supervisor Prof. Johan Harmenberg for giving me this golden opportunity to do this wonderful project that may change the fencing sport and providing advice and feedback on my work.

Furthermore, I would like to thank Dr. John-Olof Nilsson for helping me out with the hardware and their previous research Open Shoes are very im- pressive. I also want to direct a particular "thanks" to Chen Xiao for helping me do the verification using the Vicon system. I also want to thank Prof.

Anthony Turner and his colleagues for extending my research into future val- idation study.

Finally, I want to express my gratitude to my family and friends, my girl- friend for the encouragement and support through my years of study. This accomplishment would not have been possible without them.

Stockholm, July 12, 2019 Yu Ge

1 Introduction 1

1.1 Related work . . . 2

1.2 Research gap . . . 2

1.3 Research questions . . . 3

1.4 Application . . . 3

1.5 Outline . . . 3

2 Background 5 2.1 Inertial sensor . . . 5

2.2 Current device . . . 6

2.3 Coordinate frames . . . 7

2.4 Rotation matrix . . . 8

2.5 Euler angles . . . 9

2.6 Unit quaternions . . . 10

2.7 Acceleration . . . 11

2.8 Angular velocity . . . 12

2.9 General measurement models . . . 12

2.10 Gradient descent . . . 14

2.11 Vicon system . . . 14

2.12 Rigid transform . . . 15

2.13 Linear interpolation . . . 16

3 Filtering 17 3.1 Extended Kalman filter(EKF) . . . 17

3.1.1 Kalman filter . . . 17

3.1.2 Extended Kalman filter . . . 18

3.2 Marginalized particle filter(MPF) . . . 19

3.2.1 Particle filter . . . 19

3.2.2 Marginalized particle filter . . . 21

vi

4 Pose estimation 24

4.1 Dead reckoning . . . 24

4.2 Extended Kalman filter with zero-velocity updates . . . 26

4.2.1 State-space model . . . 26

4.2.2 Detection of stationarity . . . 27

4.2.3 Pose estimation using EKF . . . 27

4.3 MPF with zero-velocity updates . . . 28

5 Re-calibration 30 5.1 Ellipsoid fitting . . . 30

5.2 Gyroscope re-calibration . . . 32

6 Results and discussion 35 6.1 Pose estimation . . . 35

6.2 Real fencing test . . . 45

6.3 Ellipsoid fitting . . . 48

6.4 Gyroscope re-calibration . . . 49

6.5 Bike wheel test . . . 51

7 Conclusions 57 7.1 Future work . . . 57

Bibliography 58 A Something extra 60 A.1 Conversions between different parametrizations . . . 60

A.2 EKF with zero-velocity updates . . . 61

A.3 MPF with zero-velocity updates . . . 62

Introduction

In fencing competitions, fencers need to extend their arms to get the oppor- tunities to score the hit and reach a highest possible speed through the critical distance, because it limits the opponents’ choices. During this time, the arm movements are often combined with some foot movements, like the lunge.

The maximum arm extension speed is faster than the leg speed, but the dis- tance travelled is much smaller and the hit speed drops dramatically when the arm is fully extended. There is no doubt that a good combination of arm and leg movements should help fencers reach a higher speed. The current training just trains the movements separately, but always ignores training the combi- nation movements. Therefore, the summation of the highest speeds of the two movements is always much larger than the real speed of the combination movement. Maybe the current training method should be improved, which would focus more on training combination movements rather than the basic movements separately.

This thesis work aims at providing a reasonable and economical way for the Swedish Olympic Academy to track some fencing relevant movements and measure the highest possible speed at the moment of scoring, which can be used in the real fencing application; probably, this thesis work may change a sport. The thesis mainly works on proposing algorithms and evaluating them.

A full evaluation in the fencing application is beyond the scope of the thesis and it will be pursued by others.

Inertial sensors can measure the acceleration and the angular velocity, which can be used in the pose estimation. After undergoing major developments over recent years, they became smaller, lighter and more available. However, the quality of sensors greatly depends on their prices. Some costly sensors can get good measurements and even have some additional useful systems, but

1

other low-price sensor measurements always suffer from high noises and the imperfect pre-calibration.

The goal of this thesis is to use a low-quality inertial sensor to acquire good pose estimation results and use some hands-on tools to re-calibrate the sensor. Three algorithms for pose estimation including the dead reckoning, the extended Kalman filter with zero-velocity updates and the marginalized particle filter with zero-velocity updates, and two algorithms for re-calibration, the ellipsoid fitting algorithm and an algorithm for the gyroscope re-calibration will be introduced.

1.1 Related work

There are a large number of papers using different kinds of sensors to do pose estimation. In [1], a sensor which combines the IMUs with a GPS re- ceiver and a magnetometer was used. The smoothing optimization, the fil- tering optimization and the extended Kalman filter algorithms have been pre- sented. In [2], an inertial sensor with a camera was used and the particle filter has been introduced to solve the pose estimation problem. A foot-mounted, zero-velocity-update aided inertial navigation system was presented in [3], [4]

and [5].

There are also many practical calibration algorithms, which do not need any special traditional mechanical platforms. In [6], the IMU was rotated into different orientations, where the norms of the inputs were equal to the local gravity. The maximum likelihood estimation method was used to calibrate the accelerometer. In [7] and [8], a similar method was used to calibrate the gyroscope, but only with the help of a simple rotating table which provided the constraint of constant angular velocity.

1.2 Research gap

For the pose estimation, most of the previous work either uses better iner- tial sensors, which not only provide the 3-D acceleration and 3-D angular ve- locity, but also contain additional systems, like GPS, magnetometer or camera.

Or they have very good dynamic models or suitable assumptions for specific movements. For the re-calibration, most of the existing methods require addi- tional machines or accurate references, and many methods are unnecessarily costly.

1.3 Research questions

In this thesis, we just have a relatively cheap inertial sensor and want to find some methods that can make up the low-quality measurements from our low-price sensor and acquire reliable pose estimation results. The research question can be divided into two parts.

(a). The pose estimation using low-price inertial sensors: acquire the 3-D velocity and the 3-D position estimates of some movements just using the low- quality 3-D acceleration and 3-D angular velocity measured by some cheap sensors.

(b). The re-calibration without using any laboratorial machines: re-calibrate the low-price inertial sensor and estimate the error parameters just using some handy tools.

1.4 Application

The proposed algorithms will be used in the fencing application, or even more general sports. Researchers from the Swedish Olympic Academy will use the proposed techniques to assess the performance of elite fencers and provide suitable training methods for them. To be exact, they will use the tech- niques to track the fencing relevant movements and measure the highest pos- sible speed at the moment of scoring. Then they will assess the performance based on the results and potentially provide more pointed training guidance.

This thesis mainly works on proposing algorithms and on the evaluation of them. A full evaluation in the fencing application is beyond the scope of the thesis and it will be pursued by others.

1.5 Outline

The rest of the thesis continues as follows: Chapter 2 gives some back- ground knowledge of the hardware aspects of this thesis, including the current device we use, and some basic concepts and necessary measurement models.

The theories of the extended Kalman filter and the marginalized particle fil- ter will be explained in Chapter 3. In Chapter 4, three different algorithms for pose estimation will be introduced, including the dead reckoning, the extended Kalman filter with zero-velocity updates, and marginalized particle filter with zero-velocity updates. We will do re-calibration both for the accelerometer and the gyroscope using the ellipsoid fitting and the proposed gyroscope re-

calibration algorithms respectively in Chapter 5. The results for both pose estimation and re-calibration as well as the discussion and the analysis of the results will be presented in Chapter 6. Finally, Chapter 7 will summarize the results, conclude the thesis and propose some future research.

Background

This chapter provides an introduction to the hardware aspects of this thesis.

The sensor we use in this thesis will be introduced, also some basic concepts will be introduced to better understand the sensor readings. Moreover, the ba- sic measurement models for the 3D inertial sensor will be discussed in detail.

We also consider more general measurement models that contain some error parameters.

In Section 2.1, the term of the inertial sensor will be explained and the current sensor we use will be introduced in Section 2.2. After that, the basic description about different coordinate frames will be illustrated. Then, we will discuss the rotation matrix in Section 2.4. The different methods to represent the orientation, the Euler angles, and the unit quaternions will be introduced in Section 2.5 and 2.6, respectively. The following two sections will discuss the accelerometer and gyroscope measurement models, respectively, under the assumption of perfect calibration for accelerometers and gyroscopes. Section 2.9 will show more general measurement models for both accelerometer and gyroscope, in which some error parameters like biases, scales, misalignment errors, and non-orthogonal errors will be considered. Then the concept of gra- dient descent will be introduced in Section 2.10. Section 2.11 and Section 2.12 will briefly introduce the Vicon system and the rigid transform, respectively.

Finally, the linear interpolation will be described in Section 2.13.

2.1 Inertial sensor

The term of the inertial sensor is used to denote the combination of gyro- scopes and accelerometers, which provide the angular velocity and the specific acceleration, respectively[1]. After undergoing major developments over re-

5

cent years, they become smaller, lighter and more available. Therefore, it has been widely used in industry like quadcopters, smartphones and many others.

However, the quality of sensors greatly depends on their prices. Some costly devices usually can provide good measurements and even have some addi- tional useful systems like GPS and magnetometer, but other low-price sensor measurements always suffer from high noises and imperfect pre-calibration[9].

(a) quadcopter (b) smartphone

Figure 2.1: Some examples of devices containing inertial sensors

2.2 Current device

The sensor we use is Osmium MIMU22B9P(X) provided by Inertial Ele- ments. It has 4 MPU-9250 IMUs, supports both USB and Bluetooth interfaces.

The size is quite small, only 42.2mm × 27.9mm × 17.0mm, also very light, which would never affect users’ movements[10]. Figure 2.2 shows the PCB of the device, its components and the case.

(a) top side of the PCB (b) bottom side of the PCB (c) the completed device

Figure 2.2: PCB of the device, its components and the case

The sensor measures both 3-D angular velocity and 3-D acceleration, Fig- ure 2.3 is an example of sensor readings when the sensor is stationary.

(a) Gyroscope readings (b) Accelerometer readings

Figure 2.3: An example of sensor readings

The measurements are corrupted by some noises. Apart from the noises, the measurements would be also distorted by other types of errors, like bi- ases, scales, misalignment errors and non-orthogonal errors, which will be discussed in detail in Section 2.9.

2.3 Coordinate frames

The acceleration and angular velocity are usually studied in variable co- ordinate frames. Therefore, we need to understand some typical coordinate frames before looking into the problem. Figure 2.4 shows the relation among the navigation, earth and inertial frames intuitively.

The body frame b is the coordinate frame of the moving sensor. All the inertial measurements are described in this coordinate frame. It is aligned to the frame of the accelerometer in the inertial sensor.

The navigation frame n is the local geographic frame where we want to navigate. For example, if we want to track the movement of an indoor robot, the frame of the room will be the navigation frame. The navigation frame is usually stationary with respect to the earth. However, it could be rotated along- side the surface of the earth, if the reference moves a lot, like for an airplane.

In this thesis, we just consider the first case, since we are only interested in the pose estimation.

The earth frame e rotates with the earth. The origin is located at the center of the earth. Its axes are aligned with respect to the earth.

The inertial frame coincides with the earth frame, but it is stationary. Its origin is the same as the origin of the earth frame, while its axes are fixed with reference to the stars[1].

Figure 2.4: Navigation, body and inertial frames

2.4 Rotation matrix

A rotation matrix can be used to rotate a vector in R³from one coordinate frame into another coordinate frame. It only changes the coordinate frame

where the vector is, but the length of the vector is retained. It is a 3 × 3 matrix and has the following properties:

RR^T = I₃ = R^TR, detR = 1 (2.1)

For example,

aⁿ= R^nba^b (2.2)

The rotation matrix R^nb rotates the vector a in the body frame into the navigation frame, but the norm of the vector does not change. Conversely, R^bncan rotate the vector in the navigation frame into the body frame[11]

a^b = (R^nb)^Taⁿ= R^bnaⁿ (2.3)

2.5 Euler angles

We can define the rotation matrix as the combination of three rotation an- gles, (roll, pitch, yaw),(θ, φ, ψ), which are the Euler angles that rotate around x, y, z axis, respectively. Figure 2.5 illustrates these three Euler angles in de- tail.

Figure 2.5: From left to right, it shows the Euler angles yaw, pitch, roll, (ψ, θ, φ), which are the angles that rotate around the z, y, x axes, respectively.

Therefore, the rotation matrix can be expressed as the cascade of three rotations around the x, y, z axes.

R^nb = R^nb(e₁, φ)R^nb(e₂, θ)R^nb(e₃, ψ)

=





1 0 0

0 cos φ sin φ 0 − sin φ cos φ









cos θ 0 − sin θ

0 1 0

sin θ 0 cos θ









cos ψ sin ψ 0

− sin ψ cos ψ 0

0 0 1





=





cos θ cos ψ cos θ sin ψ − sin θ

sin φ sin θ cos ψ − cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ − sin φ cos ψ cos φ cos θ



 (2.4) where e1, e2, e3 are defined as unit vectors

e₁ = 1 0 0
T

, e₂ = 0 1 0
T

, e₃ = 0 0 1
T

(2.5) Although the rotation matrix can be represented in Euler angles, there are two main shortcomings of this method, wrapping and gimbal lock. Both re- sult in different groups of Euler angles having the same rotation matrix. The periodicity of Euler angles is the reason for the wrapping[12]. For example, the rotations (0, 0, 0) and (2π, 0, 0) are equivalent. Due to the same value of (φ−ψ), (^π₂,^π₂, 0) is equal to (π,^π₂,^π₂), which is a typical example of the gimbal lock.

2.6 Unit quaternions

In order to overcome the shortcomings of Euler angles mentioned in Sec- tion 2.5, the unit quaternions were introduced by Hamilton and now have been widely used in orientation estimation algorithms. It uses a 4-dimensional com- plex vector to represent the orientation, having the following properties[12]:

q = q₀ q₁ q₂ q₃
T

= q₀ q_v



, kqk₂ = 1 (2.6)

A vector can be rotated using unit quaternions as [12]

aⁿ = q^nb a^b (q^nb)^∗ (2.7) where (·)^∗is the quaternion conjugate

q^∗ =

 q₀

−q_v



(2.8)

 denotes the quaternion multiplication p  q =

 p₀q₀− p_v · q_v p₀q_v+ q₀p_v − p_v × q_v



= p^Lq = q^Rp (2.9)

We have

p^L=p₀ −p^T_v p_v p₀I₃+ [p_v×]



q^R=q₀ −q_v^T q_v q₀I₃− [q_v×]



(2.10) The cross product matrix is defined as

[pv×] =





0 −p₃ p₂ p₃ 0 −p₁

−p₂ p₁ 0



 (2.11)

From the properties above, an orientation can be easily expressed either as a unit quaternion qtor as a rotation matrix Rt. It can be also viewed as a unit quaternion ˆq_tand an orientation deviation ηt. For example, a new unit quater- nion q^nb_t can be expressed using the old unit quaternion ˆq_t^nband the orientation deviation vector η_t^b, like [12]

q^nb_t = ˆq^nb_t  exp(ηˆ_t^b

2) (2.12)

where

exp(η) = cos kηk₂

η

kηk₂sin kηk₂

!

(2.13)

2.7 Acceleration

The accelerometer measures the specific acceleration in the body frame, as a^b_t. If we assume the accelerometer is well calibrated, that means there are no biases, no scales or no misalignment errors, then the measurement only be corrupted by some noises. The measurement model for acceleration at each time instance t can be expressed as

y_a = a^b_t = R^bn_t (aⁿ_t − gⁿ) + e^b_a,t (2.14) where R^bn_t is the rotation matrix from the navigation frame into the body frame.

aⁿ_t is the acceleration in the navigation frame which is also what we want to use in the pose estimation. gⁿis the local gravity, which can be easily found

online or calculated using the local latitude and altitude. e^b_a,t represents both the noise and the uncertainty of the model.

The local gravity is a function of local latitude and altitude as [13]

g = (9.78032 + 0.005172sin²θ − 0.00006sin²(2θ))( r

r + z)² (2.15) where g is the local gravity, θ is the local latitude and z is the local altitude. r is the radius of the earth, which is approximately 6371km.

2.8 Angular velocity

The gyroscope measures the angular velocity in the body frame with re- spect to the inertial frame, as ω_ib,t^b . If we assume the gyroscope is well cali- brated, the measurement model for the gyroscope at each time instance t can be expressed as

y_ω = ω_ib,t^b = R^bn_t (ωⁿ_ie,t+ ω_en,tⁿ ) + ω_nb,t^b + e_ω,t (2.16) where R^bn_t is the rotation matrix from the navigation frame into the body frame.

ωⁿ_ie,tis the earth rotation rate, which is around 7.29 × 10⁻⁵rad/s. ω_en,tⁿ is the rate of the navigation frame with respect to the earth, which is 0 when the navigation frame is stationary or does not travel a long distance. ω_nb,t^b is the angular velocity in the body frame with respect to the navigation frame, which is what we want to observe. eω,t represents both the noise and the uncertainty of the model for the gyroscope.

Hence, we only consider the case when the navigation frame is stationary;

the earth rotation rate is quite negligible. Then the measurement model can be simplified as

y_ω = ω_ib,t^b ≈ ω^b_nb,t+ e_ω,t (2.17)

2.9 General measurement models

In Section 2.7 and 2.8, we assume the accelerometer and the gyroscope are well calibrated, and introduce the measurement models for them under that assumption. However, the sensor is not well calibrated in most cases, like the sensor we are using. Moreover, it is unavoidable that the sensor will be worn and torn after a long-term use. If we use such sensors to track relatively long-time movements, the integration drift would be very serious. Therefore, a reliable and precise re-calibration is necessary.

There are some error parameters that need to be observed. To be exact, the biases and scales mainly come from the inaccurate conversion from the output voltage into angular velocity and acceleration measurements inside the sensor.

The non-orthogonal errors are because the imperfect manufacturing makes the three sensitivity axes non-orthogonal to each other[6]. The misalignment errors are due to the coordinate frame of the accelerometer does not match the coordinate frame of the gyroscope[7]. In this thesis, we only consider these four types of error parameters.

We consider the three biases, three scales and three non-orthogonal errors in our accelerometer-measurement model and consider these nine parameters together with three misalignment errors in our gyroscope-measurement model.

Accelerometer-measurement model

y_a= S_aN_aa + b_a+ e_a (2.18) Gyroscope-measurement model

y_ω = S_ωM_ωN_ωω + b_ω+ e_ω (2.19) where,

yais the measured acceleration y_ωis the measured angular velocity a is the real acceleration

ω is the angular velocity e is the noise

b is the bias vector, as b_x b_y b_z
^T

The subscript_ameans the matrix or the vector is for acceleration.

The subscript_ω means the matrix or the vector is for angular velocity.

In (2.18) and (2.19), Naand Nω are the non-orthogonal error matrices. If we set the x axis as reference, we just need to observe three parameters, and the matrix will be





1 −θ_yz θ_zy

0 1 −θ_zx

0 0 1



 (2.20)

where θ_yzis the angle rotation of the z axis with respect to the y axis. Since it is very small, we can use the small angle approximation here.

S_aand S_ωare the scale matrices. They are diagonal matrices as





k_x 0 0 0 k_y 0 0 0 k_z



 (2.21)

where k is the scaling factor for the corresponding axis.

Mωis the misalignment error matrix between the accelerometer and gyro- scope. Usually, we just combine the misalignment errors with non-orthogonal errors together to create a new matrix with 6 small angle rotations, as





1 −φ_yz φ_zy φ_xz 1 −φ_zx

−φxy φyx 1



 (2.22)

where φ also represents the small angular rotation. Again, we use the small angle approximation here.

2.10 Gradient descent

The gradient descent is also known as the steepest descent. It is a first- order iterative optimization algorithm for finding the minimum of a function.

The gradient defines the direction in which the error increases the most. The gradient descent implies that a move in the opposite direction in the weight space should be taken[14].

For example, there is a task to find suitable parameter vector θ that mini- mizes the cost function , as

θ = arg min

θ



 = e^Te

2 = (t − f (θ))^T(t − f (θ)) 2

(2.23)

where t is the target.

Then the gradient is calculated as

∂

∂θ = ∂e^T

∂θ e = −(∂f (θ)

∂θ )^Te = −J^Te (2.24) where J represents the Jacobian matrix of f (θ)

If we set a suitable learning rate η, the new parameter vector θ would be

θ_k+1 = θ_k+ ηJ^Te (2.25)

2.11 Vicon system

Vicon Tracker is a powerful camera based object-tracking solution, provid- ing unrivalled data accuracy for integration into 3D applications. It has been

widely used in many areas. In this thesis, we will use the Vicon system to get some high-accuracy pose tracking results of some movements as references, which would be used to validate the proposed pose estimation algorithms.

2.12 Rigid transform

The rigid transform is the optimal transformation between two sets of cor- responding 3D point data[15]. Finding the rigid transform is a common prob- lem in rigid-body relevant problems. Figure 2.6 shows an illustration of the problem for the simplest case of three corresponding points, which is the min- imum required points to solve this kind of problems.

Figure 2.6: An illustration of finding the rigid transform between two sets of corresponding 3D point data

In this problem, we want to find the optimal rotation R and translation t that will align the points in dataset A to dataset B, as

P_B= RP_A+ t (2.26)

where P_A/P_B represents the points in the dataset A/B.

Here, "optimal" is in terms of the least square error; the following error is minimized:

err = 1 N

N

X

i=1

RP_Aⁱ + t − P_Bⁱ

2 (2.27)

2.13 Linear interpolation

Interpolation is a method of constructing new data points within the range of a discrete set of known data points. In practice, there are often a number of data points available, which are obtained by sampling or experimentation and represent the values of a function for some independent variables. It is often required to interpolate, that is to estimate the value of that function for an intermediate value of the independent variables[16].

Linear interpolation is one of the simplest methods of interpolation. Gen- erally, linear interpolation takes two data points, say (x_a, y_a) and (x_b, y_b), and the interpolant (x, y) is given by:

y = y_a+ (y_b− y_a)x − x_a

x_b− x_a (2.28)

Filtering

Filtering algorithms have been widely used in this kind of sensor related problems. In this chapter, we will introduce the extended Kalman filter and the marginalized particle filter briefly.

3.1 Extended Kalman filter(EKF)

The extended Kalman filter is the nonlinear version of a Kalman filter, and has numerous applications in technology, like the control of spacecraft and the GPS based navigation. In this section, we will firstly give a brief idea about the Kalman filter and introduce the EKF.

3.1.1 Kalman filter

Kalman filtering is an algorithm that can produce some unknown estimates by estimating the joint probability distribution using a number of sets of ob- servations over time[17]. This algorithm is optimal if all errors are Gaussian, and the space-state model of the Kalman filter should be linear, as

x_t = F_k−1x_t−1+ G_k−1e_t−1

y_t= H_kx_t+ v_t (3.1)

where the process noise e and the measurement noise v are zero mean multi- variate Gaussian noises, with e ∼ (0, Q) and v ∼ (0, R), respectively.

The algorithm works in two steps. Firstly, it produces estimates of the current state variables, along with their uncertainties using the time update.

Secondly, these estimates are updated using a weighted average, when the next outcome is observed. Then just repeat these two steps. Because all the

17

probability distributions are viewed as Gaussian, we just need to estimate the mean-value vectors and the covariance matrices.

The time update is ˆ

x_t|t−1 = F_k−1xˆ_t−1|t−1

P_t|t−1= Ft−1P_t−1|t−1F_t−1^T + Gt−1Qt−1G^T_t−1 (3.2) The measurement update is

ye_t= y_t− H_kxˆ_t|t−1

K_t= P_t|t−1H_t^T(H_tP_t|t−1H_t^T + R_t)⁻¹ ˆ

x_t|t= ˆx_t|t−1+ K_tey_t P_t|t = (I − K_tH_t)P_t|t−1

(3.3)

where,

the subscriptameans the estimation is for time a.

the subscript_a|bmeans the estimation is for time a given measurements upto and including time b.

ˆ

x is the estimated state vector.

P is the estimated state covariance matrix.

ey is the innovation or the measurement residual.

K is the Kalman gain.

Then the algorithm is given as Algorithm 1.

Algorithm 1 Kalman filter

Input: measurement readings (y_t)^N₁

Output: state vectors (x_t)^N₀ and covariance matrices (P_t)^N₀

1: Initialization: set x_0|0, P_0|0, Q₀, R₀

2: for N ≥ t ≥ 1 do

3: Time update: using (3.2)

4: Measurement update: using (3.3)

5: end for

3.1.2 Extended Kalman filter

There is the requirement that the state-space model of the Kalman filter should be a linear model. Therefore, the extended Kalman filter is introduced for more general models, which is the non-linear version of a Kalman filter and uses the first-order Taylor expansion to do the linearization[17].

In the extended Kalman filter, the non-linear state-space model is x_t= f (x_t−1, e_t−1)

y_t= h(x_t) + v_t (3.4)

These two algorithms are very similar. Only the time update and the mea- surement update have few differences.

The time update is ˆ

x_t|t−1 = f (ˆx_t−1|t−1)

P_t|t−1 = F_t−1P_t−1|t−1F_t−1^T + G_t−1Q_t−1G^T_t−1 (3.5) The measurement update is

eyt= yt− h(ˆxt|t−1)

K_t = P_t|t−1H_t^T(H_tP_t|t−1H_t^T + R_t)⁻¹ ˆ

x_t|t = ˆx_t|t+ K_tey_t P_t|t= (I − K_tH_t)P_t|t−1

(3.6)

Since we use the first-order Taylor expansion to do the linearization, F, G and H are defined as some Jacobian matrices.

F = ^{df (x,e)}_dx |^e=0

x=ˆxt−1|t−1

G = ^{df (x,e)}_de |^e=0

x=ˆxt−1|t−1

H = ^dh(x)_dx |_x=ˆxt|t−1

3.2 Marginalized particle filter(MPF)

In the Kalman filter, the distributions are viewed as Gaussian and the state- space model should be linear or need linearization. However, the state-space model is usually non-linear, and the probability distributions of the errors are not always Gaussian. For these nonlinear, non-Gaussian filtering problems, the particle filter can be used. Since this algorithm is not very suitable for some high dimensional systems, the marginalized particle filter can be considered.

In this section, we will introduce the basic particle filter and the marginalized particle filter.

3.2.1 Particle filter

The particle filter has the same state-space model as (3.4). However, unlike the extended Kalman filter, there is no need to do the linearization and the noise

distribution could be any form. As a Monte Carlo algorithm based filter, the particle filter uses a set of random samples with associated weights to represent the required posterior distribution and compute estimates[17]

p(x_0:t|y_1:t) ≈

N

X

i=1

w_tⁱδ(x_0:t− xⁱ_0:t) (3.7)

where p(x_0:t|y_1:t) is the posterior distribution at time t. (xⁱ_0:t, wⁱ_t)^N_i=1is a set of random samples with associated weights. The weights are already normalized asPN

i=1wⁱ_t = 1.

These samples and corresponding weights can be selected using impor- tance sampling. For the principle of importance sampling, we introduce a suitable proposal density q(x). Then the distribution g(x) can be expressed as an expectation of those samples:

E[g(x)] = Z

g(x)p(x)dx = Z

g(x)p(x)

q(x)q(x)dx ≈

N

X

i=1

g(x_i)w_i (3.8)

where xi is drawn from the proposal density q(x) and wi = _{N q(x}^p(xi)

i). (xi, w_i) is called the particle distribution. Equivalently, the distribution p(x) could be estimated by

p(x) ≈

N

X

i=1

wiδ(x − xi) (3.9)

The weights can be normalized as

w_i = wi

PN n=1wn

(3.10) Then the measurement update of the particle filter is

w_t|tⁱ = w_t|t−1ⁱ p(yt|xⁱ_t) w_t|tⁱ = w_t|tⁱ

PN n=1wⁿ_t|t

(3.11)

The time update of the particle filter is

w_t+1|tⁱ = w_t|tⁱ p(xⁱ_t+1|xⁱ_t)

q(xⁱ_t+1|xⁱ_t, y1:t) (3.12) where the particles xⁱ_t+1are generated from the proposal distribution q(x_t+1|xⁱ_t, y_1:t).

After several iterations, most of the particles might just have negligible weights. That means only a few particles would contribute to the result and most of the computational effort would be paid on updating particles with almost no contribution. This is what is called the degeneracy problem[17]. We can use the resampling method to reduce its effect. Given particles (x_i, w_i)^N, we can test if weights before resampling are too different by using

Nˆ_{ef f} = 1

P(wⁿ)²  N (3.13)

if it is too different, we can draw new particle values with replacement from the set (xi, w_i)^N with distribution p(x = xi) = w_i.

Then the new samples have

p(x) =

N

X

m=1

w_mδ(x − x_m) (3.14)

where wm = _N¹.

Then the algorithm is given as Algorithm 2.

Algorithm 2 Particle filter

Input: measurement readings (y_t)^N₁ Output: state vectors (x_t)^N₁

1: Initialization: Generate K particles x^k₁ ∼ p_x0, w^k_1|0 = _K¹

2: for N ≥ t ≥ 1 do

3: Measurement update: use (3.11)

4: Resampling(optional)

5: Time update: generate prediction values from proposal distribution x^k_t+1 ∼ q(x_t+1|x^k_t, y_1:t)

update weights using (3.12)

6: end for

3.2.2 Marginalized particle filter

The particle filter is very costly, when it is applied to some high dimen- sional systems. Because the required number of samples increases exponen- tially with the dimensions. Therefore, we need to keep the state dimension small enough for the particle filter to be practical.

Usually, in some very high dimensional state-space model, it is not totally non-linear, and contains a linear part. Therefore, we can take the whole state

vector into two parts, a linear part and a non-linear part, as x_t= ((xⁿ_t)^T, (x^l_t)^T)^T[18].

Then the state-space model can be described as

xⁿ_t = f_t−1ⁿ (xⁿ_t−1) + F_t−1ⁿ (xⁿ_t−1)x^l_t−1+ Gⁿ_t−1(xⁿ_t−1)eⁿ_t−1 x^l_t= f_t−1^l (xⁿ_t−1) + F_t−1^l (xⁿ_t−1)x^l_t−1+ G^l_t−1(xⁿ_t−1)e^l_t−1 y_t= h_t(xⁿ_t) + H_t(xⁿ_t)x^l_t+ v_t

(3.15)

where x_t= xⁿ_t x^l_t



, v_t = v_tⁿ v_t^l



∼ (0, R_t), Q_t= Qⁿ_t Q^ln_t Q^nl_t Q^l_t



Then we can apply the particle filter theories on the non-linear part if fixing the linear part, and apply the Kalman filter theories on the linear part when fixing the non-linear particles[19]. Then the algorithm is given as Algorithm 3.

Algorithm 3 Marginalized particle filter Input: measurement readings (yt)^N₁ Output: state vectors (xt)^N₁

1: Initialization:

(a) Generate K particles x^n,k₁ ∼ p_xⁿ

0, w_1|0^k = _K¹ (b) Set (x^l,k_1|0, P_1|0^k , Q^k₀, R^k₀) = (x^l₀, P0, Q0, R0)

2: for N ≥ t ≥ 1 do

3: PF measurement update: use (3.11)

4: Resampling(optional)

5: PF time update and KF measurement and time updates:

(a) Kalman filter measurement update for each particle:

K_t = P_t|t−1H_t^T(H_tP_t|t−1H_t^T + R_t)⁻¹ P_t|t= (I − K_tH_t)P_t|t−1

yet= yt− Hkxˆ^l_t|t−1− ht(xⁿ_t) ˆ

x_t|t = ˆx_t|t−1+ K_tye_t

(b) Particle filter time update:Generate prediction values from proposal distribution x^n,i_t+1∼ q(xⁿ_t+1|x^n,i_t , y_1:t), update weights using (3.12) (c) Kalman filter time update for each particle:

ˆ

x^l_t+1|t = f_t^l(xⁿ_t) + ¯F_t^lxˆ^l_t|t+ G^l_tQ^nl_t (Gⁿ_tQⁿ_t)⁻¹z_t+ L_t(z_t− F_tⁿxˆ^l_t|t) P_t+1|t= ¯F_t^lP_t|t( ¯F_t^l)^T + G^l_tQ¯^l_t(G^l_t)^T − L_tN_tL^T_t

N_t = F_tⁿP_t|t(F_tⁿ)^T + Gⁿ_tQⁿ_t(Gⁿ_t)^T L_t = ¯F_t^lP_t|t(Aⁿ_t)^TN_t⁻¹

where, zt = xⁿ_t+1− f_tⁿ(xⁿ_t) F¯_t^l = F_t^l− G^l_tQ^nl_t (Gⁿ_tQⁿ_t)⁻¹F_tⁿ Q¯^l_t = Q^l_t− Q^nl_t (Qⁿ_t)⁻¹Q^ln_t

6: end for

Pose estimation

We have the 3-D acceleration and the 3-D angular velocity provided by the inertial sensor. We need to use these measurements to track the velocity and the position, that is so called the pose estimation. In this chapter, we assume the sensor is well calibrated and use measurement models in Section 2.7 and 2.8. Three algorithms will be introduced.

In Section 4.1, we will introduce the basic algorithm, dead reckoning.

Then a more advanced algorithm the extended Kalman Filter with zero-velocity updates will be introduced in Section 4.2. Finally, we will also apply the marginalized particle filter into pose estimation in Section 4.3.

4.1 Dead reckoning

The dead reckoning is the most basic algorithm in pose estimation. The theory is very obvious. The integration of the angular velocity gives the ori- entation. The integration and double integration of the acceleration in the navigation frame give the velocity and the position, respectively, as shown in figure 4.1.

24

Figure 4.1: The block diagram of dead reckoning

In this thesis, the sampling rate is high, so we can discretize the dead reck- oning as[5]



 pⁿ_t vⁿ_t q_t^nb



=





pⁿ_t−1+ T v_t−1ⁿ +^T₂²(R^nb_t−1a^b_t−1+ gⁿ+ e_a,t−1) v_t−1ⁿ + T (R^nb_t−1aⁿ_t−1+ gⁿ+ e_a,t−1)

q_t−1^nb  exp(^T₂(ω_t−1^b + e_ω,t−1))



 (4.1)

where t is the time index. T is the sampling time difference. p is the position.

v is the velocity. R is the rotation matrix. q is the unit quaternion. g is the local gravity. a is the accelerometer reading and ω is the gyroscope reading.

e is the corresponding noise.

For the initial values, we assume the sensor is stationary at the beginning and the origin coincides with the starting point, then pⁿ₀ = 0, vⁿ₀ = 0. Since the sensor is stationary, the accelerometer only measures the local gravity, as

ya = a^b₀ = R^bn₀ (−gⁿ) = R^bn₀



 0 0

−g



= −g





−sinθ sinφcosθ cosφcosθ



 (4.2)

θ, φ can be easily derived from accelerometer readings. We set ψ = 0. Then, we can convert Euler angles (φ, θ, ψ) into q₀^nb. The conversion can be found in the Appendix.

The dead reckoning algorithm is given as Algorithm 4.

Algorithm 4 Dead reckoning Input: measurement readings

 a^b_t ω^b_t

^N

Output: state vectors x_t=



 pⁿ_t vⁿ_t q_t^nb





1: Initialize the state: set pⁿ₀ = 0, v₀ⁿ= 0, calculate q₀^nbfrom readings

2: for N ≥ t ≥ 1 do

3: x_t= f (x_t−1, a^b_t−1, ω_t−1^b ) using (4.1)

4: end for

This method will give us perfect pose estimation results only if all the mea- surements are ideal. However, there are always some noises and other types of errors that corrupt the measurements, like what we mentioned in Section 2.9.

Therefore, this method always suffers from the leakage of gravity, which is due to the estimated orientation does not match the orientation of the gravity.

The result will just drift away rapidly. We need to apply better algorithms to alleviate these problems.

4.2 Extended Kalman filter with zero-velocity updates

The extended Kalman filter was introduced before. In this section, we want to apply the extended Kalman filter into the pose estimation problem.

4.2.1 State-space model

In order to apply the EKF into pose estimation, we need to create a suitable model for our problem.

Our state-space model is based on the dead reckoning. However, unlike the dead reckoning using the measurements directly, we view the acceleration in the navigation frame and the angular velocity as random walks, which means the derivatives are Gaussian. The observations are the sensor measurements, which are in the body frame.

Our state-space model is











 pⁿ_t v_tⁿ aⁿ_t q_t^nb ω_t













=













pⁿ_t−1+ T vⁿ_t−1+ ^T₂²aⁿ_t−1+ e_p,t−1 v_t−1ⁿ + T aⁿ_t−1+ e_v,t−1

aⁿ_t−1+ e_a,t−1 q^nb_t−1 exp(^T₂ω_t−1)

ω_t−1+ e_ω,t−1













 a^b,obs_t ω^b,obs_t



= R^bn_t (aⁿ_t − gⁿ) + s_a,t ω_t+ s_ω,t



(4.3)

Apart from the sensor readings, we have an additional observation, the zero velocity. To be exact, when we stand still, the sensor is stationary and the velocity should be zero[3]. Then the observation model should be



 a^b,obs_t ω_t^b,obs 0



=





R^bn_t (aⁿ_t − gⁿ) + s_a,t ω_t+ s_ω,t v_tⁿ+ s_v,t



 (4.4)

where a^b,obs and ω^b,obs are the sensor readings; s are the corresponding mea- surement noises.

4.2.2 Detection of stationarity

We have different observation equations for stationary and nonstationary cases. Therefore, we need to find a reasonable method to detect whether the system is stationary or not. This can be done by hypothesis testing, as[20]

1 N

X

k∈Wc

( 1 σ_a²

a_k− g a¯_c k ¯a_ck

2

+ 1

σ²_ω kω_kk²) < γ (4.5) where k is the time index. ¯a_cis the mean acceleration over time window Wcof N-sample length center around index c. σ_aand σ_ω are the standard deviations for acceleration and angular velocity measurements. γ is the threshold.

The system will use the current acceleration and angular velocity to cal- culate a new value using (4.5). Then compare this value with the threshold γ.

If it is smaller than γ, we view the sensor is stationary at this time interval.

Otherwise, it is not.

4.2.3 Pose estimation using EKF

We introduced the EKF and set our own state-space model in the beginning of this section. We will combine them together to show how our algorithm works in detail. The algorithm is given as Algorithm 5.

Algorithm 5 EKF with zero-velocity updates Input: measurement readings

 a^b_t ω^b_t

^N

Output: state vectors xt = pⁿ_t vⁿ_t aⁿ_t q_t^nb ωt

T

and covariance ma- trices Pt

1: Detect the stationarity using hypothesis testing as (4.5)

2: Initialization: set x₀, P₀, Q, R_sand R_u

3: for N ≥ t ≥ 1 do

4: Time update:

ˆ

x_t|t−1= f (ˆx_t−1|t−1)

P_t|t−1 = F_tP_t−1|t−1F_t^T + G_tQ_tG^T_t f (·) is the prediction model in (4.3)

5: if The sensor is nonstationary then

6: Measurement update:

ye_t= y_t− h(ˆx_t|t−1)

Kt = Pt|t−1H_t^T(HtPt|t−1H_t^T + Rt)⁻¹ ˆ

x_t|t = ˆx_t|t+ K_tey_t P_t|t= (I − K_tH_t)P_t|t−1

h(·)is the observation model in (4.3)

7: else

8: Measurement update: same as step 6, but h(·) is the observation model in (4.4)

9: end if

10: Re-normalize the unit quaternion and its convariance matrix:

ˆ

q^nb_t|t = ^q¯

nb t|t

q¯_t|t^nb

2

and P_t|t= J_tP¯_t|tJ_t^T, here J_t= ¹ ¯q_t|t^nb

3 2

¯

q_t|t^nb(¯q_t|t^nb)^T

11: end for

The details about calculating the Jacobian matrices F, G, H, Q, R for this algorithm can be found in the Appendix.

4.3 MPF with zero-velocity updates

We introduced the MPF before. In this section, it would be applied into the pose estimation problem. The algorithm is given as Algorithm 6.