# Patent

## Title
Techniques for accurate pose estimation

## Abstract

The described technology regards an augmented reality system and method for estimating a position of a location of interest relative to the position and orientation of a display based upon a retroactive adjustment of a previously rendered position and orientation of the display, by means of an adjust-update-predict (AUP) cycle, and calculating the location of interest relative to the position and orientation of the display. Systems of the described technology include including a plurality of sensors, a processing module or other computation means, and a database. Methods of the described technology use data from the sensor package useful to accurately render graphical user interface information on a display.

## Background
The inventions herein disclosed were made with government support under Federal Contract No. FA8650-09-C-7909, awarded by DARPA (Defense Advanced Research Projects Agency). The government has certain rights to the inventions described in this application.


BACKGROUND OF THE TECHNOLOGY
The described technology regards vision-aided navigation, and in particular pose estimation useful in vision aided navigation, advantageous in wearable augmented-reality (AR) systems operating in natural outdoor environments and other applications.
While a variety of GPS/INS solutions achieve pose estimation, currently available solutions do not offer the level of customization needed for a person walking over arbitrary outdoor environments. The systems and methods of the described technology take advantage of kinematics constraints, and detection of magnetic and dynamic disturbances, to achieve enhanced accuracy. Further, the use of digital terrain and elevation data (DTED) information allows systems of the described technology to provide a stable measurement of altitude with measurements of absolute orientation, systems incorporating the described technology reach levels of performance that are not possible with other GPS/INS systems.
GENERAL DESCRIPTION
The described technology regards pose estimation systems useful in augmented-reality systems and other technology, including a plurality of sensors, a processing module or other computation means, and a database.
The plurality of sensors or sensor package may include sensors such as a camera, a 3-axis accelerometer, a 3-axis angular rate gyro, a 3-axis magnetometer, a barometric pressure sensor, and a GPS receiver, and may be mounted to a rigid reference assembly. Data from the sensor package, which could include delayed measurements, is transmitted to the processing module or other computation means, which generates signals that render graphical user interface information on a display using the sensor data. The processing module also is communicatively coupled with, and uses information from a database, which receives, stores, and transmits data such as locations of interest in geodetic coordinates (latitude, longitude, altitude), and digital terrain and elevation data. The database may be local to the processing module, virtual, stored on a single or network of remote servers, on the Internet or otherwise accessible to the processing module.
The data received by the processing module may include time-stamped sensor values such as camera imagery, accelerometer measurements, rate-gyro measurements, magnetometer measurements, barometric pressure sensor measurements, GPS receiver position measurements, and GPS receiver velocity measurements. From this data and data representing the spatial position of a display relative to the spatial position of the sensors, the processing module calculates over time a position vector representing the current estimated position of a location of interest with respect to the position and orientation, such as the orientation, geodetic position (longitude, latitude, altitude), or combination thereof, of the display, expressed in display coordinates. In some embodiments the position vector derives from the sensor values received by the processing module, using an Extended Kalman Filter (EKF) structure based on a retroactive update to a previously rendered pose estimate stored in a rewind buffer, or a forward buffer to store current and predicted pose estimates calculated by the processing module, for example by means of circuitry, firmware and/or a processor. Each delayed measurement may be stored in the rewind buffer and processed using a separate sequential update. In some embodiments the processing module detects the presence of magnetic disturbances and, if detected, rejects magnetometer measurements corresponding to magnetic disturbances. In some embodiments the calculation of the position vector uses an EKF structure having an adjust-update-predict (AUP) cycle, which adjusts the previous prediction pose values by including the effects of current input data in addition to current measurement data to estimate the current state.
The processing module then generates signals to render on a display, at a position derived from the position vector, graphical user interface information including a graphical icon representing the location of interest. The processing module may implement a baseline GPS/INS to calculate the position vector, and may adjust the baseline GPS/INS with absolute orientation information when available.
Some embodiments of the described technology further include a radio to send and receive geo-spatial data relating to objects in the environment. The processing module receives, stores and uses such geo-spatial data, and generates signals to render graphical user interface information on a display such as a graphical icon representing some of the objects in the environment at a position derived from the position vector. In some embodiments of the described technology a rate-gyro measurement vector received by the processing module is selectively filtered by the processing module based on its magnitude.
The described technology further regards a method for providing an estimate of position for a location of interest relative to a display. The method includes the steps of receiving sensor data from a plurality of sensors which may include delayed data measurements and data representing the pose of a display, and applying an Extended Kalman Filter (EKF) to the received data and the geodetic coordinates of a location of interest, to estimate a pose of the display from which a position of the location of interest relative to the pose of the display may be calculated. The EKF may facilitate a retroactive update to a previously rendered pose estimate stored in a rewind buffer. In some embodiments of the described technology the EKF may also or alternatively use a forward buffer to store current and predicted state estimates. In some embodiments the method includes receiving a plurality of measurement vectors from a rate-gyroscope, and selectively filtering the measurement vectors based on a magnitude of each of the measurement vectors; a position state estimate may then be generated based upon the filtered measurement vectors and other received data. In some embodiments the method applies an Extended Kalman Filter (EKF) to the received data and geodetic coordinates of a location of interest, to estimate the position of the location of interest relative to the pose of the display, based upon a retroactive adjustment of a previously rendered pose estimate using the delayed data measurements, a recalculation of the estimated position of the location of interest relative to the pose of the display, and a prediction of future pose estimates by means of an adjust-update-predict (AUP) cycle.
The disclosed technology also regards a pose estimation system for use with a display, including means for receiving sensor data from a plurality of sensors comprising delayed data measurements and data representing the spatial position and orientation and orientation of a display, and means for applying an Extended Kalman Filter (EKF) to the received data and geodetic coordinates of a location of interest, to estimate a position of the location of interest relative to the pose of the display. The estimation may be based on a retroactive update to a previously rendered pose estimate stored in a rewind buffer, using delayed sensor values. The estimation may also or alternatively use a forward buffer to store current and predicted state estimates. The disclosed technology may also include means for receiving a plurality of measurement vectors from a rate-gyroscope, and selectively filtering the measurement vectors based on a magnitude of each of the measurement vectors, wherein the means for generating a position state estimate is based on the filtered measurement vectors and other received data. In some embodiments of the disclosed technology the estimation of the position of the location of interest may be based upon a retroactive adjustment of a previously rendered pose estimate of the display using the delayed data measurements, a recalculation of the estimated position, and a prediction of future positions by means of an adjust-update-predict (AUP) cycle.
The disclosed technology also regards one or more tangible computer-readable storage media encoding computer-executable instructions for executing on a computer system a computer process of modifying an extended Kalman Filter (EKF) prediction of a position and orientation of a display with a retroactive adjustment of a previously rendered position and orientation of the display, using delayed data measurements and predicting future positions of the display, by means of an adjust-update-predict (AUP) cycle. The predicted position and orientation of the display may then be used by the computer process to calculate the position of a location of interest relative to the estimated position and orientation of the display.
In an application of the described technology, a user can quickly orient himself and initiate movement as his next waypoint is automatically rendered in his environment as seen through the display (along with additional pertinent information), rather than having to study his map or check his compass. The user's azimuth, current position and objective destination may be constantly displayed. If asked to report his location, the user can give a status update without stopping or otherwise taking his attention away from his current field of view. If the user encounters an obstacle to movement, he can easily detour or bypass the obstacle without losing his orientation to the movement objective. His next waypoint, or other icon of interest, serves as a directional beacon regardless of occlusions to his view or obstacles to his forward progress.
Similarly, in a military application, soldiers are often required to effectively manage airspace for numerous aircraft and to orchestrate aircraft movement in support of ground forces and sensor assets. Some mission profiles mandate unaided visual acquisition of an aircraft. Aircraft that broadcast their location can be geo-referenced by the described technology, even if the aircraft is out of visual range. Once in visual range, finding the aircraft can still be very challenging due to occlusions (e.g., clouds, terrain) or other sensory challenges (e.g., acoustic multi-path effects). The described technology enables rapid acquisition by the soldier of the locations of aircraft assets and provides immediate understanding of aircraft attributes (e.g., call sign, altitude).



