WO2019177471A1

WO2019177471A1 - A method of object localization, particularly of human beings, and a device for human localization

Info

Publication number: WO2019177471A1
Application number: PCT/PL2018/050066
Authority: WO
Inventors: Michał MEINA; Krzysztof Rykaczewski
Original assignee: Uniwersytet Mikolaja Kopernika W Toruniu
Current assignee: Uniwersytet Mikolaja Kopernika W Toruniu
Priority date: 2018-01-29
Filing date: 2018-12-14
Publication date: 2019-09-19
Anticipated expiration: 2020-07-29
Also published as: PL241476B1; PL424408A1

Abstract

The method of object localization, in particular, a human being, using inertial measurement units IMU1 and IMU2, both of which contain a three-axis accelerometer and a three-axis gyroscope, using IMU2 which additionally contains a three-axis magnetometer and IMU1 contains a permanent magnet, by simultaneously measuring acceleration, angular velocity, and the magnetic field induced by the permanent magnet, and later processing the measurements in the reading and processing unit by integrating the readings over a time period. The device for object localization, in particular, a human being, consists of at least one pair of inertial measurement units, connected with a reading and processing unit. The first inertial measurement unit of the pair contains an accelerometer, a gyroscope, and a permanent magnet, and the second contains a gyroscope, an accelerometer, and a magnetometer.

Description

A method of object localization, particularly of human beings, and a device for human localization

The core of the invention is the method of establishing the location of an object (particularly, a human being) by means of inertial navigation (with dead reckoning), and a device for localizing an object (particularly, a human being) by dead reckoning, which utilizes a method of relative positioning of the human’s feet to enhance the localization precision.

The usage of inertial navigation for object localization was described by Eric Foxlin, who, in his article“Pedestrian tracking with shoe-mounted inertial sensors published in November 2015 in IEEE Computer Graphics and Applications, presented a method based on sensors implanted into a pedestrian’s footwear, simultaneously pointing out some limitations stemming from the deviation of the estimated position from the real object position. For the needs of his system, Foxlin assumed that during walking, there is a certain timespan in which a pedestrian’s foot remains stationary.

There are also other known methods of localizing an object (in particular, a human being), using sensors implanted on the object.

US patent 6549845 describes a setup enabling indoor localization which utilizes a magnetometer (a sensor that measures an matter’s magnetic properties), permanent magnets, pressure sensors (barometers), and a CPU for calculating the length and direction of every step. The data may be presented to the user, preferably on a map, or on the floor, and may be transmitted outside the building in question.

US patent 8751151 describes a computer-implemented method of localizing a trac- kee at a location and generating or updating a map of the location based on inertial sensor information, the method being implemented on a computer system that in- eludes one or more physical processors programmed by one or more modules, the method comprising:

• identifying, by a feature detection module, at least one sensor feature based on inertial sensor information that includes a plurality of measurements that measure a motion of the trackee at the location, the at least one sensor feature being identified based on a first subset of the plurality of measurements;

• identifying, by the feature detection module, a structural feature of the location using only the at least one sensor feature;

• determining, by a localization and mapping module, a location estimate that estimates a location of the trackee based on at least a second subset of the plurality of measurements that is the same as or different from the first subset of the plurality of measurements, wherein a position of the structural feature is determined based on the location estimate;

• generating or updating, by the localization and mapping module, a map of the location based on the structural feature.

The patent US5899963 describes a device that measures the distance travelled, speed, and height jumped of a moving object or a person while running or walking. Accelerometers and rotational sensors are placed in the object or in the sole of one shoe, or in a wrist watch or the waist of the user, along with an electronic circuit that performs mathematical calculations to determine the distance and height. A microprocessor calculates an output speed based upon step-distance and elapsed time, and the distance traveled from the sum of all previous steps. The output of the microprocessor is coupled to a display that shows the distance traveled, speed, or height jumped.

US patent 6786877 describes a self-contained sensor apparatus that generates a signal, which corresponds to at least two of the three orientational aspects of yaw, pitch and roll of a human-scale body, relative to an external reference frame. A sensor generates first sensor signals that correspond to rotational accelerations or rates of the body about certain body axes. The sensor may be mounted to the body. Coupled to the sensor is a signal processor for generating orientation signals relative to the external reference frame that correspond to the angular rate or acceleration signals. The first sensor signals are impervious to interference from electromagnetic, acoustic, optical and mechanical sources. The sensors may be rate sensors. An integrator may integrate the rate signal over time. A drift compensator is coupled to the rate sensors and the integrator. The drift compensator may include a gravitational tilt sensor or a magnetic field sensor or both. A verifier periodically measures the orientation of the body by a means different from the drift sensitive sate sensors. The verifier may take into account characteristic features of human motion, such as stillness periods. The drift compensator may be, in part, a Kalman filter, which may utilize statistical data about human head motion.

