Distributed Estimation System

Abstract

A hybrid distributed estimation system (HDES) jointly tracks states of multiple moving devices configured to transmit measurements indicative of the states of the moving devices and estimates of the states of the moving devices derived from the measurements. The hybrid distributed estimation system (HDES) selects between the measurements and the estimates and, based on the selection, activates different types of distributed estimation systems (DES) configured to jointly track the states of the moving devices using different types of information. The hybrid distributed estimation system (HDES) then tracks the states using the activated distributed estimation systems (DES) and enables the states to be tracked by the different distributed estimation systems (DES) at different times.

Description

[Technical Field]

[0001] The present invention relates generally to distributed estimation systems (DES), and more particularly to jointly tracking the state of multiple mobile devices, each configured to transmit over a wireless communication channel to a hybrid distributed estimation system (HDES). [Background technology]

[0002] A Global Navigation Satellite System (GNSS) is a system of satellites that can be used to determine the geographic position of a mobile receiver relative to the Earth. Examples of GNSS include GPS, Galileo, Glonass, QZSS, and BeiDou. Various Global Navigation Satellite (GNS) correction systems are known that are configured to receive GNSS signal data from GNSS satellites, process these GNSS data, calculate GNSS corrections from the GNSS data, and provide these corrections to a mobile receiver with the goal of achieving faster and more accurate calculation of the geographic position of the mobile receiver.

[0003] Various position estimation methods are known in which position calculations are based on repeated measurements of so-called pseudorange and carrier phase observables by an Earth-based GNSS receiver. The "pseudorange" or "code" observable represents the difference between the time of transmission of a GNSS satellite signal and the time of local reception of this satellite signal, and therefore includes the geometric distance covered by the satellite's radio signal. Measuring the alignment between the carrier of a received GNSS satellite signal and a copy of such a signal generated inside the receiver provides another source of information for determining the apparent distance between the satellite and the receiver. The corresponding observable is called "carrier phase," which represents the integral of the Doppler frequency due to the relative motion of the transmitting satellite and the receiver.

[0004] Any pseudorange observation contains unavoidable error contributions, including receiver and transmitter clock errors, as well as additional delays caused by the atmosphere's non-zero refractive index, instrument delays, multipath effects, and detector noise. Any carrier phase observation also contains an unknown integer number of signal cycles, i.e., an integer number of wavelengths, that elapsed before lock-in to this signal alignment was achieved. This number is called the "carrier phase ambiguity." Typically, observables are measured, or sampled, by the receiver at discrete successive times. The time indices at which the observables are measured are called "epochs." Known positioning methods generally involve dynamic numerical estimation and correction schemes for range and error components based on measurements of the observables sampled at successive epochs.

[0005] When GNSS signals are tracked continuously and no loss of lock occurs, the integer ambiguity resolved at the beginning of the tracking phase can be maintained throughout the entire GNSS positioning span. However, GNSS satellite signals may occasionally be shadowed (e.g., due to buildings in an "urban canyon" environment) or momentarily blocked (e.g., when the receiver passes under a bridge or through a tunnel). Generally, in such cases, the integer ambiguity value is lost and must be re-determined. This process may take several seconds to several minutes. In fact, the presence of significant multipath errors or unmodeled systematic biases in one or more measurements of either pseudorange or carrier phase can make ambiguity resolution difficult with current commercially available positioning systems. As receiver separation (i.e., the distance between the reference receiver and the mobile receiver whose position is being determined) increases, distance-dependent biases (e.g., orbital errors and ionospheric and tropospheric effects) increase, and as a result, reliable ambiguity resolution (or reinitialization) becomes even more challenging. Furthermore, loss of lock can also be caused by a discontinuity in the receiver's continuous phase lock to the signal, called a cycle slip. For example, a cycle slip can be caused by a power loss, a receiver software failure, or a malfunctioning satellite oscillator. Additionally, a cycle slip can be caused by changing ionospheric conditions.

[0006] GNSS enhancement refers to techniques used to improve the accuracy of positioning information provided by the Global Positioning System or other global navigation satellite systems, generally the network of satellites used for navigation. For example, some methods use differencing techniques based on satellite-to-satellite differencing, receiver-to-receiver differencing, epoch-to-epoch differencing, and combinations thereof. Single and double differences between satellites and receivers reduce error sources but do not eliminate them.

[0007] Therefore, there is a need to improve the accuracy of GNSS positioning. To address this problem, several different methods use the cooperation of multiple GNSS receivers to improve the accuracy of GNSS positioning. However, to properly cooperate, the multiple GNSS receivers need to be synchronized and their operation needs to be constrained. For example, U.S. Patent 9,476,990 describes cooperative GNSS positioning estimation by multiple mechanically connected modules. However, such constraints on the cooperative improvement of GNSS positioning accuracy are not always practical. Summary of the Invention

[0008] Some embodiments are based on the recognition that current methods for tracking the state of a moving device, such as a vehicle, assume either individual or centralized estimation based on internal modules of the vehicle, or distributed estimation, which performs state estimation in a tightly controlled and / or synchronized manner. Examples of such distributed estimation include distributed systems that estimate the vehicle's state by determining different surfaces of the state track and reaching a consensus, unbalanced systems that independently track the system's state while one type of track dominates over another, and distributed systems that include multiple synchronized receivers, preferably located at a fixed distance from each other.

[0009] Some embodiments appreciate the benefits of cooperative state tracking when internal modules of a moving vehicle use some additional information determined externally. However, some embodiments recognize that such external information is not always available. Thus, a need exists for cooperative tracking where the tracking is performed by internal modules of the vehicle but can seamlessly integrate external information when it is available.

[0010] Some embodiments are based on the recognition that collaborative estimation can be performed at a central computing node, or the estimation can be performed completely decentralized, or it can be a distributed joint approach. Some embodiments are based on the recognition that different applications require different types of information, and further, even if an application at a particular time step can optimally utilize one type of information, such optimal type of information may change from one time step to the next.

[0011] To that end, some embodiments disclose a hybrid distributed estimation system (HDES) that utilizes at least two types of information. For example, the information utilized by the HDES includes estimates from local filters running on each mobile device. Additionally or alternatively, the information utilized by the HDES includes measurements for each mobile device, which may be obtained using sensors physically or operably connected to the mobile device. In this manner, the HDES can select the best information about the current state of the merged tracking. The type of best information to be used by the DES depends on a number of factors, including the type of environment, the type of mobile device, the quality of the measurements, the quality of the estimates, and how long estimations at the DES and mobile device have been ongoing.

[0012] Other embodiments decide whether to run DES completely or revert to using only local estimates because DES only makes the estimation worse in certain settings. For example, if the measurement noise between two sensors is correlated, meaning there is a relationship between them, but the DES does not know this, the DES may end up producing an estimate with more uncertainty than a local estimator would provide. For example, for a particular type of sensor, if the delay in sending any measurements to the DES is large relative to the time the estimator was run, the estimate will be used.

[0013] Thus, some embodiments use these and / or other factors to determine which information to use at a particular time step. This allows for the best possible estimate at a given time. Additionally or alternatively, some embodiments recognize that when switching between information types, different DESs must be used because it is impractical to provide a general-purpose DES that can handle any type of measurement without significant modification. For example, one embodiment switches between a measurement-sharing probability filter and an estimate-sharing consensus-based distributed Kalman filter (DKF).

[0014] Accordingly, one embodiment discloses a hybrid distributed estimation system (HDES) for jointly tracking the states of multiple mobile devices, each of which is configured to transmit to the HDES over a wireless communication channel one or a combination of measurements indicative of the state of the mobile device and estimates of the state of the mobile device derived from the measurements.

