lecture sabatini final...29/08/15 6...

$: Lecture Sabatini final...29/08/15 6 Representation(of(orientation(We*need*to*master*the*coordinate*transformation*ofagenericvector x* resolved*in*the*Earth6\ixed*coordinate*system,*or*{E$
29/08/15

1

Milan, August 25th 2015

Orienta5on/posi5on sensing with magneto/iner5al sensors

Speaker: Angelo Maria Saba.ni

logo The BioRobo3cs Ins3tute Scuola Superiore Sant’Anna Pisa, Italy

Outline of the talk

•  Introductory remarks •  Mathematical preliminaries •  Strap-‐down inertial navigation •  Sensor fusion for pose estimation •  Worked-‐out example

29/08/15

2

Outline of the talk


Overview

Inertial sensors They are pose (position and/or orientation) or simply motion sensors, the references of which are completely internal, except possibly for initialization (alignment)*

They measure physical quantities, i.e., linear accelerations and angular velocities of the body part to which they are strapped

* Currey, R.K., Ash, M.E., Thielman, L.O., Barker, C.H. (2004). Proposed IEEE inertial system terminology standard and other inertial sensor standards. IEEE Position, Location and Navigation Symposium, PLANS 2004, Monterey, CA, USA, 26-‐29 April 2004.

DEFINITION

Inertial sensors are internally referenced or sourceless, i.e., they sense their own (change of) position and orientation – opportunity for anytime, anywhere use

29/08/15

3

Inertial Measurement Unit (IMU) An IMU contains multiple inertial sensors (accelerometers and/or gyroscopes) in Aixed orientation relative to one another

Inertial navigation system (INS) Using the outputs from an IMU, an INS estimates a body’s orientation and position (via dead-‐reckoning), in connection with a gravitational \ield model and the operation of a reference clock

Magneto-‐Inertial Measurement Unit (MIMU) A MIMU contains multiple inertial sensors (accelerometers and/or gyroscopes) and magnetic sensors in Aixed orientation relative to one another

Using a geomagnetic \ield model, magnetic sensors can help specify the orientation of an INS during initialization and estimate the heading during operation

DEFINITION

Thanks to advances in the \ield of Micro ElectroMechanical Systems (MEMS), (M)IMUs with good features in terms of •  weight – size •  power consumption •  metrological speci\ications are available for applications related to Human Movement Analysis

The focus of this lecture is not on hardware/\irmware aspects related to the development of (M)IMUs

The focus of this lecture is on some of the computational methods used for estimating the pose (orientation and position) of a rigid body in the three-‐dimensional (3D) space

29/08/15

4

Outline of the talk


Choice of coordinate systems

For motions on or near the Earth surface, two

coordinate systems are convenient to be chosen:

•  Earth-‐\ixed coordinate system

•  Body-‐\ixed coordinate system

Both coordinate systems will be

speci\ied by right-‐handed

orthonormal bases, denoted in

the following as {E} and {B},

respectively

29/08/15

5






Right-‐hand rule

North-‐East-‐Down (NED) convention






Right-‐hand rule

ZY

X ZYX

29/08/15

6

Representation of orientation

We need to master the coordinate transformation of a generic vector x resolved in the Earth-‐\ixed coordinate system, or {E}-‐frame, so as to get its representation in the body-‐\ixed coordinate system, or {B}-‐frame

Leonhard Euler proposed that the rotation of a rigid body (or 3D coordinate system with the \ixed origin) is described by: •  single rotation about some axis (axis-‐angle representation) •  sequence of three rotations about base vectors (Euler angle representation)

!!!

E x =x1x2x3

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

→ B x =

x1'

x2'

x3'

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

Euler’s theorem

The most general motion of a rigid body with one point \ixed is a rotation by an angle (rotation angle) about some axis n (rotation axis) θ

!!!

R(n,θ )= cosθ ⋅I3×3 +(1− cosθ )n ⋅nT − sinθ n×⎡⎣ ⎤⎦

R(n,θ )= I3×3 − sinθ n×⎡⎣ ⎤⎦+(1− cosθ ) n×⎡⎣ ⎤⎦2

!!!

n× v = n×⎡⎣ ⎤⎦v

n×⎡⎣ ⎤⎦ =

0 −nZ nYnZ 0 −nX−nY nX 0

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

skew-‐symmetric operator rotation matrix

rotation vector

!θ =θn n

v

n × n × v

v⊥

n × v

v

θ

R n,θ( )v

29/08/15

7

Euler’s angles

In principle, several possible ways exist to de\ine three independent body-‐referenced Euler’s angles

Z-‐Y’-‐X’’ sequence (of common use in the aeronautics community)

The orientation of the {B}-‐frame (nose-‐wing-‐belly) relative to the {E}-‐frame (NED) is described by performing the three rotations as follows (suppose initially the two frames are aligned):

1.  rotate the body about the belly axis through

(heading angle, or yaw)

2.  rotate the body about the wing axis through

(elevation angle, or pitch)

3.  rotate the body about the nose axis through

(bank angle, or roll)

ψ

ϑ

ϕ

positive rotation

!!!

R X(ϕ)=1 0 00 cosϕ sinϕ0 −sinϕ cosϕ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

RY (ϑ )=cosϑ 0 −sinϑ0 1 0

sinϑ 0 cosϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

RZ(ψ )=cosψ sinψ 0−sinψ cosψ 00 0 1

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

!!!B x =R X(ϕ)RY (ϑ )RZ(ψ )E x

Rotation matrix

ϕ

ϑψ

!!!EBC =R X(ϕ)RY (ϑ )RZ(ψ )

!!!EBC =

cϑ cψ cϑ sψ −sϑsϕ sϑ cψ − cϕ sψ sϕ sϑ sψ + cϕ cψ sϕ cϑcϕ sϑ cψ + sϕ sψ cϕ sϑ sψ − sϕ cψ cϕ cϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

29/08/15

8

Singularities and the gimbal lock

!!!


⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥


sinϑ 0 cosϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥


⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

Singularities and the gimbal lock

When the pitch angle is ± 90°, a singularity is met in the representation of the orientation using the Euler’s angles (Z-‐Y-‐X sequence)

The yaw axis becomes parallel to the roll axis, therefore we have the loss of a rotational degree of freedom

!!!


⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

RY (ϑ )=0 0 10 1 0±1 0 0

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥


⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

!!!

EBC =R X(ϕ)RY (ϑ )RZ(ψ )=

0 0 −1−sin(ψ −ϕ) cos(ψ −ϕ) 0cos(ψ −ϕ) sin(ψ −ϕ) 0

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

0 0 1−sin(ψ +ϕ) cos(ψ +ϕ) 0−cos(ψ +ϕ) −sin(ψ +ϕ) 0

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

⎧

⎨

⎪⎪⎪⎪

⎩

⎪⎪⎪⎪

29/08/15

9

Quaternion of rotation

DeKinition (due to W.R. Hamilton)

i, j, k behave such as imaginary numbers

!!!

q = q1i+q2 j +q3k +q4

q =qq4

⎡

⎣⎢⎢

⎤

⎦⎥⎥=

q1q2q3q4

⎡

⎣

⎢⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥⎥

vector part

scalar part

× 1 i j k 1 1 i j k i i –1 k –j j j –k –1 i k k j –i –1

Quaternion multiplication

DeKinition (notation that allows multiplication in natural order)

IMPORTANT NOTE: the quaternion product is not commutative

!!!

q⊗p=

q4 q3 −q2 q1−q3 q4 q1 q2q2 −q1 q4 q3−q1 −q2 −q3 q4

⎡

⎣

⎢⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥⎥

p1p2p3p4

⎡

⎣

⎢⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥⎥

=q4p+ p4q−q×p

q4p4 −q ⋅p

⎡

⎣⎢⎢

⎤

⎦⎥⎥

pp4

⎡

⎣⎢⎢

⎤

⎦⎥⎥

× 1 i j k 1 1 i j k i i –1 –k j j j k –1 –i k k –j i –1

!!!q⊗p= (q1i+q2 j +q3k +q4 )(p1i+ p2 j + p3k + p4 )

29/08/15

10

Quaternion of rotation and rotation vector

Euler symmetric parameters

The unit quaternions and describe a rotation to the same \inal coordinate system position, i.e., the quaternion representation is not unique, although it does not present singularities

quaternion that describes the rotation of E into B, with the axis of rotation n expressed in B

!!!

q =

nx sin(θ /2)ny sin(θ /2)nz sin(θ /2)

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

= nsin(θ /2), q4 = cos(θ /2)

rotation vector

!θ =θn

!!!q = q2+q4

2 =1

!q !−q

!!!EBq =

nsin(θ /2)cos(θ /2)

⎡

⎣⎢⎢

⎤

⎦⎥⎥

unit quaternion

Quaternion of rotation and rotation matrix

A vector can be transformed from one coordinate system to another coordinate system by pre-‐ and post-‐multiplying its quaternion by the quaternion of rotation and its inverse, respectively:

Homogenous quadratic functions of the components of a unit quaternion

!!!Bp=

Bp0

⎡

⎣⎢⎢

⎤

⎦⎥⎥= E

Bq⊗Ep0

⎡

⎣⎢⎢

⎤

⎦⎥⎥⊗ E

Bq−1

!!!q−1 =

−qq4

⎡

⎣⎢⎢

⎤

⎦⎥⎥

inverse quaternion

quaternion of a vector

!!!

EBC =

q12 −q2

2 −q32 +q4

2 2(q1q2 +q3q4 ) 2(q1q3 −q2q4 )2(q1q2 −q3q4 ) −q1

2 +q22 −q3

2 +q42 2(q2q3 +q1q4 )

2(q1q3 +q2q4 ) 2(q2q3 −q1q4 ) −q12 −q2

2 +q32 +q4

2

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

EBC = I3×3 −2EBq4 E

Bq×⎡⎣ ⎤⎦+2 EBq×⎡⎣ ⎤⎦

2

29/08/15

11

Rotational kinematics equation

Basic equation (expressed in terms of quaternion of rotation): The system of \irst-‐order differential equations is linear in the components of the quaternion

It can be solved from known initial conditions by using the angular velocity of the motion as the input to the differential equation

!!!

ddt E

Bq(t)= 12− Bω(t)×⎡⎣ ⎤⎦

Bω(t)− Bω(t)T 0

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥ EBq(t)

!!ΩBω(t)( )

Numerical integration

The general form of the numerical solution can be easily expressed in terms of quaternions under the assumption that the angular velocity is constant over the integration period

Advantages of the quaternion formulation •  No singularity in the representation of orientation •  Computational convenience (accuracy and computing time)

!!!EBq(tk+1)!!!E

Bq(tk )!!!

EBq(tk+1)=

ωk

ωk

sinωk Δt2

⎛

⎝⎜⎜

⎞

⎠⎟⎟

cosωk Δt2

⎛

⎝⎜⎜

⎞

⎠⎟⎟

⎡

⎣

⎢⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥⎥

⊗ EBq(tk )

!!Δt = tk+1 −tk :

29/08/15

12

Outline of the talk


Strap-‐down INS

Implementation The magneto-‐inertial sensors are mounted in body coordinates and are not mechanically moved

Approach A computer solution is used to keep track of the MIMU orientation and to rotate the acceleration measurements from the {B}-‐frame to the {E}-‐frame

gyroscope magnetic sensor accelerometer

initialization

quaternion integration

gravity compensation

29/08/15

13

Earth’s magnetic Kield

Determine heading by measuring the geomagnetism using a compass

Limitations

Variation/inclination of the earth’s magnetic \ield

Natural magnetic anomalies (e.g., solar magnetic storms, iron ore deposits)

Manmade magnetic anomalies (e.g., vehicles, buildings, phones)

Host platform own magnetic \ield or susceptibility to magnetic \ields

Dynamic conditions (accelerations) of the host platform

Earth’s magnetic Kield

The Earth’s magnetic \ield varies greatly in intensity and direction over the surface of the Earth

The magnetic \ield is very dif\icult to model within a manmade environment, even at the smallest spatial scales of interest (cities, streets, buildings, apartments, rooms, individuals)

Magnetic declination or variation Difference between the local magnetic meridian and the local geographic meridian at any particular point on the Earth Inclination or dip angle Angle between the magnetic \ield and the horizontal component (perpendicular to gravity)

DEFINITION Accurate geomagnetic models are available

29/08/15

14

Magnetic disturbances

Hard irons Due to permanent magnets and magnetized iron or steel on the compass platform These distortions remain constant and in a \ixed location relative to the compass for all headings

Soft irons Due to interactions between the Earth’s magnetic \ield and any magnetically soft material nearby the compass These distortions do not remain constant, they vary with the orientation of the platform relative to the direction of the Earth’s magnetic \ield

DEFINITION

Compensation of hard and soft irons

6

-250

-150

-50

50

150

-200 -100 0 100 200

Y-ax

is Se

nsor

(mga

uss)

X-axis Sensor (mgauss)-20

-10

0

10

20

0 90 180 270 360

Head

ing

Erro

r (de

gree

)

Heading (degree)

Figure 10— Hard iron offsets when rotatedhorizontally in the earth’s field.

Figure 11— Heading error due to hard iron effects known assingle-cycle errors.

-200

-100

0

100

200

-200 -100 0 100 200

Yh O

utpu

t (m

gaus

s)

Xh Output (mgauss)-8

-6

-4

-2

0

2

4

0 90 180 270 360

Head

ing

Erro

r (de

gree

)

Heading (degree)

Figure 12—Soft iron distortion when rotatedhorizontally in the earth’s field.

Figure 13— Heading error due to soft iron effects known astwo-cycle errors.

Compensating for soft iron effects is a bit more difficultthan for hard iron effects. This involves a bit morecalculation than a simple subtraction. One way to removethe soft iron effect is to rotate the reading by 45°, scalethe major axis to change the ellipse to a circle, then rotatethe reading back by 45°. This will result in the desiredcircular output response shown in Figure 9.

Most ferrous material in vehicles tend to have hard ironcharacteristics. The best approach is to eliminate any softiron materials near the compass and deal with the hardiron effects directly. It is also recommended to degaussthe platform near the compass prior to any hard/soft ironcompensation.

Some compass manufacturers provide calibrationmethods to compensate for the hard and soft iron effects.Each calibration method is associated with a specifiedphysical movement of the compass platform in order tosample the magnetic space surrounding the compass. Thecalibration procedure can be as simple as pointing thehost in three known directions, or as complicated asmoving in a complete circle with pitch and roll, or

pointing the host in 24 orientations including variations intilt. It is impossible for a marine vessel to perform the 24-point calibration, but easy for a hand-held platform. If thecompass is only able to sample the horizontal fieldcomponents during calibration, then there will beuncompensated heading errors with tilt. Heading errorcurves can be generated for several known headings toimprove heading accuracy [10,11].

Hard and soft iron distortions will vary from location tolocation within the same platform. The compass has to bemounted permanently to its platform to get a validcalibration. A particular calibration is only valid for thatlocation of the compass. If the compass is reoriented inthe same location, then a new calibration is required. Agimbaled compass can not satisfy these requirements andhence the advantage of using a strapdown, or solid state,magnetic sensor. It is possible to use a compass withoutany calibration if the need is only for repeatability andnot accuracy.

Calibration procedure •  Freely rotate the magnetic sensor

in the 2D (3D) space, so as to get the measurement locus

•  Apply mathematical \itting techniques for compensation

The effect of soft irons is to distort a circle (sphere) whose radius is equal to the magnitude of the constant magnetic \ield into an ellipse (ellipsoid)

The effect of hard irons is to add an offset to the origin of the circle (sphere) or ellipse (ellipsoid)

!!

HX = (1+ sfX )(HX −Hoff!X )HY = (1+ sfY )(HY −Hoff!Y )

ψ = arctan2(HY ,HX )

29/08/15

15

Leveling

The output from a tri-‐axial accelerometer gives the components of the speciKic force directed along the sensor sensitivity axes:

Null (negligible) external acceleration

!!

E f = Ea− E gB f = E

BC Ea− E g( )

!!

→

ϑ̂ = arcsin fXg

⎛

⎝⎜⎞

⎠⎟!(pitch)!!!!!

ϕ̂ = arctan fYfZ

⎛

⎝⎜⎞

⎠⎟!(roll)!!!!!!!!

⎧

⎨

⎪⎪

⎩

⎪⎪⎪

attitude determination (leveling)

!!!

B f =fXfYfZ

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

= −g−sinϑ

sinϕ cosϑcosϕ cosϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

Strap the compass in the horizontal plane using a static accelerometer

Compassing

!!!


⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥


sinϑ 0 cosϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

leveling

!!!E x =RY (ϑ̂ )R X(ϕ̂)B x !

hxhyhz

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

!

Hx

Hy

Hz

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

!!ψ̂ = arctan2(Hy ,Hx )compassing

!!!

B f =fXfYfZ

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

→

ϑ̂ = arcsin fXg

⎛

⎝⎜⎞

⎠⎟!(pitch)!!!!!

ϕ̂ = arctan fYfZ

⎛

⎝⎜⎞

⎠⎟!(roll)!!!!!!!!

⎧

⎨

⎪⎪

⎩

⎪⎪⎪

29/08/15

16

Heading errors due to pitch without tilt compensation

Heading accuracy

3

Figure 4—Heading errors due to pitch without tilt compensation(Dip Angle =40°).

COMPASS ERROR ANALYSIS

If a compass system has a requirement of better than onedegree of accuracy, then it is important to break down theerror contributed by the tilt sensor and the magnetic sensorand determine what level of signal processing is required.Specifically, heading accuracy is affected by:

✔ A/D converter resolution✔ Magnetic sensor errors✔ Temperature effects✔ Nearby ferrous materials✔ Compass tilt errors✔ Variation of the earth's field

A/D Converter Resolution—To achieve a one-degreeaccurate compass requires a magnetic sensor that canreliably resolve angular changes to 0.1°. The sensors mustalso exhibit low hysteresis (<0.08%FS), a high degree oflinearity (<0.05%FS) and be repeatable. The magneticfields in the X and Y horizontal plane will typically be inthe 200 mgauss range—more at the equator, less near thepoles.

Using the standard heading relationship of equation (1),the required A/D converter resolution for the magneticsensors can be estimated. If the magnetometer error, oruncertainty, is allowed to be 0.1° then:

if: Error = 0.1° = arcTan(Yh/Xh)then: Yh/Xh = 1/573 (4)

This implies that a ratio change of 1 part in 573 will resultin a 0.1° difference. If X and Y were read with a nine-bitA/D converter there would be only a 1:512 bit resolution.This means that a 9+ bit A/D converter is needed to meetthe 0.1° error budget for an (X,Y) magnetic field changeof 200 mgauss. Since the (X,Y) magnetic fields measure±200 mgauss for a complete heading sweep, the A/Dconverter range should be doubled, to 10+ bits. To allowfor hard iron correction and larger horizontal fields—likeat the equator—this range should be quadrupled to ±800mgauss. Now the A/D converter resolution should be 12+bits, or 12.163 bits to be more exact.

A 12 bit A/D converter can be used to provide a 0.1°resolution in a 200 mgauss horizontal field. This impliesthat the sensor must be able to resolve a 0.39 mgauss fieldover a span of ±800 mgauss (1.6 gauss/4096 counts).

Magnetic Sensor Errors—Solid state magneto-resistive(MR) sensors available today can reliably resolve <0.07mgauss fields [4-7]. This is more than a five times marginover the 0.39 mgauss field required to achieve 0.1°resolution.

Other magnetic sensor specifications should support fieldmeasurement certainty better than 0.5° to maintain anoverall 1° heading accuracy. These include the sensornoise, linearity, hysteresis, and repeatability errors.

Any gain and offset errors of the magnetic sensor will becompensated for during the hard iron calibration(discussed later) and will not be considered in the errorbudget.

MR sensors can provide a total error of less than 0.5mgauss, which corresponds to a 0.14° heading error asshown in Table 1.

ParameterSpec

Limit (1)FieldError

HeadingError

Noise(BW=10Hz)

85 ugauss 85 ugauss <0.01°

Linearity 0.05 %FS 0.2 mgauss 0.06°Hysteresis 0.08 %FS 0.32 mgauss 0.09°Repeatability

0.08 %FS 0.32 mgauss 0.09°

Total rmserror 0.49 mgauss 0.14°

(1) Typical specs for HMC1021/22 MR sensors; FS=400mgauss

Table 1—Error budget for an MR magnetic sensor

-10

-5

0

5

10

0 90 180 270 360Heading (degree)

Hea

ding

Erro

r (de

gree

)

pitch (degree)-10

-5-2

25

10

!Δψ ≈ −Δϕ ⋅tanδ ⋅cosψ −Δϑ ⋅tanδ ⋅sinψ

dip angle

!!!

B f =fXfYfZ

⎡

⎣

⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥

→

ϑ̂ = arcsin fXg

⎛

⎝⎜⎞

⎠⎟!(pitch)!!!!!

ϕ̂ = arctan fYfZ

⎛

⎝⎜⎞

⎠⎟!(roll)!!!!!!!!

⎧

⎨

⎪⎪

⎩

⎪⎪⎪

External magnetic disturbances

When the source of the magnetic \ield and the magnetic sensor move relative to one another, sudden shifts in offset/magnitude are observed in each sensed component – independent of the orientation of the magnetic sensor in the 3D space

!!!

Bhmeas =Bh+ hn

!!!!!! Bh = Bhref +Bhsource(t)

29/08/15

17

Gyroscope model

A realistic model of the gyroscope must take into account various factors:

Scale factor error Misalignment error

Bias Measurement noise

Using carefully implemented in-‐Kield calibration, the following model can be considered valid:

•  The gyroscope bias is simulated as a Gaussian random walk process:

•  The gyroscope measurement noise is Gaussian white:

!!!Bωmeas =K g ⋅R g ⋅

Bω + gb+ gn

!!!Bωmeas =

Bω + gb+ gn

!!!ddt

gb= nw , nw ∝N 0, I3×3 ⋅ gσ 2( )

!!!gn∝N 0, I3×3 ⋅σ g

2( )

Accelerometer model

A realistic model must take into account various factors:


Bias Measurement noise


•  The accelerometer bias can be neglected

•  The accelerometer measurement noise is Gaussian white:

!!!an∝N 0, I3×3 ⋅σ a

2( )

!!!B fmeas =Ka ⋅Ra ⋅

B f + ab+ an

!!!B fmeas =

B f + an= EBR Ea− E g( )+ an

29/08/15

18

Magnetic sensor model

A realistic model must take into account various factors:


Soft iron bias Measurement noise

Hard iron bias Sensor bias


•  The magnetic sensor bias can be neglected

•  The effects of hard and soft irons are canceled out

•  The magnetic sensor measurement noise is Gaussian white:

!!!hn∝N 0, I3×3 ⋅σ h

2( )

!!!Bhmeas =Km ⋅Rm ⋅Csi( Bh+bhi + hb)+ hn

!!!Bhmeas =

Bh+ hn= EBC(href + Ehsource )+ hn

Outline of the talk


29/08/15

19

Sensor fusion

Informally, combination of data from several sensors such that the information we get has less uncertainty than would be possible when each sensor is used alone

Estimating orientation using only gyros is affected by integration drifts due to gyro bias and measurement noise

Principles of sensor fusion can be adopted (using drift-‐free accelerometers and magnetic sensors):

–  Gravimetric tilt sensing – stabilization of attitude (i.e., pitch and roll angles) –  Earth’s magnetic sensing – stabilization of heading (i.e., yaw angle)

We need to learn how to prevent effects of –  Fast motions in gravimetric tilt sensing –  Magnetic disturbances nearby the MIMU

Sensor fusion using Kalman Kiltering

Estimate the state of a dynamic system using a sequence of noisy measurements made on the system

State-‐space approach to modeling dynamic systems (discrete-‐time formulation of the problem)

system

sensors Kalman estimator

disturbances

disturbances

control inputs

system’s state measurements

estimated state

model

model

29/08/15

20

Kalman Kiltering (linear time-‐varying)

•  Dynamic model

–  State vector:

–  Process noise: •  Measurement model

–  Measurement vector:

–  Measurement noise:

•  Prediction-‐update recursion –  Use the dynamic model to predict the state from tk-‐1 to tk –  Use the measurement at tk to update the prediction

!!!xk = Fkxk−1 +wk−1 , wk−1 ∝N(0,Qk−1)

!! xk ∈nx

!!! wk−1 ∈nw

!!!zk =Hkxk + vk , vk ∝N(0,Rk )!! zk ∈

nz

!! vk ∈nv

white Gaussian, mutually independent

Predic5on Measurement update

the output at k-‐1 will be the input for k (iterative nature of the \iltering process) !!

x(0|0)= x0P(0|0)=P0

initial conditions innovation term and its covariance matrix

29/08/15

21

Kalman Kiltering: general setting

•  Dynamic model – non-‐linear

–  State vector:

–  Process noise: •  Measurement model – non-‐linear

–  Measurement vector:

–  Measurement noise:

•  Extended Kalman Filter (EKF) The main feature of the EKF is that it approximates (linearizes) the non-‐linear functions in the dynamic and measurement models

!!!xk = fk(xk−1 ,wk−1)

!! xk ∈nx

!!! wk−1 ∈nw

!!!zk = hk(xk ,vk )!! zk ∈

nz

!! vk ∈nv

white Gaussian, mutually independent

!!!

x(k |k −1)= fk−1(x(k −1|k −1))P(k |k −1)= Fk−1P(k −1|k −1)Fk−1T

x(k |k)= x(k |k −1)+Kk zk −hk−1(x(k |k −1))( )P(k |k)=P(k |k −1)−KkSkKk

T

!!!

Sk =HkP(k |k −1)HkT +Rk

Kk =P(k |k −1)HkTSk

−1

prediction

update

!!!

fk−1(x(k −1|k −1))→ Fk−1hk−1(x(k |k −1)) → Hk

!!!

fk−1(x(k −1|k −1)) → Fk−1 =∂ fi∂x j x(k−1|k−1)

, i =1,...,nx ; j =1,...,nx⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

hk(x(k |k −1)) → Hk =∂hi∂x j x(k|k−1)

, i =1,...,nz ; j =1,...,nx⎧⎨⎪

⎩⎪

⎫⎬⎪

⎭⎪

29/08/15

22

Highlights on EKFs

Suboptimal solution to the nonlinear Kiltering problem The posterior density is approximated by a Gaussian random variable which is propagated analytically through the “\irst-‐order” linearization of the nonlinear system

What if the nonlinearity is very severe? Multimodal or heavily skewed posterior distributions are poorly represented by a multivariate Gaussian density

Computational burden It is dependent on the complexity of Jacobian computations, which are carried out at each \iltering step

Convergence The behavior is dependent on how the \ilter is tuned, on the initialization process and the incorporation of possibly outlying measurements

The state vector includes two parts that account for: –  Rotational degrees of freedom (DOF) –  Translational DOF

In a tightly-‐coupled algorithm, all measurements are used directly to compute the \inal result – in a loosely-‐coupled algorithm, the estimated task is partitioned in different modules communicating with each other

!!!x(t)= xR(t)

xT(t)⎡

⎣⎢⎢

⎤

⎦⎥⎥

!!! xR(t)=

12

− Bω(t)×⎡⎣ ⎤⎦Bω(t)

− Bω(t)T 0

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥xR(t)

!!! xT(t)=

E bB(t)E bB(t)

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥= 0 1

0 0⎡

⎣⎢

⎤

⎦⎥

EbB(t)E bB(t)

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥+

0E bB(t)

⎡

⎣⎢⎢

⎤

⎦⎥⎥

continuous-‐time state transition equations

Pose estimation using a single (M)IMU

29/08/15

23

Loosely-‐coupled approach

By loosely coupled approach we refer to a solution where the measurements from one or several of the individual sensors are processed before they are used to compute the \inal result (positioning)

gyroscope magnetic sensor accelerometer

measurement model

state transition model

update

!!!x̂R(t)= EB q̂(t)

Kalman Kilter

control input

aiding sensors

ORIENTATION

Loosely-‐coupled approach

accelerometer !!!EB q̂(t) !!!

B fmeas(t)gravity compensation

!!! E bB(t)= E bB(τ )dτ +

t0

t

∫ E bB(t0)

Kalman-‐based \ilter

!!! E bB(t)= E

BC( EBq((t)))T B fmeas(t)+ E g

time integration

time integration

!!! EbB(t)= E bB(τ )dτ +

t0

t

∫ EbB(t0)

e.g., dead-‐reckoning with zero-‐velocity update

The body orientation estimated in the \irst-‐stage is delivered to the cascaded second stage, where the position of the body is computed (estimated)

POSITIONING

29/08/15

24

A clever use of the constraints of ambulation allows breaking the cubic growth of positioning errors with the operation time

Park, S.K. and Suh, Y.S. (2010) “A zero velocity detection algorithm using inertial sensors for pedestrian navigation systems,” Sensors, 10(10), 9163-‐9178

Zero-‐velocity update

Highlights on loosely-‐coupled approaches

Computational complexity They are easy to implement and computationally convenient, compared with tightly-‐coupled approaches

Integration of other aiding sensors Tightly-‐coupled approaches can be preferred to handle sensor outages

Dead-‐reckoning with zero-‐velocity update When the body is detected at rest, leveling can be performed to reset gyroscope integration errors, and null initial velocity can be assumed in the time-‐integration of acceleration, e.g., shoe-‐mounted (M)IMUs

In the absence of frequent resting periods in the movement of the body, zero-‐velocity updates are ineffective, e.g., waist-‐mounted (M)IMUs

Orientation errors and strap-‐down rotation Large orientation errors and fast movements affect the positioning accuracy achieved by loosely-‐coupled approaches

29/08/15

25

Aiding sensors’ measurement equation

For a generic vector p (e.g., gravity or Earth’s magnetic \ield):

Accelerometer and magnetic sensor measurements are both cast in the same framework:

!!!

pmeas = EBC E

Bq( )p+np , np ∝N 0,I3×3 ⋅σ p2( )

∂∂ EBq E

BC EBq( )p= −2q4 p×⎡⎣ ⎤⎦+2 p×⎡⎣ ⎤⎦ q×⎡⎣ ⎤⎦−2 q×⎡⎣ ⎤⎦ p×⎡⎣ ⎤⎦( ) 2 q×⎡⎣ ⎤⎦p

⎡⎣⎢

⎤⎦⎥

!!!ψ EBq,p( )

!!!

B fmeas = EBC E

Bq( ) Ea− E g( )+na , na ∝N 0,I3×3 ⋅σ a2( )

Bmmeas = EBC E

Bq( ) Eb+ Eh( )+nm , nm ∝N 0,I3×3 ⋅σm2( )

nuisance factor: external acceleration

nuisance factor: magnetic disturbance

Threshold-‐based approach

When the norm and/or the directional differences between sensed and predicted vectors exceed given threshold values, the sensed vector is not assimilated in the \ilter, or its measurement noise covariance matrix is adapted

E.g., simple test for adapting the accelerometer noise covariance matrix R:

Threshold setting in a threshold-‐based EKF is very dif\icult: –  if too low, the rejection rate of accelerometer measurements can be very high (no sensor fusion – only gyroscope integration)

–  if too high, the risk is to have poor leveling performance

!!!R =

I3×3 ⋅Rσ a

2 03×303×3 I3×3 ⋅

Rσ h2

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥, Rσ a

2 =σ a

2 , B fmeas − BEC(k |k −1) E g < λg

∞, !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! otherwise

⎧⎨⎪

⎩⎪

29/08/15

26

State-‐augmentation technique

Add extra-‐state variables and develop simple stochastic models for them:

•  gyroscope bias

•  external acceleration

•  magnetic disturbance

!!!

ddt E

Bq = 12ΩBωmeas −

gb− gn( ) EBqddt

gb= gnw !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

ddt

ab= anw !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

ddt

hb= hnw !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

⎧

⎨

⎪⎪⎪⎪⎪

⎩

⎪⎪⎪⎪⎪

!!!

x(t)=EBq(t)gb(t)ab(t)hb(t)

⎡

⎣

⎢⎢⎢⎢⎢

⎤

⎦

⎥⎥⎥⎥⎥

continuous-‐time state transition equation

!!!

B fmeas = EBC E

Bq( ) Ea− E g( )+na , na ∝N 0,I3×3 ⋅σ a2( )

Bmmeas = EBC E

Bq( ) Eb+ Eh( )+nm , nm ∝N 0,I3×3 ⋅σm2( )

Gyroscope noise model highlighted

Continuous-‐time single-‐axis gyro model

Discretization

The angular velocity (measurement and truth) is assumed constant over the integration interval – we cannot make the same assumption for the stochastic variables!

Continuous-‐time noise model

The Dirac delta has units of Hz

!!

ω(t)=ω(t)+b(t)+ηv(t)b(t)=ηu(t)

!! 1Δt

ω(t)dtt0

t0+Δt

∫ = 1Δt

[ω(t)+b(t)+ηu(t)]dtt0

t0+Δt

∫

!!

ηv(t) → E[ηv(t)ηv(τ )]=σ v c2 δ(t −τ ), σ v c expressed!in![rad/s/ Hz]

ηu(t) → E[ηu(t)ηu(τ )]=σ uc2 δ(t −τ ), σ uc !expressed!in![(rad/s) Hz]

29/08/15

27

Gyroscope noise model highlighted

Discrete-‐time single-‐axis gyro model

The discretization must be taken carefully, because of the invalidity of the assumption that the random variables are constant over the integration interval!

Continuous-‐time and discrete-‐time noise standard deviations

•  In the discrete case, both noises (bias driving noise, i.e., process noise and rate noise, i.e., measurement noise) would have the same units!

•  For isotropic noise models, the extension to the tri-‐axis gyro model is straightforward

!!

ω k+1 =ω k +12(bk +bk+1)+

σ v c2

Δt+ 112σ uc

2 Δt Nv , Nv ∝N(0,1)

bk+1 = bk +σ uc Δt Nu , Nu ∝N(0,1)

!!

σ v d =σ v c

Δt, [rad/s]

σ ud =σ uc Δt , [rad/s]

Insights into the EKF development

Model formulation The state vector is de\ined, and the system of differential equations governing the state is elaborated

The prediction equation used within the Kalman-‐framework is obtained by taking the expectation of the above

The measurement model is set-‐up, provided that the aiding sensors are chosen for the intended application

Usually, the following approximations/assumptions are made: –  Statistical decoupling of the motion degrees of freedom –  Gaussianity and whiteness of the process and measurement noise –  Isotropy of the sensor noise models

29/08/15

28


Discretization It must be performed carefully, especially as for the speci\ication of the process noise covariance matrix (see gyro noise model discussed above)

Jacobian computation When either the state or the measurement equations are nonlinear, Jacobian matrices are needed to predict and/or update the state and the error covariance matrix

Initialization Not overly critical step, only in the presence of large calibration sensor errors the \ilter can be very dif\icult to converge


Filter parameter tuning The parameters are both sensor-‐ and motor task-‐dependent

It is important to settle, in particular, the parameters of the process noise covariance matrix for achieving good tracking behavior (trade-‐off between promptness of response and accuracy)

Sensor fusion robustness –  The magnitude/direction of the external acceleration depends on the motor task (e.g., ambulation, sedentary activity, postural transitions)

–  The magnitude/direction of the magnetic disturbance depends, indirectly, on the working volume (where the MIMU is expected to work)

–  Relevance of the (M)IMU placement site (e.g., foot, shank, waist, trunk, head)

Adaptivity The process and measurement noise covariance matrices can be adapted on-‐line, for a good trade-‐off between EKF accuracy and promptness of response

29/08/15

29

magnetic sensor accelerometer gyroscope

R-‐adaptation

EKF initialization

EKF

Jacobian computation project ahead update Kalman gain

computation

estimate

estimate

accelerometer magnetic sensor

gyroscope

Sabatini, A.M. (2011) “Estimating three-‐dimensional orientation of human body parts by inertial/magnetic sensing,” Sensors, 11(1), 1489-‐1525

Outline of the talk


29/08/15

30

An example of application

The aim of the sensor fusion algorithm is to estimate the vertical linear velocity during unconstrained motor tasks

Assumption 1 If positioning information is only needed in the vertical direction (i.e., parallel to gravity), we have to estimate attitude only (i.e., roll and pitch angles) for rotating the sensed speci\ic force in the {E}-‐frame

Assumption 2 The absence of motor tasks’ constraints does not allow to easily apply dedrifting (or zero-‐velocity updates) to remove integration drift errors

Assumption 3 An additional sensor, i.e., a barometric altimeter, is considered in the development of the sensor fusion algorithm

Proposed loosely-‐coupled approach

•  How to handle the presence of the external acceleration in the accelerometer measurements?

•  How to get a drift-‐free estimate of the vertical velocity?

Kalman \ilter

strap-‐down rotation

complementary \ilter

gyroscope

barometric altimeter

accelerometer

attitude

vertical velocity height

external vertical acceleration

gravity compensation

rotated acceleration

conditioning

!!B f = E

BC Ea− E g( )

29/08/15

31

Model-‐based approach

Our proposed approach*

A linear KF is developed that attempts to exploit the additive nature of the accelerometer model (external acceleration and gravity add to yield the speci\ic force sensed by the accelerometer)

State transition equation

Linear measurement equation Ligorio, G., Sabatini, A.M. (2015) “A novel Kalman \ilter for human motion tracking with an inertial-‐based dynamic inclinometer,” IEEE Trans. Biomed. Eng., in press

!!!x(k)=

Bg(k)Ba(k)

⎡

⎣⎢⎢

⎤

⎦⎥⎥

!!!Bg(k)= exp − Bω(k −1)×⎡⎣ ⎤⎦Δt( ) Bg(k −1)

!!!Ba(k)= ca Ba(k −1)+ cbw(k), w∝N(0,I3×3)

!!!z(k)= −I3×3 I3×3⎡

⎣⎤⎦

Bg(k)Ba(k)

⎡

⎣⎢⎢

⎤

⎦⎥⎥=Hx(k)

Model-‐based approach

Proposed approach

Kalman Filter

update

prediction accelerometer

gyroscope

Key features

•  Linear estimation framework (good for stability and consistency) •  Filter parameter tuning easy and straightforward

Shank-‐worn IMU during gait

B g, B a⎡⎣ ⎤⎦

29/08/15

32

Results

The proposed linear KF is more robust than, i.e., a threshold-‐based EKF and when the gyroscope output is integrated without sensor fusion (INT), especially when: •  the motion task-‐placement site combination yields poor leveling (EKF) •  the time duration of the experimental trial increases (INT)

Results

The threshold-‐based EKF is shown here compared with INT during a locomotion task (also including heading estimation results); the INT error growth (after 60 s) is evident from the plot

Bergamini E. et al. (2014) “Estimating orientation using magnetic and inertial sensors and different sensor fusion approaches: accuracy assessment in manual and locomotion tasks,” Sensors, 14(10), 18625-‐49

Sensors 2014, 14 18636

routine trials with respect to the locomotion task (U6 = 1, p < 0.005). Conversely, no difference was reported between the manual routine and the locomotion scenarios for INT and CF.

Figure 5. Heading and attitude errors (ȟɅ) of the INT, SF and CF methods plotted as a function of time for the locomotion task. The mean ± one standard deviation (SD) curves over the six participants are reported. Note the different scale of the axes of the ordinate in the two graphs.

4. Discussion

In the present study, two sensor fusion methods for the estimation of 3D orientation using MIMUs were selected as representative of the stochastic and complementary filtering approaches, and their performance was compared with that of the numerical integration method. The three algorithms were analyzed during manual and locomotion tasks, and their level of accuracy was assessed, in terms of heading and attitude errors, with respect to reference orientations obtained through stereophotogrammetry.

Average heading errors during the manual routine task were lower than 5.5° for the two sensor fusion approaches and lower than 10.5° for INT. Similar results were obtained for the attitude errors These results .(௧௧ < 7.3°, for the sensor fusion methods and INT, respectivelyܵܯܴ ௧௧ < 3.5° andܵܯܴ)are in agreement with the findings of two previous studies published by Luinge et al. [31,44], where the 3D orientation of an inertial sensor located on the forearm was estimated during both eating and morning routine tasks using an ad hoc developed Kalman filter. However, no magnetic sensors were included in both studies, therefore further comparison with the present work would not be appropriate.

When considering the locomotion task, average heading errors increased up to 21° for the sensor fusion methods and to 32° for the INT method, while average attitude inaccuracies remained lower than 5.5° in all cases. Also in this case, a thorough comparison with previous studies can be hardly performed, due to the variety of experimental protocols and methodologies used to assess the accuracy of the analyzed methods or devices. In one study [32], participants were asked to walk in a straight line for 10 m and trunk, thigh and shank inclination in the sagittal plane was estimated using a non-linear filter. Trial duration was approximately 4 s and RMS differences of 1.5° and 3.0° between reference and estimated angles were found for the trunk and the shank segments, respectively. In other studies,

HEADING ERROR

0 50 100 1500

20

40

60

80INTINT±1SD

CFCF±1SD

SFSF±1SD

time [s]

erro

r [°]

ATTITUDE ERROR

0 50 100 1500

5

10

15INT

CF

SFINT±1SD

CF±1SD

SF±1SD

time [s]

erro

r [°]

INT

29/08/15

33

How to estimate vertical velocity and height

Basic equation (expressed in terms of coordinate transformation matrix):

Vertical external acceleration (resolved in the {E}-‐frame):

!!!BEC = E

BCT =cϑ cψ sϕ sϑ cψ − cϕ sψ cϕ sϑ cψ + sϕ sψcϑ sψ sϕ sϑ sψ + cϕ cψ cϕ sϑ sψ − sϕ cψ−sϑ sϕ cϑ cϕ cϑ

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥

!!

BgT

g

!!!Ea = B

EC Ba → Eav =1gBgT Ba

IMU Kalman \ilter

!!! x(k |k)=

Bg(k |k)Ba(k |k)

⎡

⎣⎢⎢

⎤

⎦⎥⎥

→ E xv =Bg(k |k)⋅ Ba(k |k)

How to estimate vertical velocity and height

Complementary Kilter

Sabatini, A.M., Genovese, V. (2014) “A sensor fusion method for tracking vertical velocity and height based on inertial and barometric altimeter measurements,” Sensors, 14(10), 13324-‐47

!!!

E xv(k)E xv(k)

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥= 1 Δt

0 1⎡

⎣⎢

⎤

⎦⎥

E xv(k −1)E xv(k −1)

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥+ 1 Δt

20 1

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥Kc ⋅Δt ⋅Δxk−1 +

Δt21

⎡

⎣

⎢⎢⎢

⎤

⎦

⎥⎥⎥Δvk−1

Δx(k −1)= E xp(k −1)− E xv(k −1)Δv(k −1)= E xv(k)⋅Δt

!!!Kc = −

2σ w /σ v

σ w /σ v

⎡

⎣

⎢⎢

⎤

⎦

⎥⎥

complementary \ilter

barometric altimeter

vertical velocity height

external vertical acceleration

conditioning

!! E xv(k)

!!E xp(k)

!!

σ w =15!mgσ v =30cm

external acceleration noise standard deviation:

barometric altimeter noise standard deviation:

29/08/15

34

Fall detection The waist motion is tracked on-‐line in the vertical direction using a baro-‐IMU device

Antero-‐lateral trip: walk toward the crash mat before simulating tripping on its edge. Impact spot: left hip

Sabatini, A.M. et al. (2015) “Prior-‐to and post-‐impact fall detection using inertial and barometric altimeter measurements,” IEEE Trans. Neural Syst. Rehabil. Eng., in press

Strap-‐down integration •  Dedrifting requires that the start and end times of integration are identi\ied •  The dynamic activity cannot last for more than few seconds to get accurate results •  Dedrifting is performed off-‐line, necessarily Baro-‐IMU approach •  Biases in the estimated vertical acceleration do not turn in velocity drifts •  Estimates of the vertical velocity (height) are obtained on-‐line

29/08/15

35

> REPLACE THIS LINE WITH YOUR PAPER IDENTIFICATION NUMBER (DOUBLE-CLICK HERE TO EDIT) <

3

The barometric altimeter output was conditioned before being used in the complementary filter. Two conditioning options were considered, named N and Y in the remainder of the paper: the barometric altimeter output was smoothed by an M-point moving average filter (M = 4) (option N); in addition to smoothing, the barometric altimeter output was processed through a whitening filter (option Y). The whitening filter was designed to remove the serial correlation due to short-term pressure changes and other environment factors that can mask the height information in the barometric altimeter output.

The design of the whitening filter was based on the parameters of the Auto Regressive Moving Average (ARMA) model that was developed to capture the statistical properties of the air pressure noise [26]: for the experiments in this paper the whitening filter was designed with a DC gain ADC = 0.21, one pole at a frequency of about 1 Hz and one zero approximately two octaves down; the unity-gain condition enforced at 25 Hz allowed high-frequency noise components to be deemphasized.

C. Experimental Protocol Twenty-five subjects (14 males and 11 females) participated

in the experiments after being informed and providing written consent. Age ranged from 25 to 35 years (28.3 ± 2.9 years), body mass from 50 to 116 kg (67.6 ± 15.2 kg), and height from 1.58 to 1.85 m (1.72 ± 0.08 m). The WIMU was worn at the right anterior iliac spine, using an elastic Velcro belt to keep it firm. The orientation of the WIMU axes were X: vertical (down/up), Y: medio-lateral (right/left) and Z: antero-posterior (forward/backward). The experimental protocol comprised 12 tasks, 5 falls and 7 activities of daily living (ADLs), performed in a safe controlled laboratory setting, Table I. The WIMU data of each subject were recorded in one experimental session.

The simulated fall study involved simulating falls from floor level on a 20-cm thick crash mat, measuring 100 cm × 200 cm. The falls were initiated from both standing (F1-F2-F3) and walking (F4-F5). The subjects were asked to fall in a relaxed way, without applying specific compensation strategies to prevent the impact. No instructions were given on keeping their legs straight or flexing their knees during falls and breaking the falls with their hands. The subjects were asked to stand still for a rest period of 5 s in a given initial location before falling (F1-F2-F3) or starting to walk (F4-F5). After each fall, they returned to the initial location after a period of 10 s spent on the crash mat. Each fall was repeated 3 times, for a total of 375 falls. The ADLs activities encompassed: ambulation (A1-A2), low-activity behaviors (A3-A4) and postural transitions (A5-A6-A7). The overall duration of the ADLs including data from all subjects was about 3 hours.

D. Sensor Calibration, Filter Initialization and Tuning The in-field calibration of the tri-axial accelerometer was

performed before wearing the WIMU [29]. With the WIMU in place, the gyroscope bias was estimated by taking the average of the gyroscope output during the rest period and removed from all measurements (bias capture). A similar procedure was applied to the output from the barometric altimeter; then, the

TABLE I FALL AND ADL TYPES AND DESCRIPTION

ID Type Description F1 forward fall Impact spot: knees or trunk, even hands. F2 lateral fall Impact spot: left hip. F3 backward fall “Sit on empty air”, as if when missing a chair

while sitting down. Impact spot: buttock. F4 forward trip Initial location 7 m from the crash mat. Walk

toward the crash mat before simulating tripping on its edge. Impact spot: knees or trunk, even hands.

F5 antero-lateral trip

Initial location 7 m from the crash mat. Walk toward the crash mat before simulating tripping on its edge. Impact spot: left hip.

A1 walk-turn-walk Walk at free-selected speed along a 12-m long pathway, including a half-turn to walk in the opposite direction; return to the initial location.

A2 going up-and-down stairs

Climb a staircase of 11 steps (16-cm high), including a half-turn at the higher floor; walk downstairs along the same staircase; return to the initial location.

A3 sweep Sweep the floor with a broom while moving around the room, for a period of about 60 s.

A4 sit Stay seated on a 55-cm high ergonomic chair, being free to adjust the body position using the armrests or to cross the legs, for a period of about 60 s.

A5 stand-lie-stand Stand still close to a 60-cm high bed, then sit down by moving the trunk with a 90° rotation around the vertical axis. Lie down on the bed with a trunk rotation in the opposite direction and lie in a supine position. After resting for a few seconds, move to a semi-supine position, first leftwards and then rightwards, and then rotate to the bedside in preparation for stand-up. Repeat the transitions 3 times.

A6 stand-sit-stand Stand still close to three different chairs (55-cm high ergonomic chair; 45-cm high kitchen chair; 40-cm high ergonomic chair; stay seated for a few seconds on each of them before standing up. In the case of the 40-cm high ergonomic chair, sit and stand up by moving the trunk with a 90° rotation around the vertical axis, as if getting in and out of a car seat. Repeat the transitions 3 times (once for each chair).

A7 pick-up Squat to take an object from the floor and to bring it up to shelves at different heights from the floor (32, 72, 112, 142 and 172 cm). Repeat the transitions 5 times (once for each shelf).

TABLE II PARAMETER TUNING

EKF process noise model parameters gyroscope-driven input noise standard deviation, °/s 1.0 gyroscope bias standard deviation, °/s2 0.0 gyroscope bias correlation time constant, s 0.0 accelerometer bias standard deviation, m/s3 (×10-4) 0.1 accelerometer bias time constant, s 1.0 measurement noise standard deviation accelerometer, mg 15.00 threshold-based switching for gating acceleration measurements gating threshold, mg 100.00 complementary filter external acceleration noise standard deviation, mg 15.00 barometric altimeter noise standard deviation, cm (option Y) 15.00 barometric altimeter noise standard deviation, cm (option N) 30.00

Results

Dataset from 25 healthy adult volunteers simulating falls and ADLs

Performance assessment –  Falls with peak downward vertical velocities exceeding 1.38 m/s are detected with a prior-‐to-‐impact time of 157 ms (on average) (80% sensitivity, 100% speci\icity)

–  Falls with peak downward velocities from 0.55 m/s to 1.38 m/s are detected post-‐impact using posture and impact features (100% sensitivity, 100% speci\icity)

Key features –  It is dif\icult to exploit the height information available from the barometric altimeter, due to the low Signal-‐to-‐Noise ratio

–  The barometric altimeter output is especially useful for producing drift-‐free on-‐line vertical velocity estimates

Better conditioning schemes, better sensors, both or none?

29/08/15

36

Thank you!

lecture sabatini final...29/08/15 6...

Documents