A Chinese patent CN103591959 reveals a method comprising of the following steps:

1. positioning the plane position of the indoor personnel by a stride frequency and stride method;

2. positioning the plane position of the indoor personnel by a time-frequency trans formation integral method;

3. reading the altitude value Zi of the position of the indoor personnel detected by a pressure height sensor by a CPU (Central Processing Unit);

4. fusing the personal position results obtained in the steps 1 and 2; and finally, combining the altitude value Zi of the indoor personnel at the ith moment obtained in step 3 to obtain a three-dimensional spatial position (Xi, Yi and Zi) of the indoor personnel at the ith moment as a positioning result of the three-dimensional space to position the indoor personnel.

The positioning method provided by the invention measures the position information of the personnel in real time by means of the sensor which measures data such as pressure, motion acceleration and azimuth angle in real time, so that the method is not only high in positioning precision, but also high in reliability.

The patent EP1985233 describes a method which involves acquiring a physical measure along sensible three axes of a sensor e.g. sensible three axis magnetometer sensor. An invariant rotation axis is estimated in a space of the physical measure. The estimated axis is identified. A value of an indicator for indicating the variations of the rotation axis is calculated. The value of the indicator is compared with the predetermined threshold value. Independent claims are also included for the following: (1) a method for estimating movement of a mobile equipped at a sensor (2) a method for calibrating a sensor for determining the rotation matrix (3) a device for detecting a substantially invariant axis of rotation.

Utility model CN205066775U presents a high accuracy movement track detection device, wherein: the three-dimensional gesture angle of moving part is measured by inertial sensing locator system, three-dimensional position of moving part and three- dimensional gesture angle are measured by electromagnetism locator system, the three-dimensional positional information of moving part measures in machine vision locator system, an output that is used for data fusion’s treater to connect inertial sen sing locator system, electromagnetism locator system, machine vision locator system simultaneously to fuse the data of three subsystem, obtain moving part’s movement track.

In US patent 9341683 a method of determining a heading of a machine having an implement is provided. The method includes determining a first heading data of the machine using an inertial sensor. The method includes determining a second heading data of the machine using a magnetometer. The method includes determining a position of the implement in a stationary state. The method also includes calculating a corrected second heading data based on a predefined relation between the position of the implement in the stationary state and the second heading data. The method further includes determining the heading of the machine based on the first heading data and the corrected second heading data.

Patent CN105509736 reveals an indoor composite locating method for fire rescue. The method comprises specific steps as follows: an indoor composite locating system comprising a three-axis accelerometer, a three-axis gyroscope, a three-axis magnetic compass, a barometer, a card reader, an RFID (radio frequency identification) tag and a DSP (digital signal processor) is constructed; a signal acquisition module acqu ires signals of the three-axis accelerometer, the three-axis gyroscope, the three-axis magnetic compass and the barometer; a dead reckoning module acquires location information of rescue workers; a motion state extraction module determines motion states of the rescue workers; a matching correction module corrects locations of the rescue workers; a fixed-point correction module determines the locations of the rescue workers in combination with cartographic information so as to perform fixed-point correction on the locations of the rescue workers. The indoor composite locating me thod for fire rescue realizes high-precision indoor location and is easy to implement and low in dependence on external conditions.

US8972182 presents a method for tracking a handheld or head-mounted item, comprising:

· fixedly positioning an inertial navigation unit attached to a pedestrian’s foot;

• making a measurement related to a position of the navigation unit;

• updating a position and/or orientation of the handheld or head-mounted item to be tracked, based at least in part on the measurement, wherein updating a position and/or orientation of the item comprises determining a position of the item relative to the navigation unit. The clue of the invention is the construction of a method of object localization (particularly, a human being), outside of the range of systems like the GPS, using inertial navigation with dead reckoning, connected with determining the relative lo cation of the object’s feet, which significantly improves the precision of the estimated location, and a device necessary to utilize that method. The original technical effect is achieved by placing a magnet (which induces a magnetic field of a known shape) on one shoe, and a magnetometer on the other shoe of the object (which registers the deviations of the surrounding magnetic field from the expected characteristic). This enables establishing the relative location of the feet in short timespans, which in turn serves as basis for improvement of the precision of the location estimated by the inertial localization system.

Description

The method of object localization, as devised, bases on continuously measuring a set of physical parameters:

· acceleration (on both feet)

• angular velocity (on both feet)

• the magnetic induction vector of the field induced by the permanent magnet on one of the feet (by a sensor on the other foot)

