ITRM20120641A1 - PEDESTRIAN NAVIGATION SYSTEM USING INERIAL DATA ARTIFICIAL NEURAL NETWORKS AND PSEUDOMISURE FOR ERROR CORRECTION - Google Patents

Description

<DESCRIPTION of the invention having as TITLE:> Pedestrian Navigation System using inertial data, Artificial Neural Networks and pseudomeasures for error correction

State of the art

The system starts from the standard subsystems indicated below to synthesize an innovative system.

a) Artificial Neural Networks

It starts from subsystems such as: Multilayer Perceptrons (MLP) ([1], Fig. 4.1) and in particular two different alternative configurations, which are recurrent MLP (RMLP) ([1], Fig. 15.1 ) and systems with time back propagation (BPTT) ([1], Sec.15.7). Use of bidirectional subsystems, ie with causal parts in tandem with retrocausal parts [2], [4] and multistream technologies [3].

b) Error correction

Several approaches have been developed for applying corrections to dynamic equations for inertial sensor-based position estimation. In [5] the correction is made through a linear interpolation between the three speed components estimated between consecutive steps, imposing a null value at the end of one step and at the beginning of the next. This approach has the advantage of being simple and of modest computational load, but has shown (from the results obtained in [5]) to be less accurate and more prone to accumulation of errors over time.

The approach widely adopted in current systems is based on the Extended and Complementary Kalman Filters (ECKF) or on the Unscented Kalman Filter [6], in which, in correspondence with the stasis condition of the foot, it is assumed that the speed of the foot is null and therefore the existence of a pseudo-measurement of the error of the estimated speed is required, exactly equal to the last value of the estimated speed before encountering the stasis condition. This approach, originally developed in [7] is now adopted (practically without any relevant variant) in all bibliographic ([8], [9], [12], [14]) or patent ([10], [11], [13], [15]) from the recent past to the present day.

Other pseudo-measurements can be inferred or assumed by the use of additional instruments, external to the IMU, such as GPS, magnetometers, barometers, odometers, etc.

Description of the invention

The location and tracking of pedestrians in indoor environments where GPS can be intermittent, unreliable or absent, is of increasing importance in missions where timeliness is the critical aspect. Among these there is the location of first aid workers (such as members of a fire brigade team) in buildings where the location infrastructures are not present, while the critical timing of the operation prevents deployment of such infrastructures before starting the mission.

Fig. 1 describes a device (object of the invention) that can be worn by a pedestrian operator that provides his current position and path taken, based on the processing of the quantities measured by an inertial unit, acceleration and angular velocity.

The device includes the following cascaded subsystems:

â € ¢ An Inertial Measurements (IMU) sensor that outputs the time sequences of the acceleration and angular velocity vectors, indicated as raw data. These, whose typical cadence is hundreds of Hertz, are corrupted by additive noise and thermal drift (indicated with 1 in Fig. 1).

â € ¢ A non-linear filter for interpolation of raw data, based on machine learning of the characteristics of the human step implemented by an Artificial Neural Network (ANN) (indicated with 2 in Fig.1).

â € ¢ A subsystem for the integration of the equations of motion (navigation) to estimate position, speed and path by processing the interpolated inertial quantities (indicated with 3 in Fig.1).

â € ¢ Synthesis of pseudomeasures based on subjective biometric rules (indicated with 4 in Fig. 1) to estimate the errors (indicated with 5 in Fig. 1) to be subtracted from the preliminary estimates of position, speed and path (indicated with 6 in Fig. .1) and provide the final corrected estimates (indicated with 7 in Fig. 1).

You can have the following processing alternatives:

1. Subsystems 1 and 3 are present in all configurations.

2. Subsystem 2 is bypassed during an initial learning phase on a specific operator.

3. Subsystems 4, 5 and 6 are present only in the presence of pseudomeasures.

4. Alternatives 2 and 3 can be chosen independently.

1. Interpolative Filtering through Artificial Neural Networks (Fig. 1, Subsystem 2)

In a broad sense, during the human step the temporal structures of acceleration and angular velocity are constrained and mutually dependent because both are produced by the same structure of human locomotion. The temporal structures depend on where the device is placed by the operator, in the heel of a shoe, in the belt or in a pocket.

Artificial Neural Networks (ANNs) are used to automatically learn the temporal structures and the dependencies between acceleration and angular velocity in order to filter the noise of the inertial device and components not typical of the human step, which could be caused by the IMU, such as drift . The following structures are used:

1.1 Recurrent Multilayer Perceptron with delayed output

Fig. 2 indicates a Recurrent Multilayer Perceptron (RMLP), based on a Multilayer Perceptron (Subsystem 1) plus feedback from exit to entry:

â € ¢ Tsà ̈ the sampling interval

â € ¢ Input (2) u (n) is a vector of raw data with six components relative to the current instant (nTs), obtained by stacking the two vectors acceleration and angular velocity

â € ¢ The output y (n-k) (3) is a vector of filtered data with 6 components, obtained by stacking the filtered vectors of acceleration and angular velocity, which is obtained from a linear weighting of the state vector relative to the ™ latest hidden status of MLP. The output represents the filtering of the raw data u (n-k). With reference to RMLP in ([1], Fig. 15.1) with respect to the current instant, the output is not extrapolated by one step but is instead delayed by kTsseconds.

â € ¢ The blocks (4) are delay lines equal to Ts

â € ¢ The vector difference between the (noisy) training data u (n-k) (5) and the filtered (interpolated) vector corresponding to it y (n-k) is reported (feedback) at the input of the MLP (6) to update its weights.

<n n ∠’k − 1>

â € ¢ The raw input data {u (.)} n ∠'k 1, and the filtered data {y (.)} nâˆ' h which constitute the feedback from the output to the input, enter a Floating Window (W) in (7) of temporal data composed of two sub-windows related to data, respectively raw (8) and filtered (9).