[0015] The HDES includes: a memory configured to store a first distributed estimation system (DES) configured, when activated, to jointly track a state of a mobile device based on measurements of the state of the mobile device; and a second DES configured, when activated, to jointly track the state of the mobile device based on estimates of the state of the mobile device; a receiver configured to receive multiple types of information from a plurality of mobile devices via a communication channel, the types of information including one or a combination of a first type for measurements of the state of the mobile device and a second type for estimates of the state of the mobile device, the HDES further including: a processor configured to select between the first type of information and the second type of information, activate the first DES or the second DES based on the selected type of information, and jointly estimate the state of the mobile device using the activated DES; and a transmitter configured to transmit at least one or a combination of the selected type of information and the jointly estimated state of the mobile device to the mobile device via the communication channel.

[0016] Another embodiment discloses a computer-implemented method for jointly tracking states of multiple mobile devices, the method using a processor coupled to a memory storing a first distributed estimation system (DES) configured, when activated, to jointly track states of the mobile devices based on measurements of the mobile devices' states and a second DES configured, when activated, to jointly track states of the mobile devices based on estimates of the mobile devices' states, the processor being coupled to stored instructions that implement the method, the instructions, when executed by the processor, including receiving multiple types of information from the multiple mobile devices over a communication channel, the types of information including one or a combination of a first type for measurements of the states of the mobile devices and a second type for estimates of the states of the mobile devices derived from the measurements, the instructions further including: selecting between the first type of information and the second type of information, activating the first DES or the second DES based on the selected type of information, and jointly estimating the states of the mobile devices using the activated DES; and transmitting at least one or a combination of the selected type of information and the jointly estimated states of the mobile devices to the mobile devices over the communication channel. [Brief explanation of the drawings]

[0017] [Figure 1A] 1 is a schematic diagram illustrating some embodiments of the present invention. [Figure 1B] 1 shows a schematic diagram of a Kalman filter (KF) used by some embodiments for state estimation of a mobile device. [Figure 1C] 1B shows an extension of the schematic diagram of FIG. 1A with an additional mobile device. [Figure 1D] 1B shows an extension of the schematic diagram of FIG. 1A with an additional mobile device. [Figure 2A] 1 shows a schematic diagram of a distributed estimation system (DES) according to some embodiments. [Figure 2B] 1 shows a schematic diagram of a distributed estimation system (DES) that utilizes two types of information. [Figure 3A] 1 illustrates a flowchart of a method for jointly tracking multiple moving devices using a Hybrid Distributed Estimation System (HDES), according to some embodiments. [Figure 3B] 1 shows an HDES for jointly tracking the state of multiple moving devices. [Figure 4A] 1 shows multiple autonomous, semi-autonomous, or manually driven vehicles in close proximity to one another. [Figure 4B] Shown is an urban canyon setting. [Figure 5A] 1 shows a flowchart of a method for determining the type of information used in an HDES. [Figure 5B] 1 illustrates a flowchart of a method for determining a performance gap, according to some embodiments. [Figure 5C] 1 illustrates a flowchart of a method for using a performance gap to select a type of information, according to some embodiments. [Figure 6] 1 shows a flowchart of a method for performing a selected first DES according to some embodiments. [Figure 7] 1 shows a flowchart of a method for performing a selected second DES according to some embodiments. [Figure 8A] 1 shows a simplified schematic diagram of the results of three iterations of a particle filter, according to some embodiments. [Figure 8B] The possible assigned probabilities of the five states in the first iteration of FIG. 8A are shown. [Figure 9A] 1 shows a schematic diagram of a Global Navigation Satellite System (GNSS) according to some embodiments. [Figure 9B] 1 illustrates various variables used alone or in combination in modeling the motion and / or measurement models, according to some embodiments. [Figure 10] 1 illustrates an example of vehicle-to-vehicle (V2V) communication and planning based on distributed state estimation according to one embodiment. [Figure 11]FIG. 1 is a schematic diagram of multi-vehicle platooning shaping for an accident avoidance scenario according to one embodiment. [Figure 12] FIG. 1 illustrates a block diagram of a system for direct and indirect control of a mixed autonomous vehicle, according to some embodiments. [Figure 13A] 1 shows a schematic diagram of a vehicle being directly or indirectly controlled according to some embodiments. [Figure 13B] 1 shows a schematic diagram of the interaction between a controller receiving control commands from the system and a controller of a vehicle, according to some embodiments. [Figure 14A] 1 shows a schematic diagram of a controller for controlling a drone, according to some embodiments. [Figure 14B] 1 illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. [Figure 14C] 1 illustrates communication between drones used to determine the drones' positions, according to some embodiments. [Figure 15] 1 shows a schematic diagram of components involved in multi-device motion planning, according to an embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0018] 1A is a schematic diagram illustrating some embodiments of the present invention. A mobile device 110 moves within an environment 100, which may or may not be known. For example, the environment 100 may be a road network, a manufacturing area, an office space, or a general outdoor environment. For example, the mobile device 110 may be any mobile device, including a road vehicle, an aircraft such as a drone, a mobile robot, a mobile phone, or a tablet. The mobile device 110 is connected to at least one of a set of sensors 105, 115, 125, and 135, which provide data 107, 117, 127, and 137 to the mobile device. For example, the data may include environmental information, location information, or velocity information, or any other information useful to the mobile device for estimating the state of the mobile device.

[0019] 1B shows a schematic diagram of a Kalman filter (KF) used by some embodiments for state estimation of a mobile device. The KF is a tool for state estimation of a mobile device 110 that can be represented by a linear state-space model. It is an optimal estimator when the noise source is known and Gaussian distributed, in which case the state estimate is also Gaussian distributed. The KF estimates the mean and variance of the Gaussian distribution because the mean and variance are sufficient statistics, which are the two required quantities to describe a Gaussian distribution.

[0020] The KF starts with initial knowledge of the state 110b and determines the state's mean and its variance 111b. The KF then uses a model of the system, such as a vehicle motion model, to predict 120b the state and variance for the next time step to obtain 121b an updated state mean and variance. The KF then uses measurements 130b in an update step 140b using a measurement model of the system, where the model relates sensing device data 107, 117, 127, and 137 to determine an updated state mean and variance 141b. An output 150b is then obtained and the procedure is repeated for the next time step 160b.

[0021] Some embodiments use probabilistic filters, including various variants of KFs, such as extended KFs (EKFs), linear regression KFs (LRKFs), unscented KFs (UKFs), etc. While multiple variants of KFs exist, they conceptually function as illustrated by FIG. 1B. In particular, a KF uses measurements 130b described by a probabilistic measurement model to update the first and second moments, i.e., the mean and covariance, of the target probability distribution. In some embodiments, the probabilistic measurement model is a multi-head measurement model 170b that is structured to satisfy the principles of measurement update in a KF according to some embodiments.

[0022] 1C and 1D show an extension of the schematic diagram of FIG. 1A when additional mobile devices 120 and 130 are present. Some embodiments are based on the understanding that when multiple mobile devices are near each other, more information can be obtained by merging the aggregate information than by having each mobile device use an estimator to estimate its own state independently of the other mobile devices. For example, device 130 obtains sensed data 147 from sensor 145 that neither device 110 nor device 120 receives. Similarly, device 120 receives data 129 from sensor 125 that device 130 does not receive. Thus, combining individual states to have a collaborative or distributed estimation can improve the estimation over having only individual estimations.