For that, measurement units IMU1 and IMU2, as presented in Fig. 1, are used. Each of them contains an accelerometer and a gyroscope, and it is assumed that one of them additionally contains a magnetometer, and that there is a permanent magnet placed on the foot where the unit without the magnetometer is positioned. The data obtained from the measurement units presented in Fig. 1, is processed in the reading module in accordance with the steps described below. The processed data are imaged on a display as points corresponding to the position which the object occupies in the set coordinate system, on a plan of the space which the object has traversed. The data is also sent, via radio, to a server for further processing or presentation purposes. The Local Navigation Coordinate Frame is set as the navigation coordinate system, which is a local coordinate system set in reference to a fixed point at the location where the measurements are conducted. The coordinate system can be translated to the International Terrestrial Reference Frame when the ITRF location of a reference point is known in the original space. The processing of the acceleration, angular velocity and magnetic data (described by the magnetic field induction vector), is described by the following sequence of measurement and processing steps:

1. Calculate the difference in orientation, velocity and location, basing on the readings from IMU1 and IMU2, by integrating those measurements over a ti- mespan, preferably by utilizing methods from inertial navigation theory like Kalman filters.

The method used for calculating the changes in orientation, velocity and loca- tion based on readings from IMU1 and IMU2, is a Kalman filter, constructed as described below: (a) Let C_m be the orientation matrix of a sensor, relative to a set coordinate system, r_m be the location vector and v_m be the velocity in the m-th step of the algorithm.

(b) The equation of motion for the sensor, according to which the foot move- ment is reconstructed, is formulated as:

where At is the time increase,

is the acceleration in the navigation coordinate system, and is formulated as:

where

is the acceleration of the sensor in the Earth’s coordinate sys tem, go := [0, 0, 9.81 m,/s²], is the gravitational acceleration vector, ant the matrix C_m^_m denotes the rotation matrix updates, which are calculated by integrating the gyroscope’s readings. Those equations are introduced and fully discussed in a comprehensive work by C. Fischer, P.T. Sukumar and M. Hazas, 2013. Tutorial: Implementing a pedestrian tracker using inertial sensors. IEEE pervasive computing, 12(2), pp.17-27. The equation (1) can be rewritten as:

where x_m is an a priori state of form

denotes a transposition.

(c) The matrices A_m and B are linear transformations defined as

where I is the identity matrix,

and

(d) The vector u_m is a steering factor, and is defined as:

(e) In the Kalman filter, the error covariance matrix is updated in accordance with the equation:

where PQ := O is a 0-matrix, and Q is the noise covariance matrix. (f) The observation matrix is defined as H := [o ij.

(g) The Kalman matrix is:

where R is the observations’ covariance matrix.

(h) Using the zero-speed assumption, an a posteriori location is calculated, which is a location estimation from the inertial navigation circuit, as:

2. Determine the vector T_c and the matrix C_c, which are the updates for the location and orientation estimation, using the method laid out below. Mind that T_c = [x_c, y_c, z_c }€ R³ is a translation that corrects the location of one of the feet in the navigational coordinate system, whereas C_c is a transformation given by a unitary matrix, which is a rotation relative to a point given by the location of the last update, i.e. the last point where the last zero-speed phase occured. The point is denoted by c, and all operations relative to that point are denoted with the subscript c.

3. Each time after a zero-speed phase on the foot without the permanent magnet, later denoted as later denoted as isZV-nonmagnet (the occurrence of which is detected by verifying if the acceleration readings on the foot without the per manent magnet), perform the following updates on the foot with the permanent magnet:

where · denotes matrix multiplication.

4. Each time after a zero-speed phase on the foot with the permanent magnet (later denoted as isZV-magnet ), perform the following updates on the foot without the permanent magnet:

where isZV-nonmagnet is the time in movement, when the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer, is motionless; isZV-magnet denotes the time when the foot with the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet, is motionless; pos_magnet is the position of the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; pos_nonmagnet is the position of the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer; C_magnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; C_nonmagnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a magnetometer; T_c is the location update vector, and C_c is the rotation update matrix. The initial values of pos_magnet, O_magnet poS_nonmagnet oraz ⁽ _nonmagnet need to be set Independently of the method, e.g. can be set manually, by reference to some assumed motions implying location and orientation, or be measured by radio or acoustically.

For calculating the updates T_c and C_c, the characteristic of the magnetic induction field in each point surrounding the foot with the permanent magnet must be determi ned. To achieve that, a distribution of the induction values is assumed - a theoretical model of the magnetic field. There are multiple models of magnetic fields, some of which depend on few parameters like e.g. dipole models. In order to recreate the field with a given model, measurements of the magnetic field induction are conducted in the points ¾·€ M³, j = 1, . . . , l around the permanent magnet, and a cost function is introduced, which measures how well the theoretical model fits the empirical readings Such cost function may be defined e.g. as a sum of norms of the differences between the theoretical field and the empirical field over the points _<¾·. The theoretical model is fit (calibrated) to the empirical field by tuning the parameters in such a way, that it faithfully reproduces the empirical field induced by the permanent magnet. Prefe rably, the calibration is performed by triangulating the measurement points ¾·, where the magnetic field magnitude is estimated in a given point as a barycentric average of magnetic induction values in the vertices of the symplex wherein the point resides (created from the triangulation points, which in turn are drawn from the induction measurement points). A different desirable example is to perform the calibration by fitting a dipole field and/or a sum of dipole fields (which requires the tuning of one or a few parameters, dependent on the field parametrization) in such a way, that the theoretical model fits the directions and induction values of the empirical field in all measurement points. The result of the calibration is a theoretical field best fit (as evaluated by the cost function) to the empirical field of the permanent magnet.

The updates T_c and C_c are calculated at the beginning of each phase isZV, by minimizing the expression J(T_c, C_c ) (which sets the error of the approximation for the empirical field, as measured with a magnetometer, by the theoretical model) over the variables T_c and C_c. The optimization is ran over all vectors and unitary matrices. Thus, the minimization is the search for the variables T_c and C_c, by fitting the measured empirical properties of the field to the theoretical model, or in another possible example, by minimizing the function obtained by investigating the norm of the difference of the magnetic induction of the empirical field from the induction of the theoretical field. This difference is the value of the norm of the geomagnetic field (the Earth’s natural magnetic field). The exact formulas for both cases are laid out below.

In the case of fitting the empirical field to the theoretical field, the expression minimized is of form:

where || · || is the Euclidean norm, magpoS_j(T_c, C_c) expresses the measurement of the magnetic field at the point pi , i = 1, . . . , k, taken from a fragment of the path from the last isZV phase, which resulted from applying the transformations T_c and C_c to the whole path in that phase, and magteor^ are the values for the theoretical field. In other words, the algorithm translates and rotates the path from the last phase isZV in such a way that the magnetic field induction values, as induced by the permanent magnet, are in accordance with the magnetic induction readings in the path’s points.

For fitting the difference in fields’ norm to the geomagnetic field, the expression minimized is: magteor I - geomagnetic ) , (12)

where || · || is the Euclidean norm, and geomagnetic, is the geomagnetic field magnitude at the point ]_¾, i = 1, . . . , k in the path.

The method, as devised, recreates the movement path of the sensors, together with their orientation in a common navigational coordinate system, using the inertial sensors IMU1 and IMU2.

The device for object localization, in particular, a human being, consists of at least one pair of inertial measurement units, connected with a reading and processing unit. The first inertial measurement unit of the pair consists an accelerometer, a gyroscope, and a permanent magnet. The second unit consists of an accelerometer, gyroscope and a magnetometer. The inertial measurement units communicate with the reading and processing unit using a cable, or in a different good scenario, by radio communication, e.g. Bluetooth or Wi-Fi. The reading and processing unit may be equipped, to additionally enable displaying the object’s location. The display may be connected to the reading and processing module by a cable or by radio, prefferably by Bluetooth. To an additional advantage, the display may be placed in the frame of the object’s glasses, replacing one of the lenses. The reading and processing module may contain a database of maps of the locations which the object can traverse, upon which the location points can be drawn.

The invention is presented in example figures, where Fig. 1 presents the general schematic of the device, Fig. 2 presents a block diagram of the location calculation method, Fig. 3 presents an example of how can the IMU1 and IMU2 sensors be mounted on shoes. Fig. 4 presents a plot of the phases of the movement of a foot, and the time at which the measurement is conducted. Fig. 5 presents a comparison between the classic inertial navigation algorithm from the aforementioned work by Foxlin, and the method described in this document. Fig. 6 presents an example of a graphical interface for a firefighter crew, in which a firefighter can observe his location, as well as the locations of other members of the crew taking part in a rescue mission. Example I

Using the inertial measurement units IMU1 (1) and IMU2 (2) placed on a moving person’s footwear (where both of the units are placed on different feet), acceleration, angular velocity and the magnetic field are simultaneously measured. Each of the inertial measurement units IMU1 (1) and IMU2 (2) contains an accelerometer and a gyroscope, one of them additionally contains a magnetometer (5), while the other is equipped with a permanent magnet (4).

