Distributed Estimation Systems

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 states of multiple moving 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 the position calculation is 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 thus comprises the geometric distance covered by the satellite's radio signal. Measurement of 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" and 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, among them receiver and transmitter clock errors, as well as additional delays caused by the non-zero refractive index of the atmosphere, instrument delays, multipath effects, and detector noise. Any carrier phase observation further contains an unknown integer number of signal cycles, i.e., an integer number of wavelengths, that have elapsed before lock-in to this signal alignment is obtained. This number is called the "carrier phase ambiguity." Typically, the observables are measured, i.e., sampled, by the receiver at discrete successive times. The index of time at which the observables are measured is called an "epoch." Known position determination 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 in a tunnel). In general, in such cases, the integer ambiguity value is lost and must be determined again. This process may take from a few seconds to a few minutes. In fact, the presence of significant multipath errors or unmodeled systematic biases in one or more measurements of either pseudorange or carrier phase may make it difficult to resolve the ambiguity with current commercially available positioning systems. As the receiver separation (i.e., the distance between the reference receiver and the mobile receiver whose position is being determined) increases, the distance-dependent biases (e.g., orbit errors and ionospheric and tropospheric effects) increase, and as a result, reliable ambiguity resolution (or reinitialization) becomes even more of a challenge. Furthermore, loss of lock can also occur due to discontinuities in the receiver's continuous phase lock to the signal, called cycle slips. For example, cycle slips can be caused by power loss, receiver software failure, or a malfunctioning satellite oscillator. In addition, cycle slips 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 in general, a 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 differences 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 cooperate properly, 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 the accuracy of GNSS positioning are not always practical. Summary of the Invention

[0008] Some embodiments are based on the recognition that current methods of 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 state of the vehicle by determining different surfaces of the state tracking and reaching a consensus, unbalanced systems that track the state of the system independently while one type of tracking dominates over other types of tracking, 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 where internal modules of a moving vehicle use some additional information determined externally. However, some embodiments are based on the recognition that such external information is not always available. Thus, there is a need for cooperative tracking where the tracking is performed by internal modules of the vehicle, but where external information can be seamlessly integrated when it is available.

[0010] Some embodiments are based on the recognition that the collaborative estimation can be performed either 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 a certain type of information, such an 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 according to some embodiments. For example, the information utilized by the HDES includes estimates from local filters executed at each moving device. Additionally or alternatively, the information utilized by the HDES includes measurements for each moving device, which may be obtained using sensors physically or operatively connected to the moving device. In this way, the HDES can select the best information for 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 moving device, the quality of the measurements, the quality of the estimates, and how long the estimation at the DES and the moving device has been ongoing.

[0012] Other embodiments decide whether to run DES completely or fall back to using only local estimates since in certain settings DES only makes the estimation worse. For example, if the measurement noise between two sensors is correlated, meaning there is a relationship between them, but the DES does not know about this, the DES may end up producing an estimate that has more uncertainty than a local estimator would provide. For example, for a certain 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. In this way, it is possible to make the best possible estimate at a given time. Additionally or alternatively, some embodiments recognize that when switching between types of information, different DESs must be used, since 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 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 to jointly track a state of a mobile device based on measurements of the mobile device's state when activated, and a second DES configured to jointly track the state of the mobile device based on estimates of the mobile device's state when activated; 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 mobile device's state and a second type for estimates of the mobile device's state, 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 to jointly track states of the mobile devices based on measurements of the mobile devices' states when activated, and a second DES configured to jointly track states of the mobile devices based on estimates of the mobile devices' states when activated, the processor coupled to stored instructions implementing 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 mobile devices' states and a second type for estimates of the mobile devices' states 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 description 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 additional mobile devices. [Figure 1D] 1B shows an extension of the schematic diagram of FIG. 1A with additional mobile devices. [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] We present an HDES for jointly tracking the states of multiple moving devices. [Figure 4A] 1 shows multiple vehicles in close proximity to one another, driven autonomously, semi-autonomously, or manually. [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 selecting a type of information using a performance gap, 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 accident avoidance scenarios 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 that is directly or indirectly controlled according to some embodiments. [Figure 13B] FIG. 2 shows a schematic diagram of the interaction between a controller receiving controlled commands from the system and a controller of a vehicle, according to some embodiments. [Figure 14A] FIG. 1 illustrates 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] FIG. 1 illustrates a schematic diagram of components involved in multi-device motion planning, according to an embodiment. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0018] FIG. 1A is a schematic diagram illustrating some embodiments of the present invention. A mobile device 110 moves in 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 that provide data 107, 117, 127, and 137 to the mobile device. For example, the data may include environmental information, location information, or speed information, or any other information useful to the mobile device to estimate 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, and 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, since the mean and variance are sufficient statistics, which are the two required quantities to describe a Gaussian distribution.

