Mobile platform positioning using satellite positioning system and visual-inertial odometry

A method of determining a trajectory of a mobile platform includes obtaining a satellite positioning system (SPS) measurement from one or more SPS signals acquired by an SPS receiver of the mobile platform. The method also includes obtaining a visual-inertial odometry (VIO) measurement of the mobile platform from a VIO system of the mobile platform. A first position estimate of the mobile platform is determined based, at least in part, on the SPS measurement and the VIO measurement. The method then includes adjusting the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the trajectory. The trajectory of the mobile platform is then determined, at least in part, using the smoothed position estimate.

FIELD OF DISCLOSURE

This disclosure relates generally to techniques for use in determining positioning information for a mobile platform, and in particular, but not exclusively, techniques for use in tracking movements of a mobile platform based, at least in part, on positioning information obtained from a Satellite Positioning Systems (SPS) and a visual-inertial odometry (VIO) system.

BACKGROUND

Mobile platforms offer increasingly sophisticated capabilities associated with the motion and/or position location sensing of the mobile platform. New software applications, such as, for example, those related to personal productivity, collaborative communications, social networking, and/or data acquisition, may utilize motion and/or position sensors to provide new features and services to consumers.

Such motion and/or position determination capabilities may be provided using Satellite Positioning Systems (SPS), such as a global positioning system (GPS). However, position determinations based on SPS measurements alone may have inherent errors on the order of a few meters. Such accuracy may not be sufficient for certain applications. In mobile platforms, position accuracy can be improved by augmenting measurements derived from SPS with other available sensors/systems.

One such system that may be available to the mobile platform is a Visual-Inertial Odometry (VIO) system. Certain example VIO systems may use information from consecutive or otherwise temporally-separated images obtained from one or more digital cameras to estimate displacements of the mobile platform from one or more previous positions. These displacement estimates may, in certain instances, be of very high quality, e.g., being off by less than 1%. While the errors in the displacement estimates of some VIO systems may be relatively small, they may accumulate over time, which may lead to a significant drift in position estimates. This may be particularly problematic in vehicular applications, such as Advanced Driver Assistance Systems (ADAS) or robotics applications, such as drone navigation, where a large drift may interfere with proper route planning and/or mapping.

SUMMARY

The following presents a simplified summary relating to one or more aspects and/or embodiments associated with the mechanisms disclosed herein for determining a trajectory using a satellite positioning system (SPS) and visual-inertial odometry (VIO). As such, the following summary should not be considered an extensive overview relating to all contemplated aspects and/or embodiments, nor should the following summary be regarded to identify key or critical elements relating to all contemplated aspects and/or embodiments or to delineate the scope associated with any particular aspect and/or embodiment. Accordingly, the following summary presents certain concepts relating to one or more aspects and/or embodiments relating to the mechanisms disclosed herein to smooth a trajectory of a mobile platform using SPS and VIO in a simplified form to precede the detailed description presented below.

According to one aspect, a method of determining a trajectory of a mobile platform includes obtaining a satellite positioning system (SPS) measurement from one or more SPS signals acquired by an SPS receiver of the mobile platform. The method also includes obtaining a visual-inertial odometry (VIO) measurement of the mobile platform from a VIO system of the mobile platform. A first position estimate of the mobile platform is determined based, at least in part, on the SPS measurement and the VIO measurement. The method then includes adjusting the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the trajectory. The trajectory of the mobile platform is then determined, at least in part, using the smoothed position estimate.

According to another aspect, an apparatus for determining a trajectory of a mobile platform includes means for obtaining a satellite positioning system (SPS) measurement from one or more SPS signals acquired by an SPS of the mobile platform and means for obtaining a visual-inertial odometry (VIO) measurement of the mobile platform from a VIO system of the mobile platform. The apparatus also includes means for determining a first position estimate of the mobile platform based, at least in part, on the SPS measurement and the VIO measurement and means for adjusting the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the trajectory. The apparatus further includes means for determining the trajectory of the mobile platform, at least in part, using the smoothed position estimate.

According to yet another aspect, an apparatus for determining a trajectory of a mobile platform includes at least one processor and at least one memory coupled to the at least one processor. The at least one processor and the at least one memory are configured to direct the apparatus to: (i) obtain a satellite positioning system (SPS) measurement from one or more SPS signals acquired by an SPS receiver of the mobile platform; (ii) obtain a visual-inertial odometry (VIO) measurement of the mobile platform from a VIO system of the mobile platform; (iii) determine a first position estimate of the mobile platform based, at least in part, on the SPS measurement and the VIO measurement; (iv) adjust the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the trajectory; and (v) determine the trajectory of the mobile platform, at least in part, using the smoothed position estimate.

According to another aspect, a non-transitory computer-readable storage medium includes computer-executable instructions recorded thereon. Executing the computer-executable instructions on one or more processors causes the one or more processors to: (i) obtain a satellite positioning system (SPS) measurement from one or more SPS signals acquired by an SPS receiver of a mobile platform; (ii) obtain a visual-inertial odometry (VIO) measurement of the mobile platform from a VIO system of the mobile platform; (iii) determine a first position estimate of the mobile platform based, at least in part, on the SPS measurement and the VIO measurement; (iv) adjust the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the trajectory; and (v) determine the trajectory of the mobile platform, at least in part, using the smoothed position estimate.

Other objects and advantages associated with the mechanisms disclosed herein to smooth a trajectory of a mobile platform described herein will be apparent to those skilled in the art based on the accompanying drawings and detailed description.

DETAILED DESCRIPTION

Various aspects are disclosed in the following description and related drawings. Alternate aspects may be devised without departing from the scope of the disclosure. Additionally, well-known elements of the disclosure will not be described in detail or will be omitted so as not to obscure the relevant details of the disclosure.

