Research Article Simplified Pedestrian Tracking Filters with Positioning and Foot ...downloads.hindawi.com/journals/ijdsn/2014/850835.pdf · 2015. 11. 23. · Simplified Pedestrian

Research ArticleSimplified Pedestrian Tracking Filters with Positioning andFoot-Mounted Inertial Sensors

Henar Martin Juan A Besada Ana M Bernardos Eduardo Metola and Joseacute R Casar

Universidad Politecnica de Madrid ETSI Telecomunicacion Avenida Complutense 30 28040 Madrid Spain

Correspondence should be addressed to Juan A Besada besadagrpssssrupmes

Received 16 April 2014 Revised 6 August 2014 Accepted 7 August 2014 Published 4 September 2014

Academic Editor Andrei Gurtov

Copyright copy 2014 Henar Martin et al This is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

Pedestrian tracking is one of the bases for many ubiquitous context-aware services but it is still an open issue in indoorenvironments or when GPS estimations are not optimal In this paper we propose two novel different data fusion algorithmsto track a pedestrian using current positioning technologies (ie GPS received signal strength localization from Wi-Fi orBluetooth networks etc) and low cost inertial sensors In particular the algorithms rely respectively on an extended Kalmanfilter (EKF) and a simplified complementary Kalman filter (KF) Both approaches have been tested with real data showing clearaccuracy improvement with respect to raw positioning data with much reduced computational cost with respect to previous highperformance solutions in literature The fusion of both inputs is done in a loosely coupled way so the system can adapt to theinfrastructure that is available at a specific moment delivering both outdoors and indoors solutions

1 Introduction

The need to know where a person is dates back to centuriesago in the last decades traditional maps and compasses havebeen increasingly replaced by navigation assistance systemsAt the beginning of this century the use of the global positionsystem (GPS) became popular democratizing the availabilityof location estimation for many civilian servicesThis systemnowadays integrated in every smartphone may be not goodenough for many applications such as those involving stand-alone pedestrian localizationThis is due to theGPS accuracywithin the range of 5ndash20m for urbanindoor environmentswhere typical errors (in the order of 2ndash5meters) are increaseddue to the presence of local error sources (eg multipathsignal attenuation and diffraction) Current generationsof high sensitive GPS receivers may be good enough forsome indoor applications [1] but their performance rapidlydegrades with the number of walls and depends on theirmaterials Therefore the position error is often too largeand the systemrsquos availability is insufficient especially for deepindoor environments Research on personal localization andnavigation systems has attracted a lot of attention since theFederal Communication Commission (FCC) published theEnhanced 911 (E-911) implementation requirements in 1999[2] Many location based services (LBS) have been developed

since then and a wide offer of applications is nowadaysavailable in the most popular application stores [3] Fromintervention tools for emergencies or visually impaired guid-ance to tourism guides augmented with context-aware rec-ommendations [4] all these applications need to track theirusers Obviously the accuracy requirements vary dependingon the service leitmotif

Pedestrian localization presents some challenges whencompared to car or robot tracking On the one hand themovements of vehicles are more predictablemdashas they usuallymove along the predefined paths of the road network whilepedestrians move freely changing their path unexpectedlydue to obstacles or on-the-spot decisions [5] On the otherhand vehicles and robots may have many sensors attachedif needed However a pedestrian will reject wearing a sensorthat hinders his movements due to weight bulkiness and soforth Finally the walking characteristics not only differ fromindividual to individual but also depend on factors difficultto characterize or estimate such as state ofmind type of shoesor type of pavement

Previous works have addressed the problem of pedestriantracking in two different ways (a) relying on supportinginfrastructure or (b) using wearable sensors Solutions in thelatest group have applied traditional localization methodsbased on the information provided by inertial sensors The

Hindawi Publishing CorporationInternational Journal of Distributed Sensor NetworksVolume 2014 Article ID 850835 18 pageshttpdxdoiorg1011552014850835

2 International Journal of Distributed Sensor Networks

use of inertial sensors has privacy advantages over other tech-nologies for localization and tracking such as camera net-works Their low weight makes it possible to integrate themin wearable forms (shoes mobile phones etc) and the pro-cessing of their data requires low computational capabilitiesbeing suitable for real-time applications

Those systems are usually called strap-down systemssince the sensors used are strapped to the body that is goingto be tracked There are two main approaches to deal withthe problem of estimating the position of a person using theinformation provided by inertial sensors (step based) deadreckoning (DR) and inertial navigation systems (INS) DRis based on the analysis of the periodic signals that resultfrom the dynamics of walking in order to count steps Thisenables us to calculate the traversed distance by using an esti-mation of the step length In addition a magnetic compassor a gyroscope is employed to estimate the heading of thedirection ofwalking in order to project the traversed distancein the horizontal plane With respect to the INS method themeasurements of the accelerometer are double integrated tak-ing into account platform orientation according to compassandor gyroscopes to get an estimation of the position changewith time However the influence of the errors of inertialsensors on the position estimation makes it very difficult toachieve continuous accurate localization using stand-aloneinertial navigation systems as the inherent integration ofboth approaches tends to make the error increase alongtime The sensors for pedestrian localization are usually low-grade sensors which means that the errors in the measure-ments have a significant influence over the achieved results(accelerometer and gyroscope errors make the position errorgrow over the time [6]) unless some correctingaiding tech-nique is applied such as zero velocity updates (ZVU) [7]ZVU (ie controlling the instants when the foot is placed onthe ground ad thus the velocity and acceleration are zero) arecommonly used in pedestrian positioning systems In orderto apply such technique the inertial sensors must be in thefeet

The sources of inertial sensor errors and their influenceover the estimation of the pedestrian position are analyzed in[8ndash10] To reduce the impact of these growing errors severalaiding strategies have been proposed [8] Neither of themachieves a general solution that provides enough accuracyfor pedestrian tracking so fusion with other sensors withbounded error statistics is often consideredThe fusion of thedifferent aiding sensors with the inertial information is madeusing techniques such as particle filters [11ndash13] or Kalmanfilters For instance the algorithm in [11] attains an averageerror in the order of 15 meters using particle filters to fusebelt-mounted INS and Wi-Fi RSS positional measurementswhile the proposal in [12] with a very similar approachachieves an error in the order of 45m The similar strategyin [13] provides an average error in the order of 1m In thetwo later cases map-aiding was also implemented

The algorithms proposed in this paper would be imple-mented in a mobile device (ie smartphone) ZVU are usedwhenever the foot is detected on the ground (which occursperiodically as the pedestrian walks) For this reason the sys-tem includes a detector to accurately determine the instants

when the foot of a user is placed on the floor Moreoverthe inertial system is aided with positioning measurementstaken from an external localization system which could beeither a GPS in outdoor environments or a wireless position-ing system in indoor environments Typically those indoorwireless positioning systems perform location estimationfrom RSS measurements of the underlying communicationtechnologies (ie Wi-Fi Bluetooth or ZigBee motes) [14]Many existing solutions are based on Kalman filters (KF)which provide a reduced computational load with respect toparticle filters Some of those works propose a complemen-tary Kalman filter that estimates the errors of the positionvelocity acceleration and the bias of the sensors subtractingthem from the states and measurements for the integrationApproaches such as the ones described in [15 16] includean inertial navigation system that works in parallel with thatcomplementary KF In particular the proposal in [16] focusesspecifically in the detection of the ZVU intervals althoughvery limited experimental results support the design

In [17] the positioning system is based on processingthe RSS measurements from RFID tags in a tightly coupledway The obtained average error is 135m along 4 trajectoryscenarios of 600 550 520 and 1000m of length respectivelyMoreover GPS measurements are integrated in outdoorsenvironment using a loosely coupled complementaryKalmanfilter and magnetometer data is also used to aid in indoornavigation in [18] achieving results with an average erroraround 2m in a 300m long trajectory

In this paper a novel extended Kalman filter (EKF) and asimplified complementary Kalman filter (KF) are proposedboth filters enable the fusion of the position computedthrough one or more inertial sensors with periodic positionupdates provided by a positioning solution based on wirelesstechnologies (GPS or indoor positioning system) The fusionsystem includes some quite important simplifications of sen-sor andmovement modeling with respect to those existing inliterature (specifically on the Foxlinrsquos contribution [15] whichinspires one of the proposed solutions) In the results sectionit is shown how even with those simplifications the systemmaintains a high accuracy being robust enough for manytypical localization applications The main contribution ofour approach is the very small computational load andbatteryconsumption introduced by the filter a feature of paramountimportance in resource and energy-constrained wearableapplications The fusion of both inputs is done in a looselycoupledway so the systemcan adapt to the infrastructure thatis available at a specific moment Additionally the solution isubiquitous as it can work in indoor environments using theinformation from wireless technologies or outdoors using aGPS receiver

The paper provides an overview of both approachesperforming a comparative analysis of their quality Sections2 and 3 describe the models assumed for the problemformulation In particular Section 2 describes the pedestrianmovement in terms of the typical signalsmeasured by inertialand positioning systems while Section 3 is centered onthe description of the errors coming from typical wearablesensors Afterwards Section 4 includes the derivation of thetwo different proposed filtering solutions Performance is

International Journal of Distributed Sensor Networks 3

analyzed in Section 5 comparing it with other solutionsespecially from the point of view of their accuracy and com-putational load Finally Section 6 contains some conclusionsand limitations on the applicability of these techniques

2 Pedestrian Movement Modeling

Human walking is a process of bipedal locomotion in whichthe weight of the moving body is supported by one leg whilethe other moves forward (ie the center of gravity swapsbetween the left and the right side of the body) in an alternatesequence necessary to cover a distance [19] A gait cyclecan thus be defined This gait cycle which happens to bereasonably consistent among different individuals is mainlycomposed of a stance phase during which one foot is incontact with the ground and a swing phase during which thefoot is in the airThese two phases can be easily distinguishedwhen analyzing the measurements gathered from differentinertial sensors mounted on the pedestrianrsquos feet In additionpretty short heel strike and foot lift-off phases can be detectedalong the gait when the highest acceleration appears butwithreduced effect on the complete traversed distance In [20] itis proposed an acceleration-based strategy to count the stepsthat a person takes (which is particularly interesting for dead-reckoning systems) Authors only care about the periodicshape of the acceleration signal regardless of the result of itsdouble integration In inertial navigation the focus is on theinformation that is provided by the integration of the accel-eration signal representing the velocity applied in the step(simple integration) and the position (double integration)