â € ¢ The time extension of the window is hTsseconds.

Therefore, with respect to the central time index (n-k), relative to the release of RMLP, (W) includes raw data in the future and filtered data in the past. The time delay (latency) kTs between the current raw data u (n) and the RMLP output y (n-k) is less than the maximum acceptable delay for the presentation of the filtered data (~ two / three seconds).

The time extension of the Window (W) is about 5 seconds, and includes the duration of a few

typical step. RMLP will be pre-trained offline to get a preliminary set of weights on one or more

operators, with different types of steps, before being used in real operations by a specific one

operator. During the operations of that operator, RMLP will refine the weights on in real time

the latter.

1.2 Updating Weights via the Extended Kalman Filter (EKF)

Fig. 3 indicates a subsystem for updating RMLP weights, which includes an RMLP

(Subsystem 1) and an EKF (Subsystem 2). The quantity u (n) (3) entering the Subsystem (1) is ̈

a composite vector obtained by stacking all the vectors contained in the input window in Fig. 2. The weight vector w (n), obtained by stacking all the scalar weights of all the layers of MLP, is assumed

as the state vector of an Extended Kalman Filter (EKF). For the evolution of the weights a

dynamic model:

w (n + 1) = w (n) + z (n) (1)

Where z (n), the state noise vector, has a covariance matrix QÏ ‰.

At instant n, RMLP operates on u (n) with an extrapolated estimate of the weight vector w <Ë †> n / n ∠’1 (4) for

produce the output vector y (n-k) (5). By giving EKF the filtering error, ie the

difference between u (n-k) (6) and y (n-k), it produces as output the extrapolated estimate (w <Ë †> n 1 / n) by preparing the

overall system for the subsequent iteration relating to instant n + 1.

The observable output of RMLP is given by:

y (n∠’k) = b (w <Ë †> n / n− 1, u (n)) (2)

where b () denotes the vector nonlinear input / output relationship of RMLP, which depends on the

observable estimate of the weights w <Ë †> n / n ∠’1, from the vector input u (n) and from the structure (i.e. connections between

nodes of adjacent layers) of MLP. Nonlinearity is induced by nonlinear sigmoid functions (y = a

tanh (bx)) present in MLP synapses. The relationship between RMLP input and output is modeled as

follows:

u (n∠’k) = y (n − k) r (n ∠’k) = b (w <Ë †> n / n− 1, u (n)) r (n ∠’k) (3)

Where r (n-k) is the output noise (with QÎ1⁄2 covariance matrix) induced by the additive input noise which is the output noise from the inertial sensor. The error vector that is used to update the weights is:

e (n∠’k) = u (n − k) ∠’y (n − k) (4)

Due to the state noise vector in eq. (1), the true value of the weight w (n) is not known. The (observable) estimate of the weight w <Ë †> n / n ∠’1 and the relative quantities associated with EKF are:

w <Ë †> n + 1 / n = w <Ë †> n / nâˆ'1 + Gn [u (nâˆ'k) âˆ'b (w <Ë †> n / n âˆ'1, u (n) ]

(5)

<T ∠’1>

<G> n <= P> n / n−1 <B> n [<B T>

n <P> n / n−1 <B> n <+ Q> v] (6)

<P> n + 1 / n <= [I −G> n <B> n <] P> n / n ∠’1 <+ Q> Ï ‰ (7)

Equations from (5) to (7) indicate the evolution of the estimate of the extrapolated weight vector, of the Kalman Gne gain of the covariance matrix of the weight error. We define:

t: number of scalar weights in RMLP

p: number of RMLP output scalar quantities, ie the six components of acceleration and angular velocity.

The only non-linearity of EKF is the vector function b (.), Used to calculate the measurement matrix B, which has dimensions (p, t): Bn (h, k) is defined as the partial derivative of the h-th component of the vector function b (.) with respect to the k-th component of the weight vector w <Ë †> n / n ∠'1.

Bn is evaluated at the current point w <Ë †> n / n ∠'1. The derivatives are obtained through the Real-time Recurrent Learning algorithm ([1], Sec. 15,8), which uses a retro- propagation.

1.3 Multi-stream Training (Fig. 4).

The innovative subsystem indicated here is obtained starting from the Recurrent Multilayer Perceptron in Fig. 2.

The temporal half-window of the HW raw data in Fig. 2 is divided into m sub-half-windows (SHW) out of phase in time (1â € ™, 2â € ™, ..., mâ € ™), each with a temporal extension (1 / m) with respect to the starting semi-window. The number of raw data in each sub-half window is equal to k / m.

Similarly, the filtered data enter m sub-half windows (1â € ™ â € ™, 2â € ™ â € ™, ..., mâ € ™ â € ™) out of phase in time each of time extension (1 / m) with respect to starting semi-window. The number of data filtered in each sub-half window is equal to (h-k) / m.

The data relating to each pair of m HW are sent to a distinct Sub RMLP (SRMLP, Subsystem 1, 2, â € ¦, m).

A composite error vector is formed by stacking the m vector errors of the m SRMLPs, whose size is m times that of the starting RMLP.

The set of weights of each of the SRMLPs is bound to be identical to that of the others: the composite error is used to simultaneously update the shared set of weights of the (m) SRMLPs via an EKF, similarly that in Fig. 3.

At the current time n:

<n> The set of raw data in the (m) sub-windows (1â € ™, 2â € ™, ..., mâ € ™) is {u (.)} knâˆ'1n ∠'k m, n ∠'<(> m âˆ' <1)> k

{u (<.>)} m

2

n ∠’k1 ...., {u (<.>)} m

n ∠’k1

m,

<n> ∠'<k> âˆ' <1> The set of data filtered in the (m) sub-half windows (1â € ™ â € ™, 2â € ™ â € ™, ..., mâ € ™ â € ™) à ̈ {y (.)} <m> hn ∠', âˆ' <2 kn> ∠'m <1 n> âˆ' <k> ∠'<1>