[0023] Some embodiments are based on the understanding that mobile devices can perform their estimations locally, based on information from sensors and also from surrounding mobile devices. For example, device 110 can perform its estimation based solely on sensors 105, 115, 125, and 135, which can additionally or alternatively include information 131, 121 coming directly from the mobile devices.

[0024] Some embodiments are based on the recognition that the collaborative estimation can either be performed at a central computing node, the estimation can be performed completely decentralized, or it can be a distributed joint approach.

[0025] FIG. 2A shows a schematic diagram of a distributed estimation system (DES) according to some embodiments. Mobile devices 220, 230, and 240 transmit data 227, 237, and 247 to a DES 210. The data may be determined locally or by some other entity, and the mobile devices simply redistribute it to the DES. Based on a merged motion model 205, which is subject to process noise, the DES can predict the time evolution of the mobile device's state. Additionally, based on a merged measurement model 215, which is subject to measurement noise, the DES updates the state based on data 227, 237, and 247 received from the mobile devices; however, the prediction and update can be performed in a manner similar to that shown in FIG. 1B.

[0026] Some embodiments are based on the recognition that different applications require different types of information, and furthermore, even if an application at a particular time step can optimally utilize a certain type of information, such an optimal type of information may change from one time step to the next.

[0027] FIG. 2B shows a schematic diagram of a distributed estimation system (DES) that utilizes two types of information 225, 235, 245, according to some embodiments.

[0028] In some embodiments, the information 225, 235, 245 sent to the DES is estimates from local filters running on each mobile device 220, 230, 240. In other embodiments, the information 225, 235, 245 sent is a measurement vector for each mobile device, obtained using sensors physically or operatively connected to the mobile device. The best type of information to use by the DES depends on a number of factors, including the type of environment, the type of mobile device, the quality of the measurements, the quality of the estimates, and how long the estimation at the DES and mobile device has been ongoing. Other embodiments decide whether to run the DES fully or revert to using only local estimates, since the DES only worsens the estimation in a particular setting.

[0029] For example, if the measurement noise between two sensors is correlated, meaning that there is a relationship between them, but the DES does not know about this, the DES may end up producing estimates with more uncertainty than a local estimator would provide.

[0030] For example, if for a particular type of sensor the delay in sending any measurements to the DES is large relative to the time the estimator is performed, it may be preferable to use the estimate.

[0031] Therefore, some embodiments use these factors to determine what information to use at a particular time step, thereby enabling the best possible estimate to be made at a given time.

[0032] Other embodiments recognize that when switching between information types, different DESs must be used because it is impractical to provide a general-purpose DES that can handle any type of measurement without major modifications. For example, one embodiment switches between a measurement sharing probabilistic filter and an estimate sharing consensus-based distributed Kalman filter (DKF).

[0033] 3A shows a flowchart of a method for jointly tracking multiple mobile devices using a hybrid distributed estimation system (HDES), according to some embodiments. In an embodiment, information is received from the mobile devices over a wireless communication channel. First, the method receives 310a information transmitted from a set of mobile devices over a wireless communication channel 309a, according to some embodiments. The information includes one or a combination of measurements of the mobile device's state and estimates of the mobile device's state. Using the information 315a, the method selects 320a a type of information to be used in the DES.

[0034] Using the determined type of information 329a, the method then determines 330a a DES to be performed using the selected type of information. For example, in one embodiment, the information is selected to be measurements about the mobile device, and the DES is a measurement-sharing Kalman filter. In another embodiment, the information is an estimated state, and the corresponding DES is a consensus-based distributed Kalman filter (DKF).

[0035] Using the determined DES 335a, the method performs DES 340a to generate estimated mobile device states 345a. Finally, the method transmits 350a the estimated states 345a, generating estimates 355a that are transmitted to each of the mobile devices.

[0036] 3B illustrates an HDES 300 for jointly tracking the state of multiple mobile devices, each configured to transmit one or a combination of mobile device state measurements and mobile device state estimates to the HDES 300 over a wireless communication channel. The HDES 300 includes a receiver 360 for receiving data 339. In one embodiment, the data includes mobile device state measurements. In another embodiment, the data includes mobile device state estimates, e.g., estimated means and covariances, and in yet another embodiment, the data includes a combination of state measurements and state estimates.

[0037] The HDES includes a memory 380 that stores (381) a first DES configured, when activated, to jointly track a state of a mobile device based on measurements of the state of the mobile device. The memory 380 also stores (382) a second DES configured, when activated, to jointly track a mobile device based on estimates of the state of the mobile device. For example, in one embodiment, the first DES is a measurement-sharing Kalman filter and the second DES is a consensus-based DKF. For example, in one embodiment, the first DES is a measurement-sharing Kalman filter and the second DES is a weighted DKF. In some embodiments, the first DES includes a model of the cross-correlation of measurement noise of measurements from the mobile device. In other embodiments, the cross-correlation is unknown and is estimated in the HDES based on the transmitted (339) noise and the positions of the sensors measuring the mobile device.

[0038] The memory 380 may also include a probabilistic motion model that relates prior beliefs about the vehicle's state to predictions of the moving device's state according to the probabilistic motion model, and a probabilistic measurement model that relates beliefs about the vehicle's state to measurements of the state of each moving device (383). The memory also stores instructions 384 related to how to determine which DES to perform. The memory may additionally or alternatively store the communication bandwidth required for the first DES and the second DES as a function of the state dimension and the measurement dimension (385).

[0039] The receiver 360 is configured to receive multiple types of information from multiple mobile devices over a communication channel, the types of information including one or a combination of a first type for measurements of the state of the mobile devices and a second type for estimates of the state of the mobile devices.

[0040] The receiver 360 is operatively connected (350) to a processor 330 configured to select (331) a first information type or a second information type. Based on the determined (331) information type, the processor is further configured to switch between activating and deactivating the first DES and the second DES based on the selected information type and to run the activated DES 332 to generate (333) an estimate of the state of the mobile device.

[0041] The HDES 300 includes a transmitter 320 operatively connected (350) to the processor 330. The transmitter 320 is configured to submit (309) the selected (331) type of information and the associated tracked state 333 of the mobile device estimated by the activated DES 332 to the mobile device over a communication channel.

[0042] In some embodiments, the submitted tracked state includes a first moment of the mobile device's state. In other embodiments, the information includes a first moment and a second moment of the mobile device. In other embodiments, the information includes higher-order moments to form a general probability distribution of the mobile device's state. In other embodiments, the information includes data indicative of an estimate of the mobile device's state and estimated noise, e.g., as samples, as the first moment and the second moment, or including higher-order moments. In still other embodiments, the time of receiving the information differs from the time the information was determined. For example, in some embodiments, an active remote server includes instructions for determining the first and second moments, and execution of such instructions, possibly coupled to communication times between the remote server and the RF receiver. To this end, in some embodiments, the information 309 includes timestamps of the times the first and second moments were determined.

[0043] In some embodiments, the first type of information includes measurements and measurement statistics of the expected distribution of the measurements, e.g., noise covariance in the case of assumed Gaussian noise. In other embodiments, the first type of information further includes information necessary to build a measurement model, e.g., the location of sensors measuring the mobile device, the time of measurement. In some embodiments, the second type of information includes a mean estimate of the state plus an associated covariance. In other embodiments, the second type further includes one or a combination of the time of estimation, higher-order moments, or samples of the state estimate, e.g., as output from a particle filter.

[0044] Some embodiments are based on the understanding that deciding to use a first type of information or a second type of information is not a one-time decision, and that a first type of information that is a preferred choice at a particular time step may be a least preferred choice at a future time step, or vice versa.

