The present invention relates to localization and mapping.
There is an increasing need to provide localization and mapping in environments where global positioning systems (GPS) cannot be reliably used, such as in indoor environments where GPS signals are not typically able to register. Localization and mapping of these indoor environments is desirable both for location-based services and for network deployment optimization for pervasive computing. However, known localization and mapping systems for GPS-deprived environments lack accuracy or require too resource-intensive computations and/or specialized hardware to be widely employed.
According to an embodiment, a system for providing localization and mapping within an environment includes at least one mobile device adapted to be moved within the environment, the at least one mobile device including an inertial measurement unit and at least one radio frequency receiver. The at least one mobile device detects inertial measurements through the inertial measurement unit and values of signal strength through the at least one radio frequency receiver. The system also includes a localization module receiving the inertial measurements and the values of signal strength. The localization module provides simultaneous localization and mapping of the at least one mobile device within the environment based at least on the inertial measurements detected by the inertial measurement unit and the values of signal strength detected by the at least one radio frequency receiver.
According to an embodiment, the system also includes a display for displaying an estimated trajectory of the at least one mobile device.
According to an embodiment, the at least one mobile device may include a plurality of radio frequency receivers of different modalities, and the localization module may provide localization based on values of signal strength detected by the plurality of radio frequency receiver.
According to an embodiment, the plurality of radio frequency receivers may include a WiFi receiver and a 4G LTE receiver.
According to an embodiment, the plurality of radio frequency receivers may include a Bluetooth receiver.
According to an embodiment, the inertial sensor may include a three-axis accelerometer and a three-axis gyroscope.
According to an embodiment, the system may also include at least one of a global positioning system, a near field communication chip and a camera adapted to read two-dimensional barcodes. The simultaneous localization and mapping provided by the localization module may also be based on absolute landmark positions detected by at least one of the global positioning system, near field communication chip or camera.
According to an embodiment, a computerized method includes detecting, through an inertial sensor of at least one mobile device, inertial measurements as the at least one mobile device is moved within an environment, and detecting, through a radio frequency receiver of the at least one mobile device, values of radio frequency signal strength as the mobile device is moved within the environment. The computerized method further includes estimating a trajectory of the mobile device within the environment based at least on the inertial measurements from the inertial sensor and the values of signal strength detected by the radio frequency receiver.
According to an embodiment, the computerized method also includes displaying an estimated trajectory of the at least one mobile device.
According to an embodiment, the computerized method also includes generating a signal strength map by projecting the values of signal strength detected by the radio frequency receiver on positions of the estimated trajectory.
According to an embodiment, the values of signal strength include 4G LTE reference signal received power.
According to an embodiment, the computerized method additionally includes detecting, through a plurality of radio frequency receivers of the at least one mobile device, values of radio frequency signal strength of different modalities as the mobile device is moved within the environment, and estimating the trajectory of the mobile device within the environment based on the values of signal strength of different modalities.
According to an embodiment, the computerized method additionally includes detecting, by the mobile device, at least one absolute landmark positions through at least one of a global positioning system, a near field communication chip and a camera adapted to read two-dimensional barcodes, and estimating the trajectory of the mobile device within the environment based on that at least one absolute landmark position.
According to an embodiment, the computerized method additionally includes tracking a second mobile device within the environment based on the trajectory estimate of the first mobile device.
According to an embodiment, a non-transitory, tangible computer-readable medium storing instructions adapted to be executed by at least one processor of a mobile device to perform a method may comprise the steps of detecting, through an inertial sensor of the mobile device, inertial measurements as the mobile device is moved within an environment, detecting, through a radio frequency receiver of the mobile device, values of radio frequency signal strength as the mobile device is moved within the environment, and estimating, by a localization module executing on the at least one processor, a trajectory of the mobile device within the environment based at least on the inertial measurements from the inertial sensor and the values of signal strength detected by the radio frequency receiver.
According to an embodiment, the method may also include displaying an estimated trajectory of the mobile device.
According to an embodiment, the method may also include generating a signal strength map by projecting the values of signal strength detected by the radio frequency receiver on positions of the estimated trajectory.
According to an embodiment, the values of signal strength may include 4G LTE reference signal received power.
According to an embodiment, the method may additionally include detecting, through a plurality of radio frequency receivers of the at least one mobile device, values of radio frequency signal strength of different modalities as the mobile device is moved within the environment, and estimating, by the localization module executing on the processor, the trajectory of the mobile device within the environment based on the values of signal strength of different modalities.
According to an embodiment, the method may additionally include detecting, by the mobile device, at least one absolute landmark positions through at least one of a global positioning system, a near field communication chip and a camera adapted to read two-dimensional barcodes, and estimating, by the localization module executing on the processor, the trajectory of the mobile device within the environment based on that at least one absolute landmark position.
These and other embodiments will become apparent in light of the following detailed description herein, with reference to the accompanying drawings.
Before the various embodiments are described in further detail, it is to be understood that the invention is not limited to the particular embodiments described. It will be understood by one of ordinary skill in the art that the systems and methods described herein may be adapted and modified as is appropriate for the application being addressed and that the systems and methods described herein may be employed in other suitable applications, and that such other additions and modifications will not depart from the scope thereof.
In the drawings, like reference numerals refer to like features of the systems and methods of the present application. Accordingly, although certain descriptions may refer only to certain Figures and reference numerals, it should be understood that such descriptions might be equally applicable to like reference numerals in other Figures.
Referring to
The system 10 includes the necessary electronics, software, memory, storage, databases, firmware, logic/state machines, microprocessors, communication links, and any other input/output interfaces to perform the functions described herein and/or to achieve the results described herein. For example, the system 10 may include the processor 12 and memory 20, which may include system memory, including random access memory (RAM) and read-only memory (ROM). Suitable computer program code may be provided to the system 10 for executing numerous functions, including those discussed in connection with the localization module 18.
The processor 12 may include one or more conventional microprocessors and one or more supplementary co-processors such as math co-processors or the like. The processor 12 may communicate with other networks and/or devices such as servers, other processors, computers, cellular telephones, tablets and the like.
The processor 12 may be in communication with the memory 20, which may comprise an appropriate combination of magnetic, optical and/or semiconductor memory, and may include, for example, RAM, ROM, flash drive, an optical disc such as a compact disc and/or a hard disk or drive. The processor 12 and the memory 20 each may be, for example, located entirely within a single computer or other device; or connected to each other by a communication medium, such as a USB port, serial port cable, a coaxial cable, an Ethernet type cable, a telephone line, a radio frequency transceiver or other similar wireless or wired medium or combination of the foregoing.
The memory 20 may store inertial measurements taken by the IMU 14 and/or RF signal measurements from the plurality of RF receivers 16 and may also store any other information required by the localization module 18, an operating system for the system 10, and/or one or more other programs (e.g., computer program code and/or a computer program product) adapted to direct the localization module 18 to perform according to the various embodiments discussed herein. For example, the memory 20 may also store graphical images for outputting mapped data as discussed herein. The localization module 18 and/or other programs may be stored, for example, in a compressed, an uncompiled and/or an encrypted format, and may include computer program code executable by the processor 12. The instructions of the computer program code may be read into a main memory of the processor 12 from the memory 20 or a computer-readable medium other than the memory 20. While execution of sequences of instructions in the program causes the processor 12 to perform the process steps described herein, hard-wired circuitry may be used in place of, or in combination with, software instructions for implementation of the processes of the present invention. Thus, embodiments of the present invention are not limited to any specific combination of hardware and software.
The methods and programs discussed herein may also be implemented in programmable hardware devices such as field programmable gate arrays, programmable array logic, programmable logic devices or the like. Programs may also be implemented in software for execution by various types of computer processors. A program of executable code may, for instance, comprise one or more physical or logical blocks of computer instructions, which may, for instance, be organized as an object, procedure, process or function. Nevertheless, the executables of an identified program need not be physically located together, but may comprise separate instructions stored in different locations which, when joined logically together, comprise the program and achieve the stated purpose for the programs such providing simultaneous localization and mapping. In an embodiment, an application of executable code may be a compilation of many instructions, and may even be distributed over several different code partitions or segments, among different programs, and across several devices.
The term “computer-readable medium” as used herein refers to any medium that provides or participates in providing instructions and/or data to the processor 12 of the system 10 (or any other processor of a device described herein) for execution. Such a medium may take many forms, including but not limited to, non-volatile media and volatile media. Non-volatile media include, for example, optical, magnetic, or opto-magnetic disks, such as memory. Volatile media include dynamic random access memory (DRAM), which typically constitutes the main memory. Common forms of computer-readable media include, for example, a floppy disk, a flexible disk, hard disk, magnetic tape, any other magnetic medium, a CD-ROM, DVD, any other optical medium, punch cards, paper tape, any other physical medium with patterns of holes, a RAM, a PROM, an EPROM or EEPROM (electronically erasable programmable read-only memory), a FLASH-EEPROM, any other memory chip or cartridge, or any other medium from which a computer can read.
Various forms of computer readable media may be involved in carrying one or more sequences of one or more instructions to the processor 12 (or any other processor of a device described herein) for execution. For example, the instructions may initially be stored on a magnetic disk of a remote computer (not shown). The remote computer can load the instructions into its dynamic memory and send the instructions over an Ethernet connection, cable line, telephone line using a modem, wirelessly or over another suitable connection. A communications device local to a computing device can receive the data on the respective communications line and place the data on a system bus for the processor 12. The system bus carries the data to the main memory, from which the processor 12 retrieves and executes the instructions. The instructions received by main memory may optionally be stored in memory 20 either before or after execution by the processor 12. In addition, instructions may be received via a communication port as electrical, electromagnetic or optical signals, which are exemplary forms of wireless communications or data streams that carry various types of information.
Referring to
Referring to
At step 44, the localization module 18 uses the inertial measurements to perform pedestrian dead reckoning to incrementally estimate a trajectory of the mobile electronic device 28, shown in
To perform pedestrian dead reckoning at step 44, the localization module 18 first detects and counts steps taken by the user through the inertial measurements of the IMU 14, shown in
Once the localization module 18 determines that the user has taken a step, the localization module 18 estimates the position of the mobile electronic device 28 after the step relative to the position of the mobile electronic device 28 prior to the step using motion model equations:
(1)
(2)
(3)
where:
is the X coordinate after the step;
is the Y coordinate after the step;
is a yaw after the step;
is the X coordinate prior to the step;
is the Y coordinate prior to the step;
is a yaw prior to the step;
is a pocket offset angle;
is a yaw increment during the step;
is an offset angle between an arbitrary map coordinate system and the local ground coordinate system; and
is a length of the stride.
The localization module 18 estimates a heading of the user and mobile electronic device 28 as a combination of the yaw value θt and the pocket offset angle βt by using the three-dimensional orientation of the mobile electronic device 28, as measured by the IMU 14, shown in
The offset angle ξ is the transform angle between the arbitrary map coordinate system (e.g. the map coordinate system of a building, campus or the like) and the local ground coordinate system defined by the longitude (towards East) and latitude (towards North) axes. In using the offset angle ξ, the localization module 18 ignores curvature of the earth at the scale of the environment 30 (e.g. the building), which is at most a few hundred meters, and considers the local ground coordinate system as orthogonal within that radius, thereby enabling affine transforms between the coordinate systems.
The length of the stride dt corresponds to the distance between two steps taken by the user and detected by the mobile electronic device 28. Although the length of the stride dt may be calculated at each step, in embodiments, the length of the stride dt may instead be held constant between two known positions, thereby simplifying calculations using the motion model equations (1), (2) and (3).
The position estimation of the localization module 18 using the motion model equations (1), (2) and (3) may experience long term drift, particularly where the pocket offset angle βt and the length of the stride dt have been set as constants between known positions.
Accordingly, during the walking trajectory, the localization module 18 may frequently reset the position of the mobile electronic device 28 in pedestrian dead reckoning using absolute position data from a collection of landmarks that either encode their own position (e.g., GPS readings, Near Field Communication (NFC) tags or two-dimensional bar codes) or whose position can be known in advance, such as Bluetooth dongles. To reset the position, the localization module 18 may simply assign the position (x1,t; x2,t) of the mobile electronic device 28 the known coordinates (y1,k; y2,k) of the k-th landmark.
The system 10, shown in
In an embodiment, the system 10, shown in
In embodiments, the localization module 18 may calibrate the pedestrian dead reckoning trajectories, such as by using a Least Squares Error calibration or the like, because of long-term shifts of the mobile electronic device 28 within the user's pocket, changes of the user stride dt, and/or because the mobile electronic device 28 may be moved in and out of the pocket to read the position landmarks as discussed above. The calibration may include re-calibrating the values of βt and dt in each segment of time between two landmarks I and J, of respective coordinates and .
To re-calibrate the values of βt and dt, the localization module 18 minimizes the Least Square Error , where is the pedestrian dead reckoning trajectory re-estimated by starting from and iterating the pedestrian dead reckoning motion model equations (1), (2) and (3), using stride length and offset angle .
(4)
The values of d and β in equation (4) may be found by grid search with small step increments (e.g. 0.1 m and 5 deg, respectively) or the like. In other embodiments, the localization module 18 may not calibrate the pedestrian dead reckoning trajectories and may instead move directly to the refinement using RF signal similarities as discussed below.
After the localization module 18 performs pedestrian dead reckoning, the localization module 18 refines the estimate of position using RF signal similarities at step 46, shown in
For WiFi or Bluetooth data, where there can be hundreds of different access points scattered around the building, the localization module 18 may compute RF signal similarity using the Kullback-Leibler divergence between multinomials of access point visibility. For the 4G LTE Reference Signal Received Power or the magnitude of the magnetic field (as measured by a magnetometer (not shown) of the system 10), the localization module 18 may compute RF similarity using Euclidean distance. The KL divergence or the Euclidean distance may be input into a kernel function k(St
In the presence of multiple sources of RF data, such as WiFi Received Signal Strength, 4G LTE Reference Signal Received Power, Bluetooth Received Signal Strength or the magnitude of the magnetic field, the kernel matrices, K1, K2, K3, etc., for each signal modality are multiplied, element by element, by the localization module 18 provided that the rows and columns in each matrix correspond to the same time intervals for the same mobile electronic device 28. Thus, the system localization module 18 may pre-compute the kernel similarity between the RF signals at each time interval during the trajectory and may easily combine multiple sources of RF signal by multiplying multiple kernel matrices.
Once the localization module 18 has computed the kernel similarity, for all the time segments of the trajectory from the pedestrian dead reckoning, the localization module 18 may use the kernel similarities to refine the trajectory. To refine the trajectories, the localization module 18, considers the trajectory of a mobile electronic device 28 as a sequence of poses ; in the 2D case, each pose is a 2D position and an orientation angle . The sequence of poses may be represented by a chain graph, where each pose is associated to a vertex and each motion edge corresponds to a known motion increment (in the case of pedestrian dead reckoning, each edge is a step with stride dt and yaw θt). The sequence is produced by iterating the equations of the motion model (1), (2) and (3), as discussed above.
Whenever the mobile electronic device 28 returns, at time tj, near a location previously traversed at time t, and, if the localization module 18 is able to recognize that similarity, then a new loop closure or constraint edge is added to the graph, linking xj to xi. These constraint edges may be added, for instance, if at time tj, the mobile electronic device 28 is next to the same landmark as at time ti (e.g. touching the same Bluetooth or NFC tag or seeing the same two-dimensional bar code). The localization module 18 defines the constraint edge as having a displacement equal to zero and an unknown change of orientation. If there is a gap in the pedestrian dead reckoning trajectory (or if data is collected from a different mobile electronic device starting from the same landmark), the localization module 18 inserts a constraint edge between the disjoint ends.
The localization module 18 also defines global landmarks in the chain graph, such as the position fixes given by GPS or readings from an NFC tag or QR code with their known landmark positions . A global vertex can be added, for instance, at the origin of the coordinate system, and the displacement in the constraint landmark edge is equal to the landmark's coordinates .
The localization module 18 begins refinement using positions and orientations derived from pedestrian dead reckoning, which means that the motion edge constraints are, initially, all satisfied while the loop closure constraint edges are initially violated because of the cumulative drift due to noisy pedestrian dead reckoning. The localization module 18 iteratively optimizes the sequence of poses in the graph by minimizing the errors at all edges, including the violations of the loop closure and landmark constraint edges.
For any two vertices i and j in the pose graph that have an edge linking them, the localization module 18 defines:
as the vector of constraints: it is the observed relative displacement (2D translation and change of orientation angle) between pose i and pose j, expressed from the viewpoint of pose i;
as the calculated displacement given the current values of poses and , calculated from the viewpoint of pose i;
as the error between the current configuration of poses i and j and the observed constraint; and
as the 3×3 information matrix (inverse covariance matrix) of the constraint between pose i and j. This information matrix expresses the noise in the observation of the (pedestrian dead reckoning, loop closure, landmark) constraint but may generally be kept diagonal. The term at row, column (3; 3) in the matrix expresses the inverse covariance of the orientation angle constraint.
The localization module 18 aims at minimizing a negative log likelihood F of all the observations over a set of C edges:
(5)
(6)
The localization module 18 obtains the solution to equation (6) by iterative local linearization. At each step, a new, optimal sequence of poses is recomputed given the current edge constraints and the constraints on all edges are then re-evaluated given the new poses. The new sequence of poses after each step's update is decomposed as .
At each iteration, the localization module 18 also uses the kernel similarities of RF signals to improve the error minimization. To consider kernel similarities, the localization module 18 first restricts the set or poses to time points that correspond to time windows when signal strength was collected and between which signal similarity was computed, as discussed above. At a given iteration of equation (6) and for the current configuration of poses , the localization module 18 uses weighted kernel regression to predict the expected pose based on the poses of its immediate neighbors in the signal space (where and ). Such neighbors in the signal space are poses whose kernel function evaluates the highest. The localization module 18 may define a neighborhood Nt, in the signal space, of poses at time t, and set a limit to the number of neighbors, such as ten neighbors. The weighted kernel regression of the localization module 18 predicts the following position for :
(7)
That prediction is used by the localization module 18 as a temporary absolute landmark for the next iteration of equations (5) and (6). This means that at the next iteration of equations (5) and (6), an additional signal edge is added from the pose to the new pose and the information matrix specific to the signal edge constraints is unit diagonal with the exception of the orientation term .
After each update of x, the signal similarity based prediction of each pose is re-evaluated by the localization module 18 by re-computing equation (7). However, the localization module 18 does not change the signal similarity kernel , or the neighbors Nt of in signal space. As a consequence, the overhead introduced by using the kernel similarities of RF signals to refine the localization is negligible. Additionally, the localization module 18 is able to integrate any kind of signal similarity that is stationary (i.e. time independent), which includes for instance WiFi RSSI, 4G LTE, Bluetooth from fixed beacons or, under some circumstances, magnetic field.
The error after the update is linearized as a sum of the current error and a term linear in the pose update . The minimization of equation (6) in terms of can be expressed as a minimization of a quadratic function in terms of and therefore the global sequence of pose update becomes the solution of a linear system. The localization module 18 reaches the locally optimal solution to the sequence of poses by iteratively recalculating the error functions at each edge and solving for the pose update , until convergence.
Although the simultaneous localization and mapping provided by the system 10 has been described in connection with a single mobile electronic device 28, shown in
Referring to
The system 10 may use the estimated trajectory 50 provided by the localization module 18 for a variety of purposes. For example, referring to
Referring to
The 4G LTE and/or WiFi signal map 56 may advantageously be used for optimizing the deployment of telecommunication networks with the environment 30, shown in
In embodiments, the 4G LTE and/or WiFi signal map 56 may also be used for geo-localization. For example, the 4G LTE and/or WiFi signal map 56 may provide a fingerprint of 4G LTE and/or WiFi RF signals from all access points or small cells within the environment 30, shown in
By relying on a state-space model, where the trajectory of the user is unknown and depends both on the dynamics coming from a pedestrian motion model and on multi-sensor observations, including WiFi and/or LTE signals, the system 10, shown in
The system 10, shown in
The system 10, shown in
The pedestrian dead reckoning estimates of the localization module 18, shown in
Although this invention has been shown and described with respect to the detailed embodiments thereof, it will be understood by those skilled in the art that various changes in form and detail thereof may be made without departing from the spirit and the scope of the invention. For example, although the system 10, shown in