In order to analyze a pedestrian movement the measure-ments from the different sensors from an experimental IMUhave been considered These sensors are raw measures fromtwo orthogonal accelerometers mounted on a plane roughlyparallel to the sole of the foot along foot (longitudinal) andorthogonal to its axis (transversal) and a heading ldquosensorrdquobased on integration of IMU gyroscopes and magnetometerHeading derivation is performed within the IMU using theirproprietary algorithms These will be the same measures tobe used in our simplified filters In particular the MTx IMUsfrom Xsens have been used [21] for the experimental part ofthis work (see Figure 1)

The IMU will be foot-mounted The choice of the foot asthe position of the sensor is due to the fact that our processingwill make use of ZVU which can be applied to reset the driftsaccumulated in the filter due to the errors of the measuredsignals

For this setting the two main reference systems for iner-tial navigation will be as follows

(i) Inertial reference system (119868) it is an earth-fixed righthanded Cartesian coordinate system external to thepedestrian with three orthogonal axes119883

119868axis points

towards the local magnetic North 119885119868axis points up

and119884119868axis pointsWestThe pedestrian trajectory will

be tracked in these coordinates(ii) Foot reference system (119865) the orthogonal axes of this

coordinate system are defined by the orientation ofthe three accelerometers embedded in the IMUsThis

Figure 1 MTx sensor unit

coordinate system is important as the accelerationmeasures are initially referred to it In this coordinatesystem 119883

119865points to the longitudinal direction of

the foot 119884119865points horizontally to the left of the

foot (transversal) and 119885119865points up (following the

direction of the leg)(iii) Body reference system (119861) this reference system can

be defined from the rotation of foot reference systemso that 119883

119861points to the longitudinal direction of the

foot but in the horizontal plane Then 119884119861points to

the left of the foot (transversal) being equivalent to119884119865 and 119885

119861points up (normal to horizontal plane)

This reference system is not affected by the pitchingmovement of the foot along the stride

Figure 2 shows measurements taken along several stridesduring a typical constant course movement of a pedestrianIn this specific case each stride lasts around 50 samples or1 second since the sample frequency is 50Hz (it must benoted that each stride of the signal corresponds to one leftand one right foot step) Around 40 of the time of the stepcorresponds to the stance phase (zero acceleration zero rateof turn constant heading) and there are several peaks in thedifferent phases of the movement

(i) a quite deterministic longitudinal acceleration at thebeginning of the step and a deceleration at its endthis acceleration is measured by the longitudinalaccelerator being therefore expressed along119883

119865axis

(ii) a much smaller and less deterministic transversalacceleration swing (measured in either119884

119865or119884119861axis)

coupled with rate of turn and heading changes as thefoot moves along the step Note that the maximumheading excursion along the step is lower than 15∘for short times at the beginning of each step Thisbehavior is pretty dependent on pavement and shoes

When the pedestrian performs a maneuver the resultingsignals change causing the following situations

(i) When the pedestrian stops all acceleration signalsbecome zero and heading becomes constant Addi-tionally depending on the last foot in movement thelast stepmight be shorter and therefore the associatedlongitudinal acceleration might be smaller


50 100 150 200 250 300

05

10Transversal acceleration

Samples

minus5

minus10

minus15

minus20

Acce

lera

tion

(ms2)

(a)

50 100 150 200 250 300Samples

010203040

Longitudinal acceleration

minus10

minus20

minus30

Acce

lera

tion

(ms2)

(b)

50 100 150 200 250 300

Heading

Samples

Hea

ding

(rad

)

minus14minus145minus15

minus155minus16

minus165minus17

minus175minus18

(c)

Figure 2 Along-time measures (a) Transversal acceleration (b) Longitudinal acceleration (c) Heading provided by the IMU

(ii) Usually at the beginning or end of pedestrian move-ments the body velocity increases or decreases Thistypically has to dowith smaller acceleration althoughin those situations the movement is not so determin-istic as the balance of the body and movement is notperfect

(iii) Along turns typically the heading tends to rapidlychange to reach the new course

The following key ideas might be derived from these footmounted IMU signals

(i) There are distinct modes of longitudinal movementfor the feet stopped quite fast acceleration at thebeginning and end of the step and slow accelerationchanges along the step

(ii) Lateral movement can be seen as the movementof the pedestrian body with some additional errorintroduced by the lack of balance of the person alongeach step

Although it is perfectly possible that a person moveslaterally this kind of movement is not usual while walking(but it could be typical for some other kind of activities suchas dancing or playing tennis) Additionally the presentedmodels will demand changes for the running movementwhere the heading excursions and duration of the differentstride phases will be different Ourmovementmodels and theproposed processing methods which are based on them arenot well suited for these other types of movements A finalconsideration is that all our movement models assume thatwalking is performed over a flat surface

3 Measurement Modeling

Next we will mathematically describe the sensors that havebeen considered and the models of their errors detailingthe notation of the variables that characterize the movementand the associated measurements In addition to the termsincluded in (1) to (3) all sensors include noise error termswhich are not considered here for the sake of simplicityOf course the proposed processing systems will take intoaccount the presence of those noise terms in their (Kalmanfilter) measurement models

Position sensors may be of different kinds for indoor andoutdoor applications For indoor applications it is frequentto use RSS-based positioning systems Those systems sufferfrom quite a complex range of errors due to multipathand propagation leading to measures that have typically(time and space) varying bias and noises In our simplifiedapproach we will assume that the positioning system hasnegligible bias and constant noise statisticsTherefore we willhave

119864 119909119898 = 119909 119864 119910

119898 = 119910 (1)

where (119909 119910) is the current actual position of the pedestrianand (119909

119898 119910119898) is the associated measurement Here and in

the rest of equations 119864sdot is the expectation operationBoth actual position and position measures are taken in theinertial reference systemThe samemodelmight be usedwithdifferential GPS positioning system measurements in out-door applications although different statistical (noise covari-ance) parameters should be used in this case in the trackingfilters


The accelerometer sensor measures the force applied to abody in a specific direction and converts it into acceleration(in ms2) The longitudinal (along foot) and transversalcomponents are used to track the 2D movement In oursystems acceleration ismeasured in the foot reference systemand translated into the body reference system using theattitude of the accelerometer estimated by the inertial systemThus the input measures to our filters will be defined inthe body reference frame This rotation of the accelerationcorrupts the longitudinal acceleration measurement alongthe stance by the time-varying projection of gravity (this termis smaller than 05ms2 for a walking pedestrian but could belarger if he is running) Both acceleration (119886) measurementsare typically considered to contain an offset error due totechnological limitations [1 9 10] So we assume an almostconstant offset in both coordinates

119864 119886119871119898 = 119886119871+ Δ119886119871 119864 119886

119879119898 = 119886119879+ Δ119886119879 (2)

where 119886119871is the real longitudinal acceleration 119886

119879is the real

transversal acceleration119898 subindex stands for measure andΔ stands for offset in the measurement Therefore 119886

119871119898would

be the measurement of the longitudinal acceleration andΔ119886119879would be the offset in the measure of the transversal

acceleration The same notation will be used along thefollowing equations It should be emphasized again that thisis a very rough model of the error as the pitching movementof the foot along the stride will make the projection ofthose biases not constant in time especially due to theprojection of gravity in the longitudinal accelerometer Inour proposal we are investigating if neglecting the effect ofthis important error source enables a simpler tracking systemwithout compromising tracking accuracy and stability

The heading (120595) measurements are obtained by thecombination of the measurements gathered from varioussensors included in the IMU such as accelerometers gyro-scopes and magnetometers This processing stabilizes themeasurements from the different sensors being embedded inthe IMU hardware and computed in real time The result isthe measured heading angle (120595

119898 in radians) describing the

direction of the pedestrian movement which is affected bya small error (Δ120595) this error may be assumed to be almostconstant

119864 120595119898 = 120595 + Δ120595 (3)

This model is not so clear especially taking into accountthe time correlation of the errors induced by the IMUprocessing methods and the use of magnetometer measureswhich can be corrupted due to the presence of metallic orelectronic objects in the environment a circumstance thatis quite usual in indoor environments From our experiencewith indoors heading measures we may state that Δ120595 tendsto change along time at different rates quite slowly when thepedestrian follows constant heading paths or stops andmuchfaster along pedestrians turns This will demand specificmeans to improve the processing in our filter

It should be noted that actual foot accelerations can behigher than 5G and specially and turn rates may be way over300 degreessecond which are the default dynamic ranges

of the IMUs used in our deployment Together with thequite low 50Hz sampling rate such hardware limitations canseriously reduce the accuracy (for instance in sharp turns) ofINS systems based on those sensors Our approach is quiterobust against these problems as will be seen in the resultssection due to its capability to estimate the accumulatedinertial integration error terms unobservable from inertialmeasures But of course better sensors could allow muchimproved overall solution quality

4 Proposal of Alternative Fusion Approachesfor Position and Accelerometer Data Fusion

Two different filters for pedestrian tracking are proposedand compared in this work The first is an extended Kalmanfilter (EKF) that considers in the state variables the offsetsof the measurementsrsquo bias (both for accelerometers andheading) The other filter is a complementary Kalman filter(KF) inspired on the filters suggested by Foxlin [15]

Our filters use simplified 2D kinematic models withrespect to those in the literature not trying to estimate gyrobias and neglecting the importance of vertical accelerationbias in order to reduce overall computational load Ourcomplementary filter (second solution) estimates not only theactual dynamics of the pedestrian but also some correctionterms for the integration