[0045] For example, if the cross-correlation between measurements across mobile devices is known to the HDES, then essentially, from an information-theoretic perspective, it is better to use the first type of information than the second type of information because the first type of information contains information about the correlation between mobile devices that the second type of information does not.

[0046] For example, if the cross-correlation between measurements across mobile devices is unknown to the HDES, the HDES does not provide extra information for using the first type of information over the second type of information. Furthermore, if the first type of information is used without the cross-correlation, such an estimate may lead to erroneous conclusions about the merged state estimate. Therefore, using the second type of information may be preferable.

[0047] Some embodiments are based on the understanding that the information relevant to determining the type of information to use changes over time, for example, the cross-correlation between measurements may be unknown at one time step but known at a later time step, which may warrant selecting a first type of information over a second type of information.

[0048] For example, the number of mobile devices included in the merged estimation may change over time, resulting in both the information provided by the first type of information and the second type of information changing over time. Additionally, the quality of the types of information may change differently over time, and thus the type of information to select may change over time.

[0049] As an example application, consider the setting of FIG. 4A, in which multiple autonomous, semi-autonomous, or manually driven vehicles are in each other's vicinity 410a. The vehicles transmit information to the HDES (420a). As the vehicles move, some vehicles move out of the merged area 410a and some other vehicles enter the area 410a, but the number of vehicles is not constant. The vehicles locally use a global navigation satellite system (GNSS) to track their status, and the first type of information includes GNSS measurements. Considering FIG. 4B, we can see that in certain settings, such as an urban canyon setting, the environment 470b limits the measurement quality of a particular satellite. For example, satellites 440b and 430b do not have direct paths 438b and 428b to receiver 404b, but their measurements experience multipath 439b and 429b. However, satellites 420b and 410b have direct paths to transmit their measurements 419b and 409b. Since the ratio of good measurements to bad measurements, meaning measurements with direct line of sight and measurements without direct line of sight, varies over time, the choice of using the first or second type of information will also vary over time.

[0050] To that end, some embodiments track changes in information quality over time and select between the first and second DES by comparing a quality measure to a threshold, such as signal-to-noise ratio (SNR), the presence, type, and extent of multipath signals, and the confidence of the measurements.

[0051] 5A shows a flowchart of a method 320a for determining the type of information 329a to be used in the HDES. Using the received information 315a, the method determines 510a a measurement model 515a that describes the relationship between the state of the mobile device and measurements of the state of the mobile device. Next, the method determines 520a whether the received information 315a is sufficient to determine the cross-covariance between measurements across the mobile device. If it is not possible to determine the cross-covariance, the method selects 525a a second type of information. If it is possible to determine the cross-covariance, the method determines 530a a cross-covariance 535a. Based on the cross-covariance 535a and the measurement model 515a, the method then determines 540a a performance gap between estimates using a first DES and estimates using a second DES, where the performance gap is determined based on the predicted output of the different DESs according to a system model 537a. The step 510a of determining a measurement model is based on a mathematical probabilistic representation of the relationship between the state of the mobile device and the measurements.

[0052]

number

[0053] In other embodiments, the measurement model is nonlinear according to y=h(x)+e, where h is the nonlinear relationship. Some embodiments linearize h to obtain C, while other embodiments express the model in its original nonlinear formula. In some embodiments, the received information 315a includes the information necessary to determine the measurement model, while in other embodiments, the information is already stored in memory 385.

[0054] To determine whether the cross-covariance can be calculated, the method checks the received information 315a for its content. For example, in some embodiments, the position of the sensor, along with a nominal noise value, is required to determine the cross-covariance. In other embodiments, the sensor position is fixed, and the cross-covariance can then be determined a priori and stored in memory 380. In other embodiments, the sensor is attached to a mobile device, and therefore the cross-covariance can be calculated from an estimate of the state of the mobile device.

[0055] Some embodiments determine the type of information based on one or a combination of the bandwidth of some of the communication channels and mobile devices, the correlation between measurements collected by the mobile devices, the expected difference between the merged state estimates of the first DES and the merged state estimates of the second DES, and the communication delay in transmitting the first type of information and the second type of information to the HDES.

[0056] To determine 540a the performance gap 545a, different embodiments proceed in several ways. FIG. 5B shows a flowchart of a method 540a for determining a performance gap, according to some embodiments. First, the method determines 510b whether a moving device should be considered a currently stationary device or a currently moving device. This can be advantageous because determining the performance gap is substantially easier if the device is stationary or nearly stationary. One embodiment determines movement by comparing measurements or estimates from a previous time step with current estimates or measurements, and considers a device to be currently moving if at least one of the devices has moved more than a threshold.

[0057] If the device is stationary (520b), the method proceeds to determine (530b) a performance matrix 535b. In some embodiments, the performance matrix is determined as a combination of the measurement model, the pooled measurement covariance without the cross-covariance inserted, and the pooled measurement covariance with the cross-covariance inserted.

[0058] For example, one embodiment determines the performance gap between a DES with a first type of information and a DES with a second type of information by determining the expected uncertainty, e.g., covariance, of the DES estimates.

[0059]

number

[0060] In other words, the performance gap is a combination of the correlation between measurements collected by the mobile device and the expected difference between the merged state estimates of the first DES and the merged state estimates of the second DES. If the measurement model is non-linear, a linearized approach can be taken, or a sampling-based approach can be taken, according to other embodiments.

[0061] Performance Gap J gap is a matrix that generally indicates the general performance gap between a DES using a first type of information and a DES using a second type of information. Some embodiments provide a method for evaluating the performance gap matrix, J gap Another embodiment is a second method for estimating the performance gap matrix, J gap By finding the norm of J gap Evaluate.

[0062] If the device is not quiescent (520b), the method proceeds to predicting (550b) the state performance (555b) of the first DES using the cross-covariance (535a) interpolated into the combined measurement covariance. Next, the method predicts (560b) the state performance (565b) of the state using the second DES. Using the determined performance, the performances are compared and then a performance gap (575b) is determined (570b).

[0063] In some embodiments, performance is defined as the predicted second moment of the DES, i.e., the estimated state covariance of the DES.

[0064]

number

[0065] In other embodiments, the recurrence is determined using an LRKF, and in other embodiments, the performance includes a predicted average of the estimates.

[0066] Using the determined performance for each DES, the method determines 579b the performance gap 575b. For example, one embodiment determines the performance gap by comparing the mean squared error assuming unbiased estimators, which is directly related to the covariance of such unbiased estimators.

[0067] 5C shows a flowchart of a method for selecting (550a) a type of information using a performance gap 545a, according to some embodiments. The method relies on a threshold value stored in memory 380 or determined during runtime. Using the performance gap 545a and the communication bandwidths 505c of the first DES and the second DES, the method determines (510c) a benefit 515c of using the second type of information with the first DES compared to using the second type of information with the second DES. The method then determines (520c) whether the benefit threshold 515c is met. If so, the method selects (540c) the first type of information 545c. Otherwise, the method selects (530c) the second type of information 535c.

[0068] In some embodiments, the benefit 515c is a weighted combination of the performance gap 545a and the communication bandwidth 505c. For example, in a setting where communication resources are very high, the communication bandwidth 505c gets a relatively small weight. In a setting where communication resources are low, the communication bandwidth 505c gets a relatively large weight.

[0069]

number

[0070] Some embodiments are based on the understanding that certain elements ij may be most important, for example, specific states of a few specific mobile devices may be of interest, which provides a method for determining the specific elements ij.