DRAWINGS
FIGS. 1A, 1B and 1C depict example embodiments of the system of the described technology.
FIG. 2 is a peripheral view of an example embodiment of the system of the described technology.
FIG. 3 is a block diagram of the sensor package and the processing module of an example embodiment of the described technology.
FIG. 4 shows the various coordinate systems useful in the described technology.
FIG. 5 is a qualitative timing diagram of the EKF processing of example embodiments of the described technology.
FIG. 6 shows a close-up look at an azimuth update based on a representative absolute orientation measurement useful in the system and methods of the described technology; as depicted in the inset of the figure, the EKF goes back in time using the rewind buffer to reprocess the azimuth estimate based on the delayed absolute orientation measurement.
FIG. 7 shows an example embodiment of the user interface/display of the described technology.
FIG. 8 depicts the predict-update (PU) cycle and the update-predict (UP) of an EKF method useful in the described technology.
FIG. 9 depicts an adjust-update-predict cycle of an exemplary embodiment of the described technology.
FIG. 10 is a block diagram representing an exemplary environment in which the present disclosure or parts thereof may be implemented.
FIG. 11 depicts the geometry behind the definition of the error measure in the augmented reality application of the described technology.



DETAILED DESCRIPTION
The augmented reality system of the described technology comprises in some embodiments a motion sensing and visualization kit 1, an augmented-reality processing module 2 with a database 3, and may include a radio 4, as depicted in FIG. 1C and FIG. 2. The database may be remote from the visualization kit and the processing module
As shown in the embodiment of the described technology depicted in FIGS. 1A, 1B and 1C and FIG. 2, the motion sensing and visualization kit 1 may include a rigid reference assembly 11 with a camera (high-speed and high-resolution) 12 and a sensor package 13, and having a display 14 with a graphical user interface 141 rendered on the display to convey information in the form of text and graphics, an embodiment of which is shown in FIG. 7. In some embodiments of the system of the described technology the display 14 is see-through. The sensors and processing module of the described technology can function with a wide variety of displays, including by example and without limitation see-through displays manufactured by the BAE, Lumus, and SA Photonics.
As depicted in FIG. 3, the sensor package 13 includes a plurality of sensors, including a 3-axis accelerometer 131, a 3-axis angular-rate gyro 132, a 3-axis magnetometer 133, a barometric pressure sensor 134, a GPS receiver 135 and a camera 12. The sensors may be mounted to the rigid reference assembly as a packaged unit. While described as a package, the sensors may be individually positioned about the rigid reference assembly 11 or on the user's clothing or other equipment, in accordance with the technology as herein described. In some embodiments the rigid reference assembly is a helmet.
The sensors 13 are in wired communication (via a cable, or other hard wire) or wireless communication (via Bluetooth or other wireless communication means) with the processing module 2 or other computation means of the described technology. As hereinafter described, the processing module processes data from the sensors and data from a database to generate display pose, and renders tactically-relevant information on the motion sensing and visualization kit's display 14. In some embodiments the processing module is carried on the user's body when the system is in operation. Coupled with the processing module is a database 3 including the geodetic coordinates of locations of interest (longitude, latitude and altitude), and digital terrain and elevation data (DTED) to aid in the estimation of altitude. The processing module may further include custom software and standard libraries to receive geo-spatial data (i.e., latitude, longitude and altitude information about objects in the environment) via a radio network or otherwise, and software to render this data to a GUI 141.
The processing module or other computation means may be in wired communication (via a cable, or other hard wire) or wireless communication (via Bluetooth, or other wireless communications means) with the display 14. The processing module may also be coupled by wire or wireless communication with the radio 4, which receives signals relating to data in the database, and supports receiving and parsing XML-based messages from a digital radio network.
Further, as shown in FIG. 7, the user interface/display 141 may provide operational alerts (e.g., notification that the radio network is inoperable, that the system is magnetically-disturbed, or the GPS signal is denied or degraded), system status information (e.g., user interface mode ID, system battery level, operational time), system settings menu access, iconic visualization of geo-registered points of interest, and a situational awareness ring 213. A menu may not be displayed until activated by the user via a button/toggle switch located on or coupled with the motion sensing and visualization kit 1; with the menu, the user may access and change system configuration settings. The situational awareness ring shown in FIG. 7 is an intuitive tool that offers the user in a military application a dynamic real-time 360° understanding of where friendlies, enemies and other points of interest are located. At the center of the grid is the user's Military Grid Reference coordinate; located above the ring is the user's heading (on the fly configurable as magnetic or true). Icons may move around the ring in response to user rotation. Geo-registered icons and those on the situational awareness ring are displayed in some embodiments with range information from the user, and in some cases elevation (for aircraft icons). One or more batteries may power various components of the system of the described technology.
Suitable hardware for use in the processing module 2 include embedded processing modules with, for example, an NVidia Tegra 3 system-on-chip and DDR3L memory. Similar suitable hardware may be found in current cell-phone quad-core computing platforms.
Over time, in periodic intervals, the sensors of the sensor package 13 measure various conditions, and transmit time-stamped signals representing the measurements to the processing module 2. Specifically, the accelerometer 131 provides a measure ya of the difference between linear acceleration of the sensor and the Earth's gravity vector, the rate gyro 132 provides a measure ug of angular rate, the magnetometer 133 provides a measure ym of the Earth's magnetic field to help in determining azimuth, and the barometric pressure sensor 134 provides a measure ybp of barometric pressure for estimating altitude. Similarly, the GPS receiver 135 provides its position data yGp (latitude, longitude, and altitude) and its velocity data yGv (North, East, and Down velocities).
The processing module 2 or other computation means receives measurements from the sensors 13, and calculates over time the position vector of a location of interest s relative to the pose of the display 14.
The vector of the location of interest s calculated by the processing module is referred to as vector rdsd, representing the current estimated position of s relative to the pose of the display, expressed in display coordinates. The calculations are based upon an Extended Kalman Filter (EKF) structure, performed by an “EKF Calculator” stored in memory and executable by a processor to calculate state predictions. The EKF Calculator may include software and/or hardware elements, and may be implemented in any tangible computer-readable storage media. “Tangible computer-readable storage media” includes, but is not limited to, RAM, ROM, EEPROM, flash memory or other memory technology, CDROM, digital versatile disks (DVD) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other tangible medium which can be used to store the desired information and which can be accessed by mobile device or computer. In contrast to tangible computer-readable storage media, intangible computer-readable communication signals may embody computer readable instructions, data structures, program modules or other data resident in a modulated data signal, such as a carrier wave or other signal transport mechanism. The term “modulated data signal” means a signal that has one or more of its characteristics set or changed in such a manner as to encode information in the signal.
The Extended Kalman Filter (EKF) is a method for estimating a set of quantities describing a system, referred to as system states, given a set of measurements and a pair of mathematical models. The measurements include system data, typically collected by sensors as the system operates. The first mathematical model, referred to as the process model, describes the evolution of the system states over time. The second mathematical model, referred to as the measurement model, describes how the measurement data is related to the states. Being only an approximation, each model must include a measure of its uncertainty, which the EKF uses to produce an estimate of the system states from the information at hand. It does so by starting from initial estimates of the system states and their uncertainty, valid at some initial time t0, and using the process model to produce new estimates of both, called the prediction, valid at some later time t1 when measurement data is also available. At time t1, the new information provided by the measurement data, weighted using the measurement model, is used to make an update to the prediction, resulting in the final estimates of the system states and their uncertainty at t1. This prediction-update cycle continues as long as measurement data is available, producing estimates of system states and associated uncertainty along the way.
In some embodiments of the described technology the EKF structure includes a rewind buffer to integrate delayed measurements, or a forward buffer to store current and predicted state estimates calculated by the processing module, or both. Using the vectors rdsd calculated by the processing module, the processing module 2 or other computation means generates signals to render graphic symbology on the display 14, corresponding to the location of interest s, that appears to the user to be attached to that real-world location (i.e., geo-registered). In some embodiments the system uses DOD-standard MIL-STD-2525C symbology, which distinguishes between hostile, friendly, neutral and unknown symbols based on shape and color.
Specifically, the processing module 2 or other computation means of the described technology implements a baseline GPS/INS, and aids it with vision-based and other non-inertial absolute orientation information when available. The baseline GPS/INS is designed to provide a nominal level of performance when vision-aiding and other measurements are not available, and integrate them when available for improved performance, without assumptions about their availability or periodicity. The system of the described technology may further utilize measurements and computer processes and analysis to address latency and enhance robustness of the system in the presence of magnetic or dynamic disturbances, as hereinafter described.
The EKF structure used in the processing module 2 of the described technology further accommodates and aligns time-delayed measurements using a buffer scheme. In some embodiments the processing module selectively filters the rate-gyro measurement vector ug transmitted to the processing module, based on the magnitude of the vector ug. In some embodiments the processing module of the described technology augments the standard predict-update cycle of the EKF process with an additional step to reduce prediction error and improve responsiveness to rate gyro and accelerometer inputs, as hereinafter described.
By means of the processing module 2 or other computation means and associated standard libraries accessible thereby, the GUI 141 renders a situational awareness ring 21 and one or more icons on the display representing positions of locations of interest, using the vector rdsd.
Various coordinate systems are involved in the calculations of the described technology, as depicted in FIG. 4. The body coordinate system b is the reference for the motion sensing and visualization kit 1, with origin at the point p. The camera coordinate system c consists of a permutation of the body coordinate system's axes and shares the same origin. The display coordinate system d and the accelerometer coordinate system a are both rigidly attached to the body coordinate system. Coordinate system a is the reference for the motion sensing and sensors. Coordinate system n is the North-East-Down (NED) reference for navigation. The Earth-Centered Earth-Fixed (ECEF) coordinate system e is used to specify points in the environment. Coordinate system i is the Earth Centered Inertial (ECI) coordinate system, which is a good approximation of a true inertial reference in the context of the described technology. The WGS-84 ellipsoid is used as the world model.
The processing module calculates the position vector rdsd of a point s in the environment with respect to the origin of d, expressed in d coordinates, using the formula:

rdsd=(CneCbnCdb)T[rese−(repe+CneCbnrpdb)]

wherein

rpdb is the position of the origin of d with respect to p, expressed in b coordinates, obtained from physical measurements on the actual system or measurements on its solid-model drawing equivalent.

rese is the position vector of a point s in the environment with respect the origin of e, expressed in e coordinates, obtained by converting the known latitude, longitude, and altitude of the point s from the database into equivalent Cartesian coordinates.

repe is the position of p with respect to the origin of e, expressed in e coordinates, obtained by converting the latitude, longitude, and altitude of the point p into equivalent Cartesian coordinates. The latitude, longitude, and altitude of the point p are estimated by the system's EKF using sensor data, as hereinafter described.

The position vectors of points s (ps) and p (pp) with respect to the origin of e are specified in terms of latitude, L, longitude, λ, and altitude, h. The position vector ps is stored in the database 3; the position vector pp is calculated using the method as hereinafter described. The conversion from latitude, longitude, and altitude coordinates into their Cartesian equivalents is performed by the processing module of the described technology, by the mapping:

xe=(RN(L)+h)cos L cos λ

ye=(RN(L)+h)cos L sin λ

ze=[1−e2)(RN(L)+h)] sin L 

wherein RN(L) and e are WGS-84 ellipsoid parameters.