(i) Δ119909 Δ119910 offset in119883119868and 119884

119868derived from the accumu-

lative errors in the inertial system integration(ii) Δ|V| offset in the velocity magnitude derived from

the accumulative errors in the inertial system integra-tion

All filters will process 2D position measurements using aloosely coupled solution This solution is chosen due to thefact that the system must run in several possible scenarioswith different 2D position sensors for indoor and outdoorapplications and depending on the deployed positioningtechnologies available

In the notations section there is a summary of the com-mon notation that is used to define the variables of bothfilters

In the following sections the notation of the EKFKFusedin the book by Gelb et al [22] has been used So the EKFKFequations will be as follows

(i) Prediction

119901= 119891 (

119891)

119875119901= 119865119875119891119865119879+ 119876

(4)

where 119901is the predicted state estimate at current instant

119891

is the filtered estimate just after previous update and 119891(sdot) isthe potentially nonlinear function that enables predicting thecurrent state from the previous state Meanwhile 119875

119901is the

covariance associated to 119901119875119891is the covariance associated to

119891 119865 is the Jacobian matrix of 119891(sdot) and 119876 is the covariance

matrix of the plant noise During the prediction stages thesubindex 119901 will always refer to current time prediction and


the subindex 119891 to previous update filtered estimate We willnot include any time index in the equations in order to reduceits size to the minimum

(ii) Filtering (update)

119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)

where 119901is the predicted state estimate at current instant and

119891is the filtered estimate at the same instant after the incor-

poration of the information in the current update includedin measurement vector (

119898) ℎ(sdot) is the potentially non-

linear projection function relating dynamic and bias statesto Kalman measurements Meanwhile 119875

119901is the covariance

associated to 119901 119875119891is the covariance associated to

119891 119867 is

the Jacobian matrix of ℎ(sdot) 119877 is the covariance matrix of themeasurement noise and119870 is the Kalman gainmatrix Duringthe filtering stages both subindexes 119891 and 119901 will always referto current time filtering and prediction Subindex 119898 will beused for input measurements at this time

Next we will describe our proposed processing systemsfirst explaining the overall architecture and associated statevector (which will be always equivalent for predicted andfiltered states) and then clarifying the form of the differentcomponents in (4) and (5) Subindex119891will be used to denotefiltered state components while subindex 119901 will be used forpredicted state as described above The time instant theyrefer to will depend on the involved calculation being part ofthe prediction or filtering stage as described in the previousparagraphs

41 Centralized EKF The first proposed filter is an extendedKalman filter which may process three different kinds ofmeasurements (using appropriate specifications of ℎ(sdot) and119867)

(i) position (119909119898 119910119898) measured in the inertial reference

frame

(ii) acceleration (119886119871119898 119886119879119898) projected into the inertial

reference frame(iii) heading120595

119898 it should be noted that thismeasurement

is obtained from the IMUwhich performs integrationof gyroscope rates of turn to derive this value Theheading sensor will also be aided by the use of acompass

This EKF has the following associated state (describedhere for the filtered state)

119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)

The state variables are the position velocity (expressedwith its magnitude and heading angle) and accelerationvectors together with the offsets assumed in the measure-ments that is heading and acceleration offsets assumed to beconstant in our simplified errormodelThe idea of this filter isto jointly estimate the dynamic state of the pedestrian and theprincipal slowly changing components of the sensors errorsThose slowly changing offsets will be implicitly removed fromthe trajectory estimation resulting in the mostly bias-freetracking of the pedestrian The key to be able to do so isthe observability of the offset terms which is attained alongtime due to the presence of position measures which finallyallow their calibration and removal Other error sources forthe integration are assumed to have reduced impact in theoutput In fact the filter will estimate a set of offsets whichwill not accurately describe the actual errors terms but willbe able to calibrate the inertial equations leading to effectiveremoval of induced positional biases

411 Prediction Model In this first proposed filter there areseveral potential modes of movement related to the phasesof each step For the phases where the foot is movingwe assume a prediction of the position coordinates in theinertial coordinate system following constant accelerationmotion between samples for the position circular motionfor heading and constant bias models If the time betweenthe current measure and the previous one is Δ119905 we canapproximate the associated state changeswith time as follows

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)


The Jacobian matrix of the prediction model (119865 in (4)) is

119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where

11986514= minus|V|119891 sin (120595119891) Δ119905 minus

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)

For this mode of movement the plant noise covariancematrix (119876 in (4)) models potential changes in longitudinaland transversal acceleration the projection of those changesin velocity and heading and independent drifts in themeasurement biases leading to the following definition

119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)

where the nonzero values of the centralmatrix are parameterstuning the EKF behavior They model the different errorsand variables changes with time 1205902

119886119871is the assumed longi-

tudinal acceleration variance 1205902119886119879

the assumed transversalacceleration variance 1205902

Δ120595the assumed heading bias change

variance 1205902Δ119886119871119898

the assumed longitudinal acceleration biasdrift variance and 1205902

Δ119886119879119898the assumed transversal acceleration

bias drift varianceIn order to reduce the errors of the acceleration signal

ZVU strategy is introduced For this reason a new predictionmodel must be taken into consideration whenever the footis assumed to be on the floor This is detected according tothe procedure to be described in Section 431 If the footdoes not move the following assumptions can be made

with respect to the state vector the position is constant theheading remains constant and the acceleration and velocityrelated components are thus zero (and constant) Hence theprediction model relation in (7) may be simplified to

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)

Therefore the prediction state relation is linear and wecan simplify in this case the calculation of the Jacobian to theidentity matrix

119865 = 119868 (12)

For the plant noise covariance matrix (119876 in (4)) the sameidea may be applied which in this case consists in assigningzero variances (the values of the dynamic states are invariant)except for the cases of the offsets of the measurements thatare nonzero

It results in the following definition of this matrix

119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)

As previously said we have noticed that Δ120595 tends tochange along time at different rates quite slowly when thepedestrian follows constant heading paths or stops andmuch


faster along pedestrians turns This is addressed in this filterby changing the plant noise covariance matrix for ZVU forboth heading and heading bias In order to do so turns aredetected by differentiating ZVUheadingmeasurements fromconsecutive stances and comparing the difference against aconstant threshold of 10 degrees If a turn is detected theplant noise covariance is changed to the one described in (14)Consider

119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595

is an additional covariance for both heading andheading bias drift

Equation (7) has a problem for low velocities which isthe instability due to the division in the heading prediction(fourth component) To eliminate this problem it is assumedthat very slow foot movement is not compatible with turnsFor this reason a prediction model in which zero (constant)

transverse acceleration is taken into consideration is appliedwhen |V|

119891is small This model changes (7) to

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)

The associated Jacobian for this case would be

119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)

Finally for this case we use the following definition ofplant noise covariance (119876 in (4)) equivalent to the one in (10)with 119865 as described in (16)

119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)

In addition protection of the predicted and filtered statesis carried out if the magnitude of the filtered or predictedvelocity (|V|

119891or |V|119901) is negative it is changed to zero

412 Measurement Model This filter processes the accelero-meter and heading measurements provided by the IMUand position measurements (from GPS or from an indoorlocalization system) When several measurements arriveat the same time they are processed successively withoutprediction phasesWhenever a newmeasurement is receivedit must be checked whether the time instant corresponds toZVU using the procedure described in Section 431 Next wewill detail the filtering associated equations for each type ofmeasurement for the cases with or without ZVU


First let us consider the position measurement process-ing compatible for both with and without ZVU situationsThe position measurements vector (

119898) is a 2D vector

containing the119883 and119884 coordinates of the positionmeasuredin the inertial coordinate system at a specific time Equation(18) defines the components in (5) necessary to complete thefiltering phase for position measurements Consider

119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875

is the assumed variance of the error of thepositioning system

The case of the acceleration is more complex as we mustdistinguish the cases with and without ZVU In the caseswithout ZVU the measurements of the acceleration (119886

119871119898and

119886119879119898

) are the projection of the acceleration values initially infoot reference frame on the horizontal plane (body frame)Equation (19) specifies the terms needed to complete thefiltering according to (5) The model considers the effect ofthe acceleration bias on the acceleration measures Consider

119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860

is the assumed variance of the error of theacceleration measurements of the IMU sensors

The measurement model changes whenever it is detectedthat the foot is resting on the ground (ie it is ZVU instant)In that case if we are on a flat surface the accelerationshould be zero which means that if the value of the projectedhorizontal measurement is different from zero it is due tothe bias of the sensor In addition zero velocity magnitudeand acceleration pseudo-measurements are introduced in themeasurement vector and modelThis forces the filter velocityand acceleration states to become almost zero and makesthe position almost constant during this time interval whilecalibrating the bias terms The variances of those pseudo-measurements are nearly 0 to reaffirm the accuracy of their

values This results in the following measurement equationsfor this case

119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)

where 1205902|V|119901119898 is the variance of the error of the velocity pseudo-

measurements and 1205902119860119901119898

is the assumed variance of the errorof the acceleration pseudo-measurements Both should bevery low values but not making the filter unstable

Finally the processing of heading measurements (120595119898) is

independent of the state of the foot not changing in timeThe heading angle provided by the IMU sensor is introducedinto the Kalman filter as described in (21) The associatedmodel takes into consideration the presence of bias in thismeasurement 1205902

120595is the assumed variance of the heading

measurements Consider

119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)

42 Complementary KF The second proposed filter is a com-plementary Kalman filter which will process three differentkinds of measurements

(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898

)(iii) heading 120595

119898

The basic idea behind this solution is the aiding of asimplified INS integration process by estimating and cor-recting the accumulated bias from the offsets between theposition sensor measures and the current integrated INSbased trajectory In order to introduce this type of filter letus assume that (1) there are no position measures and (2)heading and acceleration measures have no error For shorttime intervals and for walking pedestrians we could integratethe trajectory using the IMU measures as follows in (22)where the left part of each assignment refers to current timeinertial estimates of 119909 119910 velocity magnitude and heading