According to one aspect of the disclosure,FIG. 1illustrates an exemplary operating environment100for a mobile platform108that can determine its position using one or more techniques and determine (e.g., track) a trajectory of the mobile platform108within environment100. Embodiments are directed to a mobile platform108which may determine its position utilizing data from both a Satellite Positioning System (SPS)114and a Visual-Inertial Odometer (VIO) system116. The SPS measurements124generated by the SPS receiver114may include one or more range-rate measurements (e.g., GPS Doppler measurements), one or more pseudorange measurements, and/or one or more SPS velocity measurements. The range-rate measurements may contain information, such as GPS Doppler measurements, that allow a determination of a velocity of the mobile platform108. The pseudorange measurements may provide information about the distance between the SPS receiver114and a respective satellite. The SPS velocity measurements are representative of a velocity of the mobile platform108.

The VIO system116utilizes several sequential images120captured by a camera118to estimate a relative position, velocity, acceleration, and/or orientation of the mobile platform108. The camera118may include a single monocular camera, a stereo camera, and/or an omnidirectional camera. In operation, the VIO system116acquires the images120generated by the camera118in order to generate the VIO measurements128. In one aspect, the VIO system116may apply one or more image processing techniques to the images120, detect one or more features, match those features across multiple frames to construct an optical flow, and estimate motion of the mobile platform108based on the optical flow. The VIO system116then generates VIO measurements128. In one aspect, the VIO measurements128are VIO velocity measurements representative of an estimated velocity of the mobile platform108. In another aspect, the VIO measurements128are VIO displacement measurements representative of a positional displacement of the mobile platform108.

By combining the VIO measurements128with the SPS measurements124, the mobile platform108may increase the accuracy of position determinations of the mobile platform108. However, the SPS measurements124and the VIO measurements128may be each made with respect to separate coordinate systems. For example, the SPS measurements124may be with respect to a global reference frame126, such as an Earth-Centered, Earth-Fixed (ECEF) coordinate system, such as the WGS84 coordinate system used with GPS, while the VIO measurements128may be with respect to a separate local reference frame130. While the global reference frame126may be known and common to any system using the same satellite positioning network, the local reference frame130may depend, in part, on the specific orientation of the mobile platform108. That is, the local reference frame130may change depending on the position and/or orientation of the mobile platform108within environment100. Thus, in order to combine the VIO measurements128with the SPS measurements124, the mobile platform108may align local reference frame130with the global reference frame126. In one aspect, the mobile platform108determines one or more orientation parameters (e.g., rotation matrix) to align the local reference frame130with the global reference frame126based on the SPS measurements124and the VIO measurements128.

The operating environment100may contain one or more different types of wireless communication systems and/or wireless positioning systems. In the embodiment shown inFIG. 1, one or more space vehicles (SV) (e.g., Satellite Positioning System (SPS) satellites102a,102b) may be used as an independent source of position information for the mobile platform108. The SPS receiver114of mobile platform108may include one or more dedicated SPS receivers specifically designed to receive signals for deriving geo-location information from the SPS satellites102a,102b. In certain implementations, SPS receiver114(and also like example SPS receiver208ofFIG. 2) may be configured to acquire and make use of SPS signals from a plurality of SPS, separately or together. By way of a non-limiting example, an SPS receiver114/208may acquire SPS signals from one or more GNSS, such as, e.g., GPS, GLONASS, Galileo, etc., or from one or more regional navigation systems, such as, e.g., BeiDou (China), the Indian Regional Navigation Satellite System (IRNSS), etc., or some combination thereof.

The operating environment100may also include one or more Wide Area Network Wireless Access Points (WAN-WAPs)104a,104b, which may be used for wireless voice and/or data communication, and as another source of independent position information for the mobile platform108. The WAN-WAPs104a-104bmay be part of a wide area wireless network (WWAN), which may include cellular base stations at known locations, and/or other wide area wireless systems, such as, for example, Worldwide Interoperability for Microwave Access (WiMAX) (e.g., IEEE 802.16). The WWAN may include other known network components which are not shown inFIG. 1for simplicity. Typically, each of the WAN-WAPs104a-104bwithin the WWAN may operate from fixed positions, and provide network coverage over large metropolitan and/or regional areas.

The operating environment100may further include one or more Local Area Network Wireless Access Points (LAN-WAPs)106a,106b,106c, which may be used for wireless voice and/or data communication, as well as another independent source of position data. The LAN-WAPs can be part of a Wireless Local Area Network (WLAN), which may operate in buildings and perform communications over smaller geographic regions than a WWAN. Such LAN-WAPs106a-106cmay be part of, for example, Wi-Fi networks (802.11x), cellular piconets and/or femtocells, Bluetooth networks, etc.

The mobile platform108may derive position information from any one or more of the SPS satellites102a,102b, the WAN-WAPs104a-104b, and/or the LAN-WAPs106a-106c. Each of the aforementioned systems can provide an independent estimate of the position for the mobile platform108using different techniques. In some embodiments, the mobile platform108may combine the solutions derived from each of the different types of access points to improve the accuracy of the position data. When deriving position using the SPS satellites102a,102b, the mobile platform108may utilize a receiver specifically designed for use with the SPS that extracts position, using conventional techniques, from a plurality of signals transmitted by SPS satellites102a,102b.

SPS satellites102aand102bare part of a satellite system that typically includes a system of transmitters positioned to enable entities to determine their location on or above the Earth based, at least in part, on signals received from the transmitters. Such a transmitter typically transmits a signal marked with a repeating pseudo-random noise (PN) code of a set number of chips and may be located on ground-based control stations, user equipment and/or space vehicles. In a particular example, such transmitters may be located on Earth orbiting space vehicles (SVs). For example, a SV in a constellation of Global Navigation Satellite System (GNSS) such as Global Positioning System (GPS), Galileo, Glonass or Compass may transmit a signal marked with a PN code that is distinguishable from PN codes transmitted by other SVs in the constellation (e.g., using different PN codes for each satellite as in GPS or using the same code on different frequencies as in Glonass). In accordance with certain aspects, the techniques presented herein are not restricted to global systems (e.g., GNSS) for SPS. For example, the techniques provided herein may be applied to or otherwise enabled for use in various regional systems, such as, e.g., Quasi-Zenith Satellite System (QZSS) over Japan, Indian Regional Navigational Satellite System (IRNSS) over India, Beidou over China, etc., and/or various augmentation systems (e.g., an Satellite Based Augmentation System (SBAS)) that may be associated with or otherwise enabled for use with one or more global and/or regional navigation satellite systems. By way of example but not limitation, an SBAS may include an augmentation system(s) that provides integrity information, differential corrections, etc., such as, e.g., Wide Area Augmentation System (WAAS), European Geostationary Navigation Overlay Service (EGNOS), Multi-functional Satellite Augmentation System (MSAS), GPS Aided Geo Augmented Navigation or GPS and Geo Augmented Navigation system (GAGAN), and/or the like. Thus, as used herein satellite systems used herein may include any combination of one or more global and/or regional navigation satellite systems and/or augmentation systems, and SPS signals may include SPS, SPS-like, and/or other signals associated with such one or more SPS.