Cne represents the orientation of the North-East-Down (n) coordinate system (see FIG. 4) with respect to the Earth-Centered Earth-Fixed (e) coordinate system, obtained from a known coordinate transformation that is a function of latitude, longitude, and altitude of the point p. The latitude, longitude, and altitude of the point p are estimated by the system's EKF using sensor data.

Cbn represents the orientation of the body coordinate system (b) (see FIG. 4) with respect to the North-East-Down (n) coordinate system.

Cbn is estimated by the system's EKF using sensor data.

Cdb represents the orientation of the display coordinate system (d) with respect to the body coordinate system (b) obtained from a-priori calibration based upon alignment of features in an image acquired by the camera and expressed in body coordinates with corresponding features in the same image expressed in display coordinates.

Once Cbn and pp are estimated using the system's EKF, the processing module 2 renders the GUI information on the display 14 so that an icon representing the position s can be rendered at display coordinates rdsd.
The EKF used in the processing module 2 of the described technology is based upon the general model:



  
    
      
        d
        x
      
      
        d
        t
      
    
    =
    
      f
      ⁡
      
        (
        
          x
          ,
          u
          ,
          w
          ,
          t
        
        )
      
    
  




  
    
      
        y
        ^
      
      k
    
    =
    
      
        h
        k
      
      ⁡
      
        (
        
          
            x
            k
          
          ,
          
            v
            k
          
        
        )
      
    
  



where t is time, f is a continuous-time process, hk is a discrete-time measurement (with output ŷk), x is the state vector, xk is its discrete-time equivalent, and u is the input vector. The vector w is a continuous-time zero-mean white noise process with covariance Q (denoted as w ̃N(0,Q)) and vk is a discrete-time zero-mean white-noise process with covariance Rk (denoted as vk ̃N(0,Rk)).

The state is defined as x=[pp;vepn;qnb;bg;ba] (semicolons are used to indicate column stacking), wherein vepn is the velocity of the point p with respect to the ECEF coordinate system, expressed in NED coordinates, and qnb is the quaternion representation of Cbn. The vector bg is the rate-gyro bias, and the vector ba is the accelerometer bias. The rate gyro and accelerometer data are inputs to the process model, so that u=[ua;ug], with

ua=fipb+ba+wa 

ug=ωibb+bg+wg 

where fipb=(Cbn)T[aepn−gn+(ωenn+2ωien)×vepn] is the specific force at p, ωibb is the angular rate of the body coordinate system with respect to the ECI coordinate system (i), ωenn is the angular rate of n coordinate system with respect to the e coordinate system (expressed in n coordinates), ωien is the angular rate of the e coordinate system with respect to the i coordinate system (also expressed in n coordinates), wa ̃N(0,Qa) and wg ̃N(0,Qg). The cross product in the fipb expression is a Coriolis and centripetal acceleration term due to motion over the Earth's surface, and can be neglected when the velocity is small (which is the case for pedestrian navigation).