As a result of the measurements, data was gathered, a portion of which is shown in Fig. 4. The data consists of two pairs of time series of the acceleration and angular velocity readings, and one time series of the magnetic field magnitude readings. Fig. 5 demonstrates a comparison of the classic inertial navigation method (as per the work by Foxlin) to the method described. Fig. 5 illustrates the traversal of a human of a 59m straight line, from which a location error of 22.75[cm] i 13.26[cm] is obtained for the classic method and the method described, respectively. Fig. 5 also shows the error for the path height estimation, which should be set to 0[m], as the traversal is upon a flat surface. The errors are, on average, 1.15[m] for the classic method and 31.1 [cm] for the method described.

The gathered data was processed in the reading and processing module (3), accor ding to the method below. The processing results were displayed on the display (6) as a path which the person traversed in the navigational coordinate system. The acce leration, angular velocity and magnetic field magnitude measurements are processed as per the following algorithm:

1. Calculate the orientation, velocity and location difference using the readings from IMU1 (1) and IMU2(2).

2. Determine T_c and C_c using the method described below.

3. After the conclusion of each zero-speed phase on the foot without the permanent magnet (isZV-nonmagnet) , calculate the following updates for the foot with the permanent magnet:

4. After the conclusion of each zero-speed phase on the foot without the permanent magnet ( isZV-magnet ), calculate the following updates for the foot without the permanent magnet:

where isZV-nonmagnet is the time in movement, when the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer, is motionless; isZV-magnet denotes the time when the foot with the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet, is motionless; pos_magnet is the position of the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; pos_nonmagnel is the position of the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer; C_magnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; C_nonmagnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a magnetometer; T_c is the location update vector, and C_c is the rotation update matrix. The initial values of pos_magnet, C _{magn t >} pos_nonmagnet oraz C_nonmagnet need to be set Independently of the method, e.g. can be set manually, by reference to some assumed motions implying location and orientation, or be measured by radio or acoustically.

The updates T_c and C_c are calculated by minimizing the expression J (T_c, C_c) (denoting the approximation error) over T_c and C_c, prefferably by empirically fitting the magnetic field magnitudes to the theoretical field, on, in another possible setup, by minimizing the function obtained by investigating the norm of the difference of the magnetic induction of the empirical field from the induction of the theoretical field. This difference is the value of the norm of the geomagnetic field (the Earth’s natural magnetic field). When fitting the empirical field to the theoretical field the minimized expression takes the form:

where || · || is the Euclidean norm, magpoS_j(T_c, C_c) expresses the measurement of the magnetic field at the point ¾, i = 1, . . . , k, taken from a fragment of the path from the last isZV phase, which resulted from applying the transformations T_c and C_c to the whole path in that phase, and magteor_j are the values for the theoretical field in

When fitting the norm of field differences to the geomagnetic field, the expression is:

magteor I - geomagnetic_;) . (14)

The magnet (4) placed in the IMU1 inertial measurement unit, induces a magnetic field around itself. The modelled theoretical magnetic field is fit (calibrated) to the empirical field by tuning the parameters in such a way, that the theoretical field fa ithfully reproduces the field induced by the permanent magnet (4). The calibration is performed by triangulating the measurement points ¾·, where the magnetic field magnitude is estimated in a given point as a barycentric average of magnetic induc tion values in the vertices of the symplex wherein the point resides (created from the triangulation points, which in turn are drawn from the induction measurement points). The processed data from the sensors IMU1 (1) and IMU2 (2), reproduces the movement paths of the sensors (1) and (2), together with their orientation, in the navigational coordinate system.

The device for human localization consists of at least one pair on inertial measu rement units IMU1 (1) and IMU2 (2) connected to a reading and processing module (3). The first inertial measurement unit IMU1 (1) consists of an accelerometer, a gyro- scope, and a permanent magnet 4 The second inertial measurement unit IMU2 (2) consists of an accelerometer, gyroscope, and a magnetometer (5). The inertial measu rement units IMU1 (1) and IMU2 (2) communicate with the reading and processing module by means of a cable (not shown in the figure). The reading and processing module (3) is equipped with a radio module for transmitting the person’s location and a display, on which the points corresponding to the human’s location are displayed. Example II

Two firefighters conducting a rescue/extinguishing operation in a building fully filled with smoke are equipped with the human locating devices. Two pairs of two iner tial measurement units IMU1 (1) and IMU2 (2) are integrated with the firefighters’ footwear ((1) and (2) are placed on a different shoe on each of the firefighters). Ac celeration, angular velocity and the magnetic fields surrounding the footwear were simultaneously measured. Each of thew inertial measurement units IMU1 (1) and IMU2 (2) in each device has an accelerometer and a gyroscope, one of them is addi tionally equipped with a magnetometer (5), and the other with a permanent magnet (⁴)·