(119909119868 119910119868 |V|119868 and 120595

119868) The right parts of the assignments

contain current time IMUmeasures (119886119871119898

119886119879119898

projected intobody frame and 120595

119898) and previous time inertial estimates of

119909 119910 and velocity magnitude (119909119868 119910119868 and |V|

119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)

This kind of processingwould be accurate in the followingconditions (1) measurements without bias (2) negligibletransversal-to-foot movement and (3) reduced effect ofthe pitching movement of the foot A classical alternativeapproach to the centralized smoothing consists in performingan estimation of the bias in parallel with the inertial inte-gration and removing the estimated bias values along theprocessing The state vector of the filter contains the errorsof position and velocity and the bias of the measurementsdescribed in the next equation for the filtered estimation (forthe predicted estimation just change the 119891 subindex to 119901)

119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)

The proposed implementation of a complementary filterfor our model will be formed of the following steps

(1) Subtract the previously estimated bias (from previ-ous measures) from the acceleration and headingmeasurements defining unbiased measures to bedescribed using subindex 119906

119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)

(2) Update the inertial state estimates using a slightlymodified version of (22) with the bias correctedmeasures

119909119868= 119909119868+ |V|119868 cos (120595119906) Δ119905 + [119886119871119906 cos (120595119906) minus 119886119879119906 sin (120595119906)]

Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)

(3) Perform the prediction and filtering processes des-cribed in Sections 421 and 422 in order to updatethe filtered bias state values

(4) Correct the errors due to the integration process toobtain the output of the complete processing

119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)

(5) The filter is based on linearization of the error equa-tions in (25) to be described in Section 422 Finallyto reduce the effect of the bias in this linearizationthe filter turns the position integration drift estimatesinto zero by correcting them in the inertial positionand velocity estimates Consider

119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)

(6) For velocity bias this is done if the measurement isnot related to ZVU instant Consider

|V|119868 = |V|119906 Δ|V|119891 = 0 (28)

(7) Meanwhile if we are in ZVU instant according tothe detectors described in Section 431 the inertialintegrated velocity is converted into zero

Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)

The unbiased output of the complete processingwould be the vector

119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)

So in order to complete the description wemust describethe complementary KF prediction and measurement modelsand algorithms (step (3) of previous algorithm) This filterdoes not process the raw input position and accelerationmeasurements Instead it processes modifiedmeasurementswhich are independent of pedestrian dynamics and justcontaining bias related terms In next subsections we willdescribe both the prediction and measurement processingapproaches for the complementaryKFThe complete process-ing scheme is depicted in Figure 3

421 Prediction Model The Kalman filter needs to estimatethe described offsetbias parameters The bias parametersremain mainly constant but the kinematic estimation offsetstend to accumulate due to the INS integration process So theprediction model includes terms related to this propagationresulting in a linear relation between previous filtered estima-tion and current time predicted estimation where the statepropagation matrix (119865) results from analyzing the influencethat a small error in themagnitude of the filtered estimate andin the newmeasurements can have on the estimated positionand velocity using the integration equations in (25)


Inertial integration

Complementary Kalman filter

IMU measures

IMU and positionmeasures

Unbiased integrated position and velocity++

minusminus

Figure 3 Complementary KF complete processing architecture

119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)

Regarding the definition of the prediction of the state inthe EKF equations in (4) we have

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)

The associated Jacobian of the transformation is 119865directly and the covariance matrix of the plant noise (119876 in(4)) in this case can be modeled as follows

119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)

Again the speed of change of Δ120595 along turns is addressedby changing the plant noise covariance matrix for ZVU forheading bias In the case a turn is detected (using the same

method described in Section 411) the plant noise covarianceis changed as described in (35) Consider

119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)

422 Measurement Model The measurements that are con-sidered in the filtering process should enable the estimationof the integration and measurement offsets This means thatthey are either preprocessed versions of the input measure-ments removing the pedestrian kinematics or measurementstaken during time intervals where some of the kinematic vari-ables are known For instance when the foot is placed on thefloor (ZVU) the values of the acceleration velocity and angu-lar rate are zero and the position and the heading is constant

Next we will detail the processing associated to eachkind of input measurements following the notation in (5)First we will explain the position measurement processingfor the cases with and without ZVU The unbiased positionmeasurements vector (

119898) is a 2D vector containing the 119883

and 119884 coordinates of the position of the individual measuredin the inertial coordinate system at a specific time From thismeasure we derive an observation of the inertial positionerror terms in 119883 and 119884 coordinates by subtracting theintegrated inertial position This integrated inertial positioncontains the accumulated drift (Δ119909

119901 Δ119910119901) The subtraction

removes the dependency with pedestrian position resultingin the measurement model equation (36) Consider

119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)

where 1205902119875is again the assumed variance of the error of the

positioning systemAccelerationmeasurements are just processed in the time

intervals where the foot is on the ground (ZVU) In that caseif the value of the measurement is different from zero it isdue to the bias of the sensor This results into the followingmeasurement equations for this case

119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)

Finally we can also introduce velocity pseudo-measure-ments during ZVU intervals During this phase if there issome nonzero velocity according to the inertial integration itwill be due to the presence of drift in the integration process(nonzero Δ|V|) So the value of the inertial estimation ofvelocity is taken as the pseudo-measurement resulting in thenext equation

119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)

Here 1205902Δ|V|119901119898 would be an assumed variance for the

pseudo-measurement very small but not making the filterunstable

43 Support Algorithms for Previous Filters Next we willdetail some of the supporting algorithms that can be used forthe different localization architectures the process for ZVUdetection and the processes enabling the calculation of posi-tionmeasurements in indoor deployment based onRSSmea-surements from distributed sensors network

431 Zero Velocity Update Instants Detection As it is com-mon in this kind of systems ZVU have been included in

this work to reduce the errors of the inertial sensors Thechosen algorithms to detect in which instants the foot isplaced on the ground are the acceleration moving variancedetector (MV) and angular rate energy detector (ARE) Theyare defined in (39) where 119882 is the number of samplesin a window 119886

119896the acceleration measures 119886

119882and 1205902

119886

the average and the variance of the acceleration values inthat window and 120596

119896the gyroscope samples [23] There are

two MV detectors one for each acceleration componentlongitudinal acceleration and transversal acceleration Con-sider

MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)

The first detector is based on the fact that the measuredacceleration is approximately constant in the stance phasewhereas the second one is based on the lack of rotation of thefoot during this phase (see Figure 2)The foot is decided to bein an instant of ZVU whenever both MV and ARE detectorsdecide that it is the case

In our implementation a window length contains 5samples which has been empirically found to be a suitablevalue for the characteristics of the IMU acceleration and rateof turn signals remember one step takes about 1 second thesampling frequency is 50Hz and the stance phase is about40 of the step so this phase lasts around 20 samples Thethresholds initialized to fixed values (25 for MV 1 for ARE)have been found empirically although an adaptive thresholdcould be incorporated (for instance based on a constantfalse alarm rate design) All the detector parameters wereselected in order to minimize the possibility of detecting anonzero velocity sample as a zero velocity sample as thisevent is much more problematic than not detecting a fewzero velocity samples due to the potential corruption on biasestimations potentially resulting in increased error in bothEKF and complementary filter

432 Position Estimation All presented systems are looselycoupled based on high update rate IMUmeasures and lowerrate position updates Those position updates may comeeither from GPS for outdoor localizationnavigation appli-cations or from indoor localization systems Along time thesystemwill use the different available positionmeasurementstuning the appropriate measurement model equations ((18)and (36) for each of the described systems) to the quality ofthe sensor

In the case of indoor localization in our implementationposition estimations are calculated through a weighted cir-cular multilateration algorithm that works on RSS measure-ments The estimation of the distances between the user andthe deployed beacon nodes is based on a lognormal channelmodel The details of this algorithm are out of the scope ofthis paper for further information refer to [24]


Beacon node (IRIS)Initial point

Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI

Figure 4 Scenario 1 of trajectory and deployment

5 Result and Discussion

In this section we described the following (a) the experimen-tal deployment that has been used for the evaluation of thefilters in indoor environments (Section 51) (b) the values ofthe filter parameters for both implementations (Section 52)(c) the accuracy assessment (Section 53) (d) the applicabilityof this system for outdoor pedestrian tracking (Section 54)and (e) the computational load associated with each of thefilters (Section 55) including a comparative analysis withother methods in the literature

51 Experimental Deployment Two different scenarios havebeen considered to test the performance of the trackingsolutions In those scenarios the individual that performs thetests is equipped with a pair of MTx IMUs from the companyXsens [21] which as said before gather the informationmea-sured by the inertial sensors as well as the magnetometer at asampling rate of 50Hz Two sensors are placed on both feetof the individual In addition the individual carries an IRISmote (fromMEMSIC [25]) to enable continuous localizationusing a ZigBee RSS-based positioning system This devicecommunicates with the beacon nodes deployed in the envi-ronment using the IEEE 802154 communication protocol

The first scenario is shown in Figures 4 and 5 It consistsin a trajectory of about 40 meters along a corridor in whichthere is a turn of 90∘ after walking 16 meters It also shows thedeployment of the IRIS beacon nodes The movement startsat the top right of the map in Figure 4

To carry out RSS-based localization 12 IRIS motes havebeen deployed on the walls of the corridor in a very con-trolled way in order to be able to easily reproduce the exper-iment in similar environments The distance between themotes is 4 meters and they are placed at a height of 2 metersto prevent signal shadowing due to objects In addition theyhave been placed alternatively on the walls to provide goodsignal coverage of a minimum of 3 nonalignedmotes at everypoint of the trajectory The mote obtains the RSS measure-ment every 100msThe parameters of the lognormal channel

IRIS mobile node

IRIS beacon nodesMTx Xsens

Figure 5 Picture of experimental deployment