Using the state definition and input model described above, the process model is specified by the following equations:

{dot over (p)}p=fp(x)+wp 

{dot over (v)}epn=Cbn(ua−ba−wa)+gn−(ωenn+2ωien)×vepn+wv 

{dot over (q)}nb=1⁄2Ω(qnb)(ug−bg−wg−ωinb)+wq 

bg=wbg 

ba=wba 

where





  
    
      f
      p
    
    =
    
      
        [
        
          
            
              
                1
                
                  
                    
                      R
                      M
                    
                    ⁡
                    
                      (
                      L
                      )
                    
                  
                  +
                  h
                
              
            
            
              0
            
            
              0
            
          
          
            
              0
            
            
              
                1
                
                  
                    (
                    
                      
                        
                          R
                          N
                        
                        ⁡
                        
                          (
                          L
                          )
                        
                      
                      +
                      h
                    
                    )
                  
                  ⁢
                  cos
                  ⁢
                  
                    
                  
                  ⁢
                  L
                
              
            
            
              0
            
          
          
            
              0
            
            
              0
            
            
              
                -
                1
              
            
          
        
        ]
      
      ⁢
      
        v
        ep
        n
      
    
  
  ,




RM and RN are WGS-84 parameters, gn is the acceleration due to gravity, Ω is a 4×3 matrix that transforms an angular rate vector into the corresponding quaternion derivative, and ωinb=(Cbn)T(ωien+ωenn). The process noise vector is w=[wp;wv;wq;wg;wbg;wa;wba], and its covariance matrix is Q=blkdiag(Qp,Qv,Qq,Qg,Qbg,Qa,Qba). The measurement vector is defined as:





  
    
      y
      ^
    
    k
  
  =
  
    
      [
      
        
          
            
              
                y
                ^
              
              AO
            
          
        
        
          
            
              
                y
                ^
              
              a
            
          
        
        
          
            
              
                y
                ^
              
              m
            
          
        
        
          
            
              
                y
                ^
              
              Gv
            
          
        
        
          
            
              
                y
                ^
              
              Gp
            
          
        
        
          
            
              
                y
                ^
              
              D
            
          
        
      
      ]
    
    =
    
      [
      
        
          
            
              
                q
                nb
              
              +
              
                v
                AO
              
            
          
        
        
          
            
              
                
                  
                    (
                    
                      C
                      b
                      n
                    
                    )
                  
                  T
                
                ⁢
                
                  (
                  
                    
                      a
                      ep
                      n
                    
                    -
                    
                      g
                      n
                    
                  
                  )
                
              
              +
              
                b
                a
              
              +
              
                v
                a
              
            
          
        
        
          
            
              
                
                  
                    (
                    
                      C
                      b
                      n
                    
                    )
                  
                  T
                
                ⁢
                
                  m
                  n
                
              
              +
              
                v
                m
              
            
          
        
        
          
            
              
                v
                ep
                n
              
              +
              
                v
                Gv
              
            
          
        
        
          
            
              
                p
                p
              
              +
              
                v
                Gp
              
            
          
        
        
          
            
              h
              +
              
                v
                D
              
            
          
        
      
      ]
    
  




where ŷAO is an absolute orientation measurement, ŷa is the accelerometer measurement, ŷm is the magnetometer measurement, ŷGv is the velocity measurement, ŷGp is the GPS horizontal position (i.e., latitude and longitude) measurement, and ŷD is the measurement of altitude based on DTED. The measurement noise vector is vk=[vAO;va;vm;vGv;vGp;vD], and its covariance matrix is Rk=blkdiag(RAO, Ra, Rm, RGv, RGp, σD2).