[0071] Other embodiments are based on the understanding that multiple element combinations ij may exist at times. In such cases, an embodiment may

[0072]

number

[0073] and using weightings between factor combinations to determine the selection of information types.

[0074] 3A, once an information type is selected, a corresponding DES 335a is determined (330a), and then that DES is executed. Some embodiments are based on the understanding that once a particular DES is selected for execution, it needs to be initialized. Other embodiments recognize that such initialization is better performed according to estimates of other DESs, since otherwise there would be switching between estimators.

[0075] FIG. 6 shows a flowchart of a method for executing a selected first DES 340a according to some embodiments. First, the method sets 610a an estimation model 615a, which includes setting the dimension of the state according to the number of mobile devices and the state to be estimated at each device; extracting a probabilistic motion model from memory and expanding it to the dimension of the state; and extracting a probabilistic measurement model from memory and expanding it to the dimension of measurements from the mobile devices, resulting in a probabilistic estimation model 615a. Next, the method initializes 620a a DES using a current estimated state 619a in the HDES to generate an initialized DES 627a. Finally, using the initialized DES and measurements 625a, the method executes 630a a first DES to generate a state estimate 635a.

[0076] Initialization 620a is performed by setting the initial estimate of the first DES to the estimate of the second DES. For example, if the estimator is an estimator that estimates the first two moments of the state, the most recent mean and covariance 619a are used to initialize the first DES (620a). For example, if particle filters are used in both DESs to estimate the HDES, the particles are set to align with each other.

[0077] 7 shows a flowchart of a method for executing a selected second DES 340a according to some embodiments. First, the method configures (710a) a connectivity model 715a, which includes setting the state dimensions according to the number of moving devices and the state to be estimated at each device; and extracting the communication topology from memory and weights to be used in fusing the estimates. Next, the method initializes (720a) the DES using the current estimated state 719a in the HDES to generate an initialized DES 727a. Finally, using the initialized DES, the method executes (730a) a second DES to generate a state estimate 735a.

[0078] In some embodiments, the first DES and the second DES use different types of estimators, e.g., the first DES estimates the first two moments and the second DES is a sampling-based estimator, or vice versa. In such cases, samples are used to determine the first two moments. Similarly, samples can be sampled from the first two moments in that order to initialize the sampling-based estimator.

[0079] Some embodiments implement the first DES as a KF. Other embodiments use an LRKF that functions in the spirit of a KF. Still other embodiments implement the first DES as a particle filter (PF).

[0080]

number

[0081]

number

[0082]

number

[0083] Still other embodiments recognize that the motion model and measurement model are not linear in all states. For example, for a moving device, possible states include the position, heading, and velocity of the moving device. However, with GNSS measurements, position is nonlinear in the measurement relationship, but heading and velocity are not. Therefore, various embodiments use a marginalized PF, which performs a PF on the nonlinear portion of the state vector, conditioned on the state trajectory, and a KF is performed, one for each particle.

[0084] Various embodiments employ different motion models, as many states may be nonlinear. However, some embodiments recognize that some nonlinearities are severe and others are mild. As a result, one embodiment implements the PF as a bounded particle filter, where severely nonlinear states are used in the PF and linear, mildly nonlinear states are approximated using an extended KF or LRKF.

[0085] 8A shows a simplified schematic diagram of the results of three iterations of a particle filter, according to some embodiments. An initial state 810a, which may be one of many samples or may span the state space, is predicted forward in time 811a using a model of motion, and five next states are 821a, 822a, 823a, 824a, and 825a. Probabilities are determined as a function of probabilistic measurements 826a and measurement noise 827a. At each time step, i.e., at each iteration, an aggregation of probabilities is used to generate an aggregated state estimate 820a.

[0086] Figure 8B shows the possible assigned probabilities of the five states in the first iteration of Figure 8A. These probabilities 821b, 822b, 823b, 824b, and 825b are reflected in selecting the sizes of the dots representing states 821a, 822a, 823a, 824a, and 825a.

[0087] Determining a sequence of probability distributions amounts to finding a distribution of probabilities, such as that in Figure 8B, for each time step in the sequence. For example, the distribution can be represented as a discrete distribution, such as in Figure 8B, or the discrete states associated with the probabilities can be made continuous using, for example, a kernel density smoother.

[0088]

number

[0089]

number

[0090]

number

[0091] In some embodiments, weights are used to fuse CDKF-like estimates as a weighted combination of estimates, where the weights optimize a weighted posterior covariance of the estimation errors.

[0092] 9A shows a schematic diagram of a GNSS system according to some embodiments. For example, an Nth satellite 902 transmits code and carrier-phase measurements (920 and 921) to a set of receivers 930 and 931. For example, receiver 930 is positioned to receive signals 910, 920 from N satellites 901, 903, 904, and 902. Similarly, receiver 931 is positioned to receive signals 921 and 911 from N satellites 901, 903, 904, and 902.

[0093] In various embodiments, GNSS receivers 930 and 931 may be of different types. For example, in the exemplary embodiment of FIG. 9A , receiver 931 is a base receiver whose location is known. For example, receiver 931 may be a ground-mounted receiver. In contrast, receiver 930 is a mobile receiver configured to move. For example, receiver 930 may be mounted on a mobile phone, automobile, or tablet device. In some implementations, second receiver 931 is optional and may be used to remove or at least reduce uncertainties and errors due to various sources, such as atmospheric effects and errors in the receiver and satellite internal clocks. In some embodiments, there are multiple GNSS receivers receiving code and carrier phase signals.

[0094] In some embodiments, there are multiple mobile GNSS receivers that are jointly tracked by the HDES. The first type of information is measurement information of code and carrier phase measurements.

[0095]

number

[0096] In some embodiments, the ambiguity is included in the vehicle state. Other embodiments also include bias states that account for residual errors in atmospheric delays, e.g., ionospheric delays. For receivers close enough to each other, the ionospheric delays are the same or very similar for different vehicles. Some embodiments exploit this relationship to resolve these delays and / or ambiguities.

[0097] Some embodiments may incorporate a carrier signal and a code signal into a measurement model.

[0098]

number

[0099] where e kis the measurement noise, h is the nonlinear part of the measurement equation that depends on the receiver position, n is the integer ambiguity, λ is the wavelength of the carrier signal, and y is the single or double difference between the combinations of satellites K.

[0100] In some embodiments, the stochastic filter uses the single difference (SD) and / or double difference (DD) in carrier phase to estimate the receiver state, which indicates the receiver's location. When a carrier signal transmitted from one satellite is received by two receivers, the difference between the first carrier phase and the second carrier phase is called the single difference (SD) in carrier phase. Alternatively, SD can be defined as the difference between signals from two different satellites arriving at a receiver. For example, this difference can originate from a first satellite and a second satellite, where the first satellite is called the base satellite. For example, the difference between signal 910 from satellite 901 and signal 920 from satellite 902 is a single SD signal, where satellite 901 is the base satellite. Using the receiver pair 931 and 930 of FIG. 9A, the difference between the SDs in carrier phase obtained from the wireless signals from the two satellites is called the double difference (DD) in carrier phase. When the carrier phase difference is converted to the number of wavelengths, e.g., λ = 19 cm, for example, for L1 GPS (and / or GNSS) signals, it is separated by a fractional part and an integer part. The fractional part can be measured by the positioning device, but the positioning device cannot directly measure the integer part. Therefore, the integer part is called the integer bias or integer ambiguity.