Furthermore, the disclosed method and apparatus may be used with positioning determination systems that utilize pseudolites or a combination of satellites and pseudolites. Pseudolites are ground-based transmitters that broadcast a PN code or other ranging code (similar to a GPS or CDMA cellular signal) modulated on an L-band (or other frequency) carrier signal, which may be synchronized with GPS time. Each such transmitter may be assigned a unique PN code so as to permit identification by a remote receiver. Pseudolites are useful in situations where GPS signals from an orbiting satellite might be unavailable, such as in tunnels, mines, buildings, urban canyons or other enclosed areas. Another implementation of pseudolites is known as radio-beacons. The term “satellite”, as used herein, is intended to include pseudolites, equivalents of pseudolites, and possibly others. The term “SPS signals,” as used herein, is intended to include SPS-like signals from pseudolites or equivalents of pseudolites.

When deriving position from the WWAN, each WAN-WAPs104a-104bmay take the form of base stations within a digital cellular network, and the mobile platform108may include a cellular transceiver and processor that can exploit the base station signals to derive position. Such cellular networks may include, but are not limited to, standards in accordance with GSM, CMDA, 2G, 3G, 4G, LTE, etc. It should be understood that digital cellular network may include additional base stations or other resources that may not be shown inFIG. 1. While WAN-WAPs104a-104bmay actually be moveable or otherwise capable of being relocated, for illustration purposes it will be assumed that they are essentially arranged in a fixed position.

The mobile platform108may perform position determination using known time-of-arrival (TOA) techniques such as, for example, Advanced Forward Link Trilateration (AFLT). In other embodiments, each WAN-WAP104a-104bmay comprise a WiMAX wireless networking base station. In this case, the mobile platform108may determine its position using TOA techniques from signals provided by the WAN-WAPs104a-104b. The mobile platform108may determine positions either in a stand-alone mode, or using the assistance of a positioning server110and network112using TOA techniques. Furthermore, various embodiments may have the mobile platform108determine position information using WAN-WAPs104a-104b, which may have different types. For example, some WAN-WAPs104a-104bmay be cellular base stations, and other WAN-WAPs104a-104bmay be WiMAX base stations. In such an operating environment, the mobile platform108may be able to exploit the signals from each different type of WAN-WAP104a-104b, and further combine the derived position solutions to improve accuracy.

When deriving position using the WLAN, the mobile platform108may utilize TOA techniques with the assistance of the positioning server110and the network112. The positioning server110may communicate to the mobile platform108through network112. Network112may include a combination of wired and wireless networks which incorporate the LAN-WAPs106a-106c. In one embodiment, each LAN-WAP106a-106cmay be, for example, a Wi-Fi wireless access point, which is not necessarily set in a fixed position and can change location. The position of each LAN-WAP106a-106cmay be stored in the positioning server110in a common coordinate system. In one embodiment, the position of the mobile platform108may be determined by having the mobile platform108receive signals from each LAN-WAP106a-106c. Each signal may be associated with its originating LAN-WAP based upon some form of identifying information that may be included in the received signal (such as, for example, a MAC address). The mobile platform108may then sort the received signals based upon signal strength, and derive the time delays associated with each of the sorted received signals. The mobile platform108may then form a message which can include the time delays and the identifying information of each of the LAN-WAPs, and send the message via network112to the positioning sever110. Based upon the received message, the positioning server110may then determine a position, using the stored locations of the relevant LAN-WAPs106a-106c, of the mobile platform108. The positioning server110may generate and provide a Location Configuration Indication (LCI) message or the like to the mobile platform108that includes a pointer to the position of the mobile platform108in a local coordinate system. The LCI message or other like messages may also include other points of interest in relation to the location of the mobile platform108. When computing the position of the mobile platform108, the positioning server110may take into account the different delays which can be introduced by elements within the wireless network.

The position determination techniques described above may be used for various wireless communication networks such as a WWAN, a WLAN, a wireless personal area network (WPAN), and so on. The term “network” and “system” may be used interchangeably. A WWAN may be a Code Division Multiple Access (CDMA) network, a Time Division Multiple Access (TDMA) network, a Frequency Division Multiple Access (FDMA) network, an Orthogonal Frequency Division Multiple Access (OFDMA) network, a Single-Carrier Frequency Division Multiple Access (SC-FDMA) network, a WiMAX (IEEE 802.16) network, and so on. A CDMA network may implement one or more radio access technologies (RATs) such as cdma2000, Wideband-CDMA (W-CDMA), and so on. Cdma2000 includes IS-95, IS-2000, and IS-856 standards. A TDMA network may implement Global System for Mobile Communications (GSM), Digital Advanced Mobile Phone System (D-AMPS), or some other RAT. GSM and W-CDMA are described in documents from a consortium named “3rd Generation Partnership Project” (3GPP). Cdma2000 is described in documents from a consortium named “3rd Generation Partnership Project 2” (3GPP2). 3GPP and 3GPP2 documents are publicly available. A WLAN may be an IEEE 802.11x network, and a WPAN may be a Bluetooth network, an IEEE 802.15x, or some other type of network. The techniques may also be used for any combination of a WWAN, WLAN and/or WPAN.