Because of the block-diagonal structure of Rk, the EKF measurement update step is executed by processing measurements from each sensor as separate sequential updates (in the same order as they appear in the ŷk vector above).
The gravity vector is approximated as being perpendicular to the WGS-84 ellipsoid and therefore modeled as gn=[0;0;g0(L)], where the down component g0(L) is obtained from the known Somigliana model. Since they are used as measurements of the gravity vector in body coordinates, accelerometer-based updates are only valid if the acceleration aepn is zero. If not, these measurements are considered to be corrupted by an unknown dynamic disturbance. However, this disturbance is addressed by detecting its presence and, consequently, increasing the corresponding measurement noise covariance matrix, Ra, by a large factor ρa (e.g., ρa=100). Detection is based on comparing the norm of the accelerometer measurement to ∥gn∥, and also checking that the measured angular rate is lower than a threshold whose value is application dependent (e.g., 3°/sec. in certain conditions). In head-worn applications, the location of the sensor package on the motion sensing and visualization kit, and the corresponding kinematics due to head movement, result in angular rate being a very good indicator of aepn. The approach of increasing Ra implies that the unknown acceleration aepn is modeled as a stationary white noise process. Though the actual process is not stationary or white, it was found experimentally that this approach yields better results than the alternative of completely rejecting accelerometer measurements that are deemed disturbed. In fact, when testing this alternative, it was observed that a single valid measurement after long periods of dynamic disturbance (as in the case when walking) could cause undesirable jumps in the estimates of bg and ba, while increasing Ra resulted in no such issues.
Magnetometer-based measurement updates are valid if the magnetic field being measured is the Earth's magnetic field only. Otherwise, these measurements are considered to be corrupted by an unknown magnetic disturbance. Therefore the processing module or other computation means of the described technology may detect the presence of magnetic disturbances and, if detected, rejects the corresponding magnetometer measurements. Detection is based on comparing the norm of the measured magnetic field vector ym to the Earth's field strength Bm, as well as checking that the computed inclination angle is not too far (e.g., 0.5 deg) from the nominal value. Since it is based on the inner product ymTya, the latter check is only performed if no dynamic disturbance is detected.
The processing module or other computation means may use a circular rewind buffer (RB) to maintain a record of relevant information pertaining to the last Nr samples of EKF processing. This is done to properly integrate absolute orientation measurements, which are delayed with respect to the rest of the data (as depicted in FIG. 5, a qualitative timing diagram of the EKF processing of the described technology). By this buffer, when absolute orientation information is processed and delivered, the EKF can reprocess past information. In the processing module 2 of the described technology the absolute orientation data acquisition is synchronized with the sensor data acquisition. This reprocessing of past data is handled within a single EKF epoch Δt. FIG. 6 shows a close-up look at an azimuth update based on a representative absolute orientation measurement. The EKF is able to “go back in time” and use the rewind buffer to reprocess the state estimate based on the late measurement, all within its regular processing interval. In the example illustrated in the inset in FIG. 6, the EKF goes back in time using the rewind buffer to reprocess the azimuth estimate of the pose of the display based on the delayed absolute orientation measurement.
The processing module may also or alternatively use a forward buffer (FB) to store both the current state estimate xk+ and the predicted state estimates up to Nf time steps ahead. That is FBk={xk+, xk+1−, xk+2−, . . . , xk+N−}. Through interpolation of the FB vectors, a state estimate can then be produced for any tε[tk, tk+NfΔt] where tk is the time of the current estimate and Δt is the EKF's processing interval. Given a value Δtd for system latency, the pose that is delivered at the time tk for rendering graphic on the display is based on the predicted state at t=tk+Δtd, which is extracted from the FB. Nf must be selected such that Nf>0 and NfΔt≧Δtd. Focused experiments have shown a reduction in perceived latency from about 40 ms to about 2 ms when using this forward-prediction process.
Prior to use, the sensors 13 of the described technology must be calibrated. Hardware calibration of the motion sensing and visualization kit consists of estimating Cdb, rpdb, Cab and rpab. Estimation of the spatial position and relative orientation, Cab, of the sensors with respect to the body coordinate system is performed, which also yields an estimate of the camera's intrinsic parameters. Estimation of the relative orientation, Cdb, of the display 14 with respect to the body coordinate system is performed by an iterative process based on using an initial Cdb estimate to render scene features (e.g., edges) from camera imagery onto the display 14, and adjusting it until the rendered features align with the corresponding actual scene features when reviewed through the display 14. The position vectors rpdb and rpab can be obtained by straightforward measurement, but in fact they are negligible in the context of this application, the former because ∥rpd∥<<∥rps∥, and the latter because its magnitude is very small and was empirically determined to have negligible effect. The magnetometer 133 is also calibrated prior to each operation.
The initial state x(0) is estimated by using sensor readings during the first few seconds of operation before the EKF process starts. The initial condition of all biases is set to zero.
The processing module 2 uses a number of parameter values that have been tuned experimentally prior to system use. These are values for Q, Rk, the initial estimated error covariance matrix P(0), and a number of parameters that are used for disturbance detection, filtering, etc. This tuning is performed by combining Allan variance analysis of sensor data with the models herein described, to identify a starting point, and then performing a series of focused field experiments.
The forward-prediction process extrapolates motion to predict the state at some time in the future, and is inherently sensitive to noise. This may result in jitter (i.e., high-frequency small-amplitude motion) of the rendered graphics even when the system is perfectly stationary (e.g., mounted on a tripod). Low-pass filtering of the rate gyro signal, ug, transmitted to the processing module reduces this jitter effect but also introduces a time lag between real-world motion and motion of the corresponding symbology rendered on the display. Since this lag is not noticeable when the rotation rate is near zero, and the jitter is not noticeable when there is actual motion, in some embodiments the described technology achieves a reduction in perceived jitter by low-pass filtering the rate gyro signal only when the estimated rotation rate magnitude ∥ug−bg∥ is small (e.g., less than 5 deg/s). As specified below, this is done by adjusting the low-pass filter's bandwidth using a smooth increasing function of estimated rotation rate magnitude. The adaptive gyro filtering method is implemented in the processing module of the described technology by using the discrete-time filter ũg,k=aũg,k−1+(1−a)ug,k with a=Aexp(−0.5∥ug,k−bg∥2/σa2) where 0≦A<1 and σa>0 are parameters that are established prior to use (e.g., A=0.85, σa=0.05). The resulting filtered signal can then be used in place of ug in the EKF's time-propagation steps (i.e., in the forward-prediction process). In testing, your inventors found that this method reduces jitter by a factor of three without any adverse effects in other performance measures.
A single pose estimation processing step takes as inputs the current sensor data, the RB data, and an index inow corresponding to the current-time location in the RB. It returns updates to RB, inow, and the whole FB. An example of its implementation is as follows:















 1:
pre-process sensor date



 2:
RB[inow] ← {sensor data, pre-processed data}



 3:
istop = inow



 4:
if vision data is available and ∃ ivis : tCLK in RB[ivis] =




tCLK in vision data then










 5:
inow = ivis










 6:
end if



 7:
keep_processing = true



 8:
while keep_processing = true do










 9:
{x−, P−} ← RB[inow]



10:
RB[inow] ← {x+, P+} = ekf_u(x−, P−, RB[inow])



11:
inext = inow + 1



12:
RB[inext] ← {x−, P−} = ekf_p(x+, P+, RB[inow])



13:
if inow = istop then










14:
FB[0] ← x+, FB[1] ← x−