model necessary for the positioning algorithm have beenestimated usingmethods derived from [26] To estimate thoseparameters measurements were taken in a grid of pointsevery 12 meters The estimated channel parameters are 119860 =minus84 dB and 120578 = 24 It should be noted that those parametersare specific for each deployment Therefore it is necessary toperform a calibration of the positioning system to obtain highaccuracy performance

Eight individuals have travelled this scenario The timethat the individuals needed to cover the distance variesbetween 30 and 40 secondsThis trajectory has been travelled5 times for each individual with a sensor placed on each footSo ten signals per user (5 walks with two IMUs one per foot)were available to test the system resulting in a total of 80 testtrajectories

The second scenario is the same as the first one but in thiscase it is a closed loop trajectory with a 180∘ turn at the end ofthe corridor to comeback again and return to the initial pointIn this way the total longitude is around 80 meters travelledon a time interval between 60 and 70 seconds

52 Filter Parameters Although in the derivation of the sys-tems we have used a common notation for many of the filterparameters the parameters were manually tuned for bothof the filters independently in order to attain a good com-promise between smoothing and robustnessThis tuning wasperformed on the basis of the measurements from two usersin several scenarios different than those in the evaluation(1) movement along a corridor (2) turn in a corner and (3)circularmovement (with 3meters radius) in awide roomTheparameters of both filters are described in Table 1 where NAmeans the parameter is not applicable to this kind of filterPlease note that the most adequate values of most parameterswould be dependent on the actual deployed sensors and afine-tuningwould be necessary for other sensor quality levels

The described parameters show that the centralized EKFhas a limited capability to smooth acceleration measure-ments This permits thinking on a third potential solutionbased on a EKF filter which instead of processing accel-eration as measurements will use them as control inputs


Table 1 Filter parameters

Parameter Centralized EKFvalues

ComplementaryKF value

120590119886119871

30ms2 NA120590119886119879

15ms2 NA120590Δ120595 10minus7 rad 10minus5 rad120590120575120595 001 rad 001 rad120590Δ119886119871119898

10minus4ms2 10minus4ms2

120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2

120590120595 0005 rad NA120590|V|119901119898 10minus8ms NA120590119860119901119898

10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20

minus25 minus20 minus15 minus10 minus5

Position measurementsReal trajectoryINS integration

xI (m)

yI

(m)

Figure 6 Example position (RSS measures) and INS integratedtrajectory

(with acceleration biases calculated along ZVU intervals andremoved before applying control inputs in prediction) withreduced state vector and therefore reduced computationalcost The associated accuracy would be quite similar to thatof the centralized EKF

53 Comparative Accuracy Evaluation This section gathersthe results achieved using the proposed filters together withother results to be used as a comparison benchmark as theones achieved using double integration of the acceleration orstand-alone RSS positioning

Figure 6 shows an example of the positional data receivedalong one user trip in scenario 2 Here we have both RSSderived positions and the INS integrated position providedthat no bias is estimated or corrected The INS double inte-gration of the measurements does not contain a way to

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20


Position measurementsReal trajectoryCentralized EKF

xI (m)

yI

(m)

Figure 7 Centralized EKF execution

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20minus25 minus20 minus15 minus10 minus5

Position measurementsReal trajectoryComplementary KF

xI (m)

yI

(m)

Figure 8 Complementary KF execution

counteract it so it is expected that the errors continuegrowing as long as the experiments last INS integration hasZVU correction to reduce the drift speed due to the doubleintegration of erroneous acceleration measurements

In Figures 7 and 8 the results of processing the previousmeasures with each of the proposed filters are depictedIn those figures it is very clear that once the heading isestimated both filters are capable of tracking the movement

Figure 9 shows how the 3D complementary filter byFoxlin [15] performs in the same scenario

In order to measure the performance of the differenttrackers the error considered for comparison is the Euclideandistance between the estimated point and the approximated


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20


Position measurementsReal trajectoryFoxlinrsquos filter

xI (m)

yI

(m)

Figure 9 Foxlin [15] 3D complementary KF execution

real position of the individual at that instant using a linearmovement model along the center of the corridors Thismodel assumes constant velocity between turns (the usersmaintained quasi-constant speed in each corridor) but ofcourse as perfectly smooth movement is not possible therewill always be a residual error due to this interpolation inthe order of 20ndash40 cm (related to maximum longitudinalexcursion of half of a step and less than 05 meter lateraloffset) This additive error component is smaller than theerror we are measuring as it will be shown in the followingtables of results This way the average and the standarddeviation of the error along a trajectory can be estimated Fivevalues are provided for each scenario

(a) based on RSS(b) based on INS integration without bias removal (inte-

gration of 3D acceleration)(c) proposed centralized EKF(d) proposed INS aided by a Complementary KF(e) Foxlin [15] 3D complementary Kalman filter to be

used as an accuracy benchmark

Let us start with scenario 1 whose results are summarizedin Table 2 Results show that both filters are able to improveboth raw RSS-based positioning and INS integration Com-plementary KF has better results and is more stable thancentralized KF in our deployment Meanwhile the resultsof scenario 2 included in Table 3 confirm that the filterperformances do not degrade with time which happensfor INS integration Of course Foxlin algorithm has thebest accuracy but the difference is not so big making ourproposed filters competitive

The complete dataset lasts for almost three hours In thistime span the maximum error distance error observed was43 meters for the complementary filter and 526 meters for

Table 2 Scenario 1 results

Localization algorithm Mean distance RMS distance(a) RSS positioning 155m 180m(b) INS 524m 57m(c) Centralized EKF 138m 150m(d) Complementary KF 112m 124m(e) Foxlin 3D KF 084m 102m


Localization algorithm Mean distance RMS distance(a) RSS Positioning 169m 197m(b) INS 572m 691m(c) Centralized EKF 136m 153m(d) Complementary KF 118m 132m(e) Foxlin 3D KF 087m 108m

the centralized EKF showing the robustness of the solutionespecially for the case of the complementary filter

Finally it should be emphasized that a large number ofthe filter errors are time correlated and the tracking filtersare quite often biased due to the bias terms present in thepositioning system which are clearly visible in Figure 6 (theposition measurements are biased towards the center of thefigure) The system is robust enough to reduce somehow thiseffect but of course this unmodeled bias tends to appear inthe filter output as well

Quite often in localization literature the error is providedin terms of a percentage of the overall traversed distanceThis is somehow inspired by the increasing error of inertialnavigation systems with time When an RSS or a similarsensor is used to aid inertial navigation this kind of errorstatistic is not relevant anymore as positioning accuracy doesnot degrade with time So this statistic may be done as smallas desired by just increasing the length of the scenario

54 Outdoor Scenario The proposed filters follow a looselycoupled filtering approach which enables the use of otherpositioning systems such as GPS We have made initialintegration of the systemwith amobile phoneGPS (Nexus 4)leading to a system smoothing GPS errors Future integrationof RSS and GPS aiding would be able to guarantee continuityof walking pedestrian tracking in horizontal dimensionsfor indoor and outdoor movements Surely some additionallogic for positioning measurement selectioncombinationand for reduction of jumps due to mixed positioning envi-ronments would be needed due to the different types of biasof the positioning sensors

In Figure 10 we can see the results of the complementaryKalman filter along the real trajectory raw GPS data and theideal trajectoryThe initialization suffered during theGPS ini-tial measurements for the integrated system but afterwardsonce it recovered it worked correctly It should be empha-sized that there is a minor slope of 2 in this path which is inan urban but quite open area From these reduced scenarios itis difficult to derive really representative performances as thedata collection (and specially speed) was not so controlled


0 50 100 150 200

0

50

100

150

Real trajectoryGPS Positioning

INS with ZVUComplementary KF

minus50minus100 minus50

x (m)

y(m

)

Figure 10 Complementary KF execution in outdoor application

Table 4 Example outdoor results

Localization algorithm Mean distance RMS distanceGPS positioning 82m 126mINS 97m 153mComplementary KF 32m 43m

Table 5 Computational load of proposed filters for each type ofmeasure

Types of measure Centralized EKF Complementary KFSums Products Sums Products

Inertial not ZVU 838 854 173 163Position and Inertialnot ZVU 1021 1034 259 247

Inertial low speed 797 761 173 163Position and inertiallow speed 1021 1034 259 247

Inertial ZVU 610 539 302 292Position and inertialZVU 793 719 388 378

but the averages for this and two other simple scenarios areprovided in Table 4 Note INS error is increasing with timewhile GPS and complementary KF statistics are much morestable

55 Computational Load Analysis Table 5 summarizes theamount of operations needed to process each kind of mea-surement received by the filter To calculate these numberswe have performed much optimization on the implementa-tion of the filters as follows

(i) Many matrix coefficients are 0 or 1 and their effectsin matrix summations and multiplications are wellknown Therefore many operations can be avoided

Table 6 Computational load for each type of measure for Foxlinfilter

Types of measure Foxlin complementary KFSums Products

Inertial not ZVU 3315 3581Position and Inertial not ZVU 5715 6231Inertial low speed 3315 3581Position and inertial low speed 5715 6231Inertial ZVU 5617 6043Position and inertial ZVU 8017 8147

(ii) Introduce the different measures received in the sametime instant in order not at the same time This has aminor effect on the tracking quality but reduces quitemuch the computational needs

(iii) Reuse previously calculated terms especially in thecalculation of 119865matrices

For computational load assessment we will assume asnegligible the load associated with the pair of cosines andsinuses that need to be computed in each of the systems

Different types of measurements are considered in Table5 ldquoInertialrdquo measures just contain acceleration and heading(at 50Hz rate) ldquoposition and inertialrdquomeasures contain addi-tionally position measures derived from RSS (or GPS) inindoor scenario at 10Hz rate In this table we denote asldquonot ZVUrdquo the measures that have not been detected asZVU while ldquoZVUrdquo denote the measures where ZVU wasdetected ldquoLow speedrdquomeasures are themeasures where ZVUwere not detected but speed is assumed to be too low andpotentially problematic for the centralized EFK Each typeof measurement uses different prediction andor filteringequations resulting in a different number of summations andproducts which have been counted for each specific caseAlso the ZVU detectors computational load was assessed

