Abstract:
In an embodiment of the present invention, a GraphSLAM-like algorithm for signal strength SLAM is presented. This algorithm as an embodiment of the present invention shares many of the benefits of Gaussian processes yet is viable for a broader range of environments since it makes no signature uniqueness assumptions. It is also more tractable to larger map sizes, requiring O(N 2 ) operations per iteration. In the present disclosure, an algorithm according to an embodiment of the present invention is compared to a laser-SLAM ground truth, showing that it produces excellent results in practice.

Description:
CROSS-REFERENCE TO RELATED APPLICATIONS 
       [0001]    This application claims priority to U.S. Provisional Application No. 61/558,695 filed Nov. 11, 2011, which is hereby incorporated by reference in its entirety for all purposes. 
     
    
     FIELD OF THE INVENTION 
       [0002]    The present invention generally relates to the field of localization and mapping. 
       BACKGROUND OF THE INVENTION 
       [0003]    The widespread deployment of wireless networks presents an opportunity for localization and mapping using signal-strength measurements. Wireless networks are ubiquitous, whether in the home, office, shopping malls, or airports. 
         [0004]    Recent work in signal-strength-based simultaneous localization and mapping (SLAM) uses Gaussian process latent variable models (GP-LVM). This work, however, requires that maps are limited to very specific predefined shapes (e.g. narrow and straight hallways) and WiFi fingerprints are assumed unique at distinct locations. Generally, in the absence of any odometry information, arbitrary assumptions must be made about human walking patterns and data association. 
         [0005]    GraphSLAM is a technique used in the robotics community for simultaneously estimating a trajectory and building a map offline. It shares many benefits of Gaussian processes, but can be applied to a broader range of environments. Below, it is shown how wireless signal strength SLAM can be formulated as a GraphSLAM problem. By using GraphSLAM, limitations of traditional approaches are addressed to improve runtime complexity from O(N 3 ) to O(N 2 ), where N is the dimensionality of the state space, i.e., the number of poses being estimated. 
         [0006]    In both GraphSLAM and Gaussian processes, measurement likelihoods are modeled as Gaussian random variables. Gaussian processes can improve their model fit by moving points away from each other. To prevent these trivial solutions, GP-LVM methods require special constraints. In the case of signal strength SLAM, the special constraints force similar signal strengths to similar locations. GraphSLAM requires no special constraints. This makes GraphSLAM suitable to a wider range of real-world environments. 
         [0007]    An appeal of GraphSLAM is that it reduces to a standard non-linear least squares problem. This gives GraphSLAM access to widely used and well-studied techniques for its optimization. A parameterization of the state space for typical mobile phone applications is presented below. 
         [0008]    If wireless signal strength maps are determined ahead of time, Monte Carlo localization methods can achieve high accuracy indoor localization. In a traditional approach, the signal strength map is discretized into a spatial grid and, combined with contact sensing, obtains 0.25 m accuracy using standard Monte Carlo methods while improving convergence time over contact sensing alone. In another approach, spatial discretization of the signal strength map is performed and combine with WiFi with a low-cost image sensor to localize within 3 m. In traditional approaches, however, the process for obtaining signal strength maps is expensive and time consuming. 
         [0009]    Outdoor applications have been handled by GPS and/or attenuation model or range-based SLAM methods. Other indoor signal-strength-based localization research relies on extensive training phases or incorporates other features of the signal such as time-of-arrival or angle-of-arrival measurements. In most pedestrian applications, however, such data is inaccessible to the general public without additional infrastructure costs. The implications of low-cost signal-strength SLAM are especially meaningful for large (indoor) GPS deprived environments such as shopping malls, airports, etc. where wireless internet infrastructure is readily accessible. 
         [0010]    Existing wireless mapping techniques model the signal data in different ways. Some traditional techniques assume a model of the signal propagation. Others use a connectivity graph of predetermined cells to localize coarsely. Since these techniques rely on pre-existing information about the environment, they do not handle the problem of mapping in unknown locations. 
         [0011]    The current state of the art uses Gaussian processes to determine a map of signal strength without modeling the propagation from transmitting nodes explicitly. Gaussian processes are applied to WiFi-SLAM under a specific set of assumptions. 
       SUMMARY OF THE INVENTION 
       [0012]    The widespread deployment of wireless networks presents an opportunity for localization and mapping using only signal-strength measurements. The current state of the art is to use Gaussian process latent variable models (GP-LVM). This method relies on a signature uniqueness assumption which limits its applicability to only signal-rich environments. Moreover, it does not scale computationally to large sets of data, requiring O(N 3 ) operations per iteration. 
         [0013]    In an embodiment of the present invention, a GraphSLAM-like algorithm for signal strength SLAM is presented. This algorithm shares many of the benefits of Gaussian processes, yet is viable for a broader range of environments since it makes no signature uniqueness assumptions. It is also more tractable to larger map sizes, requiring O(N 2 ) operations per iteration. Below, this algorithm is compared to a laser-SLAM ground truth, showing that it produces excellent results in practice. 
         [0014]    In an embodiment of the present invention, the restriction that similar signal strength fingerprints/signatures all correspond to a similar location on the map is lifted. If the geographic distribution of access points is sparse, there are more spatial configurations where the fingerprint uniqueness assumption breaks down. As illustrated in  FIGS. 1A and 1B , real-world hypotheses are often multi-modal. 
         [0015]    Shown in  FIGS. 1A and 1B  are two examples  100  and  150 , respectively, of wireless node deployment. In each example, location A ( 102  and  152 , respectively) and location B ( 104  and  154 , respectively) share the same signal strength signature/fingerprint. But the configuration of the buildings  110  and  164  are different. For example, note spaces  112  and  164  in  FIGS. 1A and 1B , respectively, as well as the locations of signal sources  106 ,  108 , and  114  in  FIG. 1A  and signal sources  156  and  158  in  FIG. 1B . More generally, at indoor scales, signal strength can often be relatively invariant over long regions of open areas in line-of-sight directions. Especially at lower signal strengths, due to the log relationship between signal strength and distance, signal strength can be almost completely invariant over very large sections of space. 
         [0016]    Relaxing this requirement brings modern signal-strength-based SLAM to sparse signal environments. Furthermore, explicitly mapping similar signal strengths to similar locations hurts scalability: as the dataset grows, the risk increases of erroneous measurements being incorrectly mapped by this constraint. Since such mappings are hard constraints, these errors are completely unrecoverable. The current state of the art cannot achieve signal-strength SLAM without relying upon explicit fingerprint uniqueness back-constraints. In a traditional approach, the GP-LVM method is only reasonable in dense environments. The method of an embodiment of the present invention does not make any assumptions of fingerprint uniqueness. Therefore, signal density or sparsity, which influences fingerprint uniqueness, is not a concern in an embodiment of the present invention. 
         [0017]    In order to provide a SLAM solution suitable for both rectilinear corridor-type environments as well as open atrium-type environments, in an embodiment of the present invention, low-cost IMU data is incorporated. Introducing motion measurements makes the sensor model general enough to apply to a wide range of crowd sourcing applications. In this embodiment, subjects need not explicitly cooperate with predetermined walking patterns, consistent walking speeds, etc. Motion sensors may also make the problem easier. The viability of low-cost WiFi SLAM according to an embodiment of the present invention is demonstrated below and compared to a laser-SLAM implementation. 
         [0018]    In an embodiment, the scalability of modern signal strength SLAM is improved for larger datasets. In this embodiment, the proposed GraphSLAM based method is shown to have better runtime complexity than Gaussian process latent variable models. This method is demonstrated to produce useful results on real-world data. 
         [0019]    Our results compare the GraphSLAM approach for WiFi SLAM against a LIDAR-based GraphSLAM implementation. Using real-world datasets, a localization accuracy of between 1.75 m and 2.18 m over an area of 600 square meters is demonstrated. The manner in which the resultant maps are directly applicable to online Monte Carlo Localization is discussed below. 
         [0020]    These and other embodiments can be more fully appreciated upon an understanding of the detailed description of the invention as disclosed below in conjunction with the attached figures. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0021]    The following drawings will be used to more fully describe embodiments of the present invention. 
           [0022]      FIGS. 1A and 1B  are illustrations of deployments of wireless nodes. 
           [0023]      FIGS. 2A and 2B  are sample plots of h WiFi , Gaussian weighted interpolation of WiFi points, evaluated over a grid with scale parameter τ=2.2 m, according to an embodiment of the present invention. 
           [0024]      FIGS. 3A-3C  are graphs that compare GraphSLAM initialization and resulting optimized GraphSLAM posterior as well as the unaligned ground truth trajectory is provided for reference. 
           [0025]      FIGS. 4A and 4B  are graphs that shown the mean and standard deviation of the localization error according to an embodiment of the present invention. 
           [0026]      FIG. 5  is a block diagram of a computer system on which embodiments of the present invention can be implemented. 
       
    
    
     DETAILED DESCRIPTION OF THE INVENTION 
       [0027]    Among other things, the present invention relates to methods, techniques, and algorithms that are intended to be implemented in a digital computer system  100  such as generally shown in  FIG. 5 . Such a digital computer is well-known in the art and may include the following. 
         [0028]    Computer system  500  may include at least one central processing unit  502  but may include many processors or processing cores. Computer system  500  may further include memory  504  in different forms such as RAM, ROM, hard disk, optical drives, and removable drives that may further include drive controllers and other hardware. Auxiliary storage  512  may also be include that can be similar to memory  504  but may be more remotely incorporated such as in a distributed computer system with distributed memory capabilities. 
         [0029]    Computer system  500  may further include at least one output device  508  such as a display unit, video hardware, or other peripherals (e.g., printer). At least one input device  506  may also be included in computer system  500  that may include a pointing device (e.g., mouse), a text input device (e.g., keyboard), or touch screen. 
         [0030]    Communications interfaces  514  also form an important aspect of computer system  500  especially where computer system  500  is deployed as a distributed computer system. Computer interfaces  514  may include LAN network adapters, WAN network adapters, wireless interfaces, Bluetooth interfaces, modems and other networking interfaces as currently available and as may be developed in the future. 
         [0031]    Computer system  500  may further include other components  516  that may be generally available components as well as specially developed components for implementation of the present invention. Importantly, computer system  500  incorporates various data buses  516  that are intended to allow for communication of the various components of computer system  500 . Data buses  516  include, for example, input/output buses and bus controllers. 
         [0032]    Indeed, the present invention is not limited to computer system  500  as known at the time of the invention. Instead, the present invention is intended to be deployed in future computer systems with more advanced technology that can make use of all aspects of the present invention. It is expected that computer technology will continue to advance but one of ordinary skill in the art will be able to take the present disclosure and implement the described teachings on the more advanced computers or other digital devices such as mobile telephones or smart televisions as they become available. Moreover, the present invention may be implemented on one or more distributed computers. Still further, the present invention may be implemented in various types of software languages including C, C++, and others. Also, one of ordinary skill in the art is familiar with compiling software source code into executable software that may be stored in various forms and in various media (e.g., magnetic, optical, solid state, etc.). One of ordinary skill in the art is familiar with the use of computers and software languages and, with an understanding of the present disclosure, will be able to implement the present teachings for use on a wide variety of computers. 
         [0033]    For example, embodiments of the present invention can be implemented in cellular telephones including smart telephones that include the necessary computing and communications capabilities. Also, embodiments of the present invention can be implemented in tablet computers with the necessary hardware and software capabilities. More broadly, embodiments of the present invention can be implemented in general purposes devices with the necessary capabilities as described herein. Still other embodiments of the present invention can be implemented in stand-alone devices that are especially configured to implement the teachings of the present invention. 
         [0034]    The present disclosure provides a detailed explanation of the present invention with detailed explanations that allow one of ordinary skill in the art to implement the present invention into a computerized method. Certain of these and other details are not included in the present disclosure so as not to detract from the teachings presented herein but it is understood that one of ordinary skill in the at would be familiar with such details. 
       Traditional GraphSLAM 
       [0035]    The GraphSLAM family of techniques are used in the robotics community for simultaneously estimating a trajectory and building a map offline. Such techniques have been used in applications in computer vision and robotics. The signal strength SLAM problem is reduced to an instance of GraphSLAM as discussed below. 
         [0036]    GraphSLAM is traditionally formulated as a network of Gaussian constraints between robot poses and landmarks. Measurements are assumed to contain only additive Gaussian noise and to be conditionally independent given the world states. For each measurement Z i  from any sensor, a state-to-measurement mapping function h i (X) describes the true measurement that would have taken place if the value of the state variables were known: 
         [0000]        Z   i   =h   i ( X )+ε i  
 
         [0000]      where 
         [0000]      X={{right arrow over (x)} i     1   , {right arrow over (x)} i     2   , . . . , {right arrow over (m)} 1 , {right arrow over (m)} 2 , . . . } 
         [0000]    represents the collection of all state variables (e.g. robot locations {{right arrow over (x)} i } and landmark locations {{right arrow over (m)} i }). The measurement likelihood as a function of z i  is Gaussian with mean h i  (X) and variance Var{ε i }. 
       Motion Model 
       [0037]    The typical GraphSLAM motion model can be expressed as state-to-measurement mappings. For example, a pedometry measurement on the interval between time t i  and t i+1  is related to the state space by the function 
         [0000]        h   i   pedometry ( X )=∥ {right arrow over (x)}   t     i+1     −{right arrow over (x)}   t     i   ∥ 2  
 
         [0038]    Similarly, angular velocity measurements correspond to 
         [0000]    
       
         
           
             
               
                 h 
                 i 
                 gyro 
               
                
               
                 ( 
                 X 
                 ) 
               
             
             = 
             
               
                 
                   atan 
                    
                   
                       
                   
                    
                   2 
                    
                   
                     ( 
                     
                       
                         
                           x 
                           → 
                         
                         
                           t 
                           
                             i 
                             + 
                             1 
                           
                         
                       
                       - 
                       
                         
                           x 
                           → 
                         
                         
                           t 
                           i 
                         
                       
                     
                     ) 
                   
                 
                 - 
                 
                   atan 
                    
                   
                       
                   
                    
                   2 
                    
                   
                     ( 
                     
                       
                         
                           x 
                           → 
                         
                         
                           t 
                           i 
                         
                       
                       - 
                       
                         
                           x 
                           → 
                         
                         
                           t 
                           
                             i 
                             - 
                             1 
                           
                         
                       
                     
                     ) 
                   
                 
               
               
                 Δ 
                  
                 
                     
                 
                  
                 t 
               
             
           
         
       
     
         [0000]    where Δt=(t i+1 −t i−1 )/2. Note that implementations must be careful to account for headings that cross over from −π to +π and vice versa. 
         [0039]    The definition of a state-to-measurement mapping h together with the variance of the corresponding sensors noise, σ ε =Var(ε), describes the GraphSLAM measurement likelihood function for each sensor. The methods presented herein as embodiments of the present invention will work with any class of sensors. Pedometry and gyroscopes are merely used as examples here because they are the sensors of the sample dataset as discussed below. 
       WIFI Graphslam 
       [0040]    The state-to-measurement mapping for the i th  signal strength measurement is defined to be 
         [0000]        h   i   WiFi ( X )={right arrow over (β)} i   T   {right arrow over (z)}   WiFi  
 
         [0000]      {right arrow over (β)} i ={right arrow over (ω)} i −[{right arrow over (ω)} i ] i {right arrow over (ε)} i , ∥{right arrow over (β)} i ∥ 1 =1
 
         [0000]    For notational convenience, {right arrow over (z)} WiFi  is the vector of all signal strength measurements. {right arrow over (β)} i  excludes the i th  element from {right arrow over (ω)} i . {right arrow over (ω)} i  is the vector of interpolation weights for h WiFi . The notation [{right arrow over (α)}] i  is shorthand for the i th  element of the vector {right arrow over (α)}, and {right arrow over (β)} i  is the unit vector with all elements zero except the i th  element. Note that h i  does not interpolate the i th  measurement. This allows the measurement model to operate in sparse WiFi environments. 
         [0041]    At most distances from a transmission node, propagating radio waves are expected to have nearly the same power within any small region of free space. Over larger regions across non-free space the relationship between nearby signal strengths is highly dependent on building structure/architecture. Without a model for building structure, a method according to an embodiment of the present invention interpolates over small regions likely to be free space. 
         [0042]    The quantity [{right arrow over (ω)} i ] i  can considered as related to the probability that location i and location j are inter-polatable, e.g., nearby and separated by only free space. In practice, at reasonable scales and in lieu of additional knowledge, Gaussian interpolation weights are a popular kernel choice and have been used with success to interpolate WiFi signal strengths as well as in other machine learning applications. 
         [0000]    
       
         
           
             
               
                 [ 
                 
                   
                     w 
                     → 
                   
                   i 
                 
                 ] 
               
               j 
             
             ∝ 
             
               exp 
                
               
                 ( 
                 
                   
                     - 
                     
                       1 
                       
                         2 
                          
                         
                           τ 
                           2 
                         
                       
                     
                   
                    
                   
                     
                        
                       
                         
                           
                             x 
                             → 
                           
                           
                             t 
                             i 
                           
                         
                         - 
                         
                           
                             x 
                             → 
                           
                           
                             t 
                             j 
                           
                         
                       
                        
                     
                     2 
                     2 
                   
                 
                 ) 
               
             
           
         
       
     
         [0000]    where τ is a scale parameter, related roughly to the distance between walls (e.g., 95% of walls can be considered at least 2τ away from measurement locations). In an embodiment, τ can be learned from training data (the experimental value of τ for our dataset was approximately 2.2 meters).  FIGS. 2A and 2B  illustrate typical behavior of this signal interpolation method. More particularly,  FIGS. 2A and 2B  are sample plots  200  and  202 , respectively, of h WiFi , Gaussian weighted interpolation of WiFi points, evaluated over a grid with scale parameter τ=2.2 m, according to an embodiment of the present invention. As shown, black circles denote measured values. The vertical axis represents signal strength (dBm) and the horizontal axes represent spatial location. 
         [0043]    With this formulation, for any specific measured value z i , the following is evaluated: 
         [0000]    
       
         
           
             
               
                 z 
                 i 
               
               - 
               
                 
                   h 
                   i 
                 
                  
                 
                   ( 
                   X 
                   ) 
                 
               
             
             = 
             
               
                 ɛ 
                 i 
               
                
               
                   
               
               ⇒ 
               
                 { 
                 
                   
                     
                       
                         
                           E 
                            
                           
                             [ 
                             
                               
                                 
                                   z 
                                   → 
                                 
                                 i 
                                 WiFi 
                               
                               - 
                               
                                 
                                   ∑ 
                                   j 
                                 
                                  
                                 
                                     
                                 
                                  
                                 
                                   
                                     
                                       [ 
                                       
                                         
                                           β 
                                           → 
                                         
                                         i 
                                       
                                       ] 
                                     
                                     j 
                                   
                                    
                                   
                                     z 
                                     j 
                                     WiFi 
                                   
                                 
                               
                             
                             ] 
                           
                         
                         = 
                         0 
                       
                     
                   
                   
                     
                       
                         
                           Var 
                            
                           
                             ( 
                             
                               
                                 ɛ 
                                 i 
                                 WiFi 
                               
                               + 
                               
                                 
                                   h 
                                   i 
                                   WiFi 
                                 
                                  
                                 
                                   ( 
                                   X 
                                   ) 
                                 
                               
                             
                             ) 
                           
                         
                         = 
                         
                           
                             ( 
                             
                               1 
                               + 
                               
                                 
                                    
                                   
                                     
                                       β 
                                       → 
                                     
                                     i 
                                   
                                    
                                 
                                 2 
                                 2 
                               
                             
                             ) 
                           
                            
                           
                             Var 
                              
                             
                               ( 
                               
                                 ɛ 
                                 WiFi 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                 
               
             
           
         
       
     
         [0000]    The WiFi measurement likelihood, as a function of z i , is Gaussian with mean h i (X) and variance 
         [0000]      (1+∥{right arrow over (β)} i ∥ 2   2 )σ ε   2 .
 
         [0000]      where 
         [0000]      σ ε   2 =Var(ε WiFi )
 
         [0000]    denotes the measurement noise variance associated with the WiFi sensor. In practice, 
         [0000]      ∥{right arrow over (β)} i ∥ 2   2 &lt;&lt;1
 
         [0000]    for any sufficiently dense dataset (recall that ∥{right arrow over (β)} i ∥ 1 =1). 
         [0044]    Observe that this formulation is free of any restrictions on fingerprint uniqueness, and is equally applicable to both sparse and dense signal environments. 
         [0045]    Embodiments of the present invention can be implemented using hand-held devices such as cellular telephones, sometimes called smart-phones, tablet computers, or other computing devices having signal receivers such as WiFi transceivers. In such embodiments, the described algorithms can be implemented on the hand-held device itself or can be implemented on another computing device that is in communication with the hand-held device. Likewise, the collection of signal information can be performed on the handheld device itself or by other devices. Still other embodiments may distribute certain computational steps between a hand-held device and a separately networked computer. Computer networks in such cases can include cellular networks. Many other embodiments are possible as would be known to those of ordinary skill in the art upon understanding the teachings of the present disclosure. 
       Relationship to Gaussian Processes 
       [0046]    Here, a few key differences between our measurement model according to an embodiment of the present invention and Gaussian processes are discussed. In Gaussian processes, the model fit to the WiFi measurements, as a function of z i , has mean: 
         [0000]        h   i   GP ( X )= {right arrow over (k)}   i   T ( K+σ   ε   2   I ) −1   {right arrow over (z)}   WiFi    
         [0000]        h   i   WiFi ( X )={right arrow over (β)} i   T   {right arrow over (z)}   WiFi  
 
         [0000]    σ ε   2  is measurement noise variance associated with the WiFi sensor. k i  and K come from the choice of kernel weighting function. 
         [0047]    The comparison to GraphSLAM is most clear when 
         [0000]      [{right arrow over (ω)} 1 , {right arrow over (ω)} 2 , . . . ]̂∝ K,
 
         [0000]    a square matrix whose j th  column is k j  ∝ {right arrow over (ω)} j . The two key differences in the measurement model presented here are the omission of the (K+σ ε   2 I) −1  term and exclusion of z i   WiFi  from {right arrow over (z)} WiFi  in the weighted average, i.e. [{right arrow over (β)} i ] i =0. 
         [0048]    Omission of (K+σ ε   2 I) −1    
         [0049]    The (K+σ ε   2 I) −1  term can be thought of as a whitening transform on the weighted observations and their weights. If, for example, ten observations appear at the same location {right arrow over (x)} and the same value z, they would collectively only be given one “vote” in the weighted average, rather than ten. This makes sense when attempting to make statistically consistent function value estimates over large distances. Only at small scales can signal strengths be averaged meaningfully without physical modeling of the surrounding materials. At these scales, giving (nearby) past measurements each an “equal vote” provides a larger sample size with which to predict future measurements, which is the methodology employed within the GraphSLAM formulation. Formally, treating past measurements in this way is equivalent to the approximation of 
         [0000]    
       
         
           
             
               e 
               
                 - 
                 
                   1 
                   
                     2 
                      
                     
                       τ 
                       2 
                     
                   
                 
               
             
              
             
               σ 
               ɛ 
               2 
             
           
         
       
     
         [0000]    for a Gaussian Process, e.g., the measurement noise dominates innate signal variance. At wireless frequencies this is a reasonable assumption. Error in measured signal strength is tightly coupled to innate signal variance by the dynamics of the environment. Distinguishing measurement noise from signal variance would require extensive prior information of building materials, population distributions, etc. 
         [0050]    Furthermore, the size of K grows quadratically in the size of the dataset (number of measurement locations, N). The need to invert K when computing h and its derivatives makes each iteration an O(N 3 ) operation. This is the main reason that it is difficult to scale Gaussian process techniques to larger datasets and omitting this term in the interpolation allows GraphSLAM to achieve a O(N 2 ) asymptotic runtime and in turn makes the GraphSLAM technique easier to scale to larger datasets. 
         [0051]    It should be noted that both GraphSLAM and Gaussian process latent variable models can improve their runtime complexities by means of sparsification or other approximation methods. 
       Exclusion of z i   WiFi    
       [0052]    The second key difference in interpolation methods is that a model fit according to an embodiment of the present invention always excludes z i   WiFi  from {right arrow over (z)} WiFi  when computing the weighted average for h i   WiFi . Here, an attempt is made to determine the model fit, or self-consistency, of observing certain measurements. In both GraphSLAM and Gaussian processes, model fit/measurement likelihood is modeled as Gaussian random variables/vectors and as such are defined by their mean and (co)variance. That is, it is desired to determine the fit of z i   WiFi . The fit of z i   WiFi  will be determined by a certain distribution p. In this setting, it would not make sense to use z i   WiFi  itself to compute the parameters of p. 
         [0053]    As a consequence of including z i   WiFi , its own model fit definition, for any fixed τ, the latent space optimization of GP-LVM can improve model fit by moving all points away from each other. To circumvent this behavior, GP-LVM methods almost always require explicit signal strength→location back constraints or carefully selected priors. The GraphSLAM approach according to an embodiment of the invention, on the other hand, will require no hard constraints. Excluding z i   WiFi  causes measurements to be attracted to similar neighboring measurements. This makes GraphSLAM as an embodiment of the present invention suitable to a wider range of real-world environments, including those where wireless signatures are not rich enough to guarantee uniqueness but still provide enough information to augment an existing SLAM implementation. 
         [0054]    V. Non-Linear Least Squares 
         [0055]    One of the primary appeals of GraphSLAM methods in general is that minimizing the negative log posterior reduces to standard non-linear least squares, giving GraphSLAM access to a vast set of widely used and well-studied techniques for its optimization. 
         [0056]    Here, h pedometry , h gyro , h WiFi , together with Var(ε pedometry ), Var(ε gyro ), Var(ε WiFi ) are needed to formulate the least squares problem: 
         [0000]    
       
         
           
             
               
                 - 
                 log 
               
                
               
                 
                   ∏ 
                   i 
                 
                  
                 
                     
                 
                  
                 
                   
                     P 
                     
                       Z 
                       i 
                     
                   
                    
                   
                     ( 
                     
                       
                         z 
                         i 
                       
                       | 
                       X 
                     
                     ) 
                   
                 
               
             
             = 
             
               
                 1 
                 2 
               
                
               
                 
                   ∑ 
                   i 
                 
                  
                 
                     
                 
                  
                 
                   
                     
                       
                         
                           [ 
                           
                             
                               z 
                               i 
                             
                             - 
                             
                               
                                 h 
                                 i 
                               
                                
                               
                                 ( 
                                 X 
                                 ) 
                               
                             
                           
                           ] 
                         
                         T 
                       
                        
                       
                         [ 
                         
                           Var 
                            
                           
                             ( 
                             
                               ɛ 
                               i 
                             
                             ) 
                           
                         
                         ] 
                       
                     
                     
                       - 
                       1 
                     
                   
                    
                   
                     [ 
                     
                       
                         z 
                         i 
                       
                       - 
                       
                         
                           h 
                           i 
                         
                          
                         
                           ( 
                           X 
                           ) 
                         
                       
                     
                     ] 
                   
                 
               
             
           
         
       
     
         [0000]    Depending on the application, environment, or domain, a uniform prior and maximize the likelihood can be assumed, or any number of Gaussian priors can be added (e.g., inertial priors or smoothness constraints). For the experiments discussed below, uniform priors have been assumed and the data likelihood has been maximized directly. 
       Solvers 
       [0057]    In general, non-linear least squares is a well-studied problem in numerical optimization communities and any number of solvers can be used instead. Methods such as gradient/steepest descent, Levenberg-Marquardt, BFGS and many Conjugate gradient based methods are readily available and can be applied directly. 
         [0058]    Typical solvers depend on local linearization to iterate toward an optimum. Let J hi (X) denote the derivatives of h with respect to each state variable in X (each row of J is a gradient of h). For example, if this Jacobian is known, any initial guess X 0  can be iteratively refined by solving 
         [0000]    
       
         
           
             
               X 
               new 
             
             := 
             
               
                 X 
                 0 
               
               + 
               
                 
                   
                     [ 
                     
                       
                         ∑ 
                         
                           h 
                           i 
                         
                       
                        
                       
                           
                       
                        
                       
                         
                           J 
                           
                             h 
                             i 
                           
                           T 
                         
                          
                         
                           Ω 
                           
                             h 
                             i 
                           
                         
                          
                         
                           J 
                           
                             h 
                             i 
                           
                         
                       
                     
                     ] 
                   
                   
                     - 
                     1 
                   
                 
                 [ 
                 
                   
                     ∑ 
                     
                       h 
                       i 
                     
                   
                    
                   
                       
                   
                    
                   
                     
                       J 
                       
                         h 
                         i 
                       
                       T 
                     
                      
                     
                       
                         Ω 
                         
                           h 
                           i 
                         
                       
                        
                       
                         ( 
                         
                           
                             z 
                             i 
                           
                           - 
                           
                             
                               h 
                               i 
                             
                              
                             
                               ( 
                               
                                 X 
                                 0 
                               
                               ) 
                             
                           
                         
                         ) 
                       
                     
                   
                 
                 ] 
               
             
           
         
       
     
         [0000]    with J hi  evaluated at X 0  on each iteration. This is known as Gauss-Newton iteration. 
         [0059]    Our results in as described below are obtained using standard Gauss-Newton iteration. In our case, z and h are always scalar valued so J h     i   (X)=∇h i  is a row vector and Ω h     i   =σ h     i     −2  is a scalar. Each iteration, then, is equivalent to solving the overconstrained matrix system of the form A{right arrow over (Δ)}={right arrow over (t)}x 
         [0000]    
       
         
           
             
               
                 [ 
                 
                   
                     
                       - 
                     
                     
                       
                         
                           σ 
                           
                             h 
                             1 
                           
                           
                             - 
                             1 
                           
                         
                          
                         
                           ∇ 
                           
                             h 
                             1 
                           
                         
                       
                     
                     
                       - 
                     
                   
                   
                     
                       - 
                     
                     
                       
                         
                           σ 
                           
                             h 
                             2 
                           
                           
                             - 
                             1 
                           
                         
                          
                         
                           ∇ 
                           
                             h 
                             2 
                           
                         
                       
                     
                     
                       - 
                     
                   
                   
                     
                       
                           
                       
                     
                     
                       ⋮ 
                     
                     
                       
                           
                       
                     
                   
                   
                     
                       - 
                     
                     
                       
                         
                           σ 
                           
                             h 
                             N 
                           
                           
                             - 
                             1 
                           
                         
                          
                         
                           ∇ 
                           
                             h 
                             N 
                           
                         
                       
                     
                     
                       - 
                     
                   
                 
                 ] 
               
                
               
                 Δ 
                 ⇀ 
               
             
             = 
             
               [ 
               
                 
                   
                     
                       
                         σ 
                         
                           h 
                           1 
                         
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             z 
                             1 
                           
                           - 
                           
                             h 
                             1 
                           
                         
                         ) 
                       
                     
                   
                 
                 
                   
                     
                       
                         σ 
                         
                           h 
                           2 
                         
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             z 
                             2 
                           
                           - 
                           
                             h 
                             2 
                           
                         
                         ) 
                       
                     
                   
                 
                 
                   
                     ⋮ 
                   
                 
                 
                   
                     
                       
                         σ 
                         
                           h 
                           N 
                         
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           
                             z 
                             N 
                           
                           - 
                           
                             h 
                             N 
                           
                         
                         ) 
                       
                     
                   
                 
               
               ] 
             
           
         
       
       
         
           
             
               and 
                
               
                   
               
                
               updating 
                
               
                   
               
                
               
                 X 
                 new 
               
             
             := 
             
               
                 X 
                 0 
               
               + 
               
                 Δ 
                 ⇀ 
               
             
           
         
       
     
       State Space 
       [0060]    The convergence characteristics of GraphSLAM, as an embodiment of the present invention, depend on the linearizability of h. In an embodiment, some effort is made to transform the state space to improve linearization. In certain settings, reformulation of a GraphSLAM state space has lead to dramatically improved performance and result quality. So far, all three of the state-to-measurement mappings h pedometry , h gyro  and h WiFi  are non-linear functions if the world state is represented as X={{right arrow over (x)} t     1   , {right arrow over (x)} t     2   , . . . }, describing robot location at each point in time. 
         [0061]    Due to the exponential terms created by the interpolation weight kernel, h WiFi  will be non-linear regardless of the state space parametrization. The state space is solved in terms of the headings {φ 1 , φ 2 , . . . , φ N−1 } and distances {d 1 , d 2 , . . . , d N−1 } between each WiFi scan, e.g., 
         [0000]    
       
         
           
             
               
                 x 
                 → 
               
               = 
               
                 
                   [ 
                   
                     
                       
                         
                           Σ 
                            
                           
                             
                                 
                               ‵ 
                             
                              
                             d 
                           
                            
                           
                               
                           
                            
                           cos 
                            
                           
                               
                           
                            
                           
                             ( 
                             φ 
                             ) 
                           
                         
                       
                     
                     
                       
                         
                           Σ 
                            
                           
                               
                           
                            
                           d 
                            
                           
                               
                           
                            
                           sin 
                            
                           
                               
                           
                            
                           
                             ( 
                             φ 
                             ) 
                           
                         
                       
                     
                   
                   ] 
                 
                 . 
                 Then 
               
             
             , 
             
               
 
             
              
             
               
                 
                   h 
                   pedometry 
                 
                  
                 
                   ( 
                   X 
                   ) 
                 
               
               = 
               
                 d 
                 i 
               
             
           
         
       
       
         
           
             
               
                 h 
                 gyro 
               
                
               
                 ( 
                 X 
                 ) 
               
             
             = 
             
               
                 φ 
                 
                   i 
                   + 
                   1 
                 
               
               - 
               
                 φ 
                 i 
               
             
           
         
       
     
         [0000]    which linearize with infinite radius of convergence. This allows for eliminating linearization error in all but one of the sensors. 
         [0062]    To compute derivatives ∇ {{right arrow over (d)}, {right arrow over (φ)}} h WiFi (X): 
         [0000]    
       
         
           
             
               
                 ∇ 
                 
                   { 
                   
                     
                       d 
                       → 
                     
                     , 
                     
                       φ 
                       → 
                     
                   
                   } 
                 
               
                
               
                 
                   h 
                   WiFi 
                 
                  
                 
                   ( 
                   X 
                   ) 
                 
               
             
             = 
             
               
                 [ 
                 
                   
                     ∇ 
                     
                       { 
                       
                         x 
                         → 
                       
                       } 
                     
                   
                    
                   
                     
                       h 
                       WiFi 
                     
                      
                     
                       ( 
                       X 
                       ) 
                     
                   
                 
                 ] 
               
                
               
                 
                   J 
                   
                     { 
                     
                       x 
                       → 
                     
                     } 
                   
                 
                  
                 
                   ( 
                   
                     { 
                     
                       
                         d 
                         → 
                       
                       , 
                       
                         φ 
                         → 
                       
                     
                     } 
                   
                   ) 
                 
               
             
           
         
       
       
         
           
             
               
                 ∇ 
                 
                   { 
                   
                     x 
                     → 
                   
                   } 
                 
               
                
               
                 
                   h 
                   WiFi 
                 
                  
                 
                   ( 
                   X 
                   ) 
                 
               
             
             = 
             
               
                 ∑ 
                 j 
               
                
               
                   
               
                
               
                 
                   
                     [ 
                     
                       
                         β 
                         → 
                       
                       i 
                     
                     ] 
                   
                   j 
                 
                  
                 
                   ( 
                   
                     
                       z 
                       j 
                     
                     - 
                     
                       h 
                       i 
                     
                   
                   ) 
                 
                  
                 
                   
                     - 
                     1 
                   
                   
                     2 
                      
                     
                       τ 
                       2 
                     
                   
                 
                  
                 
                   ( 
                   
                     
                       ∇ 
                       
                         { 
                         
                           x 
                           → 
                         
                         } 
                       
                     
                      
                     
                       
                          
                         
                           
                             
                               x 
                               → 
                             
                             j 
                           
                           - 
                           
                             x 
                             → 
                           
                         
                          
                       
                       2 
                     
                   
                   ) 
                 
               
             
           
         
       
     
         [0000]    The gradient of h WiFi (X) under this ‘heading and distance’ parametrization is fast to compute in practice. This is because the columns of J {T} ({{right arrow over (d)}, {right arrow over (φ)}}) are always constant valued with leading zeros, and elements of ∇ {{right arrow over (x)}} ∥{right arrow over (x)} j −{right arrow over (x)} i ∥ 2  are all zero except for those corresponding to the 
         [0000]    
       
         
           
             
               ∂ 
               
                 ∂ 
                 
                   
                     x 
                     → 
                   
                   i 
                 
               
             
              
             
                 
             
              
             or 
              
             
                 
             
              
             
               ∂ 
               
                 ∂ 
                 
                   
                     x 
                     → 
                   
                   j 
                 
               
             
           
         
       
     
         [0000]    elements. 
       Experimental Results 
     Data 
       [0063]    To evaluate the embodiments of the present invention, a trace was used of 536 WiFi scans captured over 17 minutes across a 60 m×10 m area of one floor of a university building. The trajectory covers about 1.2 km of travel distance. This dataset contains corresponding pedometry data, readings from a MEMS gyroscope, and an accompanying LIDAR-derived ground truth. The ground truth has been derived by processing the LIDAR, pedometry, and gyroscope measurements through off-the-shelf LIDAR SLAM. 
         [0064]    Maximizing the likelihood of the dataset over parameter τ yields an optimal value of 2.2 m (e.g., the average distance to a wall in all directions is roughly 4.4 m for 95% of measurement locations), but in our experiments, any values from τ≈1.5 m through τ≈3.5 m all produce good results. 
       Results 
       [0065]    The ground truth is accurate to about 10 cm. The ground truth and LIDAR are used only to evaluate the results. GraphSLAM requires no labeled data. 
         [0066]    The state space parametrization discussed herein is used.  FIG. 3A-C  are qualitative visualizations of the performance of the GraphSLAM method according to an embodiment of the present invention. More particularly, shown in  FIGS. 3A and 3B  are graphs  300  and  302  that compare GraphSLAM initialization ( FIG. 3A ) and the resulting optimized GraphSLAM posterior ( FIG. 3B ). For reference, shown in  FIG. 3C  is graph  304  that is the unaligned ground truth trajectory. 
         [0067]    By nature of the sensors, GraphSLAM output is displayed in units of steps; ground truth is plotted in meters. Notice that the results show straight halls, despite the GraphSLAM method containing absolutely no explicit shape prior. This adds further confidence to the notion that signal-strength SLAM can be achieved in a completely unsupervised manner, without relying on trajectory priors. 
         [0068]    The sensor used in our experiment measures pedometry in units of steps. The ground truth is collected using a laser-based technique that produces coordinates measured in meters. The location of the nodes in GraphSLAM are in a different reference frame than those in the ground-truth trajectory. To report error in standard units, localization accuracy is computed with a subjective-objective technique: A characterization is made of “how accurately a person detects returning to a previously visited location.” For each time t i  during the trajectory, an inferred ‘subjective’ location {tilde over ({right arrow over (x)} t     i    from GraphSLAM and a true ‘objective’ location {right arrow over (x)}* t     i    are used for the same timestamp in the ground truth. For every objective location {right arrow over (x)}* t     i    its ‘objective neighbors’ are denoted to be those points within r meters of {right arrow over (x)}* t     i    for each objective neighbor of {right arrow over (x)}* t     i   , the ‘subjective location {tilde over ({right arrow over (x)} t     i    has a corresponding ‘subjective neighbor’ {tilde over ({right arrow over (x)} t     i    . The GraphSLAM output is scaled so that the total distance traveled, and define localization error to be the mean of the distance ∥{tilde over ({right arrow over (x)} t     i   −{tilde over ({right arrow over (x)} t     i   ∥ 2  across all pairs (t i , t* i ). τ has been chosen to be the mean distance traveled between consecutive WiFi scans. 
         [0069]    With this metric, in an embodiment of the present invention, a mean localization error of 2.23 meters is achieved. The standard deviation of this value is 1.25 m. For comparison, using the same metric, the mean localization error from only pedometry and gyroscope without WiFi is 7.10 m. 
         [0070]    To demonstrate the robustness of an algorithm according to an embodiment of the present invention with respect to parameter selection,  FIGS. 4A  and  4 B are graphs that shown the mean and standard deviation of the localization error according to an embodiment of the present invention. More particularly, shown in  FIG. 4A  is graph  400  that is shown over a range of values for the Var(ε WiFi ) parameter. Shown in  FIG. 4B  is shown over a range of values for the τ parameter. In graphs  400  and  402 , the upper line is +1 standard deviation above the mean, and the lower line is −1 standard deviation below the mean. 
         [0071]    The parameter Var(ε WiFi ), which is related to the measurement noise variance associated with the WiFi sensor, can take any value from roughly 50 through 500 dB. Considering that the WiFi measurements themselves only range from about 0 to −90 dB, this implies that this embodiment is very robust against poor Var(ε WiFi ), selection. The parameter τ, which is related to the expected distance to the nearest wall, needs only be accurate to within roughly 3 meters. This also provides confidence that a slight errors in τ will not cause a catastrophic increase in error. 
         [0072]    These experiments may represent a lower bound on result quality for this algorithm according to an embodiment of the present invention. As a non-linear least squares problem, in general, benefit is expected from more robust solvers, e.g., Levenberg-Marquardt, simulated annealing, etc. Given that the GraphSLAM framework has been well-studied in the SLAM community, any number of solvers are likely to improve performance or result quality further. 
         [0073]    As described above, a signal strength SLAM has been formulated into an instance of GraphSLAM as an embodiment of the present invention. In doing so, scalability has been improved, runtime complexity has been reduced, limitations on WiFi density/richness has been relaxed, and shape priors have been removed. Experimental results on real-world data demonstrate the effectiveness of using GraphSLAM approach to solve this problem. The teaching presented here can be implemented in 3D variants of the WiFi SLAM problem, multi-agent extensions, time-of-arrival/round-trip-time sensor models, improved initialization techniques as well as more specialized solvers as would be obvious to one of ordinary skill in the art upon reading and understanding the present disclosure. 
         [0074]    It should be appreciated by those skilled in the art that the specific embodiments disclosed above may be readily utilized as a basis for modifying or designing other image processing algorithms or systems. It should also be appreciated by those skilled in the art that such modifications do not depart from the scope of the invention as set forth in the appended claims.