The data obtained was processed in the reading and processing module (3) by the algorithm presented below. The processed data was imaged on the display (6) as the paths of movement of the firefighters and a location of the object tracked in a set coordinate system. The location information is transmitted to an external pro- gress tracking system for the use of the rescue/extinguishing operation coordinator. The reading and processing module (3), by means of a radio module (7), transmits the location information to the second firefighter’s device, so that each firefighter’s location can be displayed on the display (6) of the device carried by each of the fire fighters, thus informing them of their relative positions. The display (6) also serves as a mean of storing the information about the movement path, in case of possible orientation loss, or in case of intensive visibility obstruction due to high smoke levels. The display (6) is used as a handy mobile device, or integrated into the firefighter’s helmet. Fig. 6 presents the view available to one of the firefighters on his/her display: it shows his current location and movement path, and the location and path of the other firefighter.

The acceleration, angular velocity, and magnetic field data processing is described by the algorithm below:

2. Determine T_c and C_c using the method described below.

where isZV-nonmagnet is the time in movement, when the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer, is motionless; isZV-magnet denotes the time when the foot with the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet, is motionless; pos_magnet is the position of the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; pos_nonrnagnet is the position of the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetometer; C_magnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; C_nonmagnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a magnetometer; T_c is the location update vector, and C_c is the rotation update matrix.

The updates T_c and C_c are calculated by minimizing the expression J(T_c, C_c) (denoting the approximation error) over T_c and C_c, prefferably by empirically fitting the magnetic field magnitudes to the theoretical field, on, in another possible setup, by minimizing the function obtained by investigating the norm of the difference of the magnetic induction of the empirical field from the induction of the theoretical field. This difference is the value of the norm of the geomagnetic field (the Earth’s natural magnetic field). When fitting the empirical field to the theoretical field the minimized expression takes the form:

where || · || is the Euclidean norm, magpos^T/, C_c) expresses the measurement of the magnetic field at the point pi, i = , . . . , k, taken from a fragment of the path from the last isZV phase, which resulted from applying the transformations T_c and C_c to the whole path in that phase, and magteor are the values for the theoretical field in Pi ^ 1 , · · · , &.

magteor I - geomagnetic*) . (16)

The magnet (4) placed in the IMU1 inertial measurement unit, induces a magnetic field around itself. The modelled theoretical magnetic field is fit (calibrated) to the empirical field by tuning the parameters in such a way, that the theoretical field faithfully reproduces the field induced by the permanent magnet (4). The calibration is performed by fitting a dipole field and/or a sum of dipole fields, tuning their parameters in such a way, that the theoretical field fits the directions and magnitudes of the empirical field in all points measured. The processed data from the sensors IMU1 (1) and IMU2 (2), reproduces the movement paths of the sensors (1) and (2), together with their orientation, in the navigational coordinate system.

The device for human localization consists of at least one pair on inertial measu rement units IMU1 (1) and IMU2 (2) connected to a reading and processing module (3). The first inertial measurement unit IMU1 (1) consists of an accelerometer, a gy- roscope, and a permanent magnet (4). The second inertial measurement unit IMU2 (2) consists of an accelerometer, gyroscope, and a magnetometer (5). The inertial measurement units IMU1 (1) and IMU2 (2) communicate with the reading and pro cessing module by radio. The reading and processing module (3) is equipped with a radio module for transmitting the person’s location and a display, on which the points corresponding to the human’s location are displayed.

Notation

Faza isZV-nonmagnet is the phase in motion, during which the foot equipped with the inertial measurement unit consisting of an accelerometer, a gyroscope, and a magnetometer, is motionless (determined e.g. using a statistical test). Faza isZV-magnet is the phase in motion, during which the foot with the iner tial measurement unit consisting of an accelerometer, a gyroscope, and a permanent magnet, is motionless.

Faza isZV denotes any of the phases isZV-magnet or isZV-nonmagnet. pos_magnet denotes the position of the foot equipped with the inertial measurement unit consisting of an accelerometer, a gyroscope, and a permanent magnet. poS_nonmagnet denotes the position of the foot equipped with the inertial measurement unit consisting of an accelerometer, a gyroscope, and a magnetometer.

C_magnet denotes the rotation matrix of the foot equipped with the inertial measure ment unit consisting of an accelerometer, a gyroscope, and a permanent magnet. C_nonmagnet denotes the rotation matrix of the foot equipped with the inertial me asurement unit consisting of an accelerometer, a gyroscope, and a magnetometer.