From Table 5 it is clear that the complementary KF basedsystem has much lower computational load In our systemwe have an update rate for position of 10Hz and an updaterate for inertial measurements of 50Hz From this dataassuming ZVU lasts for 35 while low velocity lasts for 5of the stance it would be needed in average nearly 80000FLOPS for the centralized EKF and 23000 FLOPS for thecomplementary EKF based integrator

For comparison Table 6 contains an assessment of thecomputational load of the complete complementary Kalmanfilter by Foxlin [15] which performs no simplification on thestate vector resulting in a much bigger state vector and asso-ciated covariance matrices This solution had several phasescalibration navigation and so forth The computational costmeasured here would be the one related to the navigationphase where the associated KF has 15 state variables

It can be seen the computational load is much higherthan in our proposals even after performing the samekinds of optimizations of the filters on Foxlin proposalWith the same measurement rates (sampling frequencies)and assumed percentage of ZVU duration this processing


would demand a computational load in the order of 475000FLOPS (which is attainable in current mobile applicationsbut results in increased 2000 computationbattery costover our complementary EKF solution) This computationalgain comes basically from the reduced size of the state andmeasurement vectors of our proposals

It should be noted that the computational load in the IMU(which provides the heading estimate) is not computed as wedo not have access to their complete algorithms Taking intoaccount the size of the associated state vectors of a Kalmanfilter for assessing heading based on 3 axes magnetometersand gyros a computational load in the order of that of thecomplementary filter could appear This load will not bepresent in Foxlin filter which operates on rawmeasuresThiswould increase our complete solution computational loadso that Foxlin solution will just have around 1000 highercomputationbattery cost Even simpler approaches couldbe used without the need of 3D magnetometer and gyrosmeasurements exploiting the flat movement restrictions

6 Conclusions and Future Work

This paper describes the application of two novel nonlinearfilters to the problem of pedestrian tracking using low-gradepositioning and inertial sensors The filters use the measure-ments projected to the horizontal plane and neglect the errorsintroduced by the projection of gravity and foot rotationalong the stance So the described filters track a reducednumber of states with respect to other previous filters in theliterature The paper contains a comparative analysis of bothsolutions in a realistic indoor scenario where the solutionsshow their robustness and accuracy even though many ofthe error sources have been ignored gyro bias accelerationprojection of gravity positioning bias and so forth

Our study shows that the filters are capable of attaining apositioning error with an RMS in the order of 13ndash15 metersfor long time intervals (summing nearly three hours of data)which is a level of accuracy compatible with many indoorapplications this level of accuracy does not degrade alongtime Specifically the designed complementary system seemsa good solution regarding both complexity and resultingaccuracy Also due to its design the system would be able tomaintain accuracy during short support positioning systemsoutages or in the reduced areas without proper positioningsystem coverage

Of course the accuracy of the system is dependent on theaccuracy andmeasurement period of the positioning systemas shown by the difference between indoor and outdoorscenarios results (Sections 53 and 54) The exact accuracyattainable for each positioning system is not easy to derivebut for high rate sensors (gt1 measuressecond) and reducederror (lt20m RMS) a division of the RMS of the inputpositioning system by a factor in the range 15 to 3 could beexpected

The IMU that has been used in the experiments requiresa connection by cable to a concentrator that the user wearsattached to the belt Obviously this is a solution that will bedifficult to generalize in commercial termsThus it is neededto consider that a sound wearable system will have to send

the data from the IMU to the mobile phone (processing unit)wirelessly Reduced computational complexity will be thenrequired to save battery It is also needed to further study thepower usage from the wireless data transfer In our approachsome intermediate data may be calculated in the foot (ieldquoheadingrdquo from magnetometer and gyros) and not all rawmeasurements need to be transmitted (only 2D acceleration)Therefore our approach reduces communication bandwidthneeds and may lower the associated power consumptionThese facts may underline the benefits of our approachesalthough a rigorous assessment of the attainable powerconsumption would need to be performed Additionally areduced IMU with an incomplete set of sensors could bedesigned ensuring that it provides accurate heading and 2Dacceleration for the application on flat movement

Some open problems remain for the future research as itwould be the extension of the filtering approaches to runningpedestrians or to other kinds of activities and the analysisof their validity for changes of level within a building boththrough stair climbing and use of elevators Additionallythe integration of GPS must be both improved and fine-tuned and the quality for outdoor applications must be rigo-rously assessed Additionally as described in Section 52 analternative centralized EKF using accelerationmeasurementsas control inputs could be investigated This solution wouldhave reduced computational cost (although larger than com-plementary filter) with respect to centralized EKF and similarperformance

Notations

119909 119910 Pedestrian 2D position coordinates (in theinertial coordinate system)

|V| Velocity magnitude (in the inertialcoordinate system)

120595 Heading of the velocity vector in theinertial coordinate system assumed equalto the movement heading

119886119871 119886119879 Longitudinal and transversal components

of the acceleration vector (in body frame)Δ120595 Heading measurement offset (or bias)Δ119886119871 Δ119886119879 Longitudinal and transversal accelerationmeasurement offset (or bias in bodyframe)

Δ119909 Δ119910 Position offset from inertial integrationΔ|V| Velocity magnitude offset from inertial

integration

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

This work has been supported by the Spanish Ministryof Economy and Competitiveness under Grants TEC2011-28626-C02-01 and IPT-2011-1052-390000 (Social Awareness


Based Emergency Situation Solver) and by Comunidad deMadrid under Grant CONTEXTS (S2009TIC-1485)

References

[1] F Dovis R Lesca DMargaria G Boiero andG Ghinamo ldquoAnassisted High-Sensitivity acquisition technique for GPS indoorpositioningrdquo in Proceeding of the IEEEION Position Loca-tion and Navigation Symposium (PLANS 08) pp 1350ndash1361Monterey Calif USA May 2008

[2] FCC Enhanced 911 httptransitionfccgovpshsservices911-services

[3] Skyhook Location apps research 2014 httpwwwskyhook-wirelesscom

[4] W Elloumi R Leconge and S Treuillet ldquoPedestrian localiza-tion literature review and illustration of a monocular visionbased approachrdquo in Proceedings of the International Conferenceon Signal Image Vision and their Applications (SIVA rsquo11)November 2011

[5] G Gartner V Radoczky and G Retscher ldquoLocation technolo-gies for pedestrian navigationrdquo GIS Development vol 9 no 4pp 22ndash25 2005

[6] C Huang Z Liao and L Zhao ldquoSynergism of INS and PDRin self-contained pedestrian tracking with a miniature sensormodulerdquo IEEE Sensors Journal vol 10 no 8 pp 1349ndash13592010

[7] S Wan and E Foxlin ldquoImproved pedestrian navigation basedon drift-reduced MEMS IMU chiprdquo in Proceedings of the Inter-national Technical Meeting of the Institute of Navigation (ION10) pp 220ndash229 January 2010

[8] P Aggarwal Z Syed A Noureldin and N El-Sheimy MEMs-Based Integrated Navigation Artech House Norwood MassUSA 2010

[9] P D Grooves Principles of GNSS Inertial and MultisensorIntegrated Navigation Systems Artech House Norwood MassUSA 2008

[10] O Woodman ldquoAn introduction to inertial navigationrdquo TechRep UCAM-CLTR-696 University of Cambridge 2007

[11] F Evennou and F Marx ldquoAdvanced integration of WiFi andinertial navigation systems for indoor mobile positioningrdquoEurasip Journal on Applied Signal Processing vol 2006 ArticleID 86706 2006

[12] HWang H Lenz A Szabo J Bamberger and U D HanebeckldquoWLAN-based pedestrian tracking using particle filters andlow-cost MEMS sensorsrdquo in Proceedings of the 4thWorkshop onPositioning Navigation and Communication (WPNC rsquo07) pp 1ndash7 March 2007

[13] L Klingbeil and T Wark ldquoA wireless sensor network for real-time indoor localisation and motion monitoringrdquo in Proceed-ings of the International Conference on Information Processing inSensor Networks (IPSN rsquo08) pp 39ndash50 April 2008

[14] J A Besada A M Bernardos P Tarrıo and J R Casar ldquoAna-lysis of tracking methods for wireless indoor localizationrdquo inProceedings of the 2nd International Symposium onWireless Per-vasive Computing (ISWPC rsquo07) pp 492ndash497 February 2007

[15] E Foxlin ldquoPedestrian tracking with shoe-mounted inertial sen-sorsrdquo IEEE Computer Graphics and Applications vol 25 no 6pp 38ndash46 2005

[16] S S Young and P Sangkyung ldquoPedestrian inertial navigationwith gait phase detection assisted zero velocity updatingrdquo in

Proceedings of the 4th International Conference on AutonomousRobots and Agents (ICARA rsquo09) pp 336ndash341 February 2009

[17] A R Jimenez F Seco J C Prieto and J I Guevara ldquoAccuratepedestrian indoor navigation by tightly coupling foot-mountedIMU and RFID measurementsrdquo IEEE Transactions on Instru-mentation and Measurement vol 61 no 1 pp 1ndash12 2011

[18] J Bird and D Arden ldquoIndoor navigation with foot-mountedstrapdown inertial navigation and magnetic sensorsrdquo IEEEWireless Communications vol 18 no 2 pp 28ndash35 2011

[19] J Rose and J G GambleHumanWalking Williams ampWilkinsPhiladelphia Pa USA 2nd edition 2004

[20] J W Kim H J Jang D H Hwang and C Park ldquoA stepstride and heading determination for the pedestrian navigationsystemrdquo Journal of Global Positioning Systems vol 3 no 1-2 pp273ndash279 2004

[21] Xsens-Technologies ldquoMTi andMTx user manual and technicaldocumentationrdquo Tech Rep 2008