As used herein, mobile platform108may be a device such as a vehicle (manned or unmanned), a robot, a cellular or other wireless communication device, a personal communication system (PCS) device, a personal navigation device, a Personal Information Manager (PIM), a Personal Digital Assistant (PDA), a laptop or other suitable mobile device that is capable of capturing or otherwise obtaining images and navigating or supporting navigation or other like motion-based processes using measurements from one or more sensors. The term “mobile platform” is also intended to include devices which communicate with a personal navigation device (PND), such as by short-range wireless, infrared, wireline connection, or other connection—regardless of whether satellite signal reception, assistance data reception, and/or position-related processing occurs at the device or at the PND. Also, “mobile platform” is intended to include all devices, including wireless communication devices, computers, laptops, etc. which are capable of communication with a server, such as via the Internet, Wi-Fi, or other network, and regardless of whether satellite signal reception, assistance data reception, and/or position-related processing occurs at the device, at a server, or at another device associated with the network. Any operable combination of the above is also considered a “mobile platform.”

Furthermore, in one embodiment, the mobile platform108may be suitably linked to a vehicle through one or more communication interfaces (e.g., a Bluetooth interface, an RF antenna, a wired connection, etc.) that enable the mobile platform108to read SPS measurements124and/or VIO measurements128obtained by the vehicle, itself. Furthermore, an application program interface (API) that supports communication between the mobile platform108and a vehicle may make the SPS measurements124and/or VIO measurements128, obtained by the vehicle, available to the mobile platform108.

FIG. 2illustrates an example mobile platform200that may be used in an operating environment100that can determine position using one or more techniques and track a trajectory of the mobile platform200, according to one aspect of the disclosure. Mobile platform200is one possible implementation of mobile platform108ofFIG. 1.

The various features and functions illustrated in the diagram ofFIG. 2are connected together using a common data bus224which is meant to represent that these various features and functions are operatively coupled together. Those skilled in the art will recognize that other connections, mechanisms, features, functions, or the like, may be provided and adapted as necessary to operatively couple and configure an actual portable device. Further, it is also recognized that one or more of the features or functions illustrated in the example ofFIG. 2may be further subdivided or two or more of the features or functions illustrated inFIG. 2may be combined.

The mobile platform200may include one or more wireless transceivers202that may be connected to one or more antennas240. The wireless transceiver202may include suitable devices, hardware, and/or software for communicating with and/or detecting signals to/from WAN-WAPs104a-104c, and/or directly with other wireless devices within a network. For example, the wireless transceiver202may comprise a CDMA communication system suitable for communicating with a CDMA network of wireless base stations; however in other aspects, the wireless communication system may comprise another type of cellular telephony network, such as, for example, TDMA or GSM. Additionally, any other type of wide area wireless networking technologies may be used, for example, WiMAX (IEEE 802.16), etc. The wireless transceiver202may also include one or more local area network (LAN) transceivers that may be connected to one or more antennas240. For example, the wireless transceiver202may include suitable devices, hardware, and/or software for communicating with and/or detecting signals to/from LAN-WAPs106a-106c, and/or directly with other wireless devices within a network. In one aspect, the wireless transceiver202may include a Wi-Fi (802.11x) communication system suitable for communicating with one or more wireless access points; however in other aspects, the wireless transceiver202comprise another type of local area network, personal area network, (e.g., Bluetooth). Additionally, any other type of wireless networking technologies may be used, for example, Ultra Wide Band, ZigBee, wireless USB etc.

As used herein, the abbreviated term “wireless access point” (WAP) may be used to refer to LAN-WAPs106a-106cand/or WAN-WAPs104a-104b. Specifically, when the term “WAP” is used, it should be understood that embodiments may include a mobile platform200that can exploit signals from a plurality of LAN-WAPs106a-106c, a plurality of WAN-WAPs104a-104b, or any combination of the two. The specific type of WAP being utilized by the mobile platform200may depend upon the environment of operation. Moreover, the mobile platform200may dynamically select between the various types of WAPs in order to arrive at an accurate position solution. In other embodiments, various network elements may operate in a peer-to-peer manner, whereby, for example, the mobile platform200may be replaced with the WAP, or vice versa. Other peer-to-peer embodiments may include another mobile platform (not shown) acting in place of one or more WAP.

As shown inFIG. 2, mobile platform200may also include a camera204. Camera204may be a single monocular camera, a stereo camera, and/or an omnidirectional camera. In one aspect, camera204is calibrated such that the camera parameters (e.g., focal length, displacement of the optic center, radial distortion, tangential distortion, etc.) are known. Camera204is coupled to control unit210to provide images244to the control unit210.

The illustrated example of mobile platform200also includes a motion sensor206. Motion sensor206may be coupled to control unit210to provide movement and/or orientation information which is independent of motion data derived from signals received by the wireless transceiver202, the SPS receiver208, and the VIO system226.

By way of example, the motion sensor206may include an accelerometer (e.g., a MEMS device), a gyroscope, a geomagnetic sensor (e.g., a compass), an altimeter (e.g., a barometric pressure altimeter), and/or any other type of movement detection sensor. Moreover, the motion sensor206may include a plurality of different types of devices and combine their outputs in order to provide motion information. For example, the motion sensor206may use a combination of a multi-axis accelerometer and orientation sensors to provide the ability to compute positions in 2-D and/or 3-D coordinate systems.

A Satellite Positioning System (SPS) receiver208may also be included in the mobile platform200. The SPS receiver208may be connected to the one or more antennas242for receiving satellite signals. The SPS receiver208may comprise any suitable hardware and/or software for receiving and processing SPS signals. The SPS receiver208requests information and operations as appropriate from the other systems, and performs the calculations necessary to determine the mobile platforms200position using measurements obtained by any suitable SPS algorithm. In one aspect, SPS receiver208is coupled to control unit210to provide one or more SPS measurements246to the control unit210. In one example, the SPS measurements246are range-rate measurements, such as GPS Doppler range-rate measurements. In another example, SPS receiver208is configured to determine an SPS velocity of the mobile platform200based on the range-rate measurements such that the SPS measurements246are the SPS velocity measurements. In yet another example, the SPS measurements246are the pseudorange measurements that are representative of a distance from the SPS receiver208to a respective satellite (e.g.,102a,102b). That is, SPS measurements246may include the range-rate measurements by themselves, the SPS velocity measurements by themselves, the pseudorange measurements themselves, and/or any combination of the three.