[0020] Starting from initial knowledge 110b of the state, the KF determines the mean of the state 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 the 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 the sensing device data 107, 117, 127, and 137 to determine the 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 KF (EKF), Linear Regression KF (LRKF), Unscented KF (UKF), etc. Although multiple variants of KFs exist, they conceptually function as illustrated by FIG. 1B. In particular, the 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's 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 updates in KFs according to some embodiments.

[0022] 1C and 1D show an extension of the schematic diagram of FIG. 1A when there are additional moving devices 120 and 130. Some embodiments are based on the understanding that when multiple moving devices are in the vicinity of each other, more information is obtained when merging the collective information than when having each moving device use an estimator to estimate its own state independently of the other moving devices. For example, 130 obtains sensed data 147 from sensor 145 that neither 110 nor 120 receive. Similarly, device 120 receives data 129 from sensor 125 that 130 does not receive. Thus, when combining individual states to have a cooperative or distributed estimation, the estimation may be improved 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 only 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 may either be performed at a central computing node, the estimation may be performed completely decentralized, or it may 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, 240 transmit data 227, 237, 247 to the DES 210. The data may be determined at each mobile device or may be determined by some other entity, and the mobile device simply redistributes it to the DES. Based on the merged motion model 205, which is subject to process noise, the DES may predict the time evolution of the state of the mobile device. In addition, based on the merged measurement model 215, which is subject to measurement noise, the DES updates the state based on the data 227, 237, and 247 received from the mobile device, but the prediction and update may be performed in a similar manner to the principle 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 an estimate from a local filter 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 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 the estimation at the DES and mobile device has been ongoing. Other embodiments decide to run the DES completely or fall back to using only local estimates, since in certain settings the DES only makes the estimation worse.

[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 that have 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] Thus, some embodiments use these factors to determine what information to use at a particular time step, thereby allowing the best possible estimate to be made at a given time.

[0032] Other embodiments recognize that when switching between types of information, 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 moving devices using a hybrid distributed estimation system (HDES), according to some embodiments. In an embodiment, information is received from the moving devices over a wireless communication channel. First, the method receives 310a information from a wireless communication channel 309a transmitted from a set of moving devices, according to some embodiments. The information includes one or a combination of measurements of the state of the moving devices and estimates of the state of the moving devices. Using the information 315a, the method selects 329a 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 on the mobile device and the DES is a measurement sharing Kalman type filter. In another embodiment, the information is a state to be estimated and the corresponding DES is a consensus-based distributed Kalman filter (DKF).

[0035] Using the determined DES 335a, the method performs 340a DES 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 a HDES 300 for jointly tracking the states of multiple mobile devices, each of which is configured to transmit one or a combination of mobile device state measurements and mobile device state estimates to the DES 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 to jointly track a state of the moving device based on measurements of the state of the moving device when activated. The memory 380 also stores (382) a second DES configured to jointly track the moving device based on estimates of the state of the moving device when activated. 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 moving device. In other embodiments, the cross-correlation is unknown and is estimated in the HDES based on the transmitted (339) noise and the position of the sensor measuring the moving 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 by 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 on 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] 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 mobile device states and a second type for estimates of the mobile device states.