[22] A Gelb J F Kasper R A Nash C F Price and A A Suther-land Applied Optimal Estimation The MIT Press CambridgeMass USA 1974

[23] I Skog P Handel J Nilsson and J Rantakokko ldquoZero-velocitydetectionmdashan algorithm evaluationrdquo IEEE Transactions onBiomedical Engineering vol 57 no 11 pp 2657ndash2666 2010

[24] HMartınLocalization and activity detection based on the fusionof environment and inertial sensors [PhD thesis] TechnicalUniversity of Madrid 2012

[25] MEMSIC ldquoIRIS mote datasheetrdquo Tech Rep 2008[26] AMBernardos J R Casar andP Tarrıo ldquoReal time calibration

for RSS indoor positioning systemsrdquo in Proceeding of theInternational Conference on Indoor Positioning and Indoor Navi-gation (IPIN 10) pp 1ndash7 Zurich Switzerland September 2010

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation httpwwwhindawicom

Journal ofEngineeringVolume 2014

Submit your manuscripts athttpwwwhindawicom

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics


Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

SensorsJournal of


Modelling amp Simulation in EngineeringHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks



use of inertial sensors has privacy advantages over other tech-nologies for localization and tracking such as camera net-works Their low weight makes it possible to integrate themin wearable forms (shoes mobile phones etc) and the pro-cessing of their data requires low computational capabilitiesbeing suitable for real-time applications

Those systems are usually called strap-down systemssince the sensors used are strapped to the body that is goingto be tracked There are two main approaches to deal withthe problem of estimating the position of a person using theinformation provided by inertial sensors (step based) deadreckoning (DR) and inertial navigation systems (INS) DRis based on the analysis of the periodic signals that resultfrom the dynamics of walking in order to count steps Thisenables us to calculate the traversed distance by using an esti-mation of the step length In addition a magnetic compassor a gyroscope is employed to estimate the heading of thedirection ofwalking in order to project the traversed distancein the horizontal plane With respect to the INS method themeasurements of the accelerometer are double integrated tak-ing into account platform orientation according to compassandor gyroscopes to get an estimation of the position changewith time However the influence of the errors of inertialsensors on the position estimation makes it very difficult toachieve continuous accurate localization using stand-aloneinertial navigation systems as the inherent integration ofboth approaches tends to make the error increase alongtime The sensors for pedestrian localization are usually low-grade sensors which means that the errors in the measure-ments have a significant influence over the achieved results(accelerometer and gyroscope errors make the position errorgrow over the time [6]) unless some correctingaiding tech-nique is applied such as zero velocity updates (ZVU) [7]ZVU (ie controlling the instants when the foot is placed onthe ground ad thus the velocity and acceleration are zero) arecommonly used in pedestrian positioning systems In orderto apply such technique the inertial sensors must be in thefeet

The sources of inertial sensor errors and their influenceover the estimation of the pedestrian position are analyzed in[8ndash10] To reduce the impact of these growing errors severalaiding strategies have been proposed [8] Neither of themachieves a general solution that provides enough accuracyfor pedestrian tracking so fusion with other sensors withbounded error statistics is often consideredThe fusion of thedifferent aiding sensors with the inertial information is madeusing techniques such as particle filters [11ndash13] or Kalmanfilters For instance the algorithm in [11] attains an averageerror in the order of 15 meters using particle filters to fusebelt-mounted INS and Wi-Fi RSS positional measurementswhile the proposal in [12] with a very similar approachachieves an error in the order of 45m The similar strategyin [13] provides an average error in the order of 1m In thetwo later cases map-aiding was also implemented

The algorithms proposed in this paper would be imple-mented in a mobile device (ie smartphone) ZVU are usedwhenever the foot is detected on the ground (which occursperiodically as the pedestrian walks) For this reason the sys-tem includes a detector to accurately determine the instants

when the foot of a user is placed on the floor Moreoverthe inertial system is aided with positioning measurementstaken from an external localization system which could beeither a GPS in outdoor environments or a wireless position-ing system in indoor environments Typically those indoorwireless positioning systems perform location estimationfrom RSS measurements of the underlying communicationtechnologies (ie Wi-Fi Bluetooth or ZigBee motes) [14]Many existing solutions are based on Kalman filters (KF)which provide a reduced computational load with respect toparticle filters Some of those works propose a complemen-tary Kalman filter that estimates the errors of the positionvelocity acceleration and the bias of the sensors subtractingthem from the states and measurements for the integrationApproaches such as the ones described in [15 16] includean inertial navigation system that works in parallel with thatcomplementary KF In particular the proposal in [16] focusesspecifically in the detection of the ZVU intervals althoughvery limited experimental results support the design

In [17] the positioning system is based on processingthe RSS measurements from RFID tags in a tightly coupledway The obtained average error is 135m along 4 trajectoryscenarios of 600 550 520 and 1000m of length respectivelyMoreover GPS measurements are integrated in outdoorsenvironment using a loosely coupled complementaryKalmanfilter and magnetometer data is also used to aid in indoornavigation in [18] achieving results with an average erroraround 2m in a 300m long trajectory

In this paper a novel extended Kalman filter (EKF) and asimplified complementary Kalman filter (KF) are proposedboth filters enable the fusion of the position computedthrough one or more inertial sensors with periodic positionupdates provided by a positioning solution based on wirelesstechnologies (GPS or indoor positioning system) The fusionsystem includes some quite important simplifications of sen-sor andmovement modeling with respect to those existing inliterature (specifically on the Foxlinrsquos contribution [15] whichinspires one of the proposed solutions) In the results sectionit is shown how even with those simplifications the systemmaintains a high accuracy being robust enough for manytypical localization applications The main contribution ofour approach is the very small computational load andbatteryconsumption introduced by the filter a feature of paramountimportance in resource and energy-constrained wearableapplications The fusion of both inputs is done in a looselycoupledway so the systemcan adapt to the infrastructure thatis available at a specific moment Additionally the solution isubiquitous as it can work in indoor environments using theinformation from wireless technologies or outdoors using aGPS receiver

The paper provides an overview of both approachesperforming a comparative analysis of their quality Sections2 and 3 describe the models assumed for the problemformulation In particular Section 2 describes the pedestrianmovement in terms of the typical signalsmeasured by inertialand positioning systems while Section 3 is centered onthe description of the errors coming from typical wearablesensors Afterwards Section 4 includes the derivation of thetwo different proposed filtering solutions Performance is









119868axis points



















119865axis


119865or119884119861axis)





50 100 150 200 250 300

05


Samples

minus5

minus10

minus15

minus20

Acce

lera

tion

(ms2)

(a)

50 100 150 200 250 300Samples

010203040


minus10

minus20

minus30

Acce

lera

tion

(ms2)

(b)

50 100 150 200 250 300

Heading

Samples

Hea

ding

(rad

)


minus155minus16

minus165minus17

minus175minus18

(c)











119864 119909119898 = 119909 119864 119910

119898 = 119910 (1)






119864 119886119871119898 = 119886119871+ Δ119886119871 119864 119886

119879119898 = 119886119879+ Δ119886119879 (2)


119879is the real


119871119898would






119864 120595119898 = 120595 + Δ120595 (3)







(i) Δ119909 Δ119910 offset in119883119868and 119884







(i) Prediction

119901= 119891 (

119891)

119875119901= 119865119875119891119865119879+ 119876

(4)


119891


119901is the







119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)








119891 119867 is





frame






119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)



((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)



119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation

















119868axis points



















119865axis


119865or119884119861axis)





50 100 150 200 250 300

05


Samples

minus5

minus10

minus15

minus20

Acce

lera

tion

(ms2)

(a)

50 100 150 200 250 300Samples

010203040


minus10

minus20

minus30

Acce

lera

tion

(ms2)

(b)

50 100 150 200 250 300

Heading

Samples

Hea

ding

(rad

)


minus155minus16

minus165minus17

minus175minus18

(c)











119864 119909119898 = 119909 119864 119910

119898 = 119910 (1)






119864 119886119871119898 = 119886119871+ Δ119886119871 119864 119886

119879119898 = 119886119879+ Δ119886119879 (2)


119879is the real


119871119898would






119864 120595119898 = 120595 + Δ120595 (3)







(i) Δ119909 Δ119910 offset in119883119868and 119884







(i) Prediction

119901= 119891 (

119891)

119875119901= 119865119875119891119865119879+ 119876

(4)


119891


119901is the







119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)








119891 119867 is





frame






119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)



((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)



119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










50 100 150 200 250 300

05


Samples

minus5

minus10

minus15

minus20

Acce

lera

tion

(ms2)

(a)

50 100 150 200 250 300Samples

010203040


minus10

minus20

minus30

Acce

lera

tion

(ms2)

(b)

50 100 150 200 250 300

Heading

Samples

Hea

ding

(rad

)


minus155minus16

minus165minus17

minus175minus18

(c)











119864 119909119898 = 119909 119864 119910

119898 = 119910 (1)






119864 119886119871119898 = 119886119871+ Δ119886119871 119864 119886

119879119898 = 119886119879+ Δ119886119879 (2)


119879is the real


119871119898would






119864 120595119898 = 120595 + Δ120595 (3)







(i) Δ119909 Δ119910 offset in119883119868and 119884







(i) Prediction

119901= 119891 (

119891)

119875119901= 119865119875119891119865119879+ 119876

(4)


119891


119901is the







119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)








119891 119867 is





frame






119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)



((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)



119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











119864 119886119871119898 = 119886119871+ Δ119886119871 119864 119886

119879119898 = 119886119879+ Δ119886119879 (2)


119879is the real


119871119898would






119864 120595119898 = 120595 + Δ120595 (3)







(i) Δ119909 Δ119910 offset in119883119868and 119884







(i) Prediction

119901= 119891 (

119891)

119875119901= 119865119875119891119865119879+ 119876

(4)


119891


119901is the







119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)








119891 119867 is





frame






119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)