T_c denotes the location vector update. The vector is a translation (update) in the navigational coordinate system of one of the feet. T_c is of form [x_c, y_c, z_c\ £ M³.

C_c denotes the matrix constituting the rotation update. It is a unitary matrix, calculated by minimizing a relevant error function. The possible error functions are discussed in the document.

J (T_c, C_c) denotes an error function describing the fitting error of the magnetometer readings to the assumed theoretical model. This function is minimized in order to obtain the optimal values of T_c and C_c in order for the algorithm to function correctly. geomagnetic^ denotes the value (vector norm) of the geomagnetic field in the point Pi of the path.

11 a; 11 is the euclidean norm of the vector r g l³. magpoS_j(T_c, C_c ) is the magnetic field reading in the point pi, i = 1, . . . , k, of the path fragment since the last isZV phase, which was a result of applying T_c and C_c to the whole path fragment since isZV. magteor_j are the theoretical field values in the point p of the path.

Pi is the Z-th point of the path.

IMU an Inertial Measurement Unit, equipped with a three-axis gyroscope, and a three-axis accelerometer. C_m is the sensor orientation matrix relative to a set coordinate system in the m-th step of the algorithm. r_m is the location vector in the m-th step of the algorithm. v_m is the velocity in the m-th step of the algorithm. denotes the acceleration in the navigational coordinate system, which is given by equation (2). denotes the acceleration in a coordinate system tied to Earth.

C_{m, m} denotes the rotation matrix updates, obtained by integrating gyroscope re adings.

At is the time increase. go [0, 0, 9.81 m/s²] is the gravitational acceleration vector.

I is an identity matrix.

O is a zero-matrix.

H is the observation matrix given by [ o i ] . x_m is the a priori state of form [C_m, r_m, v_m]^T, where T denotes transposition. A_m is a linear transformation defined by eq. (4).

S_m is a linear transformation defined by eq. (5).

B is a linear transformation defined by eq. (6). u_m is a steering factor defined by eq. (7).

P_m is the error covariance matrix given by eq. (8). Q is the noise covariance matrix in the Kalman filter. K_m is the Kalman matrix given by eq. (9).

R is the observation covariance matrix.

Claims

1. The method of object localization, in particular, a human being, using iner tial measurement units IMU1 and IMU2, both of which contain a three-axis accelerometer and a three-axis gyroscope, characterized by the fact that IMU2 additionally contains a three-axis magnetometer and IMU1 contains a perma nent magnet, by simultaneously measuring acceleration, angular velocity, and the magnetic field induced by the permanent magnet, and later processing the measurements in the reading and processing unit by integrating the readings over a time period.

2. The method as in Claim 1, characterized by the fact that the reading from the sensors IMU1 and IMU2 is performed using inertial navigation theory methods, particularly, Kalman filters.

3. The method as in Claim 2, characterized by the fact that the Kalman filter steps are defined as follows:

(a) Let C_m denote the sensor orientation matrix in a set coordinate system, r_m denote the location vector and v_m the velocity (all in the mth step of the algorithm).

(b) The sensor’s movement equation, from which the feet movement is recon structed, is of form:

where At is the time increase, and

denotes the acceleration in the navigational coordinate system, which is given by:

is the acceleration of the sensor in a coordinate system tied with Earth, go := [0, 0, 9.81 m/s²] is the gravitational acceleration vector, and the matrix C_m^_m is a rotation matrix update, calculated by integrating gyroscope readings. A detailed derivation of the above formulas can be found in Tutorial: Implementing a pedestrian tracker using inertial sensors by C. Fischer, P.T. Sukumar and M. Hazas (2013) in IEEE pervasive computing , 12(2), pp.17-27.

Equation (1) can be rewritten as: ¾m m%m- 1 + Bu_r (19) where x_m is the a priori state of form x_m := [C_m, r_m, v_m]^T, where T denotes a transposition. (c) The matrices A_m and B are linear transformations defined as:

where I is the identity matrix,

and

B := [ O At At At]^T . (22)

(d) The vector u_m is a steering factor given by:

(e) The Kalman filter’s error covariance matrix P_m is updated per:

where /¾ := O is a zero matrix, and Q is the noise covariance matrix.

(f) The observation matrix is defined as H := [o i]

(g) The Kalman Matrix is:

where R is the observation covariance matrix.

(h) By employing the zero-speed assumption, the location is calculated a po steriori as a location estimation from the inertial navigation device as:

Xm. _*— /\^' ... //. ' (26)

4. The method as in Claims 2 or 3 characterized by the fact, that movement trajectories of IMU1 and IMU2 are estimated by integrating gyroscope and accelerometer readings.

5. The method as in Claims 1, 2 or 3, characterized by the fact that IMU1 and IMU2 are placed in any place below a human’s knee on their left and right legs.

6. The method as in Claims 1, 2 or 3, characterized by the fact that the locations calculated are updated and consequently corrected by analyzing the movement trajectories of IMU1 and IMU2 using the magnetic field readings obtained in IMU1 and induced by IMU2.

7. The method as in Claim 6, characterized by the fact that the correction of trajectory estimation is performed by finding the updates T_c and C_c by fitting a theoretical magnetic field (as obtained by calibration) to actual readings, in coherence with the trajectory estimations for IMU1 and IMU2.

8. The method as in Claim 7, characterized by the fact that the updates T_c and C_c are calculated by the algorithm below: After the conclusion of each zero-speed phase on the foot without the permanent magnet (isZV-nonmagnet) , calculate the following updates for the foot with the permanent magnet:

9. After the conclusion of each zero-speed phase on the foot without the permanent magnet ( isZV-magnet ), calculate the following updates for the foot without the permanent magnet:

where isZV-nonrnagnet is the time in movement, when the foot with the inertial measurement unit containing an accelerometer, a gyroscope, and a magnetome ter, is motionless; isZV-magnet denotes the time when the foot with the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet, is motionless; pos_magnet is the position of the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; pos_nonmagnet is the position of the foot with the inertial me asurement unit containing an accelerometer, a gyroscope, and a magnetometer; Cmagnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a permanent magnet; C_nonmagnet is the rotation matrix for the foot with the inertial measurement unit containing an accelerometer, gyroscope, and a magnetometer; T_c is the location update vector, and C_c is the rotation update matrix. The initial values of pos_magnet, Cmagnet , pos_nonmagnet oraz C_nonmagnet need to be set independently of the me thod.

10. The method as in Claim 7 or 8, characterized by the fact, that the updates the updates T_c and C_c are calculated by minimizing J(T_c C_c ) over T_c and C_c , the expression minimized being the fitting error, by e.g. fitting empirical magnetic field induction vectors to a theoretical field, or by investigating the difference norm of between the empirical and theoretical fields (by equating the result to the geomagnetic field).

11. The method as in Claim 9, characterized by the fact that the expression mini mized while fitting the theoretical and empirical fields is given by:

where |[ · || is the Euclidean norm, the expression magpo_Sj(T_c, C_c) denotes the magnetic field reading in point pi on the path since the last isZV phase, which was a result of applying T_c and C_c to the whole path fragment since isZV, and magteor_; are the magnetic field values in point _t i = 1, . . . , k.

12. The method as in Claim 9, characterized by the fact that while fitting the field difference to the geomagnetic field, the expression minimized is of form:

13. The method as in Claims 1 to 11, characterized by the fact that the permanent magnet situated in the inertial measurement unit, induces a field whose shape is known after conducting a calibration process, in which magnetic field induction vectors are measurement in a defined set of points relative to IMU1.

14. The method as in Claim 12, characterized by the fact that the theoretical magnetic field is modelled as a sum of dipole fields, or by interpolating the induction values in a given point as a barycentric average of magnetic induction values in the vertices of the symplex wherein the point resides (created from the triangulation points, which in turn are drawn from the induction measurement points) .

15. The method as in Claims 1 to 13, characterized by the fact that the processed data are imaged on a display as points corresponding with the location of the object in the navigational coordinate system.

16. The method as in claims 1 to 10, characterized by the fact that the points are imaged over a map of the location which the object is traversing.

17. The device for object localization, in particular, a human being, consisting of at least one pair of inertial measurement units, connected with a reading and pro- cessing unit, characterized by the fact that the first inertial measurement unit of the pair contains an accelerometer, a gyroscope, and a permanent magnet, and the second contains a gyroscope, an accelerometer, and a magnetometer.

18. A device as in Claim 16, characterized by the fact that the inertial measure ment units communicate with the reading and processing unit with one of the known methods among cable-wise connection, or radio communication, e.g. by Bluetooth or Wi-Fi.

19. A device as in Claims 16 or 17, characterized by the fact that the reading and processing unit is equipped with a radio module for relaying th object’s location.

20. A device as in Claims 16 or 17, characterized by the fact that the reading and processing unit is equipped with a display, on which the points corresponding to the object’s location are imaged.

21. A device as in claim 19, characterized by the fact that the display is placed in the frame of the glasses/goggles worn by a human, replacing one of the lenses.

22. A device as in Claims 16 or 17 or 18 or 19 or 20, characterized by the fact that the reading and processing unit contains a database of maps of the locations which the object traverses, upon which the points corresponding to the location are displayed.