[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 activation and deactivation of 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 as estimated by an 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 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 an estimated noise, e.g., as samples, as the first moment and the second moment, or including higher moments. In still other embodiments, the time of receiving the information is different from the time the information was determined. For example, in some embodiments, an active remote server includes instructions for determining the first moment and the second moment, and execution of such instructions, possibly coupled to a communication time between the remote server and the RF receiver. To this end, in some embodiments, the information 309 includes a timestamp of the time the first moment and the second moment 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 Gaussian assumed noise. In other embodiments, the first type of information further includes information necessary to build a measurement model, e.g., the location of the sensor measuring the moving device, the measurement time. In some embodiments, the second type of information includes a mean estimate of the state, plus the associated covariance. In other embodiments, the second type further includes one or a combination of the time of the estimate, higher 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 the least preferred choice at some future time step, or vice versa.

[0045] For example, if the cross-correlation between measurements across moving devices is known to the HDES, then essentially, from an information theoretic perspective, it is better to use the first type of information over the second type of information because the first type of information contains information of the correlation between moving 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 cross-correlation, such an estimate may lead to erroneous conclusions of the merged state estimate. Therefore, it may be preferable to use the second type of information.

[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, 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. In addition, 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, where multiple vehicles, autonomous, semi-autonomous, or manually driven, 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 others enter the area 410a, but the number of vehicles is not constant. The vehicles use a global navigation satellite system (GNSS) locally to track their status, and the first type of information includes GNSS measurements. Considering FIG. 4B, it shows that in certain settings, such as an urban canyon setting, the environment 470b limits the measurement quality of certain satellites. For example, satellites 440b and 430b do not have direct paths 438b and 428b to receiver 404b, but their measurements go through multipaths 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 whether to use the first or second type of information will also vary over time.

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

[0051] FIG. 5A shows a flow chart of a method 320a for determining the type of information 329a 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 the measurements of the state of the mobile device. The method then determines 520a whether the received information 315a is sufficient to determine the cross-covariance between the 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 the estimate using the first DES and the estimate using the second DES, the performance gap being determined based on the predicted output of the different DES according to the 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 is required to determine the cross-covariance along with the nominal noise value. 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 moving device and therefore the cross-covariance can be calculated from an estimate of the state of the moving 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, an expected difference between an estimate of the merged state of the first DES and an estimate of the merged state of the second DES, and a 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. Figure 5B shows a flowchart of a method 540a for determining the performance gap, according to some embodiments. First, the method determines 510b whether the 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 the movement by comparing measurements or estimates from a previous time step with current estimates or measurements, and considers the device to be currently moving if at least one of the devices has moved beyond a threshold.

[0057] If the device is stationary (520b), the method proceeds with determining (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 having a first type of information and a DES having 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 the measurements collected by the mobile device and the expected difference between the estimate of the merged state of the first DES and the estimate of the merged state 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 Rate the following.

[0062] If the device is not quiescent (520b), the method proceeds to predict 550b the state performance 555b of the first DES using the cross-covariance 535a inserted into the pooled measurement covariance. The method then predicts 565b the state performance of the state using the second DES (560b). 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, ie, the estimated state covariance of the DES.

[0064]

number

[0065] In other embodiments, the recurrence is determined using the 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 run-time. Using the performance gap 545a and the communication bandwidth 505c of the first DES and the second DES, the method determines (510c) a benefit 515c of using the second type of information in the first DES compared to using the second type of information in 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. If not, 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 with very high communication resources, the communication bandwidth 505c gets a relatively small weight. In a setting with low communication resources, the communication bandwidth 505c gets a relatively large weight.

[0069]

number

[0070] Some embodiments are based on the understanding that certain factors ij may be most important, for example a specific state of a few specific mobile devices may be of interest, which provides a method for determining the specific factors 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 types of information.

[0074] 3A, once a type of information 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 done according to estimates of other DESs, since otherwise there would be a switch 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; it includes setting the dimension of the state according to the number of moving devices and the state to be estimated at each device; extracting a probabilistic motion model from memory and extending it to the dimension of the state; and extracting a probabilistic measurement model from memory and extending it to the dimension of the measurement from the moving device, resulting in a probabilistic estimation model 615a. Next, the method initializes 620a the DES using the current estimated state 619a in the HDES to generate an initialized DES 627a. Finally, using the initialized DES and the measurement value 625a, the method executes (630a) the first DES to generate an estimate of the state 635a.

[0076] The 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 latest mean and covariance 619a are used to initialize the first DES (620a). For example, if particle filters are used in both DES to estimate the HDES, the particles are set to align with each other.

[0077] FIG. 7 shows a flowchart of a method for executing a selected second DES 340a according to some embodiments. First, the method sets up a connectivity model 715a (710a); it includes setting the dimension of the state according to the number of moving devices and the state to be estimated at each device; extracting the communication topology from memory and the weights used in fusing the estimates. Next, the method initializes the DES (720a) using the current estimated state 719a in the HDES to generate an initialized DES 727a. Finally, using the initialized DES, the method executes the second DES (730a) to generate an estimate of the state 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 a case, 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 a LRKF that works 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 the 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, in GNSS measurements, the position is nonlinear in the measurement relationship, but the heading and velocity are not. Thus, various embodiments use a marginalized PF, which runs a PF on the nonlinear portion of the state vector, conditioned on the state trajectory, and a KF is run, 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 and 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 can be one of many samples or can 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, 825b are reflected in selecting the sizes of the dots representing states 821a, 822a, 823a, 824a, 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, for example, using 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 the estimates, where the weights optimize a weighted posterior covariance of the estimation errors.

[0092] 9A shows a schematic diagram of a GNSS 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, the GNSS receivers 930 and 931 may be of different types. For example, in the exemplary embodiment of FIG. 9A, the receiver 931 is a base receiver whose position is known. For example, the receiver 931 may be a ground-mounted receiver. In contrast, the receiver 930 is a mobile receiver that is configured to move. For example, the receiver 930 may be mounted on a mobile phone, a car, or a tablet device. In some implementations, the second receiver 931 is optional and may be used to eliminate or at least reduce uncertainties and errors due to various sources, such as atmospheric effects and errors in the internal clocks of the receivers and satellites. In some embodiments, there are multiple GNSS receivers that receive 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 are further characterized in that the carrier signal and the code signal are modeled using a measurement model.

[0098]

number

[0099] In the formula, 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 carrier phase single difference (SD) and / or double difference (DD) to estimate the receiver state, which is indicative of the receiver's position. When a carrier signal transmitted from one satellite is received at two receivers, the difference between the first carrier phase and the second carrier phase is called the single difference (SD) in carrier phase. Alternatively, the SD can be defined as the difference between signals from two different satellites arriving at a receiver. For example, the difference can come from a first satellite and a second satellite, when the first satellite is called the base satellite. For example, the difference between the signal 910 from satellite 901 and the signal 920 from satellite 902 is one 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 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. The integer part is therefore called the integer bias or integer ambiguity.

[0101] Generally, GNSS can use multiple constellations simultaneously to determine receiver states. 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 single difference or double difference, where single difference includes taking the difference between a reference satellite and another satellite, and double differencing also includes differencing between the receiver of interest and 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 by

[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 are based on the recognition that ignoring state biases such as ionospheric errors may lead to slight inaccuracies in the state estimates, since the biases are usually removed by single or double differencing of GNSS measurements. This solution works well when the desired accuracy for the vehicle's position estimate is on the order of meters, but 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, each vehicle may be any type of mobile transportation system, including a passenger car, a mobile robot, or a rover. For example, the vehicles may be autonomous or semi-autonomous vehicles.

[0110] In this example, multiple vehicles 1000, 1010, 1020 are moving 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 (1330, 1340) information to its neighbors 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 over a large portion 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 selects 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 are configured to not only allow the vehicle to transmit what it senses at the current time t, but also, or alternatively, allow the vehicle to transmit what it senses at time T+δ. t It also allows you to send what you plan to do in

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

[0113] Additionally or alternatively, the motion of the vehicles may be jointly controlled by a remote server based on state estimates determined in a distributed manner. For example, in some embodiments, the multiple 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, 1160 traveling on a freeway 1101. Now, consider that suddenly there is an accident in zone 1100, ahead of the vehicle platoon. This accident makes zone 1100 unsafe for the vehicles to travel. Vehicles 1120, 1160 sense the problem, for example using a camera, and communicate this information to vehicles 1130, 1170. The platoon then runs a distributed optimization algorithm, for example a formation keeping multi-agent algorithm, which selects the best shape of the platoon that avoids the accident zone 1100 and also keeps the flow of vehicles uninterrupted. In this illustrative example, the best shape of 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 vehicles, semi-autonomous vehicles, 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 to a network 1290 connecting the system 1200 to the mixed automaton vehicles via a bus 1206 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 and at least one uncontrolled vehicle that are attempting to join the platoon, and the traffic conditions represent the state of each vehicle in the group and the controlled vehicle. For example, in one embodiment, the traffic conditions include a current driving distance, a current speed, and a 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] The NIC 1250 also includes a transmitter adapted to transmit control commands to the controlled vehicles over the network 1290. To that end, the system 1200 includes an output interface, e.g., a control interface 1270, configured to submit the control commands 1275 to the controlled vehicles in the group of mixed autonomous vehicles over the network 1290. As such, the system 1200 may be located on a remote server that is in direct or indirect wireless communication with the mixed automaton 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 a mouse, a trackball, a touchpad, a joystick, a pointing stick, a stylus, or a touch screen, among others.

[0118] The system 1200 includes a processor 1220 configured to execute stored instructions and a memory 1240 that stores instructions executable by the processor. The processor 1220 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 1240 may include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory machine. The 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 a 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 an extended state of the mixed automaton vehicle and convert the traffic state into a target driving interval for the mixed autonomous vehicle, and to store one or more models 1233 configured to describe the motion of the vehicle. For example, the models 1233 may include a motion model, a measurement model, a traffic model, etc.

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

[0121] FIG. 13A shows a schematic diagram of a vehicle 1301 controlled directly or indirectly 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. Also, the vehicle 1301 may be an autonomous vehicle or a semi-autonomous vehicle. For example, some embodiments control the motion of the vehicle 1301. An example of the motion includes the lateral motion 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 the sensors 1304 include a distance range finder, a radar, a lidar, and a camera. The vehicle 1301 may also include one or more sensors 1305 for sensing its current momentum and internal conditions. Examples of the 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] FIG. 13B shows a schematic diagram of the interaction between the controller 1302 receiving control commands from the system 1200 and the controller 1300 of the 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, which 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. Vehicle mechanical state x t can include position, orientation, and longitudinal / lateral velocity, and the control input u t can include lateral / longitudinal acceleration, steering angle, and engine / brake torque. State constraints for the system can include lane keeping constraints and obstacle avoidance constraints. Control input constraints can include steering angle constraints and acceleration constraints. Collected data can 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 in the embodiment of the present disclosure. The drone 1400 includes an actuator for causing the motion of the drone 1400 and a sensor for perceiving the environment and the position of the device 1400. The rotor 1401 may be an actuator, and the sensor for perceiving the environment may include a light detection and ranging 1402 (LIDAR) and a camera 1403. Furthermore, the sensor for localization may include a GPS or an indoor GPS 1404. Such a sensor 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 sensor and transceiver 1405, calculating commands to 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 moves. In some embodiments, there are one or more devices (drones, such as drone 1400) whose movements are coordinated and controlled by controller 1411. Controlling and coordinating the movements of the one or more devices corresponds to solving a mixed integer optimization problem.

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

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

[0128] FIG. 14C illustrates communication between drones used to determine the drones' 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), which communicates with drones 1401b and 1401d (1480b and 1480c). In some embodiments, communication is done through Symmetric Double-Sided Two-Way Ranging (SDS-TWR) method. In some embodiments, each drone estimates its own state and measures the distance to the other drones through SDS-TWR. In other embodiments, state estimation for each drone is done through Self-Localization and Environment Mapping (SLAM).

[0129] In other embodiments, at least one drone 1401c is wirelessly connected to a remote server 1440c via a transmit / receive interface (1499c). 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 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-equipment 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, the 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, the 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. In addition, 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 invention may be embodied as methods, of 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 simultaneously some acts shown as sequential acts in the exemplary embodiments.

[0134] Although the invention has been described by examples of preferred embodiments, it will 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 which come within the true spirit and scope of the invention.

Claims

1. 1. A hybrid distributed estimation system (HDES) for jointly tracking a state of a plurality of moving devices, each of the moving 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 moving device and an estimate of the state of the moving device derived from the measurements, the hybrid distributed estimation system (HDES) comprising: a memory configured to store a first distributed estimation system (DES) configured to jointly track the state of the mobile device based on the measurements of the state of the mobile device when activated, and a second distributed estimation system (DES) configured to jointly track the state of the mobile device based on the estimates of the state of the mobile device when activated; and 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 jointly estimate the state of the mobile device using the activated distributed estimation system (DES); a transmitter configured to transmit at least one or a combination of the selected type of information and the merged estimated state of the mobile device to the mobile device over the communication channel.

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 an expected difference between an estimate of the merged state of the first distributed estimation system (DES) and an estimate of the merged state of the second distributed estimation system (DES).

3. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor is configured to switch between activation and deactivation of the first distributed estimation system (DES) and the second distributed estimation system (DES) based on the selected type of information while initializing an activated distributed estimation system (DES) based on a state estimated by a deactivated distributed estimation system (DES).

4. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the first distributed estimation system (DES) activated to process the first type of information is a measurement sharing Kalman type filter, and 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).

5. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor selects between the first and second distributed estimation systems (DES) by tracking a measure of quality of the measurements over time and comparing the measure of quality to a threshold, the measure of quality of information including one or a combination of a signal-to-noise ratio (SNR), a presence of multipath signals, and a confidence in the measurements.

6. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the first distributed estimation system (DES) determines the state of the moving device based on a cross-correlation of measurement noise of the measurement of the state of the moving device, the cross-correlation of the measurement noise being defined by a model of the cross-correlation or determined based on transmitted noise and the position of a sensor providing the measurement of the state of the moving device.

7. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor checks the availability of a cross-correlation of measurement noise of the measurements of the state of the mobile device, selects the first type of information, and activates the first distributed estimation system (DES) when a cross-correlation of measurement noise of the measurements of the state of the mobile device is available.

8. The hybrid distributed estimation system (HDES) of claim 7, wherein the processor activates the second distributed estimation system (DES) when cross-correlation of the measurement noise of the measurements of the state of the mobile device is unavailable.

9. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein the processor checks the availability of a cross-correlation of measurement noise of the measurements of the state of the mobile device and selects the first type of information, and the processor activates the second distributed estimation system (DES) when the cross-correlation of measurement noise of the measurements of the state of the mobile device is unavailable, otherwise the processor determines a performance gap between an estimate using the first distributed estimation system (DES) and an estimate using the second distributed estimation system (DES) and activates the first distributed estimation system (DES) or the second distributed estimation system (DES) based on the performance gap.

10. 2. The hybrid distributed estimation system (HDES) of claim 1, wherein 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.

11. 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; 11. The hybrid distributed estimation system (HDES) of claim 10, wherein if at least one of the moving 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 pooled 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.

12. 11. The hybrid distributed estimation system (HDES) of claim 10, 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.

13. To initialize the first distributed estimation system (DES) upon activation of the first DES, the processor: configured to set a dimension of the merged state of the moving devices in response to a number of moving devices and a number of state variables in each of the moving devices; configured to retrieve a probabilistic motion model from the memory and extend the first probabilistic motion model to the dimension of the merged state; configured to retrieve a probabilistic measurement model from the memory and extend the first probabilistic measurement model to a 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 stochastic motion model and the stochastic 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.

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

15. To initialize the second distributed estimation system (DES) upon activation of the second DES, the processor: configured to set a dimension of the merged state of the moving devices in response to a number of moving devices and a number of state variables in each of the moving devices; configured to retrieve from said memory a communication topology and weights for fusing said 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.

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

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

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

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

20. 1. A computer-implemented method for jointly tracking states of a plurality of moving 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 moving devices based on measurements of the states of the moving devices, and a second Distributed Estimation System (DES) configured, when activated, to jointly track the states of the moving devices based on estimates of the states of the moving devices, the processor coupled to stored instructions implementing the method, the instructions, when executed by the processor, performing steps of the method, the steps including: receiving, over 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 derived from the measurements, the steps further comprising: selecting between the first type of information and the second type of information, activating the first or second distributed estimation system (DES) based on the selected type of information, and jointly estimating the state of the mobile device using the activated distributed estimation system (DES); and transmitting 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 over the communication channel.

Images