((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)



119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












119870 = 119875119901119867119879(119867119875119901119867119879+ 119877)minus1

119891= 119901+ 119870(

119898minus ℎ (

119901))

119875119891= (119868 minus 119870119867)119875119901

(5)








119891 119867 is





frame






119891= [119909119891 119910119891 |V|119891 120595119891 119886119871119891 119886119879119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(6)



((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

(((((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2|V|119891 + 119886119871119891Δ119905

120595119891+119886119879119891Δ119905

|V|119891119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

)))))))))))))))

)

(7)



119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











119865 =

((((((((((((((

(

1 0 cos (120595119891) Δ119905 119865

14

cos (120595119891) Δ1199052

2

minus sin (120595119891) Δ1199052

20 0 0

0 1 sin (120595119891) Δ119905 119865

24

sin (120595119891) Δ1199052

2

cos (120595119891) Δ1199052

20 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0minus119886119879119891Δ119905

|V|2119891

1 0Δ119905

|V|1198910 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

))))))))))))))

)

(8)

where


[119886119871119891sin (120595

119891) + 119886119879119891

cos (120595119891)] Δ1199052

2

11986524= |V|119891 cos (120595119891) Δ119905 +

[119886119871119891cos (120595

119891) minus 119886119879119891

sin (120595119891)] Δ1199052

2

(9)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 12059021198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(10)






variance 1205902Δ119886119871119898






((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891) =

((((((((

(

119909119891

119910119891

|V|119891120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))

)

(11)


119865 = 119868 (12)



119876 =

(((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902

Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

)))))))

)

(13)




119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











119876 =

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 1205902

1205751205950 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 1205902Δ120595+ 12059021205751205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

(14)

where 1205902120575120595





((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

= 119901= 119891 (

119891)

=

((((((((((((

(

119909119891+ |V|119891 cos (120595119891) Δ119905 +

119886119871119891cos (120595

119891) Δ1199052

2

119910119891+ |V|119891 sin (120595119891) Δ119905 +

119886119871119891sin (120595

119891) Δ1199052

2|V|119891 + 119886119871119891Δ119905120595119891

119886119871119891

119886119879119891

Δ120595119891

Δ119886119871119891

Δ119886119879119891

))))))))))))

)

(15)


119865 =

(((((((((((

(

1 0 cos (120595119891) Δ119905 minus(|V|119891Δ119905 +

Δ1199052

2119886119871119891) sin (120595

119891)Δ1199052

2cos (120595

119891) 0 0 0 0

0 1 sin (120595119891) Δ119905 (|V|119891Δ119905 +

Δ1199052

2119886119871119891) cos (120595

119891)Δ1199052

2sin (120595

119891) 0 0 0 0

0 0 1 0 Δ119905 0 0 0 0

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 1 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)))))))))))

)

(16)


119876 = 119865

((((((((

(

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0

0 0 0 0 1205902

1198861198710 0 0 0

0 0 0 0 0 1205902

1198861198790 0 0

0 0 0 0 0 0 1205902Δ1205950 0

0 0 0 0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 0 0 0 1205902Δ119886119879119898

))))))))

)

119865119879

(17)








119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation













119898= (119909119898

119910119898

) ℎ (119901) = ℎ

((((((((

(

((((((((

(

119909119875

119910119901

|V|119901120595119901

119886119871119901

119886119879119901

Δ120595119901

Δ119886119871119901

Δ119886119879119901

))))))))

)

))))))))

)

= (119909119901

119910119901

)

119867 = (1 0 0 0 0 0 0 0 0

0 1 0 0 0 0 0 0 0)

119877 = (1205902

1198750

0 1205902119875

)

(18)

where 1205902119875



119871119898and

119886119879119898


119898= (119886119871119898

119886119879119898

) ℎ (119901) = (119886119871119901+ Δ119886119871119901

119886119879119901+ Δ119886119879119901

)

119867 = (0 0 0 0 1 0 0 1 0

0 0 0 0 0 1 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(19)

where 1205902119860




119898=(

0

0

0

119886119871119898

119886119879119898

) ℎ (119901) = (

|V|119901119886119871119901

119886119879119901

Δ119886119871119901

Δ119886119879119901

)

119867 =(

0 0 1 0 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 1 0

0 0 0 0 0 0 0 0 1

)

119877 =(

(

1205902|V|119901119898 0 0 0 0

0 12059021198601199011198980 0 0

0 0 12059021198601199011198980 0

0 0 0 12059021198600

0 0 0 0 1205902119860

)

)

(20)








119898= (120595119898) ℎ (

119901) = (120595

119901+ Δ120595119901)

119867 = (0 0 0 1 0 0 1 0 0)

119877 = (1205902

120595)

(21)


(i) position (119909119898 119910119898)

(ii) acceleration (119886119871119898

119886119879119898


119898



(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










(119909119868 119910119868 |V|119868 and 120595



119886119879119898




119868) Consider

119909119868= 119909119868+ |V|119868 cos (120595119868) Δ119905

+ [119886119871119898

cos (120595119868) minus 119886119879119898

sin (120595119868)]Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119868) Δ119905

+ [119886119871119898

sin (120595119868) + 119886119879119898

cos (120595119868)]Δ1199052

2

|V|119868 = |V|119868 + 119886119871119898Δ119905

120595119868= 120595119898

(22)


119891= [Δ119909119891 Δ119910119891 Δ|V|119891 Δ120595119891 Δ119886119871119891 Δ119886119879119891]

119879

(23)



119886119871119906= 119886119871119898minus Δ119886119871119891

119886119879119906= 119886119879119898minus Δ119886119891

120595119906= 120595119898minus Δ120595119891

(24)



Δ1199052

2

119910119868= 119910119868+ |V|119868 sin (120595119906) Δ119905 + [119886119871119906 sin (120595119906) + 119886119879119906 cos (120595119906)]

Δ1199052

2

|V|119868 = |V|119868 + 119886119871119906Δ119905

120595119868= 120595119906

(25)



119909119906= 119909119868minus Δ119909119891

119910119906= 119910119868minus Δ119910119891

|V|119906 = |V|119868 minus Δ|V|119891

(26)


119909119868= 119909119906 119910

119868= 119910119906

Δ119909119891= 0 Δ119910

119891= 0

(27)


|V|119868 = |V|119906 Δ|V|119891 = 0 (28)


Δ|V|119891 = |V|119868 + Δ|V|119891 |V|119868 = 0 (29)


119868= [119909119868 119910119868 |V|119868 120595119868]

119879

(30)






IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












IMU measures



minusminus


119865 =

(((((

(

1 0 cos (120595119906) Δ119905 119865

14

cos (120595119906) Δ1199052

2

minus sin (120595119906) Δ1199052

2

0 1 sin (120595119906) Δ119905 119865

24

sin (120595119906) Δ1199052

2

cos (120595119906) Δ1199052

20 0 1 0 Δ119905 0

0 0 0 1 0 0

0 0 0 0 1 0

0 0 0 0 0 1

)))))

)

(31)

where

11986514= minus(|V|119868Δ119905 + 119886119871119906

Δ1199052

2) sin (120595

119906) minus 119886119879119906

Δ1199052

2cos (120595

119906)

11986524= (|V|119868Δ119905 + 119886119871119906

Δ1199052

2) cos (120595

119906) minus 119886119879119906

Δ1199052

2sin (120595

119906)

(32)


(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

= 119901= 119891 (

119891) = 119865

119891= 119865(

(

Δ119909119891

Δ119910119891

Δ|V|119891Δ120595119891

Δ119886119871119891

Δ119886119879119891

)

)

(33)


119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(34)



119876 = 1198651198760119865119879= 119865((

(

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 1205902120575120595+ 1205902

Δ1205950 0

0 0 0 0 1205902Δ1198861198711198980

0 0 0 0 0 1205902Δ119886119879119898

))

)

119865119879

(35)







119898= (119909119898minus 119909119868

119910119898minus 119910119868

)


ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










ℎ (119901) = ℎ(

(

(

(

Δ119909119901

Δ119910119901

Δ|V|119901Δ120595119901

Δ119886119871119901

Δ119886119879119901

)

)

)

)

= (minusΔ119909119901

minusΔ119910119901

)

119867 = (minus1 0 0 0 0 0

0 minus1 0 0 0 0)

119877 = (12059021198750

0 1205902119875

)

(36)




119898= (119886119871119898

119886119879119898

) ℎ (119901) = (Δ119886119871119901

Δ119886119879119901

)

119867 = (0 0 0 0 1 0

0 0 0 0 0 1)

119877 = (1205902

1198600

0 1205902119860

)

(37)


119898= (|V|119868) ℎ (

119901) = (Δ|V|119901)

119867 = (0 0 1 0 0 0)

119877 = (1205902

Δ|V|119901119898)

(38)







119882and 1205902

119886




MV = 11205902119886119882

119899+119882minus1

sum119896=119899

1003817100381710038171003817119886119896 minus 11988611988210038171003817100381710038172

ARE = 11205902120596119882

119899+119882minus1

sum119896=119899

100381710038171003817100381712059611989610038171003817100381710038172

(39)







Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











Lt1 = 16m

Lt2 = 24mΔdbn = 4m

Trajectory

Lt2

Lt1

Δdbn

xI

yI







IRIS mobile node












120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation













120590119886119871

30ms2 NA120590119886119879



120590Δ119886119879119898


120590119875 05m 20m120590119860 02ms2 02ms2


10minus8ms NA120590119860|V|119901119898 NA 1ms

0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)





0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18



xI (m)

yI

(m)







0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










0

0

minus2

minus4

minus6

minus8

minus10

minus12

minus14

minus16

minus18

minus20



xI (m)

yI

(m)


















0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










0 50 100 150 200

0

50

100

150




x (m)

y(m

)
































Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation



















Notations







integration



Acknowledgments




References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











References






























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation









Documents

Research Article Simplified Pedestrian Tracking Filters with Positioning and Foot ...downloads.hindawi.com/journals/ijdsn/2014/850835.pdf · 2015. 11. 23. · Simplified Pedestrian