Upload
others
View
2
Download
0
Embed Size (px)
Citation preview
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
• Introductory remarks • Mathematical preliminaries • Strap-‐down inertial navigation • Sensor fusion for pose estimation • Worked-‐out example
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
• Introductory remarks • Mathematical preliminaries • Strap-‐down inertial navigation • Sensor fusion for pose estimation • Worked-‐out example
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
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
Right-‐hand rule
North-‐East-‐Down (NED) convention
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
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
!!!
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
⎡
⎣
⎢⎢⎢
⎤
⎦
⎥⎥⎥
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
!!!
R X(ϕ)=1 0 00 cosϕ sinϕ0 −sinϕ cosϕ
⎡
⎣
⎢⎢⎢
⎤
⎦
⎥⎥⎥
RY (ϑ )=0 0 10 1 0±1 0 0
⎡
⎣
⎢⎢⎢
⎤
⎦
⎥⎥⎥
RZ(ψ )=cosψ sinψ 0−sinψ cosψ 00 0 1
⎡
⎣
⎢⎢⎢
⎤
⎦
⎥⎥⎥
!!!
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
• Introductory remarks • Mathematical preliminaries • Strap-‐down inertial navigation • Sensor fusion for pose estimation • Worked-‐out example
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
!!!
R X(ϕ)=1 0 00 cosϕ sinϕ0 −sinϕ cosϕ
⎡
⎣
⎢⎢⎢
⎤
⎦
⎥⎥⎥
RY (ϑ )=cosϑ 0 −sinϑ0 1 0
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:
Scale factor error Misalignment error
Bias Measurement noise
Using carefully implemented in-‐Kield calibration, the following model can be considered valid:
• 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:
Scale factor error Misalignment error
Soft iron bias Measurement noise
Hard iron bias Sensor bias
Using carefully implemented in-‐Kield calibration, the following model can be considered valid:
• 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
• Introductory remarks • Mathematical preliminaries • Strap-‐down inertial navigation • Sensor fusion for pose estimation • Worked-‐out example
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
Insights into the EKF development
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
Insights into the EKF development
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
• Introductory remarks • Mathematical preliminaries • Strap-‐down inertial navigation • Sensor fusion for pose estimation • Worked-‐out example
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!