[0101] Generally, GNSS can simultaneously use multiple constellations to determine receiver state. For example, GPS, Galileo, Glonass, and QZSS can be used simultaneously. Satellite systems typically transmit information in up to three different frequency bands, and for each frequency band, each satellite transmits a code measurement and a carrier phase measurement. These measurements can be combined as either a single difference or a double difference, where single difference involves taking the difference between a reference satellite and another satellite, and double differencing involves differencing the receiver of interest with a base receiver with a known stationary position.

[0102]

number

[0103] In one embodiment, the original measurement model can be transformed by utilizing a base receiver b mounted at a known location that broadcasts to the original receiver r, eliminating most of the error sources. For example, one embodiment can calculate the difference between the two receivers 930 and 931 in FIG. 9A as

[0104]

number

[0105] and from which the error due to the satellite clock bias can be removed. Another embodiment forms a double difference between two satellites j and l. In this way, the clock error term due to the receiver can be removed. Furthermore, at a short distance between the two receivers (e.g., 30 km), the ionospheric error can be neglected, at least when centimeter accuracy is not required,

[0106]

number

[0107] Alternatively, one embodiment forms the difference between the two satellites 901 and 902 to arrive at the SD measurement.

[0108] Additionally or alternatively, some embodiments recognize that ignoring state biases, such as ionospheric errors, can lead to slight inaccuracies in the state estimates. This is because the biases are typically removed by single or double differencing of GNSS measurements. While this solution works well when the desired accuracy for the vehicle's position estimate is on the order of meters, it can be problematic when the desired accuracy is on the order of centimeters. Therefore, some embodiments include state biases in the vehicle's state and determine them as part of the state tracking provided by the probabilistic filter.

[0109] 10 illustrates an example of vehicle-to-vehicle (V2V) communication and planning based on distributed state estimates according to one embodiment. As used herein, a vehicle may be any type of mobile transportation system, including a passenger car, a mobile robot, or a rover. For example, a vehicle may be an autonomous vehicle or a semi-autonomous vehicle.

[0110] In this example, multiple vehicles 1000, 1010, 1020 are traveling on a given freeway 1001. Each vehicle can perform many movements. For example, the vehicles can stay on the same route 1050, 1090, 1080 or change routes (or lanes) 1060, 1070. Each vehicle has its own sensing capabilities, e.g., lidar, cameras, etc. Each vehicle has the possibility to send and receive information (1330, 1340) with its neighboring vehicles and / or exchange information indirectly through other vehicles via a remote server. For example, vehicles 1000 and 1080 can exchange information through vehicle 1010. In this type of communication network, information can be transmitted across large sections of the freeway or highway 1001.

[0111] Some embodiments are configured to address the following scenario: For example, vehicle 1020 wants to change its route and selects option 1070 in its route planning. However, at the same time, vehicle 1010 also chooses to change its lane and wants to follow option 1060. In this case, the two vehicles may collide, or at best vehicle 1010 will have to perform emergency braking to avoid colliding with vehicle 1020. This is where the present invention can be useful. To that end, some embodiments allow the vehicle to not only transmit what it senses at the current time t, but also, or alternatively, transmit what it senses at time T+δ. t It also allows you to send what you plan to do in

[0112] 10, after vehicle 1020 plans and commits to executing its plan to change lanes, it notifies vehicle 1010 of its plan. Thus, vehicle 1010 knows that in time interval δt, vehicle 1020 plans to move left 1070. Therefore, vehicle 1010 can select action 1090, i.e., stay in the same lane, instead of action 1060.

[0113] Additionally or alternatively, the vehicle motion can be jointly controlled by a remote server based on state estimates determined in a distributed manner. For example, in some embodiments, the vehicles determined for the joint state estimate are vehicles that may form, and potentially form, a platoon of vehicles that are jointly controlled for shared control purposes.

[0114] FIG. 11 is a schematic diagram of multi-vehicle platoon shaping for an accident avoidance scenario according to one embodiment. For example, consider a group of vehicles 1130, 1170, 1150, and 1160 traveling on freeway 1101. Now, suddenly, consider an accident in zone 1100 ahead of the vehicle platoon. This accident makes zone 1100 unsafe for vehicles to travel. Vehicles 1120 and 1160 sense the problem, for example, using cameras, and communicate this information to vehicles 1130 and 1170. The platoon then runs a distributed optimization algorithm, for example, a formation-keeping multi-agent algorithm, which selects the best shape for the platoon that avoids the accident zone 1100 and also maintains uninterrupted vehicle flow. In this illustrative example, the best shape for the platoon is to align and form a line (1195) to avoid zone 1100.

[0115] FIG. 12 shows a block diagram of a system 1200 for direct and indirect control of mixed autonomous vehicles, according to some embodiments. The system 1200 can be located on a remote server as part of an RSU to control passing mixed autonomous vehicles, including autonomous, semi-autonomous, and / or manually driven vehicles. The system 1200 can have several interfaces connecting the system 1200 to other machines and devices. A network interface controller (NIC) 1250 connects the system 1200 via a bus 1206 to a network 1290 connecting the system 1200 to the mixed automaton vehicles and includes a receiver adapted to receive traffic conditions of a group of mixed autonomous vehicles traveling in the same direction, the group of mixed autonomous vehicles including a controlled vehicle attempting to join the platoon and at least one uncontrolled vehicle, the traffic conditions representing the status of each vehicle in the group and the controlled vehicle. For example, in one embodiment, the traffic conditions include the current driving distance, current speed, and current acceleration of the mixed automaton vehicles. In some embodiments, the mixed automaton vehicles include all uncontrolled vehicles within a predetermined range of a controlled vehicle located to the side of the platoon.

[0116] NIC 1250 also includes a transmitter adapted to transmit control commands to controlled vehicles over network 1290. To that end, system 1200 includes an output interface, e.g., control interface 1270, configured to submit control commands 1275 to controlled vehicles in the group of mixed autonomous vehicles over network 1290. As such, system 1200 may be located on a remote server in direct or indirect wireless communication with the mixed autonomous vehicles.

[0117] The system 1200 may also include other types of input and output interfaces. For example, the system 1200 may include a human-machine interface 1210. The human-machine interface 1210 may connect the controller 1200 to a keyboard 1211 and a pointing device 1212, which may include, among other things, a mouse, a trackball, a touchpad, a joystick, a pointing stick, a stylus, or a touchscreen.

[0118] System 1200 includes a processor 1220 configured to execute stored instructions and a memory 1240 that stores instructions executable by the processor. Processor 1220 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. Memory 1240 may include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory machine. Processor 1220 may be connected to one or more input and output devices via a bus 1206.

[0119] The processor 1220 is operatively connected to memory storage 1230 for storing instructions and processing data used by the instructions. The storage 1230 may form part of or be operatively connected to the memory 1040. For example, the memory may be configured to store an HDES with a first DES and a second DES 1231 trained to track extended states of the mixed automaton vehicle and convert traffic states into target driving headway for the mixed autonomous vehicle, and to store one or more models 1233 configured to describe the vehicle's motion. For example, the models 1233 may include a motion model, a measurement model, a traffic model, etc.

[0120] Processor 1220 is configured to determine control commands for the controlled vehicles, which also indirectly control the uncontrolled vehicles. To do so, the processor is configured to execute control generator 1232 to determine the control commands based on the state of the vehicles. In some embodiments, control generator 1232 uses a deep reinforcement learning (DRL) controller trained to generate control commands from the augmented state for individual vehicles and / or platoons of vehicles.