{y (<.>)} <m>

k h

n ∠’− y (<.>)} n ∠’<m> − <1> k ∠’<h>

m m, ...., {

m m

The filtered outputs of m SRMLP, obtained with the common set of weights are

<2 k> y (n ∠’<k>), y (n −), ..., y (n ∠’k)

m m

The most recent (m -1) filtered data, i.e. except the last, relative to distinct temporal indices, are

sent to (m-1) shift registers (shift registers <(s1, s2, sm)>, (1â € ™ â € ™ â € ™, 2â € ™ â € ™ â € ™, ..., (m -1) â € ™ â € ™ â € ™) that

act as delay lines of decreasing lengths.

∠’filtered quantities relating to the same temporal index (n âˆ'k) are sent simultaneously in memory R (m), which are different because they are produced with sets of weights relating to different instants. These quantities are sent to a combiner (U) to obtain in F the final value y (n âˆ'k) of the filtered quantity for this instant.

The purpose of using Multi-stream training is to reduce the complexity of the starting network, i.e. the number of independent MLP weights, without compromising performance. In fact, if to reduce the complexity instead of using the (m) sub-half-windows indicated above if only one were used, due to the rapid evolution of the inertial quantities in the raw data sub-window, induced by the temporal evolution of human walking, the weights would vary rapidly and this would be problematic for their adaptation over time. Instead, using the (m) sub-windows of raw data out of phase in time, different sections of the human step occur simultaneously to the entire composite network (m), the constrained update of the weights that simultaneously takes into account the (m) errors of the (m) SRMLP, and this poses fewer dynamic problems for the simultaneous constrained adaptation of weights.

Furthermore, the composite structure has a â € œlongerâ € memory than both each of the (m) Subsystems and the starting Subsystem of Fig. 2. In fact in the latter, since the update of the weights is driven by the error at the instant (n âˆ'k) the input weights relative to raw data at instants far from the central one (n ∠' k) tend to become evanescent, effectively reducing the effective duration of the window with respect to the nominal duration (hTs). The multi-flow system alleviates this drawback.

1.4 Back propagation over time (BPTT [1], Section 15.7)

The subsystem uses h 1 cascaded copies of non-recurring MLPs of identical structure (MLPh-k, MLPh-k-1, ..., MLP1), indicated in Fig. 5 with (h-k, h-k-1, ..., 1).

a) At the instant n MLPh-k, based on the raw data u (n∠'h) in input (indicated with (nâˆ' h) in Fig. 5), it provides the value of its status in output (n∠'h 1) 'which in turn is one of the inputs to the next MLP to its right, the other input being the raw data (nâˆ' h 1). The set of the elaboration of the MLP sections in cascade is used to predict the output of the overall subsystem, i.e. the extrapolated data y (n 'k) in MLP1 (linear combination of the components of the state of the last hidden layer s (n∠'k) obtained by weighting with

<n> ∠'<k> âˆ' <1> the matrix C: y (n∠'k) = Cs (t âˆ' k) based on the inputs, i.e. raw data {u (.)} n ∠'h and values <n ∠'k âˆ' 1>

of states {s (.)} n∠’h 1. Note that only in the first MLP on the left in Fig. 5 the data from the input state s (n− h) ((n∠’h) ') is absent.

b) At the next instant (n + 1) the least recent section of the MLP relative to the instant (n∠'h) is discarded to the left and a new MLP is considered on the right with inputs u (n âˆ' k), s (n ∠'k). In this MLP you copy the weights of the MLP to its left. Then by propagating the input signal u (n∠'h 1) through all the sections, we calculate the new extrapolated value y (nk + 1) and the error u (n âˆ' k 1) ∠'y (n ∠'k 1)

c) The error is used to update the weights of the last MLP on the right, already set up via the copy indicated above, via EKF. The other MLPs have weights that were updated in the previous moments.

By placing oranâ † ’n + 1, a new cycle aâ †’ bâ † ’c starts again.

The processing system is a floating window, over time, of (h-k) MLP subsystems.

Compared to an RMLP neural network, the BPTT structure is less general but less burdensome from a computational point of view and does not incur potential instability problems, since MLPin have no feedback.

1.5 Retrocausal subsystem

The subsystem uses k cascaded copies of non-recurring MLPs of identical structure (MLPk, MLPk-1, ..., MLP1), indicated in Fig. 6 with (k, k-1, ..., 1).

The processing indicated below proceeds in the same way as in the Subsystem in Fig. 5, except that in this case the time goes backwards proceeding in the cascade of the various MLP sections of the structure. This subsystem, in which inputs to RMLP subsections at a certain instant cause outputs at previous instants, is referred to as retrocausal. Similarly, the system of Fig.5 in which the inputs to RMLP subsections at a certain instant cause outputs at subsequent instants is indicated as causal.

d) At instant n, MLPk, based on the raw data u (n) in input (indicated with (n) in Fig. 6) provides, the value of its retrocausal state in output (n âˆ'1) 'which in turn it is one of the inputs to the next MLP on its left, the other input being the raw data (n âˆ'1). The set of the elaboration of the MLP sections in cascade is used to predict the output of the overall subsystem, i.e. the extrapolated retrocausal datum y <b> (n∠'k) in MLP1 (linear combination of the components of the retrocausal state of the € ™ last hidden layer s <b> (n∠'k) obtained by weighting with the matrix C: y <b> (nâˆ' k) = Cs <b> (t ∠'k) on the basis of

<n> ∠'<k> + <1 n âˆ' k 1> inputs, i.e. raw data {u (.)} n and retrocausal state values {s <b> (.)} nâˆ'1. Note that only in the first MLP on the right in Fig. 6 the data from the retrocausal state in input s <b> (n) ((n) ') is absent.

e) At the next instant (n + 1) the least recent MLP section relative to the instant (n∠’k) is discarded on the left and a new section on the right with input u (n 1) is considered. In this section we copy the weights of the MLP to its left. Then by propagating the input signal u (n 1) through all the sections, we calculate the new extrapolated value y <b> (n-k + 1) and the error u (n ∠'k 1) âˆ' y <b> (n ∠'k 1)

f) The error is used to update the weights of the last MLP on the right, already prepared via the copy indicated above, via EKF. The other MLPs have weights that were updated in the previous moments.

By placing oranâ † ’n + 1, a new cycle of dâ †’ andâ † ’f starts again.

The processing system is a floating window, over time, of k MLP subsystems.

The global output y <g> (n-k) is obtained by combining the relative outputs from the causal subsystems and

retrocausal. The combined use of the two subsystems, having as input a window of h data

rough, it is referred to as a Bidirectional System.

1.6 BPTT and Multirate

At each instant, m Bidirectional Subsystems are considered with sub-windows out of phase in time and temporal indices of the global outputs out of phase accordingly, similarly as in Fig. 4. These outputs are sent to m-1 delay lines of variable lengths so that their outputs, relating to the same time index, are combined in order to obtain the final estimate. The weights of the m structures are updated simultaneously via an Extended Kalman Filter, Fig. 2.

1.7 Uncoupled Extended Kalman Filter

Let us consider the error covariance matrices in EKF in the various subsystems indicated above: the analysis of the initial training indicates when a diagonal block structure can conveniently approximate these matrices, without appreciably decreasing performance; with this simplification the weight of the EKF matrix inversion calculation is reduced.

In Fig. 1 the filtered output of the extrapolating filter based on neural networks (2) is sent to the processing stage (3) to obtain the position and path data.

2. Navigation Equations (Subsystem 3, Fig.1)

The prefiltered inertial measurements (acceleration and angular velocity) - represented in the operator's reference system (body frame, indicated with b) - measured with respect to an inertial reference (i) are sent to the subsystem (Subsystem 3, Fig. 1) to obtain position and route data.

It is also considered a reference system in solidarity with the Earth (e). In the following annotation, the indices refer to the reference in which the measure is represented; subscripts, such as those in <Ï ‰ ie>, refer to the angular velocity vector of the reference (e) with respect to the reference (i), which in this case is the Earth's angular velocity represented in the Earth's reference frame.

The indices (b) and (e) indicate representations of measures with respect to the references of the operator and of the Earth.

∠’to: current time

∠’Ts: sampling interval

∠’r e: position vector

∠’v <e>: velocity vector

∠’g <e>: acceleration vector of gravity

∠’R e

b: rotation matrix to pass from the operator to the terrestrial reference

Ï ‰ b, sb: angular velocity and acceleration in the reference of the operator coming from measurements of the inertial sensor and filtered by the first processing subsystem (Fig. 1, Subsystem 2). Yes it has

Ï ‰ <b> â ‰ ¡Ï ‰ b

<Ï ‰> b

ie <= R> b

and <Ï ‰> e

ie; <Ï ‰> b

eb <= Ï ‰> b b b b e

ib; ib <âˆ’Ï ‰> ie <= Ï ‰ ∠’R> e <Ï ‰> ie (8)

∠’Ï ‰ b

eb: angular velocity between the references of the operator and ground, indicated in the reference of the operator.

∠’I3: (3,3) identity matrix

∠’03: (3,3) null matrix

∠’â„ ¦ b

eb: rotation antisymmetric matrix associated with ‰ b

and b.

ï £ "0âˆ'Ï ‰ b

eb <,> y Ï ‰ b

eb <,> zï £ ¶

<ï £ ·>

<â „¦> b <ï £ ¬>

eb <=> ï £ ¬Ï ‰ b

eb, y 0 <∠’> Ï ‰ eb, xï £ <=> Ant (Ï ‰ b

and b)

ï £ ¬ b

ï £ <∠’> Ï ‰ eb, z Ï ‰ eb, x 0 ï £ ·

ï £ ̧ (9)

The indices (x, y, z) refer to the three components of the angular velocity Ï ‰ b

and b. In eq. (9) we define the antisymmetric matrix Ant (r) = R associated with the generic vector (r).

The following navigation equations indicate how the inertial measurements in output from the inertial sensor, acceleration and angular velocity, after being filtered by the Subsystem 1 of Fig. 1, are used to calculate position and speed in the earth reference, and rotation matrix R and

b.

ï £ «r e

<t> 0+ <Ts> ï £ ¶ I3 Ts I 3 r e

t0ï £ ¶ ï £ «0, 5T 2

s I 3ï £ ¶

ï £ ¬ï £ ¬ ï £ ¶ï £ «

ï £ · ï £ «

= e ï £ ¬ R

ï £ v e

t 0 Tsï £ · ï £ ¬ï £ ¬

ï £ ̧ ï £ 03 Iï £ ï £ ï £ ¬ï £ ¬ (e and b) 0

3 ï £ ̧ï £ vï £ · ï £ ·

t 0 ï £ ¬ g

ï £ ̧ ï £ Tï £ · ï £ · b s

s I <t>

3 ï £ ̧

(10)

R <e> (t0) + ï £ "<sin> Ïƒï £ ¶ − <cos> Ïƒï £ ¶ ï £ ¹

b (t0 + Ts) = R <e ï £ ®>

bï £ ¬ï £ · (T b (t0)) ï £ ¬ï £ «<1>

ï £ ̄I3 e (T b (t

σsâ „¦ <b> + s <â„ ¦ b>

0)) <2>

ï £ ° ï £ ï £ ̧ ï £ σ 2 ï £ · e

ï £ ̧ ï £ º

ï £ "(11)

σ = (Ï ‰ <b>

eb (t0)) Ts

(12)

3. Estimation of the error through the Complementary Extended Kalman Filter (CEKF)

(Subsystem 5, Fig. 1)

In addition to inertial measurements, other types of information are provided, inertial pseudomeasures, which contribute to obtaining more accurate position and speed estimates. The filtered inertial data are â € œobjectiveâ € as they come from measuring instruments while the pseudomeasures originate from subjective biometric rules, which model characteristics of human gait and are not produced by measuring instruments. Filtering through the Complementary Extended Kalman Filter (CEKF) is introduced.

State vector error with dimension (15,1), defined by

<b b>

<Î ́x> k <=> [<ε> k <; Î ́Ï ‰> k <; Î ́re>

k <; Î ́ve>

k <; Î ́ s> k] (13)

R <Ë †> e

b: true rotation matrix (not known) to pass from reference (b) to (e)

R and

b: rotation matrix estimated to pass from reference (b) to (e)

<Re>

b <= (1−⠄¦ e>

ε <) R Ë †> b (14) Î ́εk = (Î ́εk, x, Î ́εk, y, Î ́εk, z) <T> (15)

<â „¦> εk <= Ant (Î ́> εk <)> (16) It is assumed for the vector error a state evolution equation

)

Î ́xk = Î ̈kÎ ́xk−1 + wk ∠’1 (17) where E [wkw <T>

k] = Qk

and a corresponding equation for the evolution of the error estimate

<Î ́x> k / k−1 <= Î ̈> k <Î ́ x> k−1 / k ∠’1 (18) where, according to the CEKF notation

∠’Î ́xk / k − 1à ̈ the estimate of the vector error extrapolated by a step

∠’Î ́xk / kà ̈ the filtered vector error

ï £ «and ï £ ¶

ï £ ¬ I3 Ts Rbk / k âˆ'103 03 03 ï £ ·

ï £ ¬ 03 I3 03 03 03 ï £ ·

ï £ ¬

Î ̈k = 03 03 I3 T s I30 ï £ ï £ ¬ 3 ï £ (19) ï £ ¬ e

âˆ'T sS 'e

<k> ∠’<1> 03 03 I3 Ts R ï £ · ï £ ¬ bk / k âˆ'1ï £ ·

ï £ ¬

ï £ 03 03 03 03 I 3 ï £ ·

ï £ ̧

The state transition matrices in eq. (17), not specified here, and in eq. (18) and (19) contain respectively real but unknown quantities and the corresponding estimates.

s <'and e>

k∠’1 = Rb, k / k−1 (s <b>

k−1∠’Î ́ s <b>

k−1 / k ∠’1) (20) S <'e>

kAnt (s <'e>

∠’1 = k − 1) (21) The pseudomeasures model is

<z> k <= H Î ́x> k <+ n> k (22) Where

∠’(zk) is the measurement vector affected by noise

∠’H is a measurement matrix (9,15), specified below

∠’(nk) is the measurement noise

∠’Rk = E (nknk <T>) is the covariance matrix of the measurement error

∠’Kkà ̈ the gain of Kalman

The filtered estimate of the error is obtained from the extrapolated estimate as follows:

(23)

mkà is a (9,1) composite pseudo-measure vector, obtained by stacking three pseudomeasure vectors relative to angular velocity, position and velocity respectively. We define

(24)

Î ́v <e>

k / k = [03,03,03, I3,03] Î ́ xk / k (25)

r e e e

k / k = rk / k−1∠’Î ́ rk / k (26)

v and e

k / vk / k Î ́ v e

k = −1∠’k / k (27)

The quantities in eqs. (24), (25) are used in eqs. (26), (27) to correct the estimates of position and velocity in (Fig.1, Subsystem 6) so that, by the combined action of Kalman filtering and corrections, the accuracy in the estimates increases in the time.

3.1 Summary of the error equations

The equations, concerning a time step, are:

1. Corrections with respect to the drift of accelerometers and gyroscopes

Ï ‰ 'b b b

k∠’1 = Ï ‰ k−1 − Î ́Ï ‰ k−1 / k−1 (28) s' b

k−1 = s b b

k−1∠’Î ́ sk−1 / k− 1 (29)

â „¦ '<b>

k−1 = Ant (Ï ‰ <'b>

k ∠’1) (30)

eï £ ® ï £ «sin σ '2

k ∠’1ï £ ¶

ï £ · 'bï £ "1 − cos σ'

k ∠’1ï £ ¶ 'b2ï £ ¹

<R> and

b, k / kâˆ'1 = <R> b, kâˆ'1 / k âˆ'1ï £ ̄ <I> 3ï £ ¬ï £ ¬ (<T> â „¦k ∠'1)' (<T> s k ∠'1) ï £ º (31)

ï £ ̄ k ∠’1ï £ · ï £ ¬ï £ ¬

ï £ ° ï £ σ 's

ï £ ̧ ï £ σk ∠’1ï £ · ï £ · â„ ¦

ï £ ̧ ï £ ºï £ "

σ '

k∠’1 = Ï ‰ 'b

k −1 T s (32) 2. Subtraction of gravity

s <'and' b T>

k∠’1 = R <e>

b, k / k−1sk ∠’1− [0,0, g] (33) 3. Navigation equations with position and velocity estimates in the terrestrial reference.

ï £ «r e ï £« 3 T ï £ «r e ï £« 0, 5T ï £ ¶

ï £ ¬ï £ ¬ <k> / <k> −1ï £ ¶ I <s> I 2

3ï £ ¶

ï £ · ï £ ¬k−1 / k ∠’1ï £ ¶ s I 3 'e

ï £ · <ï £ ·> = e + s (34) ï £ v e

k / k ∠’1ï £ ¬ï £ ¬

ï £ ̧ ï £ 03 I 3ï £

ï £ ̧ï £ ¬

ï £ vkâˆ'1 / k âˆ'ï £ · <ï £ ·>

1ï £ ̧ï £ ¬ï £ ¬

ï £ T s I 3ï £ · ï £ · k <∠’> 1

ï £ ̧

4. Error prediction where the matrix ̈k is given by eq. (19).

<Î ́x> k / k−1 <= Î ̈> k <Î ́ x> k−1 / k ∠’1 (18) 5. Calculation of the gain of KalmanKk:

<∠’1>

<K> k <= P> k / k−1 <H (HP> k / k−1 <HT R> k <)> (35) 6. Calculation of the filtered error

<Î ́x> k / k <= Î ́x> k / kâˆ'1 <+ K> k <(m> k <âˆ'H Î ́ x> k / k âˆ'1 <)> (23) 7. Calculation of the covariance matrix of the filtered error

<T>

<P> k / k <= (Iâˆ'K> k <H) P> k / k âˆ'1 <(Iâˆ'K> k <H) K> k <R> k <K T>

k (36) 8. Calculation of the covariance matrix of the extrapolated error

P T

k + 1 / k = Î ̈kPk / kÎ ̈k + Q k (37)

9. Correction of navigation equations

Î ́r <e>

k / k = (03.03, I3.03.03) Î ́ xk / k (24)

Î ́v <e>

k / k = (03.03.03, I3.03) Î ́ xk / k (25)

r e e e

k / k = rk / k−1∠’Î ́ rk / k (26)

v e and v e

k / k = vk / k−1∠’Î ́k / k (27)

10. Calculation of the filtered rotation matrix:

Î ́Ï • k / k = (I3,03,03,03,03) Î ́ xk / k (38)

Mk = Ant (Î ́Ï • k / k) (39)

R <e> = () (−M <−1>

k) R <e>

bk / k2I Mk2Ibk / k ∠’1 (40)

The quantity Î ́xkâˆ'1 / k ∠’1à is used in eq. (18) to obtain Î ́xk / k ∠'1, which is introduced in the pseudomeasurement equation (23) to obtain the filtered error Î ́xk / k, which in turn is used to correct the position estimates and velocity in equations (26) and (27). However, before using eq. (18) all the components of Î ́xkâˆ'1 / k ∠’1, except those relating to acceleration and angular velocity, must be zeroed: this is required because otherwise a correction already implemented would be propagated over time.

Summarizing from eq. (18) we have:

ε and b

k / k−1 = TsRbk / k−1Î ́Ï ‰ k−1 / k∠’1 (41) Î ́Ï ‰ b b

k / k−1 = Î ́Ï ‰ k−1 / k∠’1 (42) Î ́r and

k / k−1 = 0 (43) Î ́v e e b

k / k−1 = TsRbk / k−1Î ́ sk−1 / k∠’1 (44) Î ́s b b

k / kâˆ'1 = Î ́ skâˆ'1 / k∠'1 (45) When pseudo-measurements are not available, the navigation equations (10), (11) and (12) are used and no error correction until a new pseudomeasure is available. In this case the following equations are used to propagate the covariance estimate of the error.

P T

k + 1 / k = Î ̈kPk / kÎ ̈k + Î ± k (46)

Pk + 1 / k + 1 = Pk 1 / k (47)

Kk = 0 (48)

4. Pseudomeasures (Subsystem 4, Fig.1)

In Subsystem 4, Fig. 1 the pseudo measures are modeled as subjective combinations of the velocity and position estimates derived from Subsystem 3.

When the inertial sensor is placed on a shoe, in each step there is a short interval (a few hundred msecs.) In which the foot has approximately zero speed. The zero speed condition is assumed for each time sample in which three subjective criteria are simultaneously valid:

g <e> âˆ'T <b>

1â ‰ ¤ s â ‰ ¤g <e> + T1 (49)

Standard deviation of s <b> â ‰ ¤T2 (50)

Ï ‰ <b> â ‰ ¤T3 (51)

where T1, T2, T3 represent suitably chosen thresholds.

The pseudomeasures in Eq. (23) are modeled as follows:

â € ¢ Update with Zero Position - Speed (ZPVUP)

ï £ «0 0

ï £ ¬ 3 03 3 03 03ï £ ¶

ï £

H = ï £ ¬03 03 Iz3 03 03ï £ · (52) ï £ ¬

ï £ 03 03 03 I3 03ï £

ï £ ̧

ï £ «1 0 0ï £ ¶

ï £ ¬ ï £ ·

<Iz 3> = ï £ ¬0 1 0ï £

ï £ ¬ (53) ï £ 0 0 0ï £

ï £ ̧

The pseudomeasurement vector (9,1) is defined as:

m]; [r <e e e T>

k = ([0,0,0 <T>

k / k−1, x−rk−1 / k−1, x, rk / k−1, y−r <e>

k−1 / k−1, y, 0]; v <e>

k / k ∠’1) (54)

Eq. (54) indicates that the pseudo errors are:

- v <e> k / k−1 â ‰ ¡ve

k / k −1 ∠’(0,0,0) T, relative to the speed

- Difference between extrapolated and filtered position in the horizontal plane.

These errors decrease over time by the action of KF, so that the speed estimate is forced towards zero and the position estimate in the horizontal plane is forced towards a constant value.

A pseudo position error is introduced only for the horizontal plane but not for the vertical, thus avoiding correction constraints along the vertical: in fact, if a constraint was introduced along the vertical direction, the vertical component of the position estimate would be affected by a drift due to the difference between the hypothesized and the actual value of the acceleration of gravity, which would not be compensated by the Kalman Filter.

â € ¢ Update with Zero Position - Speed - Angular Speed.

The constraint can be applied during an initial pause (tens of seconds) before starting the real walk, or in any interval in which a “long” pause occurs (in practice no less than about 10 seconds). In this case, the estimate of the angular velocity is also assumed as an error to be minimized through the action of the Kalman Filter, so that â € “the estimates of velocity and angular velocity tend to zero and - the estimate of position tends to a constant value.

We have

ï £ «0

ï £ ¬ 3 I3 03 03 03ï £ ¶

ï £

H = ï £ ¬03 03 I3 03 03ï £ · (55)

ï £ ¬

ï £ 03 03 03 I3 03ï £

ï £ ̧

And the pseudomeasure vector is

ï £ ® Ï ‰ b

k / k ∠’1 ï £ ¹

ï £ ̄

m = r and ∠’r and ï £ º

k ï £ ̄k / k−1 k−1 / k ∠’1ï £ º (56) ï £ ̄ï £ ° ve

k / k ∠’1 ï £ ºï £"

On the other hand, when the inertial unit (IMU) is not positioned in the heel but at the waist or in a pocket, it is assumed that there are no pseudomeasures, the subsystem hi 4, 5 and 6 of Fig. 1 are absent and the interpolator ANN in Subsystem 2, Fig. 1 plays the role of filtering, in addition to the noise of the inertial sensor, signal components that are not typical of the human step (measured from the waist or from the pocket) such as polarizations and drifts.

Bibliography

[1] S. Haykin: Neural Networks and Learning Machines, 3rd edition, Pearson 2009

[2] M. Schuster and K.K. Paliwal: Bidirectional Recurrent Neural Networks, IEEE Transactions on Signal Processing, Vol. 45, N.11, Nov.1997

[3] L. A. Feldkamp and G. V. Puskorius: Training Controllers for Robustness: Multi-stream DEKF, Proceedings of the IEEE International Conference on Neural Networks, p. 2377-2382

[4] Xiao Hu and D.C. Wunsch II: Time Series Prediction with a Weighted Bidirectional Multi-Stream Extended Kalman Filter, IEEE 2004

[5] A.R. Jimenez, F. Seco, C. Prieto and J. Guevara, â € œA Comparison of Pedestrian Dead-Reckoning Algorithms using a Low-Cost MEMS IMUâ €, WISP 2009, 6th IEEE International Symposium on Intelligent Signal Processing, Budapest, Hungary, 26â € “28 August, 2009

[6] Zampella, F., Khider, M., Robertson, P., Jimenez, A., "Unscented Kalman filter and Magnetic Angular Rate Update (MARU) for an improved Pedestrian Dead-Reckoning", IEEE / ION Position Location and Navigation Symposium (PLANS), 23-26 April 2012

[7] Elwell, J., â € œInertial navigation for the urban warriorâ €, SPIE Conference on Digitization of the Battlespace IV, SPIE vol. 3709, Orlando, FL, (1999)

[8] S. Godha, G. Lachapelle, M.E. Cannon ,, "Integrated GPS / INS System for Pedestrian Navigation in a Signal Degraded Environment", Institute of Navigation (ION), ION GNSS 2006 Best Presentation Awards, 2006

[9] R. Zhang, M. Loschonsky and L.M. Reindl, "Study of Zero Velocity Update for Both Low- and High-Speed Human Activities", International Journal of E-Health and Medical Communications (IJEHMC), Volume 2, Issue 2, 2011.

[10] Positioning Refinement Algorithm, US patent n ° US6,292,751 B1, Sept.18.2001

[11] Vision Based on Zero Velocity Update and Zero Attitude Rate Update, US2012 / 01833179 A1, July 19, 2012

[12] Chi-Chung Lo, Chen-Pin Chiu, Yu-Chee Tseng, Sheng-An Chang, Lun-Chia Kuo, "A Walking Velocity Update technique for pedestrian dead-reckoning applications", IEEE 22nd International Symposium on Personal Indoor and Mobile Radio Communications (PIMRC), 2011

[13] Enhanced Inertial System Performance, US2007 / 0050138 A1, March 1, 2007

[14] I. Skog, P. Handel, J. Nilsson, and J. Rantakokko, â € œZero-velocity detection - an algorithm evaluation, â € Biomedical Engineering, IEEE Transactions on, vol. 57, pp. 2657 â € “ 2666, Nov. 2010

[15] System and Method for Gyroscope Error Estimation, WO 2012/135960 A1 (PCT / CA2012 / 050221), 5 April 2012

Claims (5)

CLAIMS of the invention having as TITLE: Pedestrian Navigation System using inertial data, Artificial Neural Networks and pseudomeasures for error correction 1. Location device wearable by a pedestrian operator and comprising: â € ¢ an inertial sensor configured to measure accelerations and angular velocities of the pedestrian operator and to output raw data indicative of temporal sequences of the accelerations and angular velocities measured; â € ¢ a non-linear interpolation filter that is coupled to the inertial sensor to receive the raw data from the latter and that includes an artificial neural network trained to filter and interpolate the raw data in order to filter noise and thermal drift using weights obtained through automatic learning of human step characteristics; â € ¢ a subsystem of integration of the equations of motion which is coupled to the non-linear interpolation filter to receive the filtered and interpolated data from the latter and which is configured for - estimate the position, speed and path of the pedestrian operator by processing the filtered and interpolated data, and - output corresponding preliminary estimates of position, speed and path; and â € ¢ a subsystem for estimating position and velocity errors which is coupled to the integration subsystem of the equations of motion to receive from the latter the preliminary estimates of position, velocity and path and which is configured for - estimate errors of position and speed on the basis of subjective biometric rules that model characteristics of the human step and are not produced by measuring instruments, and - correct the preliminary estimates of position, speed and path on the basis of the estimated errors, thus obtaining correct final estimates. The location device of claim 1, wherein: â € ¢ the artificial neural network comprises - a plurality of recurring multilayer perceptrons with delayed outputs in a multi-flow configuration, ed - an extended Kalman filter; â € ¢ the different recurring multilayer perceptrons with delayed outputs are configured to - filter and interpolate different time-staggered portions of the raw data using the same set of weights, and - provide corresponding temporally staggered outputs indicative of the filtered and interpolated data; â € ¢ the artificial neural network is configured to temporally align and combine the temporally staggered outputs of the recurrent multilayer perceptrons thus obtaining overall filtered and interpolated data indicative of the accelerations and angular velocities of the pedestrian operator at each sampling instant; and â € ¢ The extended Kalman filter is configured to update the same set of weights used by all recurring multilayer perceptrons. The location device of claim 1, wherein: â € ¢ the artificial neural network comprises - an extended Kalman filter, ed - a plurality of bidirectional systems in multi-flow configuration based on retro-propagation over time, each of which includes a respective causal section and a respective retrocausal section; â € ¢ the different bidirectional systems are configured for - filtering and interpolating different temporally staggered portions of the raw data using the same set of weights, e - provide corresponding temporally staggered outputs indicative of the filtered and interpolated data; â € ¢ the artificial neural network is configured to temporally align and combine the temporally staggered outputs of the bidirectional systems, thus obtaining filtered and interpolated overall data indicative of the accelerations and angular speeds of the pedestrian operator at each sampling instant; and â € ¢ The extended Kalman filter is configured to update the same set of weights used by all bidirectional systems. The location device of claim 3, wherein: â € ¢ the retrocausal section of each bidirectional system includes a retrocausal cascade of k multilayer perceptrons to receive a sequence of k samples of the raw data with the first multilayer perceptron of the retrocausal cascade receiving the sample relative to the sampling instant recent n is the last multilayer perceptron of the retrocausal cascade that receives in input the sample relative to the least recent sampling instant n-k + 1, in which k and n are integers greater than zero and in which to each of the k multilayered perceptrons respective weights of the retrocausal cascade are assigned; â € ¢ each multilayer perceptron of the retrocausal cascade, except the first, receives in input, in addition to a respective sample of the raw data, also a vector indicative of an exit state of the previous multilayered perceptron in the retrocausal cascade; â € ¢ each multilayer perceptron of the retrocausal cascade provides an output indicative of its own output status on the basis of what it receives at the input and the respective assigned weights; â € ¢ the causal section of each bidirectional system includes a causal cascade of h-k multilayer perceptrons to receive a sequence of h-k samples of the raw data with the first multilayer perceptron of the causal cascade receiving the sample relative to the least sampling instant recent n-h is the last multilayer perceptron of the causal cascade that receives in input the sample relative to the most recent sampling instant n-k-1, in which h is an integer greater than k, and in which to each of the h-k multilayer perceptrons of the causal cascade are assigned respective weights; â € ¢ each multilayer perceptron of the causal cascade, except the first one, receives in input, in addition to a respective sample of the raw data, also a vector indicative of an exit state of the previous multilayered perceptron in the causal cascade; â € ¢ each multilayer perceptron of the causal cascade provides an output indicative of its own output status on the basis of what it receives at the input and the respective assigned weights; â € ¢ the causal section of each bidirectional system is configured to perform the following operations for each new sample of raw data: - discard the first multilayer perceptron of the causal cascade; - add a new multilayer perceptron at the end of the causal cascade; - assigning to said new multilayer perceiver weights identical to those of the immediately preceding multilayer perceptron in the causal cascade, in which said new multilayer perceptron provides a preliminary output which is obtained on the basis of the assigned weights identical to those of the immediately preceding multilayer perceptron in the causal cascade and is relative to the sampling instant n-k; - calculate an error indicative of the difference between the preliminary output provided by said new multilayer perceptron and the raw data sample relative to the same sampling instant n-k; and - update the weights assigned to said new multilayer perceptron on the basis of the calculated error, in which said new multilayer perceptron provides a definitive output which is obtained on the basis of the updated weights and is relative to the sampling instant n-k ; â € ¢ the retrocausal section of each bidirectional system is configured to perform the following operations for each new raw data sample: - discard the last multilayer perceptron of the retrocausal cascade; - add a new multilayer perceptron at the head of the retrocausal cascade; - assigning to said new multilayer perceptron weights identical to those of the immediately following multilayer perceptron in the retrocausal cascade, in which the multilayered perceptor at the tail of the retrocausal cascade provides a preliminary output relative to the n-k sampling instant; - calculate an error indicative of the difference between the preliminary output provided by the multilayer perceptron at the end of the retrocausal cascade and the raw data sample relative to the same sampling instant n-k; and - update the weights assigned to the new multilayer perceptor at the head of the retrocausal cascade on the basis of the calculated error, in which the multilayer perceptor at the end of the retrocausal cascade provides a definitive output after updating the weights assigned to the new multilayer perceptor at the head of the retrocausal cascade; â € ¢ for each new sample of raw data, each bidirectional system is configured to combine the final outputs of the causal and retrocausal sections, thus obtaining an overall filtered and interpolated data relating to the sampling instant n-k; and â € ¢ The extended Kalman filter is configured to simultaneously update the weights assigned to the new multilayer perceptrons of the causal and retrocausal cascades of all bidirectional systems. The location device according to any preceding claim, wherein: â € ¢ the inertial sensor is designed to be coupled to a pedestrian operator's shoe; â € ¢ the subsystem for estimating position and speed errors is configured to detect a stasis condition of the pedestrian operator's shoe and a duration of said stasis condition on the basis of filtered and interpolated data provided by the non-linear filter of interpolation and indicators of accelerations of the pedestrian operator; â € ¢ the subsystem for estimating position and velocity errors includes a complementary extended Kalman filter configured to filter and extrapolate position and velocity errors on the basis of vectors of pseudomeasures obtained on the basis of subjective biometric rules; and â € ¢ the subsystem for estimating position and speed errors is also configured for: - when it detects a stasis condition of the pedestrian operator's shoe, input to the complementary extended Kalman filter a pseudomeasurement vector that includes an extrapolated speed and a difference between an extrapolated position and a filtered position in a tangent reference plane to the earth's surface in such a way as to obtain a filtered error at the output of the complementary extended Kalman filter; said extrapolated velocity and said extrapolated and filtered positions being provided by the subsystem of integration of the equations of motion; And - correct the preliminary estimates of position, speed and distance on the basis of the filtered error.