Mobile platform200also includes a control unit210that is connected to and communicates with the wireless transceiver202, the camera204, the motion sensor206, the SPS receiver208, and user interface212, if present. In one aspect, the control unit210accepts and processes images244received from the camera204as well as SPS measurements246received from SPS receiver208. Control unit210may be provided by a processor214and associated memory220, hardware216, firmware218, and software222.

The processor214may include one or more microprocessors, microcontrollers, and/or digital signal processors that provide processing functions, as well as other calculation and control functionality. The processor214may also include memory220for storing data and software instructions for executing programmed functionality within the mobile platform200. The memory220may be on-board the processor214(e.g., within the same IC package), and/or the memory may be external memory to the processor214and functionally coupled over a data bus224. The functional details associated with aspects of the disclosure will be discussed in more detail below.

Control unit210may further include a Visual-Inertial Odometry (VIO) system226, a positioning module228, a position database230, and an application module232. VIO system226may be configured to generate VIO measurements248in response to the images244received from camera204. The positioning module228may be configured to determine a position of the mobile platform200based on one or more positioning techniques. For example, positioning module228may be configured to determine a position of the mobile platform200by combining the VIO measurements248with the SPS measurements246. The position database230may be configured to store and update the position and/or orientation of the mobile platform200. That is, as the control unit210determines a new position and/or orientation of the mobile platform200, the position database230may be updated. The updated position and orientation information may then be provided, e.g., by displaying a digital map with the new position on the display238or by providing additional navigation instructions on the display and/or via speaker234. The positions and/or orientations stored in the position database230may be representative of a trajectory of the mobile platform200as the mobile platform200moves through an environment (e.g., environment100). In combining the VIO measurements248with the SPS measurements246, the positioning module228may determine a displacement estimate of the mobile platform200based on one or more of the VIO velocity measurements. However, the data provided by the VIO system226may be subject to a VIO drift which may cause displacement errors in the estimated displacement of the mobile platform200. Accordingly, as will be discussed in more detail below, the positioning module228may compensate for this VIO drift utilizing one or more SPS measurements246. In one aspect, the positioning module228may compensate for the VIO drift by determining a trajectory of the mobile platform200such that the trajectory produced by the compensation is smooth. That is, the positioning module228may compensate for VIO drift by generating a trajectory such that large jumps in estimated positions are avoided. The requirement for the smoothness of a trajectory may be important in control applications, where relatively large jumps in position estimates could lead to instability in the route planning and control algorithms. In one example, positioning module228utilizes a smoothing parameter to adjust position estimates that provides a trade-off between the smoothness of the generated trajectory with a tracking error.

By way of example, without the smoothness constraints, the positioning module228may combine the SPS measurements246and the VIO measurements248(e.g., VIO velocity measurements) by using a filter, such as a Kalman or a particle filter, where xtis a first position estimate of the mobile platform200at time t estimated from the combined SPS and VIO measurements. Positioning module228may then adjust the first position estimate to generate a smoothed position estimate stat time t. In one aspect, the positioning module228generates the smoothed position estimate by minimizing a cost function that determines a trade-off between: (i) a difference between the smoothed position estimate stand the first position estimate xt(i.e., the tracking error), and (ii) the smoothness of the trajectory produced. The cost function utilized by positioning module228may be represented by:
∥st−xt∥2+λ∥st−st−1∥2EQ (1a)
where λ is the smoothing parameter, and st−1is a previous smoothed position estimate. The first term of equation (1), referred to herein as a tracking error, provides a constraint that the smoothed position estimate stbe relatively close to the first position estimate xt. The second term of equation (1) provides the smoothness constraint. In one example, the smoothing parameter λ is a fixed nonnegative number and may be chosen based on the particular application (e.g., ADAS, route planning, drone control system, etc.).

In another example, the VIO measurement248may be also used directly for the smoothing. Letting vtdenote the displacement as reported by the VIO system226at time t, the positioning module228may utilize the cost function
∥st−xt∥2+λ∥st−st−1−vt∥2.  EQ (1b)
The first term of equation (1b) is again the tracking error. The second term provides a constraint that the displacement of the smoothed position estimates be relatively close to those measured by the VIO system226. The smoothing parameter λ may again be chosen to trade off between these two conflicting constraints as explained above.

In some applications, the smoothed position estimate stmay be chosen by the positioning module228so as to minimize the expectation of a sum of the cost function of equations (1a) or (1b) over a horizon of T steps. For example, a value of T=1 corresponds to positioning module228taking only the cost at the current time t into consideration. For T=1, an optimal smoothed position estimate stfor cost equation (1a) may be represented by:

st=11+λ⁢(λ⁢⁢st-1+xt)EQ⁢⁢(2⁢a)
For T=1, an optimal smoothed position estimate stfor cost equation (1b) may be represented by:

Furthermore, in some applications, the positioning module228may obtain a predicted future position estimate of the mobile platform200from the SPS receiver208and/or from the VIO system226. In one example, the predicted future position estimate may be determined by a Kalman filter computing the position estimates. The positioning module228may then generate the smoothed position estimate stbased, in part, on the predicted future position estimate. For example, for T=2, the optimal smoothed position estimate stfor cost equation (1a) may be represented by:

st=11+λ+λ1+λ⁢(λ⁢⁢st-1+xt+λ1+λ⁢x^t+1)EQ⁢⁢(3⁢a)
where {circumflex over (x)}t+1is the predicted future position estimate of the mobile platform200at time t+1. for T=2, the optimal smoothed position estimate stfor cost equation (1b) may be represented by:

Returning now toFIG. 2, control unit210may further include an application module232. The application module232may be a process running on the processor214of the mobile platform200, which requests position information from the positioning module228. Applications typically run within an upper layer of the software architectures, and may include Indoor/Outdoor Navigation, Buddy Locator, Shopping and Coupons, Asset Tracking, and location Aware Service Discovery.

Processor214, VIO system226, positioning module228, and position database230are illustrated separately for clarity, but may be a single unit and/or implemented in the processor214based on instructions in the software222which is run in the processor214. Processor214, VIO system226, positioning module228can, but need not necessarily include, one or more microprocessors, embedded processors, controllers, application specific integrated circuits (ASICs), digital signal processors (DSPs), and the like. The term processor describes the functions implemented by the system rather than specific hardware. Moreover, as used herein the term “memory” refers to any type of computer storage medium, including long term, short term, or other memory associated with mobile platform200, and is not to be limited to any particular type of memory or number of memories, or type of media upon which memory is stored.

For a firmware and/or processor/software implementation, the processes may be implemented with modules (e.g., procedures, functions, and so on) that perform the functions described herein. Any non-transitory computer-readable medium tangibly embodying instructions may be used in implementing the processes described herein. For example, program code may be stored in memory220and executed by the processor214. Memory220may be implemented within or external to the processor214.

The mobile platform200may include an optional user interface212which provides any suitable interface systems, such as a microphone/speaker234, keypad236, and display238that allows user interaction with the mobile platform200. The microphone/speaker234provides for voice communication services using the wireless transceiver202. The keypad236comprises any suitable buttons for user input. The display238comprises any suitable display, such as, for example, a backlit LCD display, and may further include a touch screen display for additional user input modes.

FIG. 3illustrates an example process300of determining a trajectory of a mobile platform, according to one aspect of the disclosure. Process300may be performed by mobile platform108ofFIG. 1and/or mobile platform200ofFIG. 2.

In a process block302, positioning module228obtains SPS measurements246from one or more SPS signals acquired by SPS receiver208of the mobile platform200. In a process block304, the positioning module228obtains VIO measurements248from VIO system226. In operation, the VIO system226may utilize the images244generated by camera204as well as data provided by one or more of the motion sensors206(e.g., accelerometer and gyroscope) to generate VIO measurements248. The VIO measurements248generated by the VIO system226may, for example in certain implementations, comprise a vector of velocities and rotation matrices at each time instant along with estimates of the variances. The rate at which the VIO measurements248are generated by VIO system226may, for example in certain implementations, be about 100 per second, which may be much higher than the rate of the SPS measurements246generated by SPS receiver208, which may be, for example, about 1 per second. The rotation matrices included in the VIO measurements248may, for example, describe the camera204orientation at the current time instant with respect to an initial camera reference frame. In some aspects, certain example VIO measurements248may be very accurate and have a drift of around 1% as a function of distance, i.e., an error of 1 m over 100 m. In certain example implementations, some or all VIO measurements248may be with respect to a local reference frame130, while the range-rate measurements obtained from SPS receiver208will likely be with respect to a global reference frame126. Thus, positioning module228may be configured to determine an orientation parameter, such as a rotation matrix, to align the local reference frame130with the global reference frame126. In one aspect, aligning the local reference frame130with the global reference frame126includes translating the VIO measurements248to the global reference frame126based on the rotation matrix.

In process block306, the positioning module228may determine a first position estimate of the mobile platform200based, at least in part, on the SPS measurement246and the VIO measurement248. By way of example, the positioning module228may combine the SPS measurements246obtained from the SPS receiver208with the VIO measurements248in order to determine a first position estimate of the mobile platform200. Combining the SPS measurements246with the VIO measurements248may include determining a positional displacement of the mobile platform200between a current time epoch and a previous time epoch based on the VIO velocity measurements. The displacement may be applied to propagate previous tentative positions to the current time epoch such that the positioning module228may apply one or more algorithms to determine the initial position estimate. In some aspects, positioning module228may be configured to detect and discard one or more outliers in the SPS measurements246based, in part, on the VIO velocity measurements.

In process block308, the positioning module228adjusts the first position estimate to generate a smoothed position estimate based, in part, on a smoothing parameter that controls a smoothness of the produced trajectory of the mobile platform200. As mentioned above, the smoothness of a trajectory may be important in control applications, where relatively large jumps in position estimates could lead to instability in the route planning and control algorithms. Thus, in some aspects, the positioning module228adjusts the first position estimate such that a difference between the smoothed position estimate and a previous smoothed position estimate is less than a difference between the first position estimate and the previous smoothed position estimate. In process block310, the positioning module228determines the trajectory of the mobile platform200, at least in part, using the smoothed position estimate. In one aspect, determining the trajectory in process block310may include storing the smoothed position estimate to position database230and/or updating an orientation of the mobile platform200.

FIG. 4illustrates an example process400of adjusting a first position estimate to generate a smoothed position estimate, according to one aspect of the disclosure. Process400is one possible implementation of process block308ofFIG. 3. In process block402, the positioning module228determines a difference between the smoothed position estimate and the first position estimate (e.g., st−xtof equations 1a or 1b). In process block404, the positioning module228determines the smoothness of the trajectory based, in part, on the smoothed position estimate and the smoothing parameter. For example, in one aspect, the positioning module228may determine the smoothness of the trajectory by applying the smoothing parameter to a difference between the smoothed position estimate and a previous smoothed estimate (e.g., λ∥st−st−1∥2of equation (1a)). In another example, process block404may include the positioning module228determining the smoothness of the trajectory by applying the smoothing parameter to a difference between: (i) the smoothed position estimate and a previous smoothed estimate; and (ii) a VIO displacement measurement included in the VIO measurement248(e.g., λ∥st−st−1vt∥2of equation (1b)). In process block406, the positioning module228minimizes a cost function, such as equations (1a or 1b), that determines a trade-off between: (i) the difference (st−xt) calculated in process block402and (ii) the smoothness of the trajectory calculated in process block404.