15:
for kp = 2 to Nf do










16:
{x−, P−} = ekf_p(x−, P−, RB[inow])



17:
FB[kp] ← x−










18:
end for



19:
keep_processiag = false










20:
end if



21:
inow = inext










22:
end while









where tCLK is the reference time stamp of both sensor and vision data acquisition, and lines 10 and 12 are the EKF measurement update and prediction steps, respectively. The loop on lines 15-18 implements the forward-prediction process by repeating single EKF prediction steps.

Accuracy performance is based on a measure of error, ε, defined as the angle between the vectors rps′b and rpŝb, as depicted in FIG. 11. The point s′ is the point in the undistorted camera image corresponding to the real-world reference point s, and is obtained via semi-automatic processing (i.e., requiring some manual input) of the imagery. The vector rpŝb is the result of using the pose estimate, {pp, Cbn}, to compute rpsb. Note that, in addition to pose estimation errors, the process of generating the ‘ground-truth’ vector rpsb, also contributes to ε. This contribution was observed to be up to 3 mrad across a variety of experiments.
EKF implementations repeatedly perform either a predict-update (PU) cycle or an update-predict (UP) cycle (shown in FIG. 8). The difference between these cycles amounts to the time of availability of the state estimate: the UP cycle implementation can produce an estimate sooner relative to the time when sensor data is available. In either case only current measurement data yk are utilized to estimate the current state (current input data uk are not).
In some embodiments of the described technology, the processing module or other computation means adjusts the previous prediction by including the effects of the current input data, before executing the update step. This adjust-update-predict (AUP) cycle (shown in FIG. 9) has the effect that both current measurement data yk and current input data uk are used to estimate the current state. Therefore, the AUP cycle implementation is more responsive than the UP cycle to changing input u, provides a better estimate to the update step, and requires very little additional computation.
In some applications these systems and the methods herein described combine a novel pose estimation capability and a plurality of sensors to allow rendering of geo-registered graphics on a see-through display, thereby appearing to be part of the real environment as the user looks at the environment through a display.
The pose estimation systems and methods herein described can be implemented in a wide variety of commercial and consumer applications. First-responder or search-and-rescue personnel can see geo-registered icons representing the locations of team members, search regions, and key objects of interest during mission operations. Accessing this information in a heads-up posture enables the user to perform activities more safely, with higher operational tempo, and with greater team coordination. Construction-site or warehouse foremen can view icons representing workers and material locations to help monitor safety on the worksite and to support quick decision making about resource allocation. Oil-and-gas industry workers can view graphics representing locations of structures of interest, such as underground or underwater pipelines, system safety components, and graphical representation of important system states (e.g., pressure and temperature of storage tanks and pipeline sections). Outdoor recreational enthusiasts (e.g., runners, bicyclists, hikers) can be presented with directional information, waypoints, and details about their exact position and heading when carrying out active movement while in a heads-up posture viewing the real-world environment. For immersive training applications, users can be presented with virtual avatars that appear as part of their real-world environment as they maneuver and carry out training scenarios indoors or outdoors. Such training enables the user to practice and improve scenario-specific decision making. This immersive training may be extended to sports training applications, where athletes may use augmented and/or virtual reality to enhance their training program. The pose estimation systems and methods herein may also be applied to gaming scenarios where augmented reality and/or virtual reality is used to enhance user experience and the estimation of pose of a gaming device is required. Other applications include the transportation industry, where vehicle operators may access information that appears to part of the real-world environment, and maintenance personnel may view pertinent information overlaid on the system under maintenance/repair.
Further, the pose estimation system and methods as herein described can be implemented with a variety of display technologies, including night vision goggles, see-through displays, wearable smart glasses, and smartphone or tablet devices. For smartphone or tablet style applications, the position and orientation of the smartphone or tablet is accurately tracked while the user holds the device in an upright position in their line of sight to view the real-world while ‘looking through the phone’. In this video see-through application, geo-registered graphical information is superimposed on the device's camera imagery and presented to the user real-time on the device's display.

## Claims

1. A pose estimation system for use with a display, the system comprising:
a. a plurality of sensors,
b. a database comprising geodetic coordinates of a location of interest, and
c. a processing module communicatively coupled with the database and the sensors, the processing module receiving over time data from the sensors comprising delayed data measurements, and data representing the spatial position of the display relative to the spatial position of the sensors, and applies an Extended Kalman Filter (EKF) to the received data to estimate the position and orientation of the display, and based upon a retroactive adjustment of a previously rendered position and orientation of the display, estimates the position and orientation of the display using the delayed data measurements and predicts future positions of the display by means of an adjust-update-predict (AUP) cycle, and calculates the location of interest relative to the position and orientation of the display, wherein the position of the location of interest is the vector rdsd, calculated using the formula:


rdsd=(CneCbnCdb)T[rese−(repe+CneCbnrpdb)]


wherein rpdb is the position of the origin of the display coordinate system d with respect to the point of origin of the body coordinate system p, expressed in body system coordinates b; rese is the position vector of the location of interest s in the environment with respect to the origin of the earth centered, earth fixed coordinate system e, expressed in e coordinates, derived from the geodetic coordinates of the location of interest in the database; and repe is the position of location p with respect to the origin of e, expressed in e coordinates, wherein Cne is an orientation of a North-East-Down (n) coordinate system with respect to e, wherein Cbn is an orientation of b with respect to the North-East-Down (n) coordinate system, and wherein Cdb is the orientation of the display relative to the body system coordinates b.

2. The pose estimation system of claim 1, wherein the plurality of sensors comprise a camera, a 3-axis accelerometer, a 3-axis angular rate gyro, a 3-axis magnetometer, a barometric pressure sensor, and a GPS receiver.

