Probabilistic state tracking using a multihead measurement model.

Abstract

A probabilistic system for tracking a vehicle's state using asynchronous coordination of information includes a probabilistic multi-head measurement model that associates incoming measurements with a state of the vehicle. A first head of the model associates measurements of satellite signals subject to measurement noise with beliefs about the state of the vehicle, and a second head associates estimates of the vehicle's state subject to estimated noise with beliefs about the state of the vehicle. A probabilistic filter of the system recursively updates beliefs about the state of the vehicle based on the multi-head measurement model that accepts one or a combination of measurements of satellite signals subject to measurement noise and estimates of the vehicle's state subject to estimated noise.

Description

[Technical field]

[0001] The present invention relates generally to positioning systems, such as the global positioning system (GPS) or the Quasi-Zenith Satellite System (QZSS), and more specifically to analyzing the state of a vehicle using asynchronous coordination of information. [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 (GNSS) 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 providing a 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", which represents the integral of the Doppler frequency due to the relative motion of the transmitting satellite and the receiver.

[0004] Any pseudorange observation includes inevitable error contributions, including 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 also includes 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, or sampled, by the receiver at discrete successive times. The time index at which the observables are measured is called an "epoch." Known position determination methods typically include 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 lock loss occurs, integer ambiguities resolved at the beginning of the tracking phase can be maintained throughout the entire range of the GNSS positioning. However, GNSS satellite signals may occasionally be obscured, e.g., due to buildings in an "urban canyon" environment, or may be momentarily blocked, e.g., when the receiver passes under a bridge or through a tunnel. In such cases, the integer ambiguity values ​​are 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 may make it difficult to resolve ambiguities with current commercial positioning systems. As the distance between the receivers (i.e., the distance between the reference receiver and the mobile receiver whose position is being sought) increases, the distance-dependent biases (e.g., orbit errors and ionospheric and tropospheric effects) become larger, and as a result, reliable ambiguity resolution (or reinitialization) becomes even more challenging. Furthermore, loss of lock can occur due to discontinuities in the receiver's continuous phase lock to the signal, called cycle slips. By way of 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 extensions refer to techniques used to improve the accuracy of positioning information provided by the Global Positioning System, a network of satellites typically used for navigation, or other global navigation satellite systems. For example, some methods use differentiation techniques based on satellite-to-satellite differentiation, receiver-to-receiver differentiation, epoch-to-epoch differentiation, and combinations thereof. Single and double differences between satellites and receivers reduce but do not eliminate sources of error.

[0007] Therefore, there is a need to improve the accuracy of GNSS positioning. To address this problem, several different methods utilize the cooperation of multiple GNSS receivers to improve the accuracy of GNSS positioning. However, for proper cooperation, the multiple GNSS receivers need to be synchronized and their operation needs to be constrained. For example, U.S. Patent No. 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 vehicle based on satellite signals received from a Global Navigation Satellite System (GNSS) envisage either individual or centralized estimation based on internal modules of the vehicle, or distributed estimation, where state estimation is performed 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 aspects 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 the other, and distributed systems that include multiple synchronized GNSS receivers, preferably located at a fixed distance from each other.

[0009] Some embodiments recognize the benefits of cooperative state tracking when the 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, cooperative but asynchronous state tracking is needed where tracking is performed by the internal modules of the vehicle but can seamlessly integrate external information when such information is available.

[0010] Some embodiments are based on the realization that various probabilistic filters that recursively track the state of the vehicle include two parts: a first part estimates samples of the state of the vehicle, and a second part updates the probability distribution of the state based on these estimated samples. At first glance, these two parts are integrated with each other, since one part does not run without the other. However, some embodiments are based on the realization that these two parts are related by a causal dependency, not a temporal dependency. In particular, the probabilistic update depends on the existence of a state sample estimate, in the sense that when a new state sample is estimated, its arrival should trigger an update. However, how and when the state sample is estimated is irrelevant to the update of the probability distribution.

[0011] Some embodiments exploit this realization to disambiguate the probabilistic state estimation and update parts. This ambiguity removal allows the probabilistic updates to be reused regardless of when and how the state samples are estimated. Hence, the state estimates can be obtained by internal modules of the vehicle based on GNSS measurements or by modules external to the vehicle when such modules are available. Regardless of the principle of state estimation, the probabilistic updates work in the same way and their quality depends on the quality of the estimate and not on the frequency of arrival of new information. In this way, the internal and external state estimators can operate in an asynchronous manner.

[0012] Based on these understandings, some embodiments disclose a multi-head measurement model that allows a probabilistic filter to use different sources to update beliefs about the state of the vehicle. The multi-head measurement model includes multiple paths that generate different types of outputs but have the same probabilistic structure. For example, in some implementations, the multi-head measurement model includes two heads. The first head associates measurements of satellite signals with beliefs about the state of the vehicle subject to measurement noise, and the second head associates estimates of the state of the vehicle with beliefs about the state of the vehicle subject to estimation noise. Hence, the measurement model includes different information and different types of noise, but has a similar structure allowed by the probabilistic filter that allows the outputs of the different heads to be used individually or jointly.

[0013] In this manner, in some embodiments, the probabilistic filter recursively propagates, at each update step, parameters of a probability distribution of the vehicle's state according to a motion model of the vehicle's state transitions affected by process noise, and updates the parameters of the probability distribution upon receiving the output of one or a combination of the first head of the multi-head measurement model and the second head of the multi-head measurement model.

[0014] For example, the first head allows updating the belief of the state of the vehicle using GNSS measurements that have certain uncertainties, biases, and ambiguities that affect the accuracy and / or quality of the state estimation. The output of the first head is typically available at the frequency of the update of the probabilistic filter. In contrast, the first head allows updating the belief of the state of the vehicle using external information that may only be received occasionally but may have a higher accuracy of the state estimation. For example, the second head may process state estimates received from an external measurement module. Hence, although the second head may receive information less frequently, the outputs of the first and second heads are seamlessly integrated without breaking the probabilistic guarantees of the probabilistic filter, regardless of the execution frequency of the first and / or second heads of the multi-head measurement model.

[0015] Some embodiments are based on the understanding that the external measurement module can be passive, active, or both. In a passive mode, the external measurement module receives an estimate of the vehicle's state that is determined independently from GNSS measurements performed by the internal measurement module. For example, the vehicle can pass near a roadside unit (RSU) configured to estimate the vehicle's state using various telemetry techniques. In an active mode, the vehicle can transmit current information of the vehicle's state to a remote server, which can also be an RSU, and receive external measurements obtained in response based on corrections of the internal measurements. Here, the current information includes the mean of the vehicle's state estimate, the covariance of the estimate of the vehicle's state, and a set of code and carrier phase measurements used by the internal measurement module to update its internal state.

[0016] It is therefore an objective of some embodiments to provide a collaborative, yet asynchronous, state estimation of the vehicle's state suitable for use by the probabilistic state estimation. Another objective of some embodiments is to provide such a collaborative, yet asynchronous estimation when the external active modules run at update rates that may differ from the execution of the internal probabilistic filters.

[0017] Some embodiments are based on the realization that the GNSS positioning problem concerns estimating the state of the receiver from a set of code and carrier phase measurements received from one or several constellations of satellites. The measurement equations involved are time-varying, nonlinear with respect to the receiver position, and incorporate various biases and integer ambiguities. In carrier phase measurements, there are integer biases known as ambiguities that are specific to each carrier phase measurement from each satellite. When considering these biases in time, they follow an integer jump process, commonly called "cycle slips", where the biases remain constant before sporadic and mutually independent jumps to new integer values. Therefore, the GNSS positioning problem can be viewed as a mixed integer GNSS positioning problem that solves a constantly increasing size mixed integer problem (with a new integer bias included in each time step), and careful consideration must be given to how to optimally relax this estimation problem in order to make the resulting algorithm feasible.

[0018] Some embodiments are based on the realization that in the setting of the probabilistic filter, the uncertainty of integer ambiguities, e.g. due to cycle slips, is reflected in the second moment of the probability distribution of the state of the vehicle, i.e. in the covariance. Therefore, to benefit from the active mode of the external measurement module, some embodiments allow a remote server to modify this information by transmitting the parameters of the probability distribution. The remote server can collect similar information from other vehicles in the vicinity, fuse the received information to modify the parameters of the probability distribution of the state of the vehicle, and transmit the updated probabilistic parameters back to the vehicle. Upon receiving the updated probabilistic parameters, the external measurement module estimates the state of the vehicle at the current time that is consistent with the updated parameters and triggers the execution of the probabilistic filter. For example, the external measurement module can propagate the mean of the probability distribution in time based on a motion model of the vehicle, sample the updated probability distribution defined by the propagated mean and the received variance, and use the probabilistic filter together with the sampled state estimate to update the probability distribution of the state of the vehicle.

[0019] In some embodiments, the vehicle state includes vehicle position, vehicle velocity, integer ambiguities associated with the vehicle's carrier phase measurements from specific satellites, and residual bias states that model atmospheric, e.g., ionospheric and tropospheric delays. For example, some methods use discriminating techniques based on satellite-to-satellite discrimination, receiver-to-receiver discrimination, epoch-to-epoch discrimination, and combinations thereof. Single and double differences between satellites and receivers reduce sources of error but do not eliminate them, thereby reducing the accuracy of the state estimate.

[0020] Some embodiments are based on the realization 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. This solution works well when the desired accuracy for the vehicle 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.

[0021] Some embodiments are based on the realization that a multi-head measurement model that allows for asynchronous but cooperative communication with an external server can address the problem of accuracy of state estimation when the external state estimates determined at the remote server are sufficiently accurate, however, there is still a need for that server to determine the state with the desired accuracy.

[0022] Some embodiments recognize that it is impractical to accurately determine the atmospheric time delay for each individual vehicle, since the probabilistic filter for each vehicle would need to resolve both the integer ambiguity uncertainty and the atmospheric delay. However, for vehicles that are close enough to each other, e.g., 2-10 km apart, the atmospheric delays are similar. Hence, for an external active module that collects similar information from multiple vehicles, there is significant overlap between the states of the different vehicles, such that the external measurement module can update the states of the individual vehicles in a way that the individual vehicle's probabilistic filter cannot. For example, ionospheric state biases can be shared between vehicles that are close to each other.

[0023] Additionally or alternatively, some embodiments are based on the recognition that inaccuracies in state estimates determined by a probabilistic filter may be caused by measurement noise as well as ambiguities and residual delays. Some embodiments are based on the recognition that when the states of multiple vehicles are determined jointly, the states of the vehicles may or may not overlap, and the ambiguities or delays may or may not overlap, but the measurement noises of different vehicles have correlations that can be explored by the joint state estimation. For example, errors in measurements of different pairs of vehicles may be related to each other.

[0024] Therefore, some embodiments perform probabilistic tracking of multiple vehicles based on an extended probabilistic filter that combines information from different vehicles into an extended domain. For example, the extended probabilistic filter fuses the states of multiple vehicles into an extended state and fuses measurements of satellite signals of multiple vehicles into extended measurements of the extended state that are subject to extended measurement noise defined by an off-diagonal covariance matrix. This off-diagonal covariance matrix has non-zero off-diagonal elements that capture the correlation between the measurement noise of different vehicles. This correlation is additional information that the extended probabilistic filter can explore to jointly track the states of multiple vehicles.

[0025] In some embodiments, the probabilistic filter is a mixed integer least squares Kalman filter, and the nonlinear measurement equation is linearized around the current estimate, resulting in a mixed integer extended Kalman filter. Other embodiments recognize that the linearization in the EKF may be inaccurate. Therefore, in some embodiments, the probabilistic filter is a mixed integer linear regression Kalman filter. Such a filter more accurately represents the first and second moments of the probability distribution, but is computationally very expensive.

[0026] In one embodiment, it is recognized that parts of the vehicle state, e.g. integer ambiguities, are linear in the model, but the position is non-linear, therefore parts of the first and second moments can be found analytically, while other parts are estimated using a linear regression Kalman filter.

[0027] The GNSS positioning problem can be viewed as a mixed-integer GNSS positioning problem that solves a constantly growing mixed-integer problem (with a new integer bias included at each time step), and careful consideration must be given to how to optimally relax this estimation problem in order for the resulting algorithm to be feasible. Especially in the case of external estimation of states when combining information from multiple vehicles, the resulting estimation problem is very high-dimensional and therefore computationally expensive.

[0028] In some embodiments, the relaxation is solved by determining two related but separate probability distributions, one of which considers only real-valued parameters, i.e., ignoring the fact that the ambiguities are in fact integers, and the second distribution is determined based on the integer ambiguities determined from the first distribution by solving an optimization problem.

[0029] Accordingly, one embodiment discloses a probabilistic system for tracking a state of a vehicle using asynchronous coordination of information received from satellite signals transmitted by a Global Navigation Satellite System (GNSS) and information transmitted over a Radio Frequency (RF) channel, the probabilistic system comprising a memory configured to store a probabilistic multi-head measurement model relating incoming measurements to a state of the vehicle, the probabilistic multi-head measurement model including a first head relating measurements of the satellite signals subject to measurement noise to beliefs about the state of the vehicle and a second head relating estimates of the state of the vehicle subject to estimated noise to beliefs about the state of the vehicle, and further comprising at least one processor configured to process executable instructions of modules of the probabilistic system.

[0030] The modules of the probabilistic system include a GNSS measurement module operatively connected to the GNSS receiver and configured to determine satellite signal measurements and measurement noise; an RF measurement module operatively connected to the vehicle's RF receiver and configured to receive data indicative of an estimate of the vehicle's state and the estimated noise; and a probabilistic filter configured to generate a belief on the vehicle's state by recursively updating parameters of a probability distribution of the vehicle's state based on a multi-head measurement model that accepts one or a combination of the satellite signal measurements affected by the measurement noise and the vehicle's state estimate affected by the estimated noise. [Brief description of the drawings]

[0031] [Figure 1A] 1 shows a schematic diagram of a Global Navigation Satellite System (GNSS), according to some embodiments. [Figure 1B] FIG. 1 shows an example in which there are two additional vehicles 140 and 150 equipped with GNSS receivers. [Figure 1C] FIG. 2 illustrates various variables used alone or in combination in modeling a motion and / or measurement model according to some embodiments. [Figure 1D] 1 shows a diagram of a scenario in which multipath interferes with a signal to a receiver. [Figure 1E] 1 illustrates a conceptual diagram of a probabilistic filter used by some embodiments. [Figure 1F] 1 illustrates a diagram of asynchronous reception of different signals according to some embodiments. [Figure 1G] 1 illustrates a diagram of a multi-head measurement model structure according to some embodiments. [Figure 1H] FIG. 2 illustrates a flowchart of an example method for updating first and second moments of a state estimation at a time instance according to one embodiment. [Figure 1I] FIG. 13 illustrates another flowchart of a method for updating first and second moments of a state estimation according to another embodiment. [Figure 1J] FIG. 13 illustrates the reverse situation, where only the first and second moments of a state estimate are received at a particular time instance, according to another embodiment. [Figure 1K] FIG. 13 is a flowchart of an example of a procedure. [Figure 2A] FIG. 2 illustrates a flowchart of a method for tracking a vehicle state using asynchronous information coordination according to some embodiments. [Figure 2B] FIG. 1 illustrates a probabilistic system for tracking vehicle states using asynchronous coordination of information according to some embodiments. [Figure 2C] FIG. 2 illustrates a flowchart of an example method for determining 240a an estimate, according to some embodiments. [Figure 2D] FIG. 1 illustrates an example of a method for determining beliefs regarding a state of a vehicle, according to some embodiments. [Figure 3A] FIG. 2 illustrates a flowchart of an exemplary implementation of a method for vehicle state estimation according to some embodiments. [Figure 3B] FIG. 2 illustrates a flowchart of a method for resolving carrier phase ambiguity according to some embodiments. [Figure 3C] FIG. 2 illustrates a flowchart of an exemplary implementation of a method for sampling floating values ​​consistent with a motion model and its process noise, according to one embodiment. [Figure 3D] FIG. 1 is a schematic diagram illustrating some principles used by some embodiments. [Figure 4] FIG. 2 illustrates a flowchart of a method for executing a probabilistic filter to update parameters of a probability distribution of a state of a vehicle, according to one embodiment. [Figure 5A] FIG. 2 illustrates a traffic scenario, illustrating a scenario according to some embodiments. [Figure 5B] FIG. 1 illustrates a synchronization message exchange in IEEE Precision Time Protocol (PTP) based synchronization. [Figure 5C] FIG. 1 illustrates a probabilistic system for tracking vehicle states using asynchronous coordination of information according to some embodiments. [Figure 5D] FIG. 2 illustrates a traffic scenario according to another embodiment. [Figure 6A] FIG. 2 illustrates a flowchart of a recursive method for determining first and second moments of a vehicle's state using multiple transmissions from multiple vehicles, according to some embodiments. [Figure 6B] FIG. 1 illustrates a flowchart of a recursive method for updating parameters of an extended state at a remote server according to some embodiments. [Figure 6C] FIG. 2 illustrates a flowchart of a method for fusing multiple states of a vehicle with an augmented state according to some embodiments. [Figure 6D] FIG. 2 illustrates an example of state space overlap according to some embodiments. [Figure 6E] FIG. 1 illustrates a flowchart of a method for blending an augmented state with a vehicle state, according to some embodiments. [Figure 6F] FIG. 2 illustrates a flowchart of a method for determining cross-covariance according to one embodiment. [Figure 6G] FIG. 2 illustrates an example of cross-covariance in measurement noise for a full measurement model including measurements from multiple transmissions, according to some embodiments. [Figure 6H] FIG. 6C shows a flowchart of a method for determining a measurement noise matrix such that the measurement noise matrix has the structure of FIG. 6G. [Figure 7] FIG. 1 illustrates a probabilistic system for tracking states of multiple vehicles using asynchronous cooperation of information received from the vehicles transmitted over an RF channel according to some embodiments. [Figure 8] 1 illustrates an example of vehicle-to-vehicle (V2V) communication and planning based on state estimation, according to one embodiment. [Figure 9] 1 is a schematic diagram of multi-vehicle platoon forming for accident avoidance scenarios according to one embodiment. [Figure 10] FIG. 1 illustrates a block diagram of a system 1000 for direct and indirect control of a mixed autonomous vehicle, according to some embodiments. [Figure 11A] 1 shows a schematic diagram of a vehicle 1101 that is directly or indirectly controlled according to some embodiments. [Figure 11B] 1 shows a schematic diagram of the interaction between a controller 1100 of a vehicle 1101 and a controller 1102 that receives control commands from the system 1000, according to some embodiments. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0032] 1A illustrates a schematic diagram of a Global Navigation Satellite System (GNSS) according to some embodiments. As an example, an Nth satellite 102 transmits 120 and 121 codes and carrier-phase measurements to a set of receivers 130 and 131. For example, receiver 130 is positioned to receive signals 110, 120 from N satellites 101, 103, 104, and 102. Similarly, receiver 131 is positioned to receive signals 121 and 111 from N satellites 101, 103, 104, and 102.

[0033] In various embodiments, the GNSS receivers 130 and 131 may be of different types. For example, in the exemplary embodiment of FIG. 1A, the receiver 131 is a base station receiver whose position is known. By way of example, the receiver 131 may be a ground-based receiver. In contrast, the receiver 130 is a mobile receiver configured to move. By way of example, the receiver 130 may be mounted on a mobile phone, a car, or a tablet. In some implementations, the second receiver 131 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 internal clocks of the receivers and satellites. In some embodiments, there are multiple GNSS receivers that receive the code and carrier phase signals.

[0034] 1B shows an example where there are two additional vehicles 140 and 150 equipped with GNSS receivers. The satellites can transmit code and carrier phase signals to several receivers such that these receivers share the same source of code and carrier phase signals. For example, satellite 104 can transmit signals to both receiver 140 and receiver 150, and satellite 101 can transmit signals to both receivers 130, 140, and 150. Additionally or alternatively, the receivers of the GNSS signals can communicate information to each other over radio waves 161.

[0035] It is an object of some embodiments to disclose a system and method for improving satellite-based tracking of the state of a vehicle equipped with a GNSS receiver. Another object is to provide such a system and method that uses asynchronous coordination of information received from satellite signals. Another object of some embodiments is to provide such a system and method that is probabilistic, i.e. that takes into account stochastic disturbances and error sources. It is an object of other embodiments to track the state of the vehicle using different information from different sources, rather than relying only on satellite signals. For example, in some embodiments, the state of the vehicle will be tracked using GNSS signals received from satellites using a GNSS receiver and first and second moments of a probability distribution of the state of the vehicle received from a remote server using a radio frequency (RF) receiver.

[0036] Additionally or alternatively, it is an objective of some embodiments to provide a filter that is probabilistic, such as a state estimation filter that provides state estimates based on a motion model and a measurement model.

[0037] One example of a state estimator is a Kalman filter, which uses a series of measurements observed over time, including statistical noise and other imprecision, to generate estimates of unknown variables that tend to be more accurate than those based on a single measurement alone by estimating a joint probability distribution of the variables for each time frame. The Kalman filter tracks the estimated state of the system and the uncertainty of the estimate. The estimates are updated using a motion model of the state transitions and measurements. Some embodiments use a Kalman filter-based system that uses a motion model that is subject to the process noise of the GNSS receiver and a measurement model of the satellite signals that is subject to measurement noise. In some embodiments, the measurement model is probabilistic and multi-headed, i.e., includes multiple paths that generate different types of outputs. In some implementations, the multi-head measurement model includes two heads. A first head relates measurements of the satellite signals to beliefs about the state of the vehicle that are subject to measurement noise, and a second head relates estimates of the state of the vehicle to beliefs about the state of the vehicle that are subject to estimation noise. Therefore, the measurement models contain different information and different types of noise, but have a similar structure allowing for probabilistic filters that allow the outputs of the different heads to be used individually or jointly.

[0038] Thus, in some embodiments, in each update step, the probabilistic filter recursively propagates parameters of a probability distribution of the vehicle's state according to a motion model of the vehicle's state transitions affected by process noise, and updates the parameters of the probability distribution based on the output of one or a combination of the first head of the multi-head measurement model and the second head of the multi-head measurement model.

[0039]

number

[0040] In some embodiments, the ambiguities are included in the vehicle's state. Other embodiments 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.

[0041]

number

[0042] In some embodiments, the stochastic filter uses carrier phase single difference (SD) and / or double difference (DD) to estimate the receiver state, which is indicative of the receiver's location. When two receivers receive a carrier signal transmitted from one satellite, the difference between the first carrier phase and the second carrier phase is called the single difference (SD) of the 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 come from a first and a second satellite, where the first satellite is called the base satellite. For example, the difference between the signal 110 from satellite 101 and the signal 120 from satellite 102 is one SD signal, where satellite 101 is the base satellite. Using the pair of receivers 131 and 130 of FIG. 1A, the difference between the SDs in the carrier phase obtained from the wireless signals from the two satellites is called the double difference (DD) of the carrier phase. The carrier phase difference can be converted to a number of wavelengths, e.g., λ=19 cm for L1 GPS (and / or GNSS) signals, which is separated by a fractional 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.

[0043] 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 or double differencing, where single differencing involves taking the difference between a reference satellite and another satellite, and double differencing also involves the difference between the receiver of interest and a base station receiver with a known static position.

[0044]

number

[0045]

number

[0046] Additionally or alternatively, some embodiments are based on the recognition that ignoring state biases such as ionospheric errors can 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 of 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 states and determine them as part of the state tracking provided by the probabilistic filter.

[0047] In certain scenarios, for example in deep urban canyons, there are multiple distortions of the satellite signals, making the information contained in the code and carrier phase signals difficult to perform high-precision state estimation. As an example, FIG. 1D shows a diagram of a scenario where multipath blocks signals to a receiver 101d. The receiver 101d receives various signals 109d and 119d from satellites 110d and 120d. There are other satellites 130d and 140d transmitting signals 128d, 129d, 138d, 139d, but due to obstructions 170d, for example urban buildings, these signals are not transmitted directly to the receiver.

[0048] Previously, signal 138d transmitted from satellite 140d was unavailable, but suddenly satellite signal 139d arrives at the receiver after multipath 102d. Such a scenario can severely impair the performance of the probabilistic filter in tracking the state of the vehicle, as the estimator locks onto the wrong ambiguity estimate, causing large estimation errors. However, there may be other receivers nearby that do not have such problems, for example, a receiver may have multipath and be shadowed by a large building, so only a few satellites are visible, but a vehicle a few blocks away has good estimation performance. In some embodiments, this is exploited by using measurements from the GNSS measurement module as well as remote estimates of the state of the vehicle, which are determined using data from multiple vehicles, to update the parameters of the probability distribution in the probabilistic filter.

[0049] An example of a probabilistic filter is the Kalman filter. Figure 1E shows a schematic diagram of the Kalman filter (KF) used by some embodiments for state estimation. The KF is a tool for state estimation in a linear state space model, and is an optimal estimator when the noise source is known and Gaussian, 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 two quantities, sufficient statistics, required to describe the Gaussian distribution.

[0050] The KF starts with initial knowledge 110e of the state to determine the mean of the state and its variance 111e. The KF then predicts 120e the state and variance to the next time step using a model of the system, such as a motion model of the vehicle, to obtain an updated mean and variance of the state 12le. The KF then uses measurements 130e in an update step 140e using a measurement model of the system to determine an updated mean and variance 141e of the state. An output 150e is then obtained and the procedure is repeated for the next time step 160e.

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

[0052] Some embodiments are based on the realization that the outputs of different heads of the multi-head measurement model of some embodiments do not need to be synchronized, allowing signals to be received at different time steps. Other embodiments are based on the understanding that in order to run the KF as is, the measurement model needs to be constructed such that the KF can accept it.

[0053] FIG. 1F shows a diagram of asynchronous reception of different signals according to some embodiments. Along the progression of time, a GNSS signal is received at one time step and the first and second moments are received at some other time steps. For example, both the GNSS signal and the first and second moments are received at time 110f. However, a second GNSS signal is received at time 120f and the first and second moments are received at time 130f. That is, the signals may or may not be received simultaneously. To process such signals, possibly received at different time steps, a measurement model is constructed in such a way that the different signals can be used independently of each other in the KF.

[0054] To that end, in some embodiments the measurement model is probabilistic and multi-headed, i.e. includes several parts, a first head relating measurements of satellite signals to beliefs about the state of the vehicle subject to measurement noise, and a second head relating estimates of the state of the vehicle to beliefs about the state of the vehicle subject to the estimated noise. Hence the measurement model includes different information and different types of noise.

[0055] In one embodiment, a first head of the measurement model subject to measurement noise is used to relate GNSS measurements to vehicle state beliefs and a second head of the measurement model subject to measurement noise is used to relate vehicle state estimates to vehicle state beliefs received by other means.

[0056] Some embodiments are based on the principle of processing measurements using a measurement model and correcting and updating the first and second moments 140e using the measurements 130e and the measurement model. In order to use the KF, the measurement model and the measurement noise must be described in a probabilistic form so that they can be handled in a balanced way and that the measurements do not inaccurately scale the first and second moments.

[0057] Another embodiment is based on the realization that the probabilistic structure needs to be consistent between the first and second heads of the measurement model: in particular, the output has the same structure, regardless of whether the update step 140e of the probability distribution of the state of the vehicle is performed using GNSS signals or using the received estimates of the state of the vehicle and the estimated noise, expressed as first and second moments.

[0058] FIG. 1G shows a diagram of the structure of a multi-head measurement model 100g, according to some embodiments. The multi-head measurement model receives inputs and associates GNSS measurements to beliefs about the state of the vehicle using a first head 110g of the measurement model, and associates alternative estimates of the state of the vehicle to beliefs about the state of the vehicle using a second head 120g. For example, the multi-head measurement model receives GNSS measurements 130g and measurement noise 140g. The multi-head measurement model then converts the measurement noise 140g to a second moment (150g) that represents the measurement noise. For example, the measurement noise has zero mean and covariance R kThe representation of the measurement noise 150g is represented using a Gaussian distribution with . Using the representation of the measurement noise 150g, the first head 110g relates the measurements 130g to beliefs 160g for the vehicle's states (111g). The output 112g from the first head of the measurement model is in a form that the KF can accept. For example, in one embodiment, the output 112g is a relation 111g described as the error between the measurements, the beliefs for the states inserted in the measurement model, and the measurement noise. In another embodiment, the output 112g is a relation 111g that is decomposed into the measurements, the beliefs for the states inserted in the measurement model, and the measurement noise covariance.

[0059]

number

[0060] Some embodiments are based on the understanding that the same probabilistic representation of outputs 112g and 122g enables updates in the KF to be performed on the same principles regardless of whether the update information is 112g or 122g, i.e., regardless of whether the information originates from the first head or the second head.

[0061] Other embodiments are based on the recognition that the updates using the first head and the updates using the second head can be performed sequentially or together. Figure 1H shows a flow chart of an example of a method for updating the first and second moments at a time instance according to one embodiment. Using the output 112g from the multi-head measurement model, the method updates the first and second moments using the first head of the measurement model (110h). The updated moments 111h are then updated (120h) again using the output 122g from the second head, resulting in updated moments 121h.

[0062] Other embodiments are based on the understanding that the GNSS measurements and received estimates of the vehicle's state and the estimated noise may not be received simultaneously, as illustrated in Figure 1F. Figure II shows another flow chart of a method for updating the first and second moments, for example using the KF. Using the output 112g from the first head of the measurement model, the method updates (110h) the moment 111h.

[0063] 1J shows the reverse situation, where only the first and second moments have been received at a particular time instance. Thus, one embodiment uses the output 122g from the second head of the measurement model to update 110h the moment 111j.

[0064] Other embodiments are based on the recognition that updates using the first and second heads can be performed together, i.e. simultaneously. Figure 1K shows a flow chart of one example of such a procedure. Using the output from the first head 112g and the output from the second head 122g, the method updates the moments using the first and second heads (110k), resulting in updated moments 111k. In one embodiment, the joint update is performed by stacking the two heads of the measurement model, resulting in an augmented measurement model output. That is, GNSS measurements and estimates of the vehicle's state and associated relationships are integrated, resulting in a joint large-scale update.

[0065] FIG. 2A illustrates a flow chart of a method for tracking a vehicle's state using asynchronous coordination of information, according to some embodiments. In an embodiment, the information is received from satellite signals transmitted by a Global Navigation Satellite System (GNSS) and transmitted through radio frequency (RF) channels. First, the method receives (210a) signals transmitted from a set of satellites 101, 102, 103, 104 from a GNSS receiver 209a, according to some embodiments. The signals include code signals and carrier phase signals, each of which includes carrier phase ambiguity as an unknown integer number of wavelengths of the carrier signal sent between the satellite 101, 102, 103, or 104 and the vehicle 130. Using the GNSS signals 215a, the method determines (220a) measurements of the satellite signals and corresponding measurement noise 225a. The method also receives (230a) estimates of the vehicle's state and the estimated noise determined by an external server from the vehicle's RF receiver 229a via RF signal 235a. In various embodiments, the estimates and estimated noise 235a are determined probabilistically as first moments of the vehicle state and second moments of the vehicle state 235a. Additionally or alternatively, the estimates and estimated noise 235a can be converted to the first moments of the vehicle state and second moments of the vehicle state using various probabilistic techniques.

[0066] Using the first and second moments, the method determines 240a an estimate of the vehicle's state and associated estimated noise for the estimate of the vehicle's state. Using the estimates 245a, the measurements 225a, and the multi-head measurement model 249a, the method relates 250a the GNSS measurements 225a to beliefs about the vehicle's state subject to measurement noise using a first head of the measurement model, and relates 250a the estimate of the state to beliefs about the vehicle's state subject to estimated noise using a second head of the measurement model. Using the associated measurements and estimates 255a, the method recursively updates 260a parameters of a probability distribution for the vehicle's state to generate 265a beliefs for the vehicle's state.

[0067] FIG. 2B illustrates a probabilistic system 200 for tracking a vehicle state using asynchronous coordination of information received from satellite signals 209 transmitted by a navigation satellite system (GNSS) and information transmitted over a radio frequency (RF) channel 239, according to some embodiments. The probabilistic system includes a GNSS receiver 220 for receiving satellite signals 209. In one embodiment, the signals include code signals, carrier phase signals, timestamps, navigation messages, and observation messages necessary to determine various delays and disturbances affecting the satellite signals. As an example, the navigation messages may include parameters necessary to determine atmospheric delays. For example, if the Klobuchar atmospheric delay model is used, the navigation messages may include parameters for determining ionospheric time delays according to the ionospheric effects on signal propagation as determined by the atmospheric delay model. In other embodiments, such messages may include parameters for determining ionospheric delays according to an SBAS delay model.

[0068] The system includes a memory 280 that stores a probabilistic multi-head measurement model (281) that relates incoming measurements to the state of the vehicle. In some embodiments, the multi-head measurement model consists of two heads. The first head relates measurements of satellite signals to beliefs about the state of the vehicle subject to measurement noise. The second head relates estimates of the state of the vehicle to beliefs about the state of the vehicle subject to the estimated noise. The memory also stores instructions on how to determine such a relationship (283), and the memory stores instructions for executing a probabilistic filter according to some embodiments of the invention (284). The memory also stores a probabilistic motion model (285) that relates previous beliefs about the state of the vehicle to predictions of the state of the vehicle according to a motion model. For example, the motion model may be a vehicle dynamics model, a constant acceleration model, a constant position model, a coordinated turning model, a Singer model, a kinematic vehicle model, a dynamic vehicle model, or a combination of different models.

[0069] In some embodiments, relating the measurements of the satellite signals to beliefs about the state and relating the estimates of the state to beliefs about the state are performed by inserting the above quantities into first and second heads of a measurement model, respectively.

[0070] The GNSS receiver is operatively connected (250) to a GNSS measurement module that, after determining the delays and disturbances, determines measurements of the satellite signals and associated measurement noise. By way of example, in one embodiment, the measurement noise is determined based on a nominal measurement noise stored in memory 282 scaled as a function of the elevation angle of the satellite.

[0071] The probabilistic system 200 includes an RF measurement module 240 operatively connected (250) to an RF receiver 260 that receives information 239 transmitted over an RF channel. In some embodiments, the information includes a first moment of the state of the vehicle. In other embodiments, the information includes a first and second moment of the vehicle. In other embodiments, the information includes higher order moments to form a general probability distribution of the state of the vehicle. In other embodiments, the information includes data indicative of an estimate of the state of the vehicle and an estimated noise, e.g., the first and second moments as samples, and includes higher order moments instead. 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 and second moments, 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 239 includes a timestamp of the time the first and second moments were determined.

[0072] In various embodiments, the GNSS measurement module and the RF measurement module generate their estimates related to beliefs about the state of the vehicle at a current time point. For example, a GNSS signal is received and upon receiving the signal, the processor 230 determines (231) a measurement at a current time step. However, in some implementations, the RF measurement module receives first and second moments of the state of the vehicle, determined for a previous time step, e.g., as the remote server took time to execute the instructions. This leads to determined measurements, and the first and second moments received do not correspond to the same time point.

[0073] To address this issue, using a kinematic model, some embodiments relate the first and second moments of the state of the previous time step to the beliefs for the state at the current time step in time to obtain an estimate of the state that indicates the beliefs for the state of the vehicle at the current time point. For example, one embodiment uses a model of time propagation, e.g., a kinematic model stored in the vehicle's memory, to propagate the first and second moments of the state of the vehicle at the time up to the current time point. Examples of models of time propagation include the Singer model, the constant acceleration model, and the single track vehicle model. Using both moments in time propagation is more advantageous than simply propagating the first moment, because it takes into account the propagation of uncertainties and therefore how much the first moment should be trusted when updating the beliefs for the state. In other embodiments, time propagation is performed by a combination of a kinematic model of the vehicle and measurements of the vehicle.

[0074] Some embodiments are based on the realization that, irrespective of the time instance at which the first and second moments of the vehicle's state are received, it is necessary to ensure that the estimation noise is configured such that when updating the parameters of the probability distribution with an estimate of the state subject to measurement noise, the probability distribution of the beliefs for the state converges to the probability distribution of the received second moment of the state. Some embodiments are based on the realization that the convergence property can be guaranteed by balancing the covariance of the received second moment with the covariance of the probability distribution of the current beliefs for the state. In other words, the estimates of the state and the associated measurement noise are sought such that a balance is maintained to maintain the convergence.

[0075] In some embodiments, processor 230 executes a probabilistic filter 232 that is used to update parameters of the probability distributions and determine 233 an estimate of the vehicle's state.

[0076] Some embodiments are based on the realization that it may be attractive to directly set the parameters of the probability distribution to match the parameters of the received first and second moments. However, implementing the convergence property is advantageous because it ensures that the parameters of the probability distribution can be updated using the same probabilistic filter from the vehicle side. In addition, since the vehicle may process additional measurements, e.g., from additional on-board sensors of the vehicle, directly using the received first and second moments may cause discontinuities in the updated state estimate of the vehicle. When using the estimate to control the vehicle, such discontinuities may be harmful because the continuity of control is lost.

[0077] FIG. 2C illustrates a flow chart of an example of a method 240a for determining an estimate, according to some embodiments. First, based on the received information indicating an estimate of the state of the vehicle and the estimated noise, the method determines (210c) a distribution of the estimate of the state of the vehicle. For example, the method determines the distribution by inserting the first and second moments into a Gaussian probability density function. Next, the method determines (220c) a distribution of beliefs for the state of the vehicle. By way of example, the method inserts parameters of the probability distribution of the state of the vehicle, which is assumed to follow a predefined shape. By way of example, in the case of a Gaussian distribution, the parameters of the probability distribution are the first and second moments. By way of example, assuming a distribution of student t, the parameters are the first moment, scale, and degrees of freedom. Using the distribution of the estimate of the vehicle 215c and the distribution of the beliefs for the state 225c, the method determines the difference between the distributions 215c and 225c. Finally, using the determined difference 235c, the method determines (240c) a covariance of the estimated noise.

[0078] Finding the difference 230c can be done in a number of ways. For example, for Gaussian distributions, the difference can be defined as the amount by which the second moment is scaled and the first moment is shifted to achieve the same distribution. In some embodiments, the difference is found by the well-known Kullback-Leiber divergence.

[0079] Some embodiments are based on the realization that even when the underlying distribution is not Gaussian, typically the first two moments adequately describe the distribution. In some embodiments, this realization is utilized in determining 240c the covariance of the estimated noise as a weighted combination of the second moment received by the RF measurement module and the second moment of the probability distribution of the vehicle's state. In other embodiments, the estimate of the vehicle's state is determined as a weighted combination of the beliefs on the vehicle's state and the received first moment of the estimate of the vehicle's state, where the weighting includes the covariance of the estimated noise.

[0080]

number

[0081]

number

[0082] Some embodiments recognize that one purpose of the second head of the measurement model is to allow the parameters of the probability distribution to be updated using the same principles as is done when using the first head of the measurement model. Some embodiments recognize that in order to update the parameters of the probability distribution using an estimate of the state of the vehicle determined using the received first and second moments, the second head of the measurement module must ensure certain properties to ensure that the convergence of the beliefs on the state of the vehicle converges to the estimate of the state of the vehicle. By way of example, in one embodiment, the second head of the measurement model includes the difference between the estimated state of the vehicle and the beliefs on the state of the vehicle determined during the previous iteration, weighted with the covariance of the estimated noise. By way of example, in one embodiment, the second head of the measurement model includes the difference between the estimated state of the vehicle and the beliefs on the state of the vehicle determined during the previous iteration, weighted with the covariance of the estimated noise. k =x k +r k In the formula, z k is the estimated vehicle state, and x k is the belief about the state of the vehicle, and r k is the estimated noise and k is the time step in the iteration.

[0083] Some embodiments are based on the realization that in order to update the parameters of a probability distribution using a probabilistic filter, the probabilistic filter must be able to update the parameters by taking into account the fact that some parts of the vehicle's state are real-valued and some are integer-valued, since some parts, e.g. the vehicle's position, are in the state and are real-valued, whereas ambiguities are in the state and are integer-valued.

[0084]

number

[0085] Some embodiments are based on the understanding that a particle filter is a filter that can solve a mixed integer estimation problem without having to resort to optimization methods. Other embodiments understand that in order to avoid having to resort to optimization methods, a particle filter requires a large number of particles that can be computationally prohibitive. To this end, some embodiments solve a recursive mixed integer weighted least squares problem by employing a mixed integer extension KF. Other embodiments solve a mixed integer linear regression KF.

[0086] FIG. 3A shows a flow chart of an exemplary implementation method for vehicle state estimation, which receives state measurements and estimates including a multi-head measurement model (310a). Each carrier signal includes carrier phase ambiguity as an unknown integer number of wavelengths of the carrier signal sent between the satellite 101, 102, 103, or 104 and the receiver 130. The method then retrieves from memory (322a) a motion model that relates the previous state of the vehicle to the current state of the vehicle, and a multi-head measurement model that uses the carrier phase ambiguity of the carrier signal to relate the carrier measurements and code signals to current beliefs about the state of the vehicle and relate the estimates of the state of the vehicle to beliefs about the state of the vehicle. Both models, the motion model and the measurement model, are probabilistic. For example, the motion model is a probabilistic model that is subject to process noise, and the measurement model is a probabilistic model that is subject to measurement noise.

[0087] Next, the method determines 320a a set of possible combinations of integer values ​​325a of carrier phase ambiguities that match the measurements of the carrier and code signals according to one or a combination of the motion and measurement models 322a within the bounds defined by one or a combination of the process noise and the measurement noise. This step is based on the understanding that instead of attempting to solve for the carrier phase ambiguities to perform the state estimation, it is beneficial to determine and test different possible combinations of carrier phase ambiguities for the state estimation. In this way, the best carrier phase ambiguities can be selected using a probability model that better reflects the nuances of the state estimation.

[0088] Therefore, the method executes (320a) a set of state estimators that determine the state of the vehicle by jointly using the motion model and measurement model 322b. Each state estimator includes its corresponding combination of integer values ​​of carrier phase ambiguities and determines a joint probability distribution 335a of the state of the vehicle for the motion model and measurement model 322a. In this way, the combination of integer values ​​of carrier phase ambiguities can be probabilistically evaluated since the measurement models of at least some of the different state estimators include different combinations of integer values ​​of carrier phase ambiguities selected from the set of possible combinations 325a. The method then determines (340a) the state of the vehicle using the state estimator with the highest joint probability of the state of the vehicle according to the measurements of the carrier and code signals and the estimate of the state of the vehicle using the multi-head measurement model.

[0089] Therefore, some embodiments are based on the recognition that the estimation of the range of possible integer values ​​of the carrier phase ambiguity and the selection of an integer value of the carrier phase ambiguity from that range can be performed probabilistically using the consistency of the motion model and the measurement model to the probability density function (PDF) of the noise of the first head in the measurement model and the noise of the second head in the measurement model.

[0090] 3B illustrates a flowchart of a method 320a for determining possible combinations of integer values ​​of carrier phase ambiguity according to some embodiments. The method may be implemented using a processor. For at least one state of the vehicle that is consistent with the process noise of the motion model, the method samples 310b the floating value of the carrier phase ambiguity for at least one satellite on a PDF of the measurement noise centered on a noiseless fit of the measurements of the carrier phase ambiguity, state, carrier and code signals to the estimates of the measurement model and state.

[0091] In some embodiments, the PDF of the measurement noise is predetermined, for example based on the characteristics of the GNSS receiver. A noiseless fit as used herein is a fit of the carrier phase ambiguities, position, and measurements of the carrier and code signals when the obtained measurements of the carrier and code signals are assumed to be correct. That is, the measurements of the carrier and code signals are assumed to be correct, and the remaining errors are due to the carrier phase ambiguities. In this way, the noiseless fit places the PDF in a position where the sampling of the PDF de-emphasizes the stochastic noise of the motion and measurement model and emphasizes the effect of the carrier phase ambiguities on the position estimate. In other words, the errors in the fit to the measurement model after inserting the carrier phase ambiguities, position, and measurements of the carrier and code signals into the measurement model are due to errors in the carrier phase ambiguities.

[0092] Next, the method forms 320b a union of the sampled float values ​​to generate a union that includes all of the sampled float values ​​321b, and integer-based discretizes 310b the union of the float values ​​to generate possible integer values ​​331b of the carrier phase ambiguity for the set of GNSS satellites. Finally, the method uses the possible integer values ​​331b of the carrier phase ambiguity for all satellites to generate 340b a set of possible combinations 325a of integer values.

[0093] FIG. 3C illustrates a flow chart of an exemplary implementation of a method 310b for sampling floating values ​​according to one embodiment. However, sampling floating values ​​310b is realized in several ways by different embodiments. The method of FIG. 3C samples floating values ​​that are consistent with a motion model and its process noise (309c). The method determines consistency with measurements of the code and carrier phase signals by inserting the sampled carrier phase ambiguities, the estimated states, and measurements of the carrier and code signals into a measurement model (311c). Based on the consistency with the measurements, the method corrects each sampled floating value as a function of the process noise and the measurement noise (312c), updates the probability of each ambiguity based on the consistency with the corrected measurements (313c), and preserves the floating values ​​of the carrier phase ambiguities that have a probability of fitting the measurement model that exceeds a threshold by removing the sampled floating values ​​of the carrier phase ambiguities that have been corrected (314c).

[0094] FIG. 3D is a schematic diagram illustrating some principles used by some embodiments. In particular, some embodiments are based on the recognition that a finite number of possible integer values ​​of carrier phase ambiguities allows for determining (340d) different combinations of possible integer values ​​for tracking the state of the vehicle. Such recognition allows for replacing (380d) the evaluation 310d of the carrier phase ambiguities for the estimation 320d of the state 330d of the vehicle with evaluation 360d of different states of the vehicle determined (350d) using different combinations 340d of carrier phase ambiguities. This replacement is advantageous because the stochastic nature of the receiver's motion is more suitable for testing the position than for testing derivatives of the position such as carrier phase ambiguities. In this way, the best state 370d selected using the stochastic nature of the vehicle's motion automatically indicates the corresponding combination of carrier phase ambiguities used to determine such state 370d.

[0095] Some embodiments are based on the recognition that particle filters can be computationally prohibitive and that a Kalman-type filter that embeds an optimization solver can be advantageous for computational reasons.

[0096] FIG. 4 shows a flow chart of a method 260a for executing a probabilistic filter to update parameters of a probability distribution of a state of a vehicle when the parameters are the first and second moments of the distribution, according to one embodiment. The method receives (410a) measurements and estimates of the state, including a multi-head measurement model. Each carrier signal includes carrier phase ambiguities as an unknown integer number of wavelengths of the carrier signal traveling between a satellite 101, 102, 103, or 104 and the vehicle 130. The method then uses the carrier phase ambiguities of the carrier signals and the estimates of the state of the vehicle to the beliefs of the state of the vehicle to retrieve (422a) from memory a motion model that relates previous states of the vehicle to the current state of the vehicle, and a multi-head measurement model that relates measurements of the carrier and code signals to the current beliefs of the state of the vehicle. Both models, the motion model and the measurement model, are probabilistic. For example, the motion model is a probabilistic model that is subject to process noise, and the measurement model is a probabilistic model that is subject to measurement noise. Using the multi-head measurement model, measurements 255a, motion model, and parameters of the probability distribution determined during the previous iteration, the method runs 420a a KF that updates the first and second moments of the probability distribution, where the updated first moments of the ambiguities in the states are real-valued. The method then solves 430a a weighted least squares optimization problem that fixes the ambiguities to have integer values, where the optimization problem cost function is the squared Euclidean norm of the deviations of the first moments of the states from the real-valued estimates 425a determined by the Kalman filter 420a. Using the resulting integer ambiguities 435a, the method then runs 440a a Kalman filter initialized with the real-valued portion of the first moments and the integer-valued portion of the first moments, resulting in updated parameters of the probability distribution 445a.

[0097] In some embodiments, the KF is an extended KF, where the nonlinear parts of the motion and measurement models are linearized around the current belief of the state. In other embodiments, the KF is a linear regression KF, such as an unscented KF, a cubature KF, or a smart sampling KF. The linear regression KF avoids linearization around the current state estimate as in the extended KF, and is generally more accurate, but computationally more complex. Instead of linearization, the linear regression KF can also be used to solve the moment integrals involved through a set of weighted integration points, and to determine other parameters of the probability distribution, such as higher-order moments. Such higher-order moments can be useful in situations where the first and second moments do not adequately represent the underlying distribution.

[0098] The first and second moments received from the RF receiver may be information received from multiple sources. For example, in some embodiments, a passive roadside unit (RSU) with a camera uses highly accurate computer vision algorithms to determine a portion of the vehicle's state, e.g., position and speed. In other embodiments, the RSU includes a range sensor, e.g., radar or lidar, that directly measures the vehicle's relative position and speed to the RSU. If a portion of the state can be resolved directly with high accuracy using direct sensing, the RSU can determine and transmit the remaining portion of the vehicle's state to the vehicle.

[0099] 5A illustrates a traffic scenario that illustrates a scenario according to some embodiments. Some embodiments are based on the recognition that an active remote server is located in the cloud as an edge computing device, and in order to establish communication between the vehicle and the cloud, the information transfer between the cloud and the vehicle needs to be done through the core infrastructure network and the RSU as shown in FIG. 5A, and the communication between the vehicle V1 510a and the cloud 520a needs to be done through the RSU 530a and the core network 540a.

[0100] Other embodiments recognize that computing in the cloud introduces significant communication delays, and in such situations it is advantageous to have an edge computing device directly in the RSU.

[0101] Some embodiments are based on the recognition that the use of a remote server to communicate with a vehicle requires the use of different clocks for different devices, and synchronization between such clocks is necessary.

[0102] The vehicle clocks can be synchronized to a GNSS clock in communication with the vehicle's GNSS receiver. However, some embodiments are based on the recognition that individually synchronizing the vehicle clocks to the GNSS clock does not guarantee synchronization of the vehicles in the vehicle communication network due to different clock offset errors that different vehicles may have. In addition, the GNSS synchronization may be adversely affected by multipath of satellite signals in urban environments. In addition, some vehicles in the vehicle communication network may not be equipped with a GNSS receiver.

[0103] Various standards provide protocols for the mutual synchronization of clocks. For example, the IEEE has developed a precision time protocol (PTP)-based synchronization standard. However, these protocols require information exchange between synchronized devices, which may become impractical in the context of a multi-purpose vehicle communication network.

[0104] Some embodiments are based on the recognition that vehicular communication networks have special requirements for participating vehicles. Unlike nodes in other networks, vehicles in a vehicular communication network periodically announce their presence. For example, in the case of IEEE Dedicated Short Range Communications (DSRC) for Wireless Access in Vehicular Environments (WAVE), a vehicle is required to transmit a heartbeat message every 100 ms to announce its presence to surrounding vehicles. Attributes of the heartbeat message include one or a combination of the following: temporary ID, time, latitude, longitude, altitude, position accuracy, speed and transmission, heading, acceleration, steering wheel angle, brake system status, and vehicle size.

[0105] Some embodiments are based on the realization that this heartbeat message contains the necessary synchronization data and can therefore be used for synchronization. This approach reduces network traffic and mitigates interference. By automatically transmitting the synchronization data, vehicles can achieve silent synchronization without the message exchange performed in conventional synchronization methods.

[0106] For example, some embodiments are based on the realization that, under the assumption that the vehicle's clock offset and vehicle's location are unknown, the vehicle's clock can be synchronized to the clocks of other vehicles based on trilateration of information received in multiple heartbeat messages. The reason is that the vehicle's trilateration can be done in two different ways. One way uses the distance that light travels between the transmission and reception times of the heartbeat message, which is a function of the unknown clock offset. The other way uses the distance between the locations of the multiple vehicles, the time of transmitting the heartbeat message, and the unknown location of the vehicle at a particular time. By comparing these two types of distances, it is possible to simultaneously determine the unknown clock offset and the unknown location of the vehicle at a particular time.

[0107]

number

[0108] In some embodiments, the parameters of the probability distribution of the vehicle's states are transmitted together with the GNSS measurements to a remote server.

[0109] 5C illustrates a probabilistic system 200 for tracking a vehicle state using asynchronous coordination of information received from satellite signals 209 transmitted by a GPS satellite system (GNSS) and information transmitted over a radio frequency (RF) channel 239, according to some embodiments. The system includes a transmitter 510c that transmits parameters of the probability distributions and satellite measurement signals 511c to a remote server.

[0110] In other embodiments, the multiple vehicles transmit their parameters of the probability distribution and the GNSS measurement signals to the remote server. The parameters include the first and second moments of the probability distribution of the state of the vehicles. Such information transmission may be beneficial when the remote server, located, for example, in the RSU or in the cloud, does not have additional sensing capabilities. In such cases, the remote server may use information received from the multiple vehicles to merge such information, thus improving the estimation of the state of each vehicle, and transmit the first and second moments to the vehicles. In other words, the probabilistic system of the vehicles transmits its parameters of the probability distribution along with the satellite measurements and receives the first and second moments of the state of the vehicles in response to the transmission. In some embodiments, the remote server forms an extended state based on the states of the multiple vehicles.

[0111] In some embodiments, the extended state is a union of the states of multiple vehicles. For example, the extended state includes vehicle positions and velocities, carrier phase ambiguities, and residual bias states, such as ionospheric residual delays. For vehicles that are close enough to each other, e.g., less than 3-10 km apart, the ionospheric delays can be considered to be identical for different vehicles. For example, referring to FIG. 5D, vehicles in region 510d have very similar ionospheric delays. Thus, in some embodiments, the dimension of the extended state is smaller than the sum of the states of all vehicles, since such ionospheric delays are the same for multiple vehicles.

[0112] In the majority of GNSS applications, the residual biases are suppressed by single or double difference operations, and the focus of these applications is to recover the integer ambiguities that form part of the GNSS measurements. However, some embodiments are based on the recognition that due to the nature of the physics of the propagation of GNSS signals, residual biases still affect GNSS measurements. However, these biases cannot be determined only from a single GNSS receiver, but using a measurement extension of multiple measurements from multiple satellites measuring multiple receivers. The reason is that integer ambiguities are unique for each satellite-vehicle combination, but the residual biases are not. For example, ionospheric delays are included as residual biases in the state, which is the same for vehicles measured in close proximity to each other. Therefore, in order to recover these residual biases, the measurement noise must be estimated in order to determine the correlation between multiple vehicle measurements of satellite signals, a task that is very different from that of integer ambiguity estimation.

[0113] Some embodiments are based on the realization that such a task of estimating the residual bias is difficult to address at the level of an individual vehicle, since there are many unknowns. The reason is that each vehicle receives multiple GNSS signals from multiple satellites, and the residual bias is similar for different GNSS signals, but is not ambiguous. Therefore, using more measurements, even from a single receiver, does not help much with estimating the residual bias.

[0114] On the other hand, when using multiple measurements from multiple vehicles, there is cross-correlation between GNSS measurements from different vehicles since such measurements originate from the same satellites.

[0115] 6A illustrates a flowchart of a recursive method 600 for cooperative state tracking of multiple vehicles, according to some embodiments. The method is executed by a processor of a remote server, e.g., an RSU or an edge device in the cloud. The remote server receives 610a multiple transmissions 605a from multiple vehicles. Each transmission 605a from a vehicle includes measurements of satellite signals received at the vehicle and parameters of a probability distribution of the vehicle's state.

[0116] The remote server fuses 620a the states of the multiple vehicles defined by the parameters of the corresponding probability distributions to form an extended state 625a of the multiple vehicles. In one embodiment, method 600 simply concatenates all states together in a vector, matrix, or tensor. However, in some other embodiments, some residual biases of the neighboring vehicles are identical, and method 600 removes 620a from forming the extended state 625a as a union of the states of the multiple vehicles, and thus the dimensionality of the extended state is less than the sum of the dimensionality of each state of the multiple vehicles.

[0117] In addition, the method 600 fuses 630a measurements of satellite signals of multiple vehicles into an extended measurement of an extended state that is subject to extended measurement noise 635a. Some embodiments are based on the recognition of the dependency of the measurement noise of different vehicles. Thus, the extended measurement noise 623a is defined by an off-diagonal covariance matrix with non-zero off-diagonal elements, each of which relates the error in the measurements of a corresponding pair of vehicles.

[0118] The method 600 executes 640a a probabilistic filter that updates 645a the extended state based on the extended measurements that are subject to the extended measurement noise. Due to one or a combination of redundancy of state variables and cross-correlation of measurement noise, the accuracy of the updated extended state 645a for each individual vehicle may be greater than the accuracy of the vehicle's individually determined state.

[0119] To avoid control discontinuities, the method 600 fuses (650a) parameters of the probability distributions of at least some of the states of the multiple vehicles with corresponding portions of the updated augmented states 645a to output fused parameters of the probability distributions of at least some of the states of the multiple vehicles (655a). The fused parameters 655a can be transmitted to the vehicles and used for control of the vehicles and for other purposes that utilize the state estimate.

[0120] The execution 640a of the probabilistic filter can be done in a number of ways, using different measurement models, such as multi-head or single-head measurement models, and using various motion models, such as a constant velocity motion model, a constant acceleration motion model, a Singer model, a kinematic vehicle model, and a dynamic vehicle model, etc. Examples of probabilistic filters include a particle filter or a mixed integer Kalman filter.

[0121] Some embodiments are based on the understanding that the states of multiple vehicles can be tracked jointly using principles of probabilistic estimation similar to those of probabilistic tracking of individual vehicles. At first glance, such joint tracking does not make sense, since it increases the dimension of the tracking problem without immediately bringing any obvious benefits. The reason is that the integer ambiguities of different vehicles, which are the focus of most GNSS applications, are independent of each other in such collective tracking, so collective tracking does not seem to help resolve ambiguities.

[0122] However, some embodiments are based on the recognition that the measurement noise imposed on the collective states of the multiple vehicles, referred to in this disclosure as the extended states, during collective state estimation of the multiple vehicles, does in fact have useful cross-correlations between the noise of measurements of the states of the different vehicles that can be explored to reduce the uncertainty about the estimation of the extended states. For example, when a satellite measures two vehicles, there is a direct cross-correlation between the two measurements because the two measurements originate from the same source and therefore have similar noise characteristics.

[0123] Some embodiments are based on the realization that the extended state estimation can correct or at least improve the state estimation performed by the individual vehicles. However, to avoid discontinuities in the control of the vehicles, it is necessary to soft-merge the results of the extended and individual state tracking. The reason is that the individual vehicles can have additional on-board sensors, e.g. inertial measurement units and wheel encoders, which can be updated at a faster rate and provide different information. Thus, simply using the results from the extended state directly can result in a loss of continuity of the estimation, which can be detrimental to vehicle safety. Therefore, some embodiments fuse the parameters of the probability distribution of the state of the individual vehicles with the corresponding part of the updated extended state, as opposed to updating the state of the individual vehicles themselves. In this way, the resulting estimate is a trade-off between additional information, such as correlated measurement noise, and similarity of the residual bias used by the server, with smoothness provided by always using the individual vehicle estimates.

[0124] Additionally or alternatively, some embodiments recognize the need to reduce the computational burden of such high-dimensional probabilistic filters that track extended states. As an example, the state of a vehicle may have a state dimension of up to 100 dimensions, depending on the number of estimated residual biases and the number of satellites currently measuring the vehicle. On the other hand, there may be hundreds of vehicles in the vicinity of the vehicle, and in the worst case, the dimension of the extended state may be tens of thousands. Therefore, the computations associated with the remote server are more computationally demanding, and in particular the solution of mixed-integer estimation problems, where a large number of integer ambiguities need to be fixed, becomes prohibitive. Some embodiments recognize that it is necessary to use a mixed-integer Kalman filter to solve the mixed-integer least-squares problem of the extended state.

[0125] Some embodiments are based on the realization that when designing a recursive mixed-integer estimator, this implies the need to solve mixed-integer problems of ever-increasing size, since the current estimate of the state depends on the entire history of integer values. Therefore, in order to solve such large estimation problems, various relaxations need to be performed, as the complexity would otherwise grow exponentially over time.

[0126] One embodiment is based on the realization that instead of determining integer ambiguities by optimization from real-valued ambiguities and then updating the parameters of the distributions based on said integer ambiguities, a method to alleviate the mixed integer estimation problem is to recursively and jointly determine the parameters of two probability distributions, one distribution that works only for real-valued states and one distribution that models integer ambiguities, because when using two distributions, the real-valued estimates are not mixed with the mixed integer estimates and therefore the true distribution of the real-valued states is not contaminated.

[0127] 6B shows a flowchart of a recursive method 620a for updating the parameters of the probability distribution of the extended states at a remote server, according to some embodiments. Using measurements 605a of multiple transmissions, the method jointly updates two sets of distribution parameters, one unconstrained using real values ​​and one constrained to have ambiguities as integer values. First, using a motion model and a measurement model 622b, the method determines 620b a one-step prediction of the parameters of the probability distribution of the unconstrained extended states. Then, the method updates 630b the parameters of the probability distribution of the unconstrained extended states using satellite measurements.

[0128] The method fixes the ambiguities (640b) using real-valued parameters of the ambiguities, and constrains the extended states (619b) by determining a relationship between the satellite measurements and the extended states with the ambiguities as integers using the constrained ambiguity and measurement model 622b, resulting in the constrained relationship. The method then determines (639b) one-step predictions of the parameters of the probability distribution of the constrained extended states, and finally updates (649b) the parameters of the probability distribution of the constrained extended states.

[0129]

number

[0130]

number

[0131]

number

[0132]

number

[0133]

number

[0134] 6C illustrates a flowchart of a method 630a for fusing multiple vehicle states with an extended state, according to some embodiments. Since the vehicle state and the extended state generally do not have the same dimensions, the states need to be transformed into a joint state space that equally represents the different states. In one implementation, the method 630a transforms (610c) the parameters of the vehicle state into the same state space as the parameters of the extended state.

[0135]

number

[0136]

number

[0137] Figure 6D shows an example of state space overlap, according to some embodiments. In the left part of Figure 6D, the state spaces are similar between the three different vehicles 610d, 620d, 630d and the extended state 640d, i.e., no transformation is required between the two. In the center part of Figure 6D, the state spaces do not intersect at all, and in the right diagram, there is an overlap between the states.

[0138] The above fusion rule is conservative since it does not consider the cross-covariance between the augmented states and the vehicle states.

[0139] 6E shows a flowchart of a method 620c for fusing the extended state with the vehicle state, according to some embodiments. If necessary, such as in the third case in FIG. 6D, the method recursively updates the vehicle state and the cross-covariance between the vehicle state and the extended state using the parameters 610e transformed into the joint state space and the measurement model 615e of the satellite measurements. Finally, the method fuses 630e the first and second moments according to the determined cross-covariance.

[0140] The recursive determination of the cross-covariance (620e) can be done in multiple ways. For example, an embodiment recognizes that if a Kalman filter is used, such as an extended Kalman filter, a linear regression Kalman filter, a mixed integer extended Kalman filter, or a mixed integer linear regression Kalman filter, the cross-covariance can be recursively determined at each time step k of the estimation. This is because the probability distribution in such a filter is represented in the form of a Gaussian distribution by the first two moments.

[0141] 6F shows a flow chart of a method 620c for determining cross-covariance according to one embodiment. Using the motion model and measurement models 615f of the extended state and vehicle state, the method determines 610f the Kalman gains of the involved Kalman filters. For example, one embodiment determines a Kalman gain for the vehicle and a Kalman gain for a Kalman filter estimating the extended state, the Kalman gains being determined according to well-established techniques.

[0142]

number

[0143]

number

[0144]

number

[0145]

number

[0146] Alternatively or in lieu of this, the Kalman gain is part of the information 215a received from the vehicle.

[0147] Other embodiments are based on the recognition that recursively calculating the cross-covariances can be problematic when using particle filters, because in such approaches the distributions are not represented solely by parameters as first and second moments, but as mixtures of first and second moments as Gaussian mixtures. However, other embodiments recognize that for particle filters the Kalman gain for the most likely particles can be used instead.

[0148]

number

[0149] One embodiment recognizes that there are several inversions of potentially very high dimensional matrices involved in the fusion of parameters. One embodiment uses Schur complements to reduce the size of the matrices that need to be inverted to generate the fused covariances.

[0150] Other embodiments are based on the realization that when measurements from multiple vehicles are used, there may be redundant information that can be inferred from the measurements.

[0151] FIG. 6G illustrates an example of the cross-covariance in the measurement noise of a full measurement model including measurements from multiple transmissions from multiple vehicles, according to some embodiments. FIG. 6G illustrates a matrix representing the elements of the covariance of the measurement noise. When stacking measurements resulting in an extended measurement model of the extended state, if satellites are not shared between the vehicles, the diagonal 610g will be the only part with non-zero components. However, if the vehicles share the same satellites used in generating differential satellite measurements, i.e., single differences or double differences, the off-diagonal 620g and 630g may have non-zero components 640g if properly modeled. Such non-zero components are exploited by the probabilistic filter to enhance knowledge of the extended state estimate.

[0152] Some embodiments appreciate that when the measurement noise matrix R is formed, off-diagonal non-zero elements may be automatically used in the KF regardless of whether they are present or not. However, when they have non-zero off-diagonals, i.e., non-zero cross-covariances, the KF can take advantage of this to better determine the Kalman gains that determine the first and second moment updates based on the measurement errors.

[0153] In one embodiment, the measurement noise matrix is ​​formed by single and / or double differences of the code and carrier phase measurements using the principle that there is mutual covariance between the measurements. For example, in some implementations, the non-zero off-diagonal elements 640 correspond to pairs of vehicles and are determined by the combination of the covariances of the corresponding pairs of vehicles.

[0154] Figure 6H shows a flow chart of a method for determining a measurement noise matrix such that the measurement noise matrix has the structure of Figure 6G. The method sorts (610g) the satellite measurements and forms (615h) a structure of sorted satellite measurements. For example, the measurements may be sorted according to satellite elevation angles, with the satellite with the highest elevation angle being sorted highest, the second highest being sorted second highest, and the satellite with the lowest elevation angle being sorted last.

[0155]

number

[0156]

number

[0157]

number

[0158]

number

[0159] 7 illustrates a server 700 for tracking the state of multiple vehicles using asynchronous coordination of information received from the vehicles transmitted over an RF channel 709, for example, according to some embodiments. The server includes an RF receiver 760 for receiving first and second moments, satellite measurements, and corresponding measurement noise of said measurements from multiple transmissions from multiple vehicles.

[0160] The system includes a memory 780 that stores a probabilistic measurement model that relates incoming satellite measurements to the state of the vehicle (781). The memory also stores a probabilistic motion model that relates previous states of the vehicle to a prediction of the state of the vehicle by the motion model (782). For example, the motion model can be a stacked vehicle dynamics model, a stacked constant acceleration model, a stacked constant position model, a stacked coordinated turn model, a stacked Singer model, or a combination of different stacked models, where stacking means stacking multiple models on top of each other.

[0161] The memory also stores instructions 783 for implementing probabilistic filters according to some embodiments of the present invention.

[0162] The probabilistic system 700 includes an RF measurement module 740 operatively connected 750 to an RF receiver 760 that receives information 709 transmitted over an RF channel from a plurality of vehicles. In some embodiments, the information includes first and second moments of the plurality of vehicles. In other embodiments, the information includes satellite signals received from the vehicles, the signals including satellite measurements, position information, and timing information.

[0163] Some embodiments use a kinematic model to relate the first and second moments of the state at a previous time step to the first and second moments of the state at a current time step. For example, one embodiment uses a model of time propagation using a kinematic model to propagate the first and second moments of the state of the vehicle to the current time. Examples of models of time propagation include the Singer model, the constant acceleration model, and the single track vehicle model. Using both moments in time propagation is advantageous over propagating only the first moment because it takes into account the propagation of uncertainties and therefore how much the first moment should be trusted when updating the state. In other embodiments, time propagation is performed by a combination of a kinematic model of the vehicle and measurements of the vehicle.

[0164] In various embodiments, the probabilistic system uses the first and second moments and the satellite measurements to update the first and second moments using information received from multiple transmissions from multiple vehicles. For example, a GNSS signal is received and upon receiving the signal, the processor 730 uses a probabilistic filter 732 to determine (733) updated first and second moments.

[0165] In another embodiment, the probabilistic system 700 includes an RF transmitter 770 for transmitting 739 the updated first and second moments, i.e., the mean and covariance, corresponding to the vehicles. For example, the mean and covariance of vehicle 1 are transmitted to vehicle 1. Illustrative Embodiments

[0166] 8 illustrates an example of vehicle-to-vehicle (V2V) communication and planning based on state estimation 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 autonomous or semi-autonomous.

[0167] In this example, multiple vehicles 800, 810, 820 are moving on a given highway 801. Each vehicle can perform many movements. For example, the vehicles can stay on the same route 850, 890, 880 or can change routes (or lanes) 860, 870. Each vehicle has its own sensing capabilities, e.g., lidar, camera, etc. Each vehicle has the possibility to send and receive information 830, 840 to its neighbors and / or exchange information indirectly through other vehicles via a remote server. For example, vehicles 800 and 880 can exchange information through vehicle 810. In this type of communication network, information can be transmitted across a highway or a large part of the highway 801.

[0168] Some embodiments are configured to address the following scenario: For example, vehicle 820 wants to change its route and selects option 870 in its route plan. However, at the same time, vehicle 810 also selects to change lanes and wants to follow option 860. In this case, the two vehicles may collide, or the best vehicle 810 will have to perform emergency braking to avoid colliding with vehicle 820. The present invention can help in this case. Therefore, some embodiments may be configured such that the vehicle not only transmits what it senses at the current time t, but also, or alternatively, transmits what it senses at time T+δ. t This allows you to send what you plan to do in

[0169] In the example of FIG. 8, vehicle 820 informs vehicle 810 of its plan to change lanes after planning and committing to execute the plan. Thus, vehicle 810 determines whether δ t 8. In the time interval t1, vehicle 810 knows that vehicle 820 plans to move to the left 870. Vehicle 810 can therefore choose to move 890 instead of 860, i.e. to stay in the same lane.

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

[0171] FIG. 9 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 930, 970, 950, 960 moving on a highway 901. Now, suddenly, there is an accident ahead of the vehicle platoon in zone 900. This accident makes the zone 900 where the vehicles are moving unsafe. The vehicles 920, 960 detect the problem, for example using a camera, and communicate this information to the vehicles 930, 970. The platoon then runs a distributed optimization algorithm, for example a formation-maintenance multi-agent algorithm, which selects the optimal shape of the platoon to avoid the accident zone 900 and keep the flow of vehicles uninterrupted. In this specific example, the optimal shape of the platoon is to line up and form a line 995 to avoid the zone 900.

[0172] FIG. 10 illustrates a block diagram of a system 1000 for direct and indirect control of mixed autonomous vehicles, according to some embodiments. The system 1000 may 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 1000 may have several interfaces connecting the system 1000 to other machines and devices. A network interface controller (NIC) 1050 includes a receiver configured to connect the system 1000 to a network 1090 connecting the system 1000 to the mixed autonomous vehicles via a bus 1006 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 willing to join the platoon and at least one non-controlled vehicle. 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 roadway, a current speed, and a current acceleration of the mixed autonomous vehicles. In some embodiments, the mixed autonomous vehicles include all non-controlled vehicles within a predetermined range from an adjacent controlled vehicle in the platoon.

[0173] The NIC 1050 also includes a transmitter adapted to transmit control commands to the controlled vehicles over the network 1090. To that end, the system 1000 includes an output interface, e.g., a control interface 1070, configured to provide control commands 1075 to the controlled vehicles in the group of mixed autonomous vehicles over the network 1090. In this manner, the system 1000 may be located on a remote server in direct or indirect wireless communication with the mixed autonomous vehicles.

[0174] The system 1000 may also include other types of input / output interfaces. For example, the system 1000 may include a human-machine interface 1010. The human-machine interface 1010 may connect the controller 1000 to a keyboard 1011 and a pointing device 1012, which may include a mouse, a trackball, a touchpad, a joystick, a pointing stick, a stylus, or a touch screen, among others.

[0175] The system 1000 includes a processor 1020 configured to execute stored instructions and a memory 1040 that stores instructions executable by the processor. The processor 1020 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 1040 may include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory machine. The processor 1020 may be connected to one or more input / output devices via a bus 1006.

[0176] The processor 1020 is operatively connected to a memory storage device 1030 that stores instructions and stores processing data used by the instructions. The storage device 1030 may form part of or be operatively connected to the memory 1040. For example, the memory may be configured to store a probabilistic filter 1031 trained to track extended states of the mixed autonomous vehicle and transform traffic conditions into a target roadway for the mixed autonomous vehicle, and to store one or more models 1033 configured to describe vehicle motion. For example, the models 1033 may include a motion model, a measurement model, a traffic model, etc.

[0177] The processor 1020 is configured to determine control commands for the controlled vehicles, which also indirectly control the non-controlled vehicles. To that end, the processor is configured to determine the control commands based on the states of the vehicles by executing a control generator 1032. In some embodiments, the control generator 1032 uses a deep reinforcement learning (DRL) controller trained to generate control commands from augmented states for individual and / or platooning of vehicles.

[0178] FIG. 11A illustrates a schematic diagram of a directly or indirectly controlled vehicle 1101 according to some embodiments. As used herein, the vehicle 1101 can be any type of wheeled vehicle, such as a car, a bus, or a rover. The vehicle 1101 can also be an autonomous or semi-autonomous vehicle. For example, some embodiments control the movement of the vehicle 1101. An example of the movement includes the lateral movement of the vehicle, which is controlled by a steering system 1103 of the vehicle 1101. In one embodiment, the steering system 1103 is controlled by a controller 1102 in communication with the system 1000. Additionally or alternatively, the steering system 1103 can be controlled by a driver of the vehicle 1101.

[0179] The vehicle may also include an engine 1106 that may be controlled by the controller 1102 or by other components of the vehicle 1101. The vehicle may also include one or more sensors 1104 for sensing the surrounding environment. Examples of the sensors 1104 include a distance range finder, a radar, a lidar, and a camera. The vehicle 1101 may also include one or more sensors 1105 for sensing its current amount of motion and internal status. Examples of the sensors 1105 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 1102. The vehicle may also include a transceiver 1106 that enables the communication functions of the controller 1102 through wired or wireless communication channels.

[0180] FIG. 11B shows a schematic diagram of the interaction between a controller 1102 receiving control commands from the system 1000 and a controller 1100 of a vehicle 1101, according to some embodiments. For example, in some embodiments, the controller 1100 of the vehicle 1101 is a steering controller 1110 and a brake / throttle controller 1120 that control the rotation and acceleration of the vehicle 1100. In such a case, the controller 1102 outputs control inputs to the controllers 1110 and 1120 to control the state of the vehicle. The controller 1100 may also include a higher level controller, for example a lane keep assist controller 1130, which further processes the control inputs of the predictive controller 1102. In either case, the controller 1100 maps and uses the output of the predictive controller 1102 to control the movement of the vehicle by controlling at least one actuator of the vehicle, such as the steering and / or brakes of the vehicle. The vehicle machine state xt may include position, orientation, and forward / lateral speed, and the control inputs ut may include lateral / forward acceleration, steering angle, and engine / brake torque. The state constraints for the system may include lane keeping constraints and obstacle avoidance constraints. The control input constraints may include steering angle constraints and acceleration constraints. The data collected may include position, orientation, speed profile, acceleration, torque, and / or steering angle.

[0181] 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, which may be located in one computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit, with one or more processors in an integrated circuit component. However, the processor may be implemented using circuitry in any suitable format.

[0182] Also, the various methods or processes outlined herein may be coded as software executable on one or more processors employing 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.

[0183] Also, embodiments of the invention may be implemented as a method, an example of which is provided. The order of operations performed as part of the method may be determined in any suitable manner. Thus, embodiments may be configured to perform operations in an order different from that illustrated, which may include performing some operations simultaneously, although in the illustrated embodiment they are shown as a sequence of operations.

[0184] While the invention has been described by examples of preferred embodiments, it is to be understood that various other adaptations and modifications are possible 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 probabilistic system for tracking a state of a vehicle using information received from satellite signals transmitted by a Global Navigation Satellite System (GNSS) and information received at a radio frequency (RF) receiver in the vehicle, the probabilistic system comprising: a memory configured to store a probabilistic multi-head measurement model relating incoming measurements to a state of the vehicle, the probabilistic multi-head measurement model including a first head relating measurements of the satellite signals subject to measurement noise to beliefs about a state of the vehicle, and a second head relating estimates of the vehicle's state subject to estimated noise to beliefs about the state of the vehicle, the probabilistic system further comprising: at least one processor configured to process executable instructions of a module of the stochastic system, the module comprising: a GNSS measurement module operatively connected to a GNSS receiver and configured to determine measurements of the satellite signals and the measurement noise; an RF measurement module configured to receive data indicative of an estimate of a state of the vehicle and the estimated noise contained in information received by an RF receiver of the vehicle; and a probabilistic filter configured to generate a belief on a state of the vehicle by recursively updating parameters of a probability distribution of the state of the vehicle based on a multi-head measurement model that accepts one or a combination of measurements of the satellite signals affected by the measurement noise and estimates of the state of the vehicle affected by the estimation noise.

2. 2. The probabilistic system of claim 1, wherein the probabilistic filter recursively propagates parameters of a probability distribution of the vehicle's state according to a motion model of the vehicle's state transitions affected by process noise, and updates the parameters of the probability distribution upon receiving an output of one or a combination of the first head of a multi-head measurement model and the second head of a multi-head measurement model.

3. The probabilistic system of claim 1 , wherein the vehicle states include one or more state biases that capture ambiguities in satellite signal propagation and residual errors in atmospheric delays.

4. The probabilistic filter receives the satellite signal measurements and the measurement noise and executes the first head of a multi-head measurement model to: estimating measurements of states of the vehicle that match the measurements of the satellite signals to generate a measured state of the vehicle; converting the measurement noise into a second moment that represents the covariance of the measured state of the vehicle; The probabilistic system of claim 1 , further comprising: a processor configured to compare a belief about a state of the vehicle with a measured state of the vehicle measured using the covariance; and output a comparison result.

5. 5. The probabilistic system of claim 4, wherein the output of the first head of the multi-head measurement model includes an error between the measured state of the vehicle and a current belief about the state of the vehicle inserted into the measurement model subject to a covariance of the measurement noise.

6. 5. The probabilistic system of claim 4, wherein the output includes a measured state of the vehicle, a current belief of the state given to the first head of the multi-head measurement model inserted into the measurement model, and a covariance of the measurement noise.

7. The probabilistic filter receives the vehicle state estimate and the estimated noise and executes the second head of the multi-head measurement model to: Transforming the received vehicle state estimates into an estimated state of the vehicle; Transforming the received estimated noise into a covariance of an estimated state of the vehicle that matches the covariance of the measured states of the vehicle; The probabilistic system of claim 4 , further comprising: a processor configured to compare a current belief regarding the state of the vehicle with an estimated state of the vehicle estimated using a covariance of the estimated state of the vehicle; and output a comparison result.

8. 8. The probabilistic system of claim 7, wherein the received estimate of the vehicle's state is a first moment of the vehicle's state and the received estimated noise is a second moment of the vehicle's state.

9. 9. The probabilistic system of claim 8, wherein the covariance of the estimated vehicle state is determined as a weighted combination of a second moment of the estimate of the vehicle state and a second moment of a probability distribution of beliefs over the vehicle state.

10. 9. The probabilistic system of claim 8, wherein a covariance of the estimated state of the vehicle is determined based on a difference between a probability distribution defined by the received first and second moments of the estimate of the state of the vehicle and a probability distribution of beliefs regarding the state of the vehicle defined by the parameters maintained by the probabilistic filter.

11. 9. The probabilistic system of claim 8, wherein the estimated state of the vehicle is determined as a weighted combination of beliefs on the state of the vehicle and the received first moments of the state of the vehicle, the weighting including a covariance of the estimation noise.

12. 3. The probabilistic system of claim 2, wherein the probabilistic filter propagates parameters of a probability distribution of the vehicle state at each time point and updates the parameters of the probability distribution of the vehicle state for a current time point based on the output of the first head or the output of the second head of the multi-head measurement model.

13. 3. The probabilistic system of claim 2, wherein the probabilistic filter propagates parameters of a probability distribution of the vehicle's state at each time point and updates, for a current time point, the parameters of the probability distribution of the vehicle's state based on a combination of the output of the first head and the output of the second head of the multi-head measurement model merged to become an extended output of the multi-head measurement model.

14. 2. The probabilistic system of claim 1, wherein the received data indicates an estimate of the vehicle's state and the estimation noise for a previous time point, and the estimate of the vehicle's state and the estimation noise for the previous time point are propagated to the current time point using a model of time propagation to update parameters of a probability distribution of the vehicle's state at the current time point.

15. The probabilistic system of claim 14 , wherein the model of time propagation includes one or a combination of a kinematic model of the vehicle, a Singer model, a constant acceleration model, and a single track vehicle model.

16. 2. The probabilistic system of claim 1, wherein the probabilistic filter is one or a combination of a mixed integer extended Kalman filter, a mixed integer linear regression Kalman filter, a linear Kalman filter, an extended Kalman filter, and a particle filter.

17. The probabilistic system of claim 1 , wherein the RF measurement module receives the estimate of the vehicle's state and the estimated noise transmitted by a roadside unit (RSU).

18. 1. A probabilistic method for tracking a state of a vehicle using information received from satellite signals transmitted by a Global Navigation Satellite System (GNSS) and information received at a radio frequency (RF) receiver of the vehicle, the method comprising: The method includes using a processor coupled to a memory storing a probabilistic multi-head measurement model relating incoming measurements to a state of the vehicle, the probabilistic multi-head measurement model including a first head relating measurements of the satellite signals subject to measurement noise to beliefs about the state of the vehicle, and a second head relating estimates of the vehicle's state subject to estimated noise to beliefs about the state of the vehicle, the processor being coupled to stored instructions which, when executed by the processor, perform steps of the method, the steps of the method including: determining measurements of said satellite signals and said measurement noise using a GNSS receiver; receiving data from an RF receiver of the vehicle indicative of an estimate of a state of the vehicle and an estimated noise included in information received by the RF receiver; and generating a belief on the state of the vehicle by recursively updating parameters of a probability distribution of the state of the vehicle based on a multi-head measurement model that accepts one or a combination of measurements of the satellite signals affected by the measurement noise and estimates of the state of the vehicle affected by the estimation noise.

Images