Following is an example implementation of one or more procedures implemented by a positioning module228in determining a trajectory of a mobile platform200that includes utilizing a smoothing parameter to control a smoothness of the produced trajectory. First, let {xt}tϵNbe an M-dimensional vector-valued stochastic Markov process. For example, consider the first-order autoregressive model:
xtFt−1xt−1+ztEQ (4)
for t∈{2, 3, . . . }, with initial value x1z1. The {ztare independent Gaussian vector with mean μtand covariance matrix Σt. The parameters Ft, μt, and μtare assumed to be known.

Positioning module228may attempt to smooth the stochastic process {xt}. At each time t, the positioning module228produces a smoothed position estimate stof xt, having access to the first position estimate xt, and the past smoothed position estimates st−1. The quality of the smoothed position estimate stmay be measured by the cost function:
c(st−1,xt,st)∥st−xt∥2+λ∥st−st−1∥2EQ (5)
for some fixed smoothing parameter λ≥0. As with equation (1a), discussed above, the first term of equation (5) measures the deviation from the value xnand thus captures the requirement that sttrack xt. The second term of equation (5) measures the deviation from the previous smoothed value st−1and thus captures the requirement that stbe smooth. The smoothing parameter λ determines the balance between these two conflicting requirements. In the following example, the initial state of the system is s00. The smoothness term (i.e., λ∥st−st−1∥2) of equation (5) couples the values of stacross time. As a result, when choosing stat time t, the expected future behavior of the stochastic process {xt} may need to be taken into account.

For example, let M=1, and assume that the {xt} are independent and identically distributed random variables with mean zero and variance one. Fix a time t, and assume that st−1=0. At time t, assume a value of the first position estimate xt=10. Choosing a large value of the smoothed position estimate stwould yield two penalties. First, an immediate penalty of st2. Second, since the next value of xtis likely to be closer to zero, a large expected penalty(st+1−st)2. Thus, taking the predicted future position estimate st+1into account, positioning module228may choose a smaller value of the smoothed position estimate st(i.e., smaller than the first position estimate xt=10).

Thus, the positioning module228may minimize at each time t, the expected cost over the next T time steps, as follows:

minπ⁢∑τ=0T-1⁢Yτ⁢𝔼⁡(c⁡(st+τ-1,xt+τ,st+τ)❘xt,st-1),EQ⁢⁢(6)
where the optimization is over all smoothing policies π={πt} and with st+τπt+τ(st+τ1,xt+τ). Here, γ∈(0,1] is a fixed discount factor. For some applications, such as in drone positioning, the case T=2 with one-step look ahead may be adequate, but any number of T may be utilized in accordance with the teachings herein.

Accordingly, with a look ahead of T−1, the positioning module228may determine the smoothed position estimate st(T)according to:

From equation (7) it can be seen that solving for the smoothed position estimate is a convex combination of the past smooth position estimate st−1, the first position estimate xt, and the predicted future position estimate(xt+τ∥xt), with weights determined as a function of the smoothing parameter λ and a discounting parameter γ.

If the alternate cost function (1b) is used, then with a look ahead of T−1, the positioning module228may determine the smoothed position estimate st(T)according to:

st(T)=1W(T)⁢(λ⁡(st-1+vt)+⁢∑τ=0T-1⁢wτ(T)⁢𝔼⁡(xt+τ-vt+1-…-vt+τ❘xt,vt))EQ⁢⁢(9)
For a large horizon T (e.g., T>2), the quantity w(T)has a limit, represented by

W(∞)⁢⩵△⁢limT→∞⁢W(T)=12⁢(1+(1+γ)⁢λ+(1+(1+γ)⁢λ)2-4⁢γλ2EQ⁢⁢(10)
where convergence to this limit is exponentially fast. Thus,

wτ(∞)⁢⩵△⁢limT→∞⁢wτ(∞)=(γ⁢⁢λW(∞))τEQ⁢⁢(11)
for every τ∈{0,1, . . . }. The limiting weights wτ(∞)can be used as an approximation for wτ(T).

In some aspects, the positioning module228may take into account the steady-state behavior of the trajectory when determining the smoothed position estimate. For example, note that
(xt+τ|xt)=Gt,τxt+gt,τEQ (12)
for some deterministic Gt,τand gt,τ. Accordingly, equation (7) may be written as:

In equation (14), α is a function of T, but not of the time index t. Moreover, α∈[0,1) by definition of W(T). Thus, for ease of explanation define ytas:
ytHtxt+htEQ (15)
for all tϵ. Thus, in some aspects, the positioning module228may solve the recursion for the smoothed position estimate for st(T)according to equation (16), as follows:

FIGS. 5A-5Dillustrate example trajectories500of a mobile platform, according to various aspects of the disclosure. Trajectories500ofFIGS. 5A-5Drepresent possible trajectories tracked by mobile platform108and/or200. The illustrated trajectory500, ofFIG. 5A, includes a first position estimate502, a smoothed position estimate504, and a previous smoothed position estimate506. The first position estimate502is generated by the positioning module228based, in part, by combining the SPS measurements246and the VIO measurements248. The positioning module228then adjusts the first position estimate502by an amount512to generate the smoothed position estimate504. The smoothed position estimate504is then added to the trajectory500that is tracked by the mobile platform. As shown inFIG. 5A, the difference508between the smoothed position estimate504and the previous smoothed position estimate506is less than the difference510between the first position estimate502and the previous smoothed position estimate506. In some embodiments, the amount512that the positioning module228may adjust the first position estimate502may be limited by a fixed threshold that controls the maximum difference508between the smoothed position estimate504and the previous smoothed position estimate506. Providing a fixed threshold to control the maximum difference508may control the smoothness of the trajectory500, but may not provide the desired accuracy in the trajectory error of trajectory500. Thus, in some examples, the amount512that positioning module228adjusts the first position estimate502may be dynamic, based, in part, on a magnitude in change of the first position estimate502from the previous smoothed position estimate506.

For example, the first position estimate502ofFIG. 5B, illustrates a larger magnitude in difference514, when compared to difference510ofFIG. 5A. Thus, the first position estimate502, ofFIG. 5B, may be adjusted by an amount516, such that the difference518is greater than the difference508ofFIG. 5A. As discussed above, the difference512,516between the smoothed position estimate504and the first position estimate502may be referred to as the tracking error, where the smoothed position estimate504is determined by attempting to minimize this tracking error while still considering the desired smoothness of trajectory500.

FIG. 5Cillustrates the use of a predicted future position estimate520by positioning module228in determining the smoothed position estimate504. As shown inFIG. 5C, the trajectory500indicates a general movement of the mobile platform to the left (i.e., along the x-axis), while the first position estimate502indicates a displacement of the mobile platform to the left. Since the predicted future position estimate520indicates that the trajectory is predicted to continue to the left, the amount522of adjustment to the first position estimate502may be relatively small (depending, in part, on the desired smoothness of trajectory500). This is in contrast to the example shown inFIG. 5D, which illustrates a first position estimate502that indicates a displacement of the mobile platform to the right. The displacement of the first position estimate502, ofFIG. 5D, is inconsistent with the predicted future position estimate518which indicates continued movement to the left. Thus, in the example ofFIG. 5D, the positioning module228may adjust the first position estimate502by an amount524that is relatively large. That is, the amount524, ofFIG. 5D, may be larger than the amount522, ofFIG. 5C, for adjusting the first position estimate502to generate the smoothed position estimate504.

FIG. 6illustrates several sample aspects of components that may be employed in a mobile platform apparatus600configured to support determining a mobile platform trajectory, as taught herein. Mobile platform apparatus600is one possible implementation of mobile platform108ofFIG. 1and/or mobile platform200ofFIG. 2.

A module602for obtaining an SPS measurement from one or more SPS signals acquired by an satellite positioning system of the mobile platform may correspond at least in some aspects to, for example, a SPS receiver208and/or positioning module228ofFIG. 2. A module604for obtaining a visual-inertial odometry (VIO) measurements from a VIO system of the mobile platform may correspond at least in some aspects to, for example, VIO system226and/or positioning module228ofFIG. 2. A module606for determining a first position estimate of the mobile platform based, at least in part, on the SPS measurement and the VIO measurement may correspond at in some aspects to, for example, positioning module228and/or processor214, ofFIG. 2. A module608for adjusting the first position estimate to generate a smoothed position estimate based on a smoothing parameter that controls a smoothness of a trajectory may correspond at least in some aspects to, for example, positioning module228and/or processor214, ofFIG. 2. A module610for determining the trajectory of the mobile platform, at least in part, using the smoothed position estimate may correspond at least in some aspects to, for example, positioning module228, processor214, and/or position database230.

The functionality of the modules602-610ofFIG. 6may be implemented in various ways consistent with the teachings herein. In some designs, the functionality of these modules602-610may be implemented as one or more electrical components. In some designs, the functionality of these modules602-610may be implemented as a processing system including one or more processor components. In some designs, the functionality of these modules602-610may be implemented using, for example, at least a portion of one or more integrated circuits (e.g., an ASIC). As discussed herein, an integrated circuit may include a processor, software, other related components, or some combination thereof. Thus, the functionality of different modules may be implemented, for example, as different subsets of an integrated circuit, as different subsets of a set of software modules, or a combination thereof. Also, it will be appreciated that a given subset (e.g., of an integrated circuit and/or of a set of software modules) may provide at least a portion of the functionality for more than one module.

In addition, the components and functions represented byFIG. 6, as well as other components and functions described herein, may be implemented using any suitable means. Such means also may be implemented, at least in part, using corresponding structure as taught herein. For example, the components described above in conjunction with the “module for” components ofFIG. 6also may correspond to similarly designated “means for” functionality. Thus, in some aspects, one or more of such means may be implemented using one or more of processor components, integrated circuits, or other suitable structure as taught herein.

Those of skill in the art will appreciate that information and signals may be represented using any of a variety of different technologies and techniques. For example, data, instructions, commands, information, signals, bits, symbols, and chips that may be referenced throughout the above description may be represented by voltages, currents, electromagnetic waves, magnetic fields or particles, optical fields or particles, just to name a few examples.

Further, those of skill in the art will appreciate that the various illustrative logical blocks, modules, circuits, and algorithm steps described in connection with the aspects disclosed herein may be implemented as electronic hardware and computer software. To clearly illustrate this interchangeability of certain portions of hardware and software, various illustrative components, blocks, modules, circuits, and steps have been described above generally in terms of their functionality. Whether such functionality is implemented as part of the hardware or software portion of an embodiment may depend upon the particular application and design constraints imposed on the overall system. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted to depart from the scope of the present disclosure.

The methods, sequences and/or algorithms described in connection with the aspects disclosed herein may, by way of example, be embodied directly in hardware, firmware, or via one or more software modules in combination with such hardware and/or firmware. A software module may, by way of example, reside in RAM, flash memory, ROM, EPROM, EEPROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art. An exemplary storage medium is coupled to the processor such that the processor can read information from, and write information to, the storage medium. In the alternative, the storage medium may be integral to the processor. The processor and the storage medium may reside in an ASIC. The ASIC may reside in an IoT device. In the alternative, the processor and the storage medium may reside as discrete components in a user terminal.

In one or more exemplary aspects, the functions described may be implemented in hardware, software, firmware, or any combination thereof. If implemented in software, the functions may be stored on or transmitted over as one or more instructions or code on a computer-readable medium. Computer-readable media includes both computer storage media and communication media including any non-transitory medium that facilitates transfer of a computer program from one place to another. A storage media may be any available media that can be accessed by a computer. By way of example, and not limitation, such computer-readable media can comprise RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, or any other non-transitory medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer. Disk and disc, as used herein, includes CD, laser disc, optical disc, DVD, floppy disk and Blu-ray disc where disks usually reproduce data magnetically and/or optically with lasers. Combinations of the above should also be included within the scope of computer-readable media.