[0121] 13A shows a schematic diagram of a directly or indirectly controlled vehicle 1301 according to some embodiments. As used herein, the vehicle 1301 may be any type of wheeled vehicle, such as a car, a bus, or a rover. The vehicle 1301 may also be an autonomous or semi-autonomous vehicle. For example, some embodiments control the movement of the vehicle 1301. Examples of movement include the lateral movement of the vehicle, which is controlled by the steering system 1103 of the vehicle 1301. In one embodiment, the steering system 1303 is controlled by a controller 1302 in communication with the system 1200. Additionally or alternatively, the steering system 1303 may be controlled by a driver of the vehicle 1301.

[0122] The vehicle may also include an engine 1306, which may be controlled by the controller 1302 or other components of the vehicle 1301. The vehicle may also include one or more sensors 1304 for sensing the surrounding environment. Examples of sensors 1304 include a distance range finder, radar, lidar, and a camera. The vehicle 1301 may also include one or more sensors 1305 for sensing its current momentum and internal state. Examples of sensors 1305 include a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, a shaft rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors provide information to the controller 1302. The vehicle may be equipped with a transceiver 1306 that enables communication capabilities of the controller 1302 through wired or wireless communication channels.

[0123] 13B shows a schematic diagram of the interaction between a controller 1302 receiving control commands from the system 1200 and a controller 1300 of a vehicle 1301, according to some embodiments. For example, in some embodiments, the controller 1300 of the vehicle 1301 is a steering controller 1310 and a brake / throttle controller 1320 that control the turning and acceleration of the vehicle 1300. In such a case, the controller 1302 outputs control inputs to the controllers 1310 and 1320 to control the state of the vehicle. The controller 1300 may also include a higher-level controller, e.g., a lane-keeping assist controller 1330, that further processes the control inputs of the predictive controller 1302. In both cases, the controller 1300 uses the output of the predictive controller 1302 to control at least one actuator of the vehicle, such as the steering wheels and / or brakes of the vehicle, to control the motion of the vehicle. The vehicle mechanical state x t can include position, orientation, and longitudinal / lateral velocity, and the control input u t may include lateral / longitudinal acceleration, steering angle, and engine / brake torque. State constraints for the system may include lane keeping constraints and obstacle avoidance constraints. Control input constraints may include steering angle constraints and acceleration constraints. Collected data may include position, orientation, and velocity profiles, acceleration, torque, and / or steering angle.

[0124] FIG. 14A illustrates a schematic diagram of a controller 1411 for controlling a drone 1400 according to some embodiments. FIG. 14A illustrates a schematic diagram of a quadcopter drone as an example of the drone 1400 according to an embodiment of the present disclosure. The drone 1400 includes an actuator for causing the drone 1400 to move and sensors for perceiving the environment and the location of the device 1400. The rotor 1401 may be an actuator, and the sensors for perceiving the environment may include a light detection and ranging (LIDAR) 1402 and a camera 1403. Furthermore, the sensors for localization may include a GPS or indoor GPS 1404. Such sensors may be integrated with an inertial measurement unit (IMU). The drone 1400 also includes a communication transceiver 1405 for transmitting and receiving information and a control unit 1406 for processing data obtained from the sensors and transceiver 1405, calculating commands for the actuator 1401, and calculating data to be transmitted via the transceiver 1405. In addition, it may include an estimator 1407 that tracks the state of the drone.

[0125] Further, based on the information transmitted by drone 1400, controller 1411 is configured to control the movement of drone 1400 by calculating a motion plan for drone 1400. The motion plan for drone 1400 may include one or more trajectories along which the drone will travel. In some embodiments, there are one or more devices (drones, such as drone 1400) whose movement is coordinated and controlled by controller 1411. Controlling and coordinating the movement of the one or more devices corresponds to solving a mixed integer optimization problem.

[0126] In different embodiments, controller 1411 obtains task parameters from drone 1400 and / or a remote server (not shown). The task parameters include the state of drone 1400, but may include more information. In some embodiments, the parameters may include one or a combination of an initial position of drone 1400, a target position of drone 1400, the geometry of one or more stationary obstacles that define at least a portion of the constraints, and the geometry and motion of moving obstacles that define at least a portion of the constraints. The parameters are submitted to a motion planner to obtain an estimated motion trajectory for performing the task, and the motion planner is configured to output the estimated motion trajectory for performing the task.

[0127] FIG. 14B illustrates a multi-device motion planning problem according to some embodiments of the present disclosure. FIG. 14B illustrates multiple devices (e.g., drone 1401b, drone 1401a, drone 1401c, drone 1401d) required to reach their assigned final positions 1402c, 1402b, 1402b, and 1402d. Additionally, obstacles 1403a, 1403b, 1403c, 1403d, 1403e, and 1403f in the surrounding environment of drones 1401a-1401d are illustrated. Drones 1401a-1401d are required to reach their assigned final positions 1402a-1402d while avoiding obstacles 1403a-1403f in the surrounding environment. A simple trajectory (such as trajectory 1404 shown in FIG. 14B) may result in collisions. Thus, embodiments of the present disclosure calculate trajectories 1405 that avoid obstacles 1403a-1403f and avoid collisions between drones 1401a-1401d, which may be achieved by avoiding overlapping trajectories or, in the case of overlapping trajectories (1406), by ensuring that corresponding drones reach their overlapping point at times sufficiently separated within the future planning time horizon.

[0128] FIG. 14C illustrates communication between drones used to determine their positions, according to some embodiments. For example, drone 1401b communicates its range to drone 1401c (1480b), which also communicates to drone 1401a (1480d). Drone 1401a communicates its range to drone 1401c (1480a), and drone 1401c communicates with drones 1401b and 1401d (1480b and 1480c). In some embodiments, communication occurs through a symmetric two-sided two-way ranging (SDS-TWR) method. In some embodiments, each drone estimates its own state and measures the distance to other drones through SDS-TWR. In other embodiments, state estimation for each drone occurs through self-localization and mapping (SLAM).

[0129] In other embodiments, at least one drone 1401c is wirelessly connected 1499c to a remote server 1440c via a transmit / receive interface. For example, in one embodiment, the HDES is located at 1440c, and the communication topology between the drones is part of the first and second types of information.

[0130] FIG. 15 shows a schematic diagram of components involved in multi-device motion planning, according to an embodiment. FIG. 15 is a schematic diagram of a system for coordinating the motion of multiple devices 1502. The multi-device planning system 1501 may correspond to the controller 1411 of FIG. 14A. The multi-device planning system 1501 receives information from at least one of the multiple devices 1502 and from the HDES 1505 via its corresponding communication transceiver. The multi-device planning system 1501 calculates a motion plan for each device 1502 based on the obtained information. The multi-device planning system 1501 transmits the motion plan for each device 1502 via the communication transceiver. The control system 1504 of each device 1502 receives the information and uses it to control the corresponding device hardware 1503.

[0131] The above-described embodiments of the present invention may be implemented in any of numerous ways. For example, embodiments may be implemented using hardware, software, or a combination thereof. If implemented in software, the software code may be executed on any suitable processor or collection of processors, whether provided on a single computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit with one or more processors within an integrated circuit component. However, a processor may be implemented using any suitable form of circuitry.

[0132] Also, the various methods or processes outlined herein may be coded as software executable on one or more processors using any one of a variety of operating systems or platforms. Additionally, such software may be written using any of a number of suitable programming languages and / or programming or scripting tools, and may be compiled as executable machine language code or intermediate code that runs on a framework or virtual machine. Typically, the functionality of the program modules may be combined or distributed as desired in various embodiments.

[0133] Also, embodiments of the present invention may be embodied as methods, for which examples are provided. The acts performed as part of the method may be ordered in any suitable manner. Thus, embodiments may be constructed in which acts are performed in an order different from that illustrated, which may include performing some acts shown as sequential acts in the exemplary embodiment simultaneously.