3. The pose estimation system of claim 1, wherein the database further comprises digital terrain and elevation data, and the processing module ingests said data to aid in the estimation of altitude of the display.

4. The pose estimation system of claim 1, further comprising the display, and wherein graphical user interface information comprising a graphical icon representing the location of interest, is rendered on the display at the position estimated by the processing module.

5. The pose estimation system of claim 4, further comprising a radio configured to send and receive geo-spatial data relating to objects in the environment, wherein the processing module receives, stores and uses the geo-spatial data to render graphical user interface information on the display comprising a graphical icon representing some of the objects in the environment.

6. The pose estimation system of claim 1, wherein the processing module implements a baseline GPS/INS to estimate the position of the location of interest, and adjusts the GPS/INS values over time with absolute orientation information.

7. The pose estimation system of claim 1, wherein the plurality of sensors comprises a 3-axis magnetometer, and wherein the processing module compares the norm of data from the 3-axis magnetometer to the Earth's field strength to detect the presence or absence of magnetic disturbances and, if detected, rejects data from the magnetometer corresponding to the times of detected magnetic disturbances.

8. A method for providing an estimate of position for a location of interest relative to a display, the method comprising:
a. receiving sensor data from a plurality of sensors comprising delayed data measurements and data representing the spatial position and orientation of the display,
b. applying an Extended Kalman Filter (EKF) to the received data and geodetic coordinates of a location of interest, to estimate the position and orientation of the display, and calculating the location of interest relative to the position of the display based upon a retroactive adjustment of a previously rendered position estimate of the display, using the delayed data measurements, a recalculation of the estimated position, and a prediction of future positions by means of an adjust-update-predict (AUP) cycle wherein the position of the location of interest is the vector rdsd, calculated using the formula:


rdsd=(CneCbnCdb)T[rese−(repe+CneCbnrpdb)]


wherein rpdb is the position of the origin of the display coordinate system d with respect to the point of origin of the body coordinate system p, expressed in body system coordinates b; rese is the position vector of the location of interest s in the environment with respect to the origin of the earth centered, earth fixed coordinate system e, expressed in e coordinates, derived from the geodetic coordinates of the location of interest in a database; and repe is the position of location p with respect to the origin of e, expressed in e coordinates, wherein Cne is an orientation of a North-East-Down (n) coordinate system with respect to e, wherein Cbn is an orientation of b with respect to the North-East-Down (n) coordinate system, and wherein Cdb is the orientation of the display relative to the body system coordinates b.

9. The method of claim 8, wherein the plurality of sensors comprise a camera, a 3-axis accelerometer, a 3-axis angular rate gyro, a 3-axis magnetometer, a barometric pressure sensor, and a GPS receiver.

10. The method of claim 8, rendering a graphical icon on the display at a position representing the location of interest relative to the position and orientation of the display.

11. The method of claim 8, further comprising sending and receiving geo-spatial data relating to objects in the environment, and calculating a position vector of an object in the environment using the estimated position and orientation of the display and the geo-spatial data relating to the object.

12. The method of claim 8, further comprising implementing a baseline GPS/INS to estimate the position of the location of interest, and adjusting the GPS/INS values over time with absolute orientation information.

13. The method of claim 8, wherein the plurality of sensors comprises a 3-axis magnetometer, and wherein the processing module compares the norm of data from the 3-axis magnetometer to the Earth's field strength to detect the presence or absence of magnetic disturbances and, if detected, rejects data from the magnetometer corresponding to the times of detected magnetic disturbances.

14. A pose estimation system for use with a display, the system comprising
a. means for receiving sensor data from a plurality of sensors comprising delayed data measurements and data representing the spatial position of the display,
b. means for applying an Extended Kalman Filter (EKF) to the received data and geodetic coordinates of a location of interest, to estimate the position and orientation of the display based upon a retroactive adjustment of a previously rendered position estimate using the delayed data measurements, and recalculating the estimated position and prediction of future positions, by means of an adjust-update-predict (AUP) cycle, and calculating the position of the location of interest relative to the position and orientation of the display wherein the position of the location of interest is the vector rdsd, calculated using the formula:


rdsd=(CneCbnCdb)T[rese−(repe+CneCbnrpdb)]


wherein rpdb is the position of the origin of the display coordinate system d with respect to the point of origin of the body coordinate system p, expressed in body system coordinates b; rese is the position vector of the location of interest s in the environment with respect to the origin of the earth centered, earth fixed coordinate system e, expressed in e coordinates, derived from the geodetic coordinates of the location of interest in a database; and repe is the position of location p with respect to the origin of e, expressed in e coordinates, wherein Cne is an orientation of a North-East-Down (n) coordinate system with respect to e, wherein Cbn is an orientation of b with respect to the North-East-Down (n) coordinate system, and wherein Cdb is the orientation of the display relative to the body system coordinates b.

15. The pose estimation system of claim 14, wherein the plurality of sensors comprise a camera, a 3-axis accelerometer, a 3-axis angular rate gyro, a 3-axis magnetometer, a barometric pressure sensor, and a GPS receiver.

16. The pose estimation system of claim 14, further comprising a means for rendering a graphical icon on the display, at the position of the location of interest relative to the position and orientation of the display.

17. The pose estimation system of claim 14, wherein the plurality of sensors comprises a 3-axis magnetometer, and wherein the processing module compares the norm of data from the 3-axis magnetometer to the Earth's field strength to detect the presence or absence of magnetic disturbances and, if detected, rejects data from the magnetometer corresponding to the times of detected magnetic disturbances.