[0134] Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.

Claims

1. 1. A hybrid distributed estimation system (HDES) for jointly estimating states of a plurality of mobile devices, each of the mobile devices being configured to transmit to the hybrid distributed estimation system (HDES) over a wireless communication channel one or a combination of measurements indicative of a state of the mobile device and an estimate of the state of the mobile device at a next time step derived from the measurements, the hybrid distributed estimation system (HDES) comprising: a memory configured to store a first distributed estimation system (DES) configured, when activated, to jointly estimate the state of the mobile device based on the measurements of the state of the mobile device, and a second distributed estimation system (DES) configured, when activated, to jointly estimate the state of the mobile device based on the estimates of the state of the mobile device; a receiver configured to receive multiple types of information from the plurality of mobile devices over the communication channel, the types of information including one or a combination of a first type for the measurements of the states of the mobile devices and a second type for the estimates of the states of the mobile devices, the hybrid distributed estimation system (HDES) further comprising: a processor configured to select between the first type of information and the second type of information, activate the first distributed estimation system (DES) or the second distributed estimation system (DES) based on the selected type of information, and use the activated distributed estimation system (DES) to merge and estimate the state of the mobile device to obtain a merged state estimate; a transmitter configured to transmit at least one or a combination of the selected type of information and the merged state estimate to the mobile device over the communication channel; the first distributed estimation system (DES) activated to process the first type of information is a measurement sharing Kalman filter; A hybrid distributed estimation system (HDES), wherein the second distributed estimation system (DES) activated to process the second type of information is a distributed Kalman filter (DKF) including one or a combination of a consensus-based distributed Kalman filter (DKF) and a weighted distributed Kalman filter (DKF).

2. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor is configured to select the first type or the second type of information based on one or a combination of the bandwidth of the communication channel and the number of the mobile devices, the correlation between the measurements collected by the mobile devices, and the expected difference between the merged state estimate of the first distributed estimation system (DES) and the merged state estimate of the second distributed estimation system (DES).

3. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor tracks a measure of the quality of the measurements over time when the mobile device transmits the measurements of the state and selects between the first distributed estimation system (DES) and the second distributed estimation system (DES) by comparing the measure of the quality to a threshold, and the measure of the quality of the information includes one or a combination of a signal-to-noise ratio (SNR), the presence of multipath signals, and the reliability of the measurements.

4. The processor determines a performance gap between the estimate of the state of the mobile device using the first distributed estimation system (DES) with the first type of information and the estimate using the second distributed estimation system (DES) with the second type of information, and activates the first distributed estimation system (DES) or the second distributed estimation system (DES) based on the performance gap; the processor determines whether the mobile device is currently stationary or currently moving; When all of the moving devices are currently stationary, the processor determines the performance gap based on a performance matrix determined as one or a combination of a measurement model, a combined measurement covariance without a cross-covariance inserted, and a combined measurement covariance with the cross-covariance inserted; 2. The hybrid distributed estimation system (HDES) of claim 1, wherein if at least one of the mobile devices is currently moving, the processor estimates a first performance value of the first distributed estimation system (DES) using a cross-covariance of measurement noise inserted into a combined measurement covariance, estimates a second performance value of the second distributed estimation system (DES), and compares the first performance value with the second performance value to estimate the performance gap.

5. 5. The hybrid distributed estimation system (HDES) of claim 4, wherein the processor selects between activating the first distributed estimation system (DES) and activating the second distributed estimation system (DES) based on a weighted combination of the performance gap and a bandwidth of the communication channel.

6. To initialize the first distributed estimation system (DES) upon activation of the first distributed estimation system (DES), the processor: configured to set a dimension of the merged state of the moving devices according to the number of moving devices and the number of state variables in each of the moving devices; configured to retrieve a probabilistic motion model from the memory and extend the probabilistic motion model to the dimension of the merged state; configured to retrieve a probabilistic measurement model from the memory and extend the probabilistic measurement model to the dimension of the dimension of the merged state; configured to retrieve a current estimate of the second distributed estimation system (DES) from the memory and convert the current estimate of the second distributed estimation system (DES) into parameters of one or a combination of the probabilistic motion model and the probabilistic measurement model; The hybrid distributed estimation system (HDES) of claim 1 , configured to initialize one or a combination of the probabilistic motion model and the probabilistic measurement model based on the transformed parameters.

7. 7. The hybrid distributed estimation system (HDES) of claim 6, wherein the second distributed estimation system (DES) uses a particle filter such that the processor converts particle values of the particle filter into first and second moments of one or a combination of the probabilistic motion model and the probabilistic measurement model.

8. To initialize the second distributed estimation system (DES) upon activation of the second distributed estimation system (DES), the processor: configured to set a dimension of the merged state of the moving devices according to the number of moving devices and the number of state variables in each of the moving devices; configured to retrieve from the memory communication topology and weights for fusing the received estimates of different mobile devices; configured to retrieve current estimates of the first Distributed Estimation System (DES) from the memory and convert the current estimates of the first Distributed Estimation System (DES) into parameters of the second Distributed Estimation System (DES); The hybrid distributed estimation system (HDES) of claim 1 , configured to initialize parameters of the second distributed estimation system (DES) based on the transformed parameters.

9. 9. The hybrid distributed estimation system (HDES) of claim 8, wherein the first distributed estimation system (DES) is a stochastic filter that uses a stochastic motion model and a stochastic measurement model, and the second distributed estimation system (DES) uses a particle filter, and the processor samples one or a combination of the stochastic motion model and the stochastic measurement model to initialize particles of the particle filter.

10. The hybrid distributed estimation system (HDES) of claim 1 , wherein the moving device is a vehicle, and the vehicle is controlled based on the corresponding state of the vehicle transmitted by the transmitter.

11. The hybrid distributed estimation system (HDES) of claim 1 , wherein the moving devices are vehicles, and the vehicles are controlled based on the corresponding states of the vehicles as a platoon.

12. The hybrid distributed estimation system (HDES) of claim 1 , wherein the mobile devices include one or a combination of robots and drones.

13. 1. A computer-implemented method for jointly estimating states of a plurality of mobile devices, the method using a processor coupled to a memory storing a first Distributed Estimation System (DES) configured, when activated, to jointly estimate states of the mobile devices based on measurements of the states of the mobile devices, and a second Distributed Estimation System (DES) configured, when activated, to jointly estimate states of the mobile devices based on estimates of the states of the mobile devices, the processor coupled to stored instructions implementing the method, the instructions, when executed by the processor, performing steps of the method, the steps comprising: receiving, via a communication channel, multiple types of information from the plurality of mobile devices, the types of information including one or a combination of a first type for the measurements of the states of the mobile devices and a second type for estimates of the states of the mobile devices at a next time step derived from the measurements, the step further comprising: selecting between the first type of information and the second type of information, activating the first distributed estimation system (DES) or the second distributed estimation system (DES) based on the selected type of information, and estimating the state of the mobile device using the activated distributed estimation system (DES) to obtain a merged state estimate; transmitting at least one or a combination of the selected type of information and the merged state estimate to the mobile device over the communication channel; the first distributed estimation system (DES) activated to process the first type of information is a measurement sharing Kalman filter; A computer-implemented method, wherein the second distributed estimation system (DES) activated to process the second type of information is a distributed Kalman filter (DKF) including one or a combination of a consensus-based distributed Kalman filter (DKF) and a weighted distributed Kalman filter (DKF).

Images