Cooperative state tracking of multiple vehicles using individual and joint estimation
The cooperative and asynchronous state tracking system using a multi-head measurement model and mixed integer extended Kalman filter addresses GNSS positioning challenges by integrating internal and external data, enhancing accuracy and continuity in vehicle state estimation.
Patent Information
- Application Number
- JP2024500700
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- Priority Date
- 2021-03-30
- Filing Date
- 2022-01-13
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2042-01-13
AI Technical Summary
Existing Global Navigation Satellite System (GNSS) positioning technologies face challenges in maintaining accurate vehicle state estimation due to loss of lock, multipath errors, and atmospheric delays, especially in urban environments, leading to difficulties in resolving integer ambiguities and cycle slips, which affect positioning accuracy.
A cooperative but asynchronous state tracking system using a multi-head measurement model that integrates internal and external vehicle information, allowing probabilistic filters to update vehicle states seamlessly, even with asynchronous data from external sources, and incorporates a mixed integer extended Kalman filter to handle non-linear and mixed-integer positioning problems.
Enhances GNSS positioning accuracy by reducing the impact of integer ambiguities and atmospheric delays, ensuring continuous and precise vehicle state estimation even in challenging environments, such as urban canyons, through cooperative and asynchronous data integration.
Smart Images

Figure 0007592212000027 
Figure 0007592212000028 
Figure 0007592212000029
Abstract
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 resolving vehicle states 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. To achieve faster and more accurate calculation of the geographic position of a mobile receiver, various Global Navigation Satellite (GNS) correction systems are known that are configured to receive GNSS signal data from GNSS satellites, process these GNSS data, calculate GNSS corrections from the GNSS data, and provide these corrections to the mobile receiver.
[0003] Various methods of position estimation are known. In these methods, the position calculation is based on repeated measurements of so-called pseudo range and carrier phase observations by an Earth-based GNSS receiver. The "pseudo range" or "code" observations represent the difference between the time of transmission of a GNSS satellite signal and the time of local reception of this satellite signal, and therefore contain the geometric distance covered by the satellite's radio signal. Measuring the alignment between the carrier of a received GNSS satellite signal and a copy of such a signal generated in the receiver provides another source of information for determining the apparent distance between the satellite and the receiver. This corresponding observation is called the "carrier phase" and represents the integral of the Doppler frequency due to the relative motion between the transmitting satellite and the receiver. Summary of the Invention [Problem to be solved by the invention]
[0004] All pseudorange observations contain unavoidable error contributions, including receiver and transmitter clock errors, and additional delays caused by the non-zero refractive index of the atmosphere, instrument delays, multipath effects, and detector noise. All carrier phase observations further contain an unknown integer number of signal cycles, or wavelengths, that have elapsed until lock-in to this signal alignment is obtained. This number is called the "carrier phase ambiguity." Typically, the observations are measured, or sampled, by the receiver at successive discrete times. The index of time at which an observation is measured is called an "epoch." Known position determination methods generally involve dynamic numerical estimation and correction schemes for range and error components based on measurements of sampled observations at successive epochs.
[0005] If GNSS signals are tracked continuously and no loss of lock occurs, the integer ambiguity resolved at the beginning of the tracking phase can be maintained for the entire GNSS positioning period. However, GNSS satellite signals may be occasionally obstructed (e.g., by buildings in an "urban canyon" environment) or momentarily blocked (e.g., when the receiver goes under a bridge or through a tunnel). In general, in such cases, the integer ambiguity value is lost and must be re-determined. This process may take from a few seconds to a few minutes. In fact, the presence of significant multipath errors or unmodeled systematic biases in one or more measurements of either the pseudorange or carrier phase may make it difficult to resolve the ambiguity using current commercial positioning systems. As the receiver separation (i.e., the distance between the reference receiver and the mobile receiver whose position is being determined) increases, the distance-dependent biases (e.g., orbit errors and ionospheric and tropospheric effects) increase, making reliable ambiguity resolution (or reinitialization) even more difficult. Loss of lock can also occur due to discontinuities in the receiver's continuous phase lock to the signal, called cycle slips. For example, cycle slips can be caused by power loss, receiver software failure, or satellite oscillator malfunction. Additionally, cycle slips can be caused by changing ionospheric conditions.
[0006] GNSS enhancement refers to techniques used to increase the accuracy of positioning information provided by the Global Positioning System or other Global Navigation Satellite Systems, networks of satellites commonly used for navigation. For example, methods use differential techniques based on satellite-to-satellite differentials, receiver-to-receiver differentials, epoch-to-epoch differentials, and combinations thereof. Single and double differentials between satellites and receivers reduce but do not eliminate error sources.
[0007] Therefore, there is a need to improve the accuracy of GNSS positioning. To address this issue, a number of different methods use the cooperation of multiple GNSS receivers to improve the accuracy of GNSS positioning. However, to properly cooperate, the multiple GNSS receivers need to be synchronized and their operation needs to be constrained. For example, U.S. Pat. No. 9,476,990 describes cooperative GNSS positioning estimation by multiple mechanically connected modules. However, such restrictions on the cooperative improvement of the accuracy of GNSS positioning are not always realistic. [Means for solving the problem]
[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) presuppose either individual or centralized estimation based on internal modules of the vehicle, or distributed estimation, which performs the state estimation in a tightly controlled and / or synchronized manner. Examples of such distributed estimation include decentralized 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 type of tracking, 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 internal modules of a moving vehicle use additional information derived from the outside. 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 internal modules of the vehicle, but where external information can be seamlessly integrated 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. The first part estimates samples of the state of the vehicle, and the second part updates the probability distribution of the state based on these estimated samples. At first glance, these two parts are integrated, since one part cannot be executed without the other. However, some embodiments are based on the realization that these two parts are related by a causal dependency, not a time dependency. Specifically, 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, when and how the state sample is estimated does not depend on the update of the probability distribution.
[0011] Some embodiments utilize this understanding to disambiguate the probabilistic state estimation and update parts. This disambiguation allows the probabilistic updates to be reused regardless of when and how the state samples are estimated. Thus, the state estimates can be determined by an internal module of the vehicle based on GNSS measurements, or by an external module to the vehicle if such a module is 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 estimation, not on the frequency of arrival of new information. In this way, the internal and external state estimators can operate asynchronously.
[0012] Based on these understandings, some embodiments disclose a multi-head measurement model that allows a probabilistic filter to update the confidence in the state of the vehicle using different sources. The multi-head measurement model includes multiple paths that generate different types of output 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 confidence in the state of the vehicle subject to measurement noise, and the second head associates estimates of the state of the vehicle with confidence in the state of the vehicle subject to estimation noise. Thus, the measurement model includes different information and different types of noise, but has a similar structure that is accepted 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 parameters of a probability distribution of the vehicle's state according to a motion model of the vehicle's state transitions subject to process noise at each update step, and updates the parameters of the probability distribution upon receiving outputs 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 confidence in the state of the vehicle using GNSS measurements with 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 update frequency of the probabilistic filter. Second The first head allows updating the confidence in the vehicle's state using external information that may be received only occasionally but may have a higher state estimation accuracy. For example, the second head may process state estimates received from an external measurement module. Thus, although the second head may receive information less frequently, regardless of how frequently the first and / or second heads of the multi-head measurement model run, the outputs of the first and second heads are seamlessly integrated without compromising the probabilistic guarantees of the probabilistic filter.
[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 estimates of the vehicle's state determined independently from GNSS measurements performed by the internal measurement module. For example, the vehicle may 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 may also be an RSU, and in response receive external measurements obtained based on corrections of the internal measurements. Here, the current information includes the mean of the vehicle's state estimate, the covariance of the vehicle's state estimate, and a set of code and carrier phase measurements that the internal measurement module uses to update its internal state.
[0016] It is therefore an objective of some embodiments to provide a cooperative, yet asynchronous, estimation of the vehicle's state suitable for use by a probabilistic state estimation. Another objective of some embodiments is to provide such a cooperative, 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 the estimation of the receiver state 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 in the receiver position, and incorporate various biases and integer ambiguities. In the 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 they are held constant and then jump sporadically and independently of each other to new integer values. Thus, the GNSS positioning problem can be viewed as a mixed-integer GNSS positioning problem that solves a mixed-integer problem of ever-increasing size (each time step contains a new integer bias), and careful consideration must be given to how best to 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, i.e., the covariance, of the probability distribution of the state of the vehicle. Therefore, to benefit from the active mode of the external measurement module, some embodiments transmit the parameters of the probability distribution to allow a remote server to correct this information. The remote server can collect similar information from other vehicles in the vicinity, fuse the received information to correct the parameters of the probability distribution of the state of the vehicle, and send 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 moment 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 update the probability distribution of the state of the vehicle using the probabilistic filter with the sampled state estimates.
[0019] In some embodiments, the vehicle state includes the vehicle position, the vehicle velocity, integer ambiguities associated with the vehicle's carrier phase measurements from specific satellites, and residual bias states that model atmospheric delays, such as ionospheric and tropospheric delays. For example, some methods use differencing techniques based on satellite-to-satellite differences, receiver-to-receiver differences, epoch-to-epoch differences, and combinations thereof. Single and double differences between satellites and receivers reduce state estimation accuracy because they reduce but do not eliminate error sources.
[0020] Some embodiments are based on the realization that ignoring state biases such as ionospheric errors can result in slight inaccuracies in the state estimates, since the biases are typically 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. Some embodiments therefore 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 asynchronous but cooperative communication with an external server can address the problem of state estimation accuracy if the external state estimates determined at the remote server are accurate enough, however that server still needs to determine the state to the desired accuracy.
[0022] Some embodiments recognize that it may be impractical to accurately determine the atmospheric time delay for each vehicle because the probabilistic filter for each vehicle must resolve both the integer ambiguity uncertainty and the atmospheric delay. However, the atmospheric delay is comparable for vehicles that are close enough to each other, e.g., 2-10 km. Thus, for an external active module that collects similar information from multiple vehicles, the external measurement module can update the state of each vehicle in a way that individual vehicle probabilistic filters cannot, since the states of different vehicles overlap significantly with each other. 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 probabilistic filters can be due to ambiguities and residual delays as well as measurement noise. Some embodiments are based on the recognition that when determining the states of multiple vehicles cooperatively, 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 cooperative state estimation. For example, errors in measurements of different pairs of vehicles may be correlated with 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 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 its current estimate, resulting in a mixed integer extended Kalman filter (EKF). Other embodiments recognize that the linearization in the EKF may be inaccurate. Thus, 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] One embodiment recognizes that some of the vehicle's states, e.g., integer ambiguities, are linear in the model, but the position is non-linear, and therefore some of the first and second moments can be determined analytically, while others are estimated with a linear regression Kalman filter.
[0027] The GNSS positioning problem can be viewed as a mixed-integer GNSS positioning problem solving a mixed-integer problem that keeps growing in size (each time step contains a new integer bias), and careful consideration must be given to how best to relax this estimation problem to make the resulting algorithm feasible. Especially for external estimation of states when combining information from multiple vehicles, the resulting estimation problem becomes very high-dimensional and therefore computationally expensive.
[0028] In some embodiments, this relaxation is solved by determining two related but separate probability distributions, one of which considers only real-valued parameters, i.e., ignoring that the ambiguity is in fact integer, and the second distribution is determined based on the integer ambiguity determined from the first distribution by solving an optimization problem.
[0029] Accordingly, one embodiment discloses a server for jointly tracking states of multiple vehicles, each state including one or more state biases incorporating residual error of atmospheric delays, the server comprising at least one processor and a memory storing instructions which, when executed by the at least one processor, cause the server to receive a plurality of transmissions from the multiple vehicles, each transmission from a vehicle including a measurement of a satellite signal received at the vehicle and a parameter of a probability distribution of the state of the vehicle, further cause the server to fuse the states of the multiple vehicles defined by the corresponding parameters of the probability distribution into an extended state of the multiple vehicles, and fuse the measurements of the satellite signals of the multiple vehicles into an extended measurement of the extended state subject to extended measurement noise defined by an off-diagonal covariance matrix having non-zero off-diagonal elements, each non-zero off-diagonal element being related to an error of the measurements of a pair of corresponding vehicles. The server is further configured to execute a probabilistic filter to update the extended state based on the extended measurements subject to the extended measurement noise, and to fuse the parameters of the probability distribution of the states of at least some of the plurality of vehicles with corresponding portions of the updated extended state to output fused parameters of the probability distribution of the states of at least some of the plurality of vehicles. [Brief description of the drawings]
[0030] [Figure 1A] FIG. 1 illustrates 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] FIG. 2 illustrates a scenario in which multipath disrupts a signal for a receiver. [Figure 1E]FIG. 2 is a conceptual diagram of a probabilistic filter used by some embodiments. [Figure 1F] FIG. 2 illustrates asynchronous reception of different signals according to some embodiments. [Figure 1G] FIG. 2 illustrates the structure of a multi-head measurement model 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 shows another flowchart of a method for updating the first and second moments of the 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's state using asynchronous coordination of information, 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. 2 illustrates an example of a method for determining confidence in a state of a vehicle according to some embodiments. [Figure 3A] FIG. 2 illustrates a flowchart of an example implementation method for vehicle state estimation according to some embodiments. [Figure 3B] FIG. 2 illustrates a flowchart of a method for determining carrier phase ambiguity according to some embodiments. [Figure 3C] FIG. 2 illustrates a flowchart of an exemplary implementation of a method for sampling floating-point values consistent with a motion model and its process noise, according to one embodiment. [Figure 3D] FIG. 1 shows a schematic illustrating some principles employed by some embodiments. [Figure 4] FIG. 2 illustrates a flowchart of a method for implementing a probabilistic filter for updating parameters of a probability distribution of a state of a vehicle, according to one embodiment. [Figure 5A] FIG. 1 illustrates a traffic scenario illustrating a scenario according to some embodiments. [Figure 5B] FIG. 1 illustrates 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 so as to obtain the structure of FIG. 6G. [Figure 7] FIG. 1 illustrates a probabilistic system for tracking states of multiple vehicles using asynchronous coordination of information received from the vehicles and transmitted over an RF channel according to some embodiments. [Figure 8] FIG. 1 illustrates an example of vehicle-to-vehicle (V2V) communication and planning based on state estimation, according to one embodiment. [Figure 9] FIG. 1 is a schematic diagram of multi-vehicle platoon shaping for accident avoidance scenarios, according to one embodiment. [Figure 10] FIG. 10 is a block diagram of a system 1000 for direct and indirect control of a mixed autonomous vehicle, according to some embodiments. [Figure 11A] FIG. 11 shows a schematic diagram of a vehicle 1101 that is directly or indirectly controlled according to some embodiments. [Figure 11B] FIG. 1 illustrates 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
[0031] Description of the embodiments 1A illustrates a schematic of a Global Navigation Satellite System (GNSS) according to some embodiments. For example, an Nth satellite 102 transmits code and carrier-phase measurements (120 and 121) 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.
[0032] 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 receiver whose location is known. For example, the receiver 131 may be a ground-based receiver. In contrast, the receiver 130 is a mobile receiver configured to move. For example, the receiver 130 may be mounted on a cell phone, a car, or a tablet. In some implementations, the second receiver 131 is optional and may be used to eliminate or at least reduce uncertainties and errors due to various sources, such as atmospheric effects and errors in the internal clocks of the receivers and satellites. In some embodiments, there are multiple GNSS receivers that receive the code and carrier phase signals.
[0033] 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, which 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 with each other over the airwaves 161.
[0034] An object of some embodiments is 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. Yet another object of some embodiments is to provide such a system and method that is probabilistic, i.e., that takes into account probabilistic disturbances and error sources. An object of other embodiments is to track the state of the vehicle using different information from different sources without relying solely on satellite signals. For example, in some embodiments, the state of the vehicle is 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.
[0035] Additionally or alternatively, it is an object of some embodiments to provide a filter that is probabilistic, such as a state estimation filter, that provides a state estimate based on a motion model and a measurement model.
[0036] One example of a state estimator is a Kalman filter. A Kalman filter uses a series of measurements observed over time, including statistical noise and other inaccuracies, to generate estimates of unknown variables that tend to be more accurate than estimates based on a single measurement alone, by estimating a joint probability distribution for the variables for each time window. 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 subject to the process noise of the GNSS receiver and a measurement model of the satellite signals subject to measurement noise. In some embodiments, the measurement model is probabilistic and multi-headed, i.e., it includes multiple paths that generate different types of outputs. In some implementations, the multi-head measurement model includes two heads. The first head associates measurements of the satellite signals with a confidence in the state of the vehicle subject to measurement noise, and the second head associates an estimate of the state of the vehicle with a confidence in the state of the vehicle subject to the estimated noise. Thus, the measurement models include different information and different types of noise, but have a similar structure that is accepted by the probabilistic filter, allowing for separate or joint use of the outputs of the different heads.
[0037] In this manner, in some embodiments, the probabilistic filter recursively propagates parameters of a probability distribution of the vehicle's states in accordance with a motion model of the vehicle's state transitions subject to process noise at each update step, and updates the parameters of the probability distribution upon receiving outputs 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.
[0038]
number
[0039] In some embodiments, the ambiguity is included in the vehicle state. Other embodiments also include bias states that incorporate residual errors in atmospheric delays, e.g., ionospheric delays. If the receivers are close enough to each other, the ionospheric delays are the same or nearly similar for different vehicles. Some embodiments exploit this relationship to resolve these delays and / or ambiguities.
[0040]
number
[0041] 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 position. When a carrier signal transmitted from one satellite is received by two receivers, the difference between the first carrier phase and the second carrier phase is called the single difference (SD) of the carrier phase. Alternatively, SD can be defined as the difference between signals from two different satellites that reach the receiver. For example, the difference can come from the first and second satellites, 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 a single SD signal, where satellite 101 is the base satellite. The difference between the SD of the carrier phase obtained from the wireless signals from the two satellites using the paired receivers 131 and 130 of FIG. 1A is called the double difference (DD) of the carrier phase. When the carrier phase difference is converted to a number of wavelengths, e.g. λ=19 cm for L1 GPS (and / or GNSS) signals, this is divided by a fractional part and an integer part. Positioning equipment can measure the fractional part, but cannot directly measure the integer part. The integer part is therefore called the integer bias or integer ambiguity.
[0042] In general, GNSS can use multiple constellations simultaneously to determine receiver state. For example, GPS, Galileo, Glonass, and QZSS can be used simultaneously. Satellite systems typically transmit information in up to three different frequency bands, and for each frequency band, each satellite transmits a code measurement and a carrier phase measurement. These measurements can be combined as single differences or double differences. Single differences involve taking the difference between a reference satellite and the other satellites, while double differences involve also taking the difference between the receiver of interest and a base receiver with a known stationary position.
[0043]
number
[0044]
number
[0045] Additionally or alternatively, some embodiments are based on the recognition that ignoring state biases such as ionospheric errors may result in slight inaccuracies in the state estimates, since the biases are typically 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 state and determine them as part of the state tracking provided by the probabilistic filter.
[0046] In certain scenarios, e.g., deep urban canyons, the presence of multiple distortions of the satellite signals makes the information contained in the code and carrier phase signals difficult to perform high-precision state estimation. For example, FIG. 1D shows a diagram of a scenario in which multipath disrupts the signal for receiver 101d. 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 these signals are not transmitted directly to the receiver due to obstructions 170d, e.g., urban buildings.
[0047] Up until now, the signal 138d transmitted from satellite 140d was unavailable, but suddenly, satellite signal 139d arrives at the receiver through multipath 102d. In such a scenario, the performance of the probabilistic filter in tracking the state of the vehicle may be significantly impaired, as the estimator locks onto an incorrect ambiguity estimate, resulting in large estimation errors. However, there may be other receivers nearby that do not have such problems. For example, one receiver may have multipath and be in the shadow of a large building, so that few satellites are visible, but the estimation performance of a vehicle several blocks away is good. In some embodiments, this is exploited by using measurements from the GNSS measurement module and a remote estimate of the state of the vehicle, which is determined using data from multiple vehicles, to update the parameters of the probability distribution in the probabilistic filter.
[0048] An example of a probabilistic filter is the Kalman filter. Figure 1E shows a schematic of the Kalman filter (KF) used by some embodiments for state estimation. The KF is a tool for state estimation in linear state space models 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 the two necessary quantities, sufficient statistics, to describe the Gaussian distribution.
[0049] Starting from initial knowledge 110e of the states, the KF determines the mean of the states and their variance 111e. The KF then predicts 120e the states and variance to the next time step using a model of the system, such as a vehicle motion model, to obtain updated mean and variance 121e of the states. The KF then uses measurements 130e in an update step 140e using a measurement model of the system to determine updated mean and variance 141e of the states. An output 150e is then obtained and the procedure is repeated for the next time step 160e.
[0050] Some embodiments employ probabilistic filters including various variants of KFs, e.g., extended KFs (EKFs) such as unscented KFs (UKFs), linear regression KFs (LRKFs). Even with multiple variants of KFs, they conceptually function as illustrated by FIG. 1E. Notably, the KF uses measurements 130e described by the probabilistic measurement model to update the first and second moments, i.e., mean and covariance, of the target probability distribution. In some embodiments, the probabilistic measurement model is a multi-head measurement model structured to satisfy the principle of measurement update in the KF.
[0051] Some embodiments are based on the realization that the outputs of different heads of a multi-head measurement model of some embodiments may not be synchronized and may receive signals 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 structured such that the KF can accept it.
[0052] FIG. 1F shows a diagram of asynchronous reception of different signals, according to some embodiments. As time progresses, a GNSS signal is received at a certain time step, and the first and second moments are received at other time steps. For example, the GNSS signal and the first and second moments are both received at time 110f. However, a second GNSS signal is received at time 120f, but the second reception of the first and second moments is at time 130f. That is, the signals may or may not be received simultaneously. To handle such signals that may be received at different time steps, the measurement model is structured to be in a form that allows the different signals to be used in the KF independently of each other.
[0053] Thus, in some embodiments, the measurement model is probabilistic and multi-headed, i.e., it includes several parts, where a first head associates measurements of satellite signals with a confidence in the state of the vehicle subject to measurement noise, and a second head associates estimates of the state of the vehicle with a confidence in the state of the vehicle subject to the estimated noise. Thus, the measurement model includes different information and different types of noise.
[0054] In one embodiment, a first head of the measurement model subject to measurement noise is used to map GNSS measurements to a confidence in the vehicle state, and a second head of the measurement model subject to measurement noise is used to map estimates of the vehicle state to a confidence in the vehicle state received by other means.
[0055] Some embodiments are based on the principle of correcting and updating 140e the first and second moments using measurements 130e and a measurement model by processing the measurements using a measurement model. In order to use a KF, the measurement model and the measurement noise must be described in a probabilistic form such that they can be handled in a balanced way so that the measurements do not erroneously scale the first and second moments.
[0056] Another embodiment is based on the realization that the probabilistic structure needs to be harmonized between the first and second heads of the measurement model. In particular, the output has the same structure, regardless of whether the updating step 140e of the probability distribution of the vehicle state is performed using GNSS signals or using the received vehicle state estimate and the estimated noise, e.g. expressed as first and second moments.
[0057] FIG. 1G shows a diagram of the structure of a multi-head measurement model 100g, according to some embodiments. The multi-head measurement model accepts inputs and associates GNSS measurements with a confidence in the vehicle state using a first head 110g of the measurement model, and associates alternative estimates of the vehicle state with a confidence in the vehicle state using a second head 120g. For example, the multi-head measurement model accepts 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 may have zero mean and covariance R k The 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 associates the measurements 130g with confidences 160g of the vehicle's states (111g). The output 112g from the first head of the measurement model is in a form that can be accepted by the KF. For example, in one embodiment, the output 112g is a relation 111g described as an error between the measurement, the confidence of 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 measurement, the confidence of the states inserted in the measurement model, and the measurement noise covariance.
[0058]
number
[0059] Some embodiments are based on the understanding that representing outputs 112g and 122g in the same probabilistic form allows the KF to be updated in the same manner regardless of whether the update information is 112g or 122g, i.e., whether the information comes from the first head or the second head.
[0060] 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 jointly. 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 again (120h) using the output 122g from the second head, resulting in updated moments 121h.
[0061] Other embodiments are based on the understanding that the GNSS measurements and the received vehicle state estimates and estimated noise may not be received at the same time instance, as illustrated in Figure 1F. Figure 1I shows another flow chart of a method for updating the first and second moments, for example using a KF. Using the output 112g from the first head of the measurement model, the method updates (110h) the moment 111h.
[0062] 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.
[0063] Other embodiments are based on the recognition that updates using the first and second heads can be performed jointly, i.e. simultaneously. Figure 1K shows a flow chart of an 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 extended output of the measurement model. That is, the GNSS measurements, the estimates of the vehicle state, and the associated relationships are combined, resulting in a joint large-scale update.
[0064] 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 further transmitted via radio frequency (RF) channels. First, the method receives 210a from a GNSS receiver 209a signals transmitted from a set of satellites 101, 102, 103, 104, 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 traveled 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 estimated noise, determined by an external server, from the vehicle's RF receiver 229a via RF signals 235a. In various embodiments, the estimates and estimated noise 235a are probabilistically determined as first moments of the vehicle state and second moments of the vehicle state 235a. Additionally or alternatively, the estimates and estimated noise 235a may be converted to first moments of the vehicle state and second moments of the vehicle state using various probabilistic techniques.
[0065] Using the first and second moments, the method determines 240a an estimate of the vehicle's state and an 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 associates 250a the GNSS measurements 225a with a confidence for the vehicle's state using a first head of the measurement model subject to the measurement noise, and associates 250a the estimate of the state with a confidence for the vehicle's state subject to the 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 confidence for the vehicle's state.
[0066] 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 impairments affecting the satellite signals. For example, the navigation messages may include parameters necessary to determine atmospheric delays. For example, when using the Klobuchar atmospheric delay model, the navigation messages include parameters for determining ionospheric time delays according to the ionospheric effects on signal propagation determined by the atmospheric delay model. In other embodiments, such messages may include parameters for determining ionospheric delays according to an SBAS delay model.
[0067] The system includes a memory 280 that stores a probabilistic multi-head measurement model (281) that correlates received measurements with vehicle states. In some embodiments, the multi-head measurement model consists of two heads. A first head correlates measurements of satellite signals with a confidence for the vehicle state subject to measurement noise. A second head correlates estimates of the vehicle state with a confidence for the vehicle state subject to the estimated noise. The memory also stores instructions on how to determine such a relationship (283), and the memory stores instructions for implementing a probabilistic filter according to some embodiments of the invention (284). The memory also stores a probabilistic motion model (285), that correlates previous confidence for the vehicle state with a prediction of the vehicle state according to the motion model. For example, the motion model can 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.
[0068] In some embodiments, associating the measurements of the satellite signals with a confidence for the state, and associating the estimates of the state with a confidence for the state, is performed by inserting the above quantities into first and second heads of a measurement model, respectively.
[0069] The GNSS receiver is operatively connected (250) to a GNSS measurement module which, after determining the delays and impairments described above, determines measurements of the satellite signals and associated measurement noise, for example, in one embodiment, the measurement noise is determined based on a nominal measurement noise scaled (282) as a function of the satellite elevation angle and stored in memory.
[0070] 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 vehicle's state. In other embodiments, the information includes a first moment and a second moment of the vehicle. In other embodiments, the information includes higher moments to form an overall probability distribution of the vehicle's state. In other embodiments, the information includes data indicative of an estimate of the vehicle's state and an estimated noise, such as first and second moments as samples, or includes higher moments. In still other embodiments, the time of receiving the information is different from the time the information was determined. For example, in some embodiments, an active remote server includes instructions to determine the first and second moments, and execution of such instructions may be associated with the communication time between the remote server and the RF receiver. Thus, in some embodiments, the information 239 includes a timestamp of the time the first and second moments were determined.
[0071] In various embodiments, the GNSS measurement module and the RF measurement module generate respective estimates related to the confidence in the state of the vehicle at the current time instance. For example, a GNSS signal is received, and upon receiving the signal, the processor 230 determines (231) a measurement value for the current time step. However, in some implementations, for example if the remote server takes time to execute the instructions, the RF measurement module receives the first and second moments of the state of the vehicle determined for a previous time step. As a result, the determined measurement value and the received first and second moments do not correspond to the same instant in time.
[0072] To address this issue, using a motion model, some embodiments temporally associate the first and second moments of the state of the previous time step with the confidence of the state at the current time step to obtain an estimate of the state that indicates the confidence in the state of the vehicle at the current time instance. For example, one embodiment uses a model of vehicle time propagation, e.g., a motion model stored in memory, to propagate the first and second moments of the state of the vehicle in time to the current time instance. Examples of models of time propagation include the Singer model, the constant acceleration model, and the single-track vehicle model. Using both moments for time propagation is more advantageous than propagating only the first moment, since it takes into account the propagation of uncertainty and therefore how much the first moment can be trusted when updating the confidence of the state. In other embodiments, the time propagation is performed by combining the vehicle motion model and vehicle measurements.
[0073] Some embodiments are based on the realization that, regardless of the time instance of receiving the first and second moments of the vehicle's state, when updating the parameters of the probability distribution with the estimate of the state subject to measurement noise, the estimation noise needs to be configured such that the probability distribution of the state's confidence converges to the probability distribution of the received second moment of the state. Some embodiments are based on the realization that convergence can be ensured by balancing the covariance of the received second moment with the covariance of the probability distribution of the current confidence of the state. In other words, the estimate of the state and the associated measurement noise are required to be balanced to maintain convergence.
[0074] In some embodiments, processor 230 executes a probabilistic filter 232 that is used to update parameters of the probability distribution to determine an estimate 233 of the vehicle's state.
[0075] Some embodiments are based on the understanding that it may be tempting to directly set the parameters of the probability distribution to match the parameters of the received first and second moments. However, enforcing convergence is advantageous because it ensures that from the vehicle's perspective, the same probabilistic filter can be used to update the parameters of the probability distribution. Moreover, 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 introduce discontinuities in the updated state estimate of the vehicle. If the estimate is used to control the vehicle, such discontinuities may be disadvantageous because they result in a loss of continuity in control.
[0076] FIG. 2C illustrates a flow chart of an example method for determining 240a an estimate, according to some embodiments. First, based on received information indicative of an estimate of the vehicle's state and an estimated noise, the method determines 210c a distribution of the estimate of the vehicle's state. 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 the vehicle's state confidence. For example, the method inserts parameters of the probability distribution of the vehicle's state, which is assumed to follow a predetermined shape. For example, for a Gaussian distribution, the parameters of the probability distribution are the first and second moments. For example, assuming a Student's t distribution, the parameters are the first moment, the scale, and the degrees of freedom. Using the distribution of the vehicle's estimate 215c and the distribution of the state confidence 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.
[0077] The differencing (230c) can be done in a number of ways. For example, for Gaussian distributions, the differencing 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 differencing is done by the well-known Kullback-Leibler divergence.
[0078] Some embodiments are based on the realization that even if 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 estimated noise covariance 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 vehicle's state confidence and the received first moment of the estimate of the vehicle's state, where the weighting includes the estimated noise covariance.
[0079]
number
[0080]
number
[0081] Some embodiments recognize that one purpose of the second head of the measurement model is to enable the update of the parameters of the probability distribution 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 the 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 such that the convergence of the confidence of the state of the vehicle converges to the estimate of the state of the vehicle. For example, in one embodiment, the second head of the measurement model includes the difference between the estimated state of the vehicle and the confidence of the state of the vehicle determined in the previous iteration, weighted by the covariance of the estimation noise. For example, in one embodiment, the second head of the measurement model includes z k =x k +r k In the formula, z k is the estimated vehicle state, and x k is the confidence in the vehicle state, and rk is the estimated noise and k is the time step of the recursion.
[0082] Some embodiments are based on the realization that to update the parameters of a probability distribution using a probabilistic filter, the probabilistic filter should be able to update the parameters taking into account that some of the vehicle's states are real-valued and some are integer-valued, because some parts, e.g. the vehicle's position, are in the states and are real-valued, but the ambiguity is in the states and is integer-valued.
[0083]
number
[0084] Some embodiments are based on the understanding that a particle filter is a filter that can solve a mixed integer estimation problem without using optimization methods. Other embodiments understand that to avoid using optimization methods, a particle filter requires a large number of particles, which may be computationally prohibitive. For this reason, some embodiments solve the recursive mixed integer weighted least squares problem by employing a mixed integer extension KF. Other embodiments solve a mixed integer linear regression KF.
[0085] FIG. 3A shows a flow chart of an exemplary implementation method for vehicle state estimation. The method receives state measurements and estimates including a multi-head measurement model (310a). Each carrier signal includes a carrier phase ambiguity as an unknown integer number of wavelengths of the carrier signal that traveled between the satellite 101, 102, 103 or 104 and the receiver 130. The method then uses the carrier phase ambiguity of the carrier signals and the estimate of the vehicle's state to the confidence of the vehicle's state to retrieve 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 relates the measurements of the carrier and code signals to the current confidence of the vehicle's state. Both models, the motion model and the measurement model, are probabilistic. For example, the motion model is a probabilistic model subject to process noise and the measurement model is a probabilistic model subject to measurement noise.
[0086] The method then determines 320a a set 325a of possible combinations of integer values of carrier phase ambiguities that match the measurements of the carrier and code signals according to one or a combination of the motion model and the measurement model 322a, within a range defined by one or a combination of the process noise and the measurement noise. This step is based on the understanding that instead of determining the carrier phase ambiguity and trying to perform a state estimation, it is beneficial to determine and test different possible combinations of carrier phase ambiguities to perform a state estimation. In this way, the best carrier phase ambiguity can be selected using a probability model that better reflects the nuances of the state estimation.
[0087] Therefore, this method uses a motion model and a measurement model. 322a run a set of state estimators that jointly use 330a). Each state estimator includes a corresponding combination of integer values of carrier phase ambiguity to determine a joint probability distribution 335a of the vehicle's state relative to the motion model and measurement model 322a. In this manner, combinations of integer values of carrier phase ambiguity 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 ambiguity 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 vehicle's state according to the measurements of the carrier and code signals using the multi-head measurement model and the estimates of the vehicle's state.
[0088] As such, 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 done probabilistically using a match of the motion and measurement model to the probability density function (PDF) of the noise of the first head of the measurement model and the noise of the second head of the measurement model.
[0089] 3B illustrates a flowchart of a method for determining 320a possible combinations of integer values of carrier phase ambiguity according to some embodiments. The method may be performed 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 floating-point values of the carrier phase ambiguity relative to the PDF of the measurement noise centered on a noiseless fit of the carrier phase ambiguity, the state, the measurements of the carrier and code signals to the measurement model, and the estimate of the state for at least one satellite.
[0090] In some embodiments, the PDF of the measurement noise is predetermined, for example based on the characteristics of the GNSS receiver. As used herein, a "noiseless fit" is a fit of the carrier phase ambiguity, position, and measurements of the carrier and code signals, assuming that the obtained measurements of the carrier and code signals are correct. That is, assuming that the measurements of the carrier and code signals are correct, the residual error is due to the carrier phase ambiguity. In this way, the noiseless fit places the PDF in a position where sampling on the PDF does not emphasize the stochastic noise of the motion and measurement model, but emphasizes the effect of the carrier phase ambiguity on the position estimate. In other words, the error of the fit to the measurement model after inserting the carrier phase ambiguity, position, and measurements of the carrier and code signals into the measurement model is due to the error of the carrier phase ambiguity.
[0091] Next, the method forms a union of the sampled floating-point values (320b) to generate a union 321b that includes all of the sampled floating-point values, and discretizes the union of the floating-point values on an integer basis ( 330b ), to generate possible integer values 331b of carrier phase ambiguity for the set of GNSS satellites. Finally, the method uses the possible integer values 331b of carrier phase ambiguity for all satellites to generate 340b a set of possible combinations of integer values 325a.
[0092] FIG. 3C illustrates a flow chart of an example implementation of a method 310b for sampling floating-point values according to one embodiment. However, sampling floating-point values 310b is performed in several ways according to different embodiments. The method of FIG. 3C samples floating-point values that are consistent with the motion model and its process noise (309c). The method determines a match with the measurements of the code and carrier phase signals by inserting the sampled carrier phase ambiguity, the estimated state, and the measurements of the carrier and code signals into a measurement model (311c). Based on the match with the measurements, the method corrects each sampled floating-point value as a function of the process noise and the measurement noise (312c), updates the probability of each ambiguity based on the match with the corrected measurements (313c), removes the corrected sampled floating-point values of the carrier phase ambiguity, and preserves the floating-point values of the carrier phase ambiguity that have a probability of fitting the measurement model above a threshold (314c).
[0093] FIG. 3D shows a schematic illustrating some principles employed by some embodiments. In particular, some embodiments are based on the recognition that a finite number of possible integer values of carrier phase ambiguity allows for determining (340d) different combinations of those possible integer values for tracking the state of the vehicle. Such recognition allows for the replacement (380d) of evaluating (310d) the carrier phase ambiguity to estimate (320d) the state of the vehicle 330d with evaluating (360d) different states 350d of the vehicle determined using different combinations 340d of carrier phase ambiguities. This replacement is advantageous because the stochastic nature of the motion of the receiver is more suitable for testing position than testing derivatives of position such as carrier phase ambiguities. In this way, the best state 370d selected using the stochastic nature of the motion of the vehicle automatically indicates the corresponding combination of carrier phase ambiguities used to determine such state 370d.
[0094] Some embodiments are based on the recognition that particle filters may be computationally prohibitive and that for computational reasons a Kalman-type filter that embeds an optimization solver may be advantageous.
[0095] FIG. 4 illustrates a flow chart of a method 260a for performing a probabilistic filter for updating parameters of a probability distribution of a state of a vehicle where 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 a carrier phase ambiguity as an unknown integer number of wavelengths of the carrier signal that traveled between a satellite 101, 102, 103 or 104 and the vehicle 130. The method then retrieves (422a) from memory a motion model that relates a previous state of the vehicle to a current state of the vehicle, and a multi-head measurement model that relates measurements of the carrier and code signals to a current confidence in the state of the vehicle, using the carrier phase ambiguity of the carrier signals and the estimates of the state of the vehicle to the confidence in 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 subject to process noise, and the measurement model is a probabilistic model subject to measurement noise. Using the multi-head measurement model, the measurements 255a, the motion model, and the parameters of the probability distribution determined in 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 deviation 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 445a parameters of the probability distribution.
[0096] In some embodiments, the KF is an extended KF, where the nonlinear parts of the motion and measurement models are linearized around the current confidence 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. A linear regression KF avoids linearization around the current state estimate as in an extended KF, and is generally more accurate, but more computationally complex. Instead of linearization, a linear regression KF solves the moment integrals involved by a set of weighted integration points, and can also be used 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 represent the underlying distribution well enough.
[0097] The first and second moments received from the RF receiver can be information received from multiple sources. For example, in some embodiments, a passive roadside unit (RSU) with a camera uses computer vision algorithms to determine a portion of the vehicle's state, e.g., position and velocity, with high accuracy. In other embodiments, the RSU is equipped with a range sensor, e.g., radar or lidar, that directly measures the vehicle's position and velocity relative to the RSU. If a portion of the state can be directly resolved with high accuracy using direct sensing, the RSU can then determine the remaining state of the vehicle and transmit it to the vehicle.
[0098] 5A illustrates a traffic scenario illustrating 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 to establish communication between the vehicle and the cloud, information transfer between the cloud and the vehicle needs to go through a core infrastructure network and an RSU as shown in FIG. 5A, and communication between the vehicle V1 510a and the cloud 520a needs to go through an RSU 530a and a core network 540a.
[0099] Other embodiments recognize that performing computations in the cloud introduces significant communication delays, and in such situations it is advantageous to have edge computing devices directly on the RSU.
[0100] Some embodiments are based on the recognition that the use of a remote server in communication with the vehicle necessitates the use of different clocks for different devices, and synchronization between such clocks is necessary.
[0101] The vehicle clocks can be synchronized with a GNSS clock that communicates with the vehicle's GNSS receiver. However, some embodiments are based on the recognition that individually synchronizing the vehicle clocks with 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 have a GNSS receiver.
[0102] 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, those protocols require information exchange between the devices to be synchronized, which may be impractical in the context of a versatile vehicular communication network.
[0103] Some embodiments are based on the recognition that vehicular communication networks have special requirements for participating vehicles. Unlike nodes in other networks, vehicles in vehicular communication networks periodically announce their presence. For example, the IEEE Dedicated Short Range Communications (DSRC) for Wireless Access in Vehicular Environments (WAVE) requires vehicles to transmit a heartbeat message every 100 ms to announce their presence to nearby 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.
[0104] 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. Because the synchronization data is transmitted automatically, vehicles can achieve silent synchronization without the message exchanges that are performed in conventional synchronization methods.
[0105] 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 with the clocks of other vehicles based on trilateration of information received in multiple heartbeat messages. The reason is that the trilateration of the vehicle can be performed in two different ways. One method 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 method uses the distance between the locations of multiple vehicles, the transmission times of the heartbeat messages, 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.
[0106]
number
[0107] 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.
[0108] 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.
[0109] 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 a first moment and a second moment of the probability distribution of the state of the vehicles. Such information transmission may be beneficial when the remote server, located, for example, in an RSU or in the cloud, does not have additional sensing capabilities. In such a case, the remote server can use information received from the multiple vehicles to combine such information, thus improving the estimate 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 the parameters of the probability distribution together with the satellite measurements to the vehicle 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 state of the multiple vehicles.
[0110] 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 residual ionospheric delays. If the vehicles are close enough, e.g., less than 3-10 km apart, the ionospheric delays can be considered to be the same for different vehicles. For example, with reference to FIG. 5D, the vehicles in region 510d have approximately similar ionospheric delays. Thus, in some embodiments, the dimension of the extended state is smaller than the sum of the states of all the vehicles, since such ionospheric delays are the same for multiple vehicles.
[0111] In most GNSS applications, the residual biases are suppressed through 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 are determined using a measurement extension of multiple measurements from multiple satellites measuring multiple receivers. The reason is that while integer ambiguities are specific to each combination of satellite and vehicle, the residual biases are not. For example, ionospheric delays are included in the state as residual biases, which are the same for vehicles measured in close proximity to each other. Therefore, in order to recover these residual biases, it is necessary to estimate the measurement noise and determine the correlation between the measurements of multiple vehicles of the satellite signals, which is a very different task from that of integer ambiguity estimation.
[0112] Some embodiments are based on the realization that such a task of estimating the residual bias is difficult to address at the individual vehicle level due to the many unknowns. The reason is that each vehicle receives multiple GNSS signals from multiple satellites, and while the residual bias is similar for different GNSS signals, the ambiguity is not. Therefore, using more measurements, even for a single receiver, does not help much in terms of estimating the residual bias.
[0113] On the other hand, when using multiple measurements from multiple vehicles, cross-correlation exists between GNSS measurements from different vehicles since such measurements originate from the same satellites.
[0114] 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 in a remote server, e.g., an edge device in an RSU or 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.
[0115] 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, the method 600 simply concatenates all the states together into a vector, matrix, or tensor. However, in some other embodiments, the residual biases of some of the neighboring vehicles are identical, and the method 600 forms 620a the extended state 625a as a union of the states of the multiple vehicles to remove duplicates, and the dimensionality of the extended state is less than the sum of the dimensionality of each of the states of the multiple vehicles.
[0116] In addition, the method 600 fuses 630a measurements of satellite signals of multiple vehicles into an extended measurement 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, where each non-zero off-diagonal element is related to the error of the measurements of a pair of corresponding vehicles.
[0117] The method 600 executes 640a a probabilistic filter that updates the extended state 645a based on the extended measurements subject to 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 higher than the accuracy of the vehicle's individually determined state.
[0118] To avoid control discontinuities, the method 600 fuses 650a parameters of the probability distribution of at least some of the states of the plurality of vehicles with corresponding portions of the updated augmented states 645a to output fused parameters 655a of the probability distribution of at least some of the states of the plurality of vehicles. The fused parameters 655a may be transmitted to the vehicles and used for control of the vehicles and for other purposes that utilize the state estimates.
[0119] The probabilistic filter execution 640a can be performed in many ways, using different measurement models such as multi-head or single-head measurement models, and using various motion models such as constant velocity motion model, constant acceleration motion model, Singer model, kinematic vehicle model, and dynamic vehicle model, etc. Examples of probabilistic filters include particle filters or mixed integer Kalman filters.
[0120] Some embodiments are based on the realization 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 dimensionality of the tracking problem without any immediately visible benefits. The reason is that the integer ambiguities of different vehicles, which are the focus of most GNSS applications, become independent of each other in such collective tracking, and thus collective tracking does not help in resolving the ambiguities.
[0121] However, some embodiments are based on the recognition that the measurement noise imposed on the collective state of multiple vehicles (i.e., referred to in this disclosure as the extended state) during collective state estimation of multiple vehicles actually has useful cross-correlations between the noise of measurements of the states of different vehicles that can be explored to reduce the uncertainty regarding the estimation of the extended state. For example, when a satellite measures two vehicles, there is a direct cross-correlation between the two measurements since they originate from the same source and therefore have similar noise characteristics.
[0122] 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 softly combine the results of the extended and individual state tracking. The reason is that the individual vehicles may have additional on-board sensors, e.g., inertial measurement units and wheel encoders, that can be updated at a faster rate and provide different information. Thus, only using the results from the extended state directly may result in a loss of continuity in the estimation, which may have a negative impact on the safety of the vehicle. Therefore, some embodiments fuse the parameters of the probability distribution of the states of the individual vehicles with the corresponding parts of the updated extended state, as opposed to updating the states of the individual vehicles themselves. In this way, the resulting estimate is a trade-off between the additional information, such as the similarity of the correlated measurement noise and residual biases used by the server, and the smoothness provided by always using the estimates of the individual vehicles.
[0123] Additionally or alternatively, some embodiments recognize the need to reduce the computational burden of such high-dimensional probabilistic filters that track extended states. For 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 that currently measure the vehicle. Meanwhile, hundreds of vehicles may be in close proximity to each other, and in a worst-case scenario, the dimension of the extended state may be tens of thousands. Thus, the calculations involving the remote server become computationally intensive, and in particular the solution of mixed-integer estimation problems that require a large number of integer ambiguities to be fixed becomes prohibitive. Some embodiments recognize that a mixed-integer Kalman filter should be used to solve the mixed-integer least-squares problem for the extended state.
[0124] Some embodiments are based on the realization that designing a recursive mixed-integer estimator implies the need to solve mixed-integer problems of ever-increasing size, since the current estimate of a state depends on the entire history of integer values. Therefore, various relaxations need to be applied to solve such large estimation problems, since the complexity would otherwise grow exponentially over time.
[0125] One embodiment is based on the realization that, instead of determining the integer ambiguity by optimization from the real-valued ambiguity and then updating the parameters of the distribution based on said integer ambiguity, as a way to alleviate the mixed integer estimation problem, it is beneficial to recursively jointly determine the parameters of two probability distributions, one of which works only for real-valued states and the other distribution models the integer ambiguity, because when using two distributions, the estimates of the real values are not mixed with the estimates of the mixed integers, and therefore the true distribution of the real-valued states is not degraded.
[0126] FIG. 6B shows a flowchart of a recursive method for updating 620a 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 parameters of the distributions: one unconstrained set using real values, and the other set constrained to have ambiguities as integer values. First, using the motion model and measurement model 622b, the method determines 620b one-step predictions of the parameters of the probability distribution of the unconstrained extended states. Then, the method uses satellite measurements to update 630b parameters of the probability distribution of the unconstrained extended states.
[0127] Using the real-valued parameters of the ambiguity, the method fixes the ambiguity (640b), and using the constrained ambiguity and measurement model 622b, the method constrains the extended states by determining a relationship between the satellite measurements and the extended states with the ambiguity as an integer (619b), resulting in a constrained relationship. Next, the method determines a one-step prediction of the parameters of the probability distribution of the constrained extended states (639b), and finally updates the parameters of the probability distribution of the constrained extended states (649b).
[0128]
number
[0129]
number
[0130]
number
[0131]
number
[0132]
number
[0133] 6C illustrates a flowchart of a method 630a for fusing multiple vehicle states with an extended state, according to some embodiments. Since the vehicle states and the extended states generally do not have the same dimensions, it is necessary to transform the states into a combined state space that equally represents the different states. In one embodiment, the method 630a transforms (610c) the parameters of the vehicle states into the same state space as the parameters of the extended states.
[0134]
number
[0135]
number
[0136] Figure 6D shows an example of state space overlap, according to some embodiments. On the left side of Figure 6D, the state space is similar between the three different vehicles 610d, 620d, 630d and the extended state 640d, i.e., no transformation is required between them. In the center of Figure 6D, the state spaces are completely separate, while in the right diagram, the states overlap.
[0137] The above fusion rule is conservative since it does not consider the cross-covariance between the augmented states and the vehicle states.
[0138] Figure 6E shows a flowchart of a method 620c for fusing the extended state with the vehicle state, according to some embodiments. Optionally, as in the third case of Figure 6D, the method recursively updates the cross-covariance between the vehicle states and between the vehicle states and the extended state using the parameters transformed into the joint state space (610e) and the measurement model of the satellite measurements 615e. Finally, the method fuses the first and second moments according to the determined cross-covariance (630e).
[0139] The recursive determination of the cross-covariance (620e) can be done in multiple ways. For example, an embodiment recognizes that when using a Kalman filter, 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, since the probability distribution in such a filter is represented by the first two moments in the form of a Gaussian distribution.
[0140] 6F shows a flow chart of a method 620c for determining cross-covariance according to one embodiment. Using the motion and measurement models 615f of the extended state and vehicle state, the method determines 610f the Kalman gains of the associated Kalman filters. For example, one embodiment determines a Kalman gain for the vehicle and a Kalman gain for a Kalman filter that estimates the extended state, where the Kalman gains are determined according to established techniques.
[0141]
number
[0142]
number
[0143]
number
[0144]
number
[0145] Alternatively or additionally, the Kalman gain is part of the information 215a received from the vehicle.
[0146] Other embodiments are based on the recognition that when using particle filters, recursively calculating the cross-covariances can be problematic because in such an approach the distributions are not represented solely by parameters as first and second moments, but as a mixture of first and second moments as a Gaussian mixture. However, other embodiments recognize that in the case of particle filters, the Kalman gain for the most likely particles can be used instead.
[0147]
number
[0148]
number
[0149] Other embodiments are based on the realization that when measurements from multiple vehicles are used, there may be extra information that can be inferred from the measurements.
[0150] 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. If the measurements resulting in the extended measurement model for the extended state are stacked, but no satellites are shared between the vehicles, then the diagonal elements 610g will be the only part with non-zero components. However, if the vehicles share the same satellites that are used in creating differential satellite measurements, i.e., single differences or double differences, then the off-diagonal elements 620g and 630g can have non-zero components 640g if properly modeled. Such non-zero components are utilized by the probabilistic filter to increase knowledge of the extended state estimate.
[0151] Some embodiments appreciate that once the measurement noise matrix R is formed, it can be automatically used by the KF regardless of whether it has off-diagonal non-zero elements. However, if it does have non-zero off-diagonal elements, i.e., non-zero cross-covariances, the KF can utilize this to better determine the Kalman gains that determine the first and second moment updates based on the measurement errors.
[0152] 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 combining the covariances of the corresponding pairs of vehicles.
[0153] FIG. 6H shows a flow chart of a method for determining the measurement noise matrix to obtain the structure of FIG. 6G. This method involves sorting the satellite measurements ( 610h ), forming a structure of sorted satellite measurements 615h. For example, the measurements may be sorted according to the elevation angle of the satellites, with the satellite with the highest elevation angle being sorted first, the satellite with the second highest elevation angle being sorted second, and the satellite with the lowest elevation angle being sorted last.
[0154]
number
[0155]
number
[0156]
number
[0157]
number
[0158] 7 illustrates a server 700 for tracking the state of multiple vehicles using asynchronous coordination of information received from the vehicles, e.g., transmitted over an RF channel 709, 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 the multiple vehicles.
[0159] The system includes a memory 780 that stores a probabilistic measurement model that correlates received satellite measurements with a state of the vehicle (781). The memory also stores a probabilistic motion model that correlates a previous state of the vehicle with a prediction of the state of the vehicle according to 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 turning model, a stacked Singer model, or a combination of different stacked models, where "stacking" means stacking multiple models on top of each other.
[0160] The memory also stores 783 instructions for implementing a probabilistic filter according to some embodiments of the present invention.
[0161] 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.
[0162] Some embodiments use a motion model to relate the first and second moments of the state of a previous time step to the first and second moments of the state at the current time step. For example, one embodiment uses a model of time propagation using a motion model to propagate the first and second moments of the state of the vehicle in time to the current time instance. Examples of models of time propagation include the Singer model, the constant acceleration model, and the single-track vehicle model. Using both moments for time propagation is more advantageous than propagating only the first moment because it takes into account the propagation of uncertainty and therefore how much the first moment can be trusted when updating the state. In other embodiments, the time propagation is performed by combining a motion model of the vehicle with measurements of the vehicle.
[0163] 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 the probabilistic filter 732 to determine (733) updated first and second moments.
[0164] 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. Exemplary embodiments
[0165] 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.
[0166] In this example, multiple vehicles 800, 810, 820 are traveling on a given highway 801. Each vehicle can perform a number of motions. For example, the vehicles can stay on the same route (850, 890, 880) or change route (or lane) (860, 870). Each vehicle has its own sensing capabilities, e.g., lidar, camera, etc. Each vehicle can send and receive information to nearby vehicles (830, 840) and / or indirectly through other vehicles via a remote server. For example, vehicles 800 and 820 can exchange information through vehicles 810. This type of communication network can transmit information over a large portion of a highway or arterial road 801.
[0167] Some embodiments are configured to handle the following scenario: For example, vehicle 820 wants to change route and selects option 870 in its route planning. However, at the same time, vehicle 810 also selects to change lanes and wants to follow option 860. In this case, these two vehicles may collide and at best vehicle 810 will have to apply emergency brakes to avoid colliding with vehicle 820. This is where the present invention can help. Therefore, some embodiments may not only require the vehicle to transmit what it senses at the current instant t, but also, or instead, to transmit what it senses at time T+δ. t It also allows the vehicle to transmit what it plans to do to the
[0168] In the example of Fig. 8, after vehicle 820 plans and commits to execute its plan to change lanes, it notifies vehicle 810 of the plan. To this end, vehicle 810 notifies vehicle 820 of the plan during the time interval δ t It is known that vehicle 820 is going to move to its left side 870 after 860. Therefore, vehicle 810 can choose motion 890 instead of 860, i.e., stay in the same lane.
[0169] Additionally or alternatively, the motion of the vehicles can be jointly controlled by a remote server based on the collaboratively determined state estimates. For example, in some embodiments, the multiple vehicles determined for the joint state estimate are those that form, and potentially could form, a jointly controlled vehicle platoon with a shared control objective.
[0170] FIG. 9 is a schematic diagram of multi-vehicle platoon shaping for an accident avoidance scenario, according to one embodiment. For example, assume that a group of vehicles 930, 970, 950, 960 are traveling on a highway 901. Now, suddenly, assume that an accident occurs in front of the vehicle platoon in zone 900. Due to this accident, zone 900 becomes unsafe for vehicles to travel. Vehicles 920, 960 detect the problem, for example with a camera, and communicate this information to vehicles 930, 970. The platoon then runs a distributed optimization algorithm, for example a formation-maintaining multi-agent algorithm, that selects the best shape of the platoon to avoid the accident zone 900 and not disrupt the flow of vehicles. In this illustrative example, the best shape of the platoon is to avoid zone 900 and form a line (995).
[0171] FIG. 10 shows 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 multiple interfaces connecting the system 1000 with other machines and devices. A network interface controller (NIC) 1050 includes a receiver adapted to connect the system 1000 with a network 1090 via a bus 1006, which connects the system 1000 with the mixed automaton vehicles to receive traffic conditions of a group of mixed autonomous vehicles traveling in the same direction. The group of mixed autonomous vehicles includes a control vehicle and at least one non-control vehicle willing to join the platoon formation, and the traffic conditions indicate the state of each vehicle in the group and the control vehicle. For example, in one embodiment, the traffic conditions include a current driving distance, a current speed, and a current acceleration of the mixed automaton vehicles. In some embodiments, the mixed automaton vehicles include all non-controlled vehicles within a predetermined range of an adjacent controlled vehicle in the platoon.
[0172] The NIC 1050 also includes a transmitter adapted to transmit control commands to the controlled vehicles over the network 1090. As such, the system 1000 includes an output interface, e.g., a control interface 1070, configured to present 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 that is in wireless communication directly or indirectly with the mixed automaton vehicles.
[0173] The system 1000 may also include other types of input and 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.
[0174] 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 other configuration. The memory 1040 may include random access memory (RAM), read-only memory (ROM), flash memory, or other suitable memory machines. The processor 1020 may be connected to one or more input and output devices via a bus 1006.
[0175] The processor 1020 is operatively connected to a memory storage 1030 for storing instructions and processing data used by the instructions. The storage 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, which is trained to track extended states of the mixed automaton vehicle, convert traffic states into target driving intervals for the mixed autonomous vehicle, and store one or more models 1033 configured to describe the vehicle's motion. For example, the models 1033 may include a motion model, a measurement model, a traffic model, etc.
[0176] The processor 1020 is configured to determine control commands for a controlled vehicle that also indirectly controls uncontrolled vehicles. To that end, the processor is configured to execute a control generator 1032 to determine the control commands based on the states of the vehicles. In some embodiments, the control generator 1032 uses a deep reinforcement learning (DRL) controller trained to generate control commands from the extended states for individual vehicles and / or vehicle platoons.
[0177] FIG. 11A illustrates a schematic of a directly or indirectly controlled vehicle 1101 according to some embodiments. As used herein, the vehicle 1101 may be any type of wheeled vehicle, such as a car, a bus, or a rover. The vehicle 1101 may also be autonomous or semi-autonomous. For example, some embodiments control the motion of the vehicle 1101. An example of the motion is the lateral motion 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 may be controlled by a driver of the vehicle 1101.
[0178] 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 sensors 1104 include distance range finders, radar, lidar, and cameras. The vehicle 1101 may also include one or more sensors 1105 for sensing its current amount of motion and internal conditions. Examples of 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 controller 1102 to communicate over wired or wireless communication channels.
[0179] 11B illustrates a schematic 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 1101 The predictive controller 1102 may include a steering control 1110 and a brake / throttle controller 1120 that control the rotation and acceleration of the vehicle. 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 that further processes the control inputs of the predictive controller 1102. In either case, the controller 1100 maps the use of the output of the predictive controller 1102 to control at least one actuator of the vehicle, such as the steering wheel and / or brakes of the vehicle, to control the motion of the vehicle. The vehicle machine state x t may include position, orientation, and longitudinal / lateral velocity, and the control input u tmay include lateral / longitudinal acceleration, steering angle, and engine / brake torque. State constraints for the system may include lane keeping constraints and obstacle avoidance constraints. Control input constraints may include steering angle constraints and acceleration constraints. Collected data may include position, orientation and velocity profiles, acceleration, torque, and / or steering angle.
[0180] The above-described embodiments of the present invention can be implemented in numerous ways. For example, the embodiments may be implemented using hardware, software, or a combination thereof. When implemented in software, the software code may be executed on any suitable processor or collection of processors, whether located in one computer or distributed across multiple computers. Such a processor may be implemented as an integrated circuit, where one or more processors are within an integrated circuit component. However, the processor may be implemented using circuitry in any suitable format.
[0181] 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 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.
[0182] 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.
[0183] Although the invention has been described with examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the following claims to cover all such variations and modifications which come within the true spirit and scope of the invention.
Claims
1. A server for collaboratively tracking states of a plurality of vehicles, each vehicle's state including one or more state biases that incorporate residual error of atmospheric delays, the server comprising at least one processor and a memory storing instructions that, when executed by the at least one processor, cause the server to: receiving a plurality of transmissions from the plurality of vehicles, each transmission from a vehicle including measurements of satellite signals received at the vehicle and parameters of a probability distribution of the states of the vehicles; fusing the states of the plurality of vehicles defined by the parameters of the corresponding probability distributions into an augmented state of the plurality of vehicles; and fusing the measurements of the satellite signals of the plurality of vehicles into extended measurements of the extended state subject to extended measurement noise defined by an off-diagonal covariance matrix having non-zero off-diagonal elements, each non-zero off-diagonal element being associated with an error in the measurements of a pair of corresponding vehicles; and causing the server to further: implementing a probabilistic filter that updates the extended state based on the extended measurements subject to the extended measurement noise; blending the parameters of the probability distribution of the states of at least some of the plurality of vehicles with corresponding portions of the updated augmented states to output blended parameters of the probability distribution of the states of at least some of the plurality of vehicles; the extended measurements are obtained by combining measurements of the satellite signals at the plurality of vehicles; The extended measurement noise is obtained by combining measurement noises at the multiple vehicles.
2. The server of claim 1 , wherein the extended state is a union of states of the plurality of vehicles, and a dimension of the extended state is less than a sum of dimensions of each state of the plurality of vehicles.
3. The server of claim 1 , wherein the probabilistic filter comprises a mixed integer Kalman filter that solves a mixed integer least squares problem to update first and second moments of a probability distribution of the extended states of the plurality of vehicles.
4. The probabilistic filter solves a recursive mixed-integer least squares problem (RMILS) of increasing size to estimate the number of vehicles.
2. The server of claim 1, comprising a recursive mixed integer Kalman filter that updates first and second moments of the extended state.
5. 5. The server of claim 4, wherein the probabilistic filter solves the RMILS by maintaining an unconstrained probability distribution having real-valued elements of the extended state and a constrained probability distribution that imposes integer constraints on some elements of the extended state that are determined by solving a weighted integer least squares problem based on the updated real-valued elements of the extended state.
6. The probabilistic filter comprises: updating the unconstrained probability distribution for the extended states based on the extended measurements to generate updated real-valued extended states; solving the weighted integer least squares problem for the updated real-valued extended state to generate constrained ambiguity values for some elements of the extended state; updating the extended measurement with the constrained ambiguity value to generate a constrained extended measurement; The server of claim 5 , configured to update the constrained probability distribution of the extended states based on the constrained extended measurements to generate updated extended states.
7. To determine the off-diagonal covariance matrix, the server forming a single or double difference between measurements of the satellite signals of the pair of vehicles relative to a common base satellite; The server of claim 1 , configured to determine a covariance between the determined single or double differences based on a number of satellites shared by the pair of vehicles.
8. The server of claim 1 , wherein the fusion parameters of the probability distributions of the states of the vehicle are determined as a weighted combination between the augmented states and the states of the vehicle.
9. The server of claim 8 , wherein weights in the weighted combination are determined by minimizing a determinant of the blended covariances of the probability distributions of the states of the vehicle.
10. The server of claim 8 , wherein weights in the weighted combination are determined by minimizing a trace of the fused covariances of the probability distributions of the states of the vehicle.
11. To fuse the first and second moments of the parameters of the probability distribution of the state of the vehicle based on the first and second moments of the updated extended state, the server: Transforming the augmented states and the states of the vehicle into a common state space; determining a cross-covariance of the fusion between the augmented states and the states of a vehicle in the common state space; fusing the first moments as a weighted combination of the cross-covariances; The server of claim 8 , configured to fuse the second moments based on weighted differences between the second moments, the weights being the weighted combinations of the cross-covariances.
12. The server of claim 11 , wherein the cross-covariance of the fusion is determined based on a Kalman gain of a probabilistic filter of the vehicle and a cross-covariance between the state and the augmented state of the vehicle.
13. The server of claim 12 , wherein the cross-covariance between the state of the vehicle and the extended state is determined as a function of joint Kalman gains for the states of the multiple vehicles.
14. The server of claim 1 , further comprising a transceiver configured to receive the plurality of transmissions from the plurality of vehicles and to transmit the fused parameters of the probability distributions of the states to at least some of the plurality of vehicles.
15. 11. A method for collaboratively tracking states of multiple vehicles, each state of the vehicle including one or more state biases that incorporate residual error of atmospheric delay, the method using a processor coupled to stored instructions that perform the method, the instructions, when executed by the processor, perform steps of the method, the steps including: receiving a plurality of transmissions from the plurality of vehicles, each transmission from a vehicle including a measurement of a satellite signal received at the vehicle and a parameter of a probability distribution of the state of the vehicle, the step further comprising: fusing the states of the plurality of vehicles defined by the parameters of the corresponding probability distributions into an augmented state of the plurality of vehicles; and fusing the measurements of the satellite signals of the plurality of vehicles into extended measurements of the extended state subject to extended measurement noise defined by an off-diagonal covariance matrix having non-zero off-diagonal elements, each non-zero off-diagonal element being related to an error in the measurements of a pair of corresponding vehicles, the step further comprising: running a probabilistic filter to update the extended state based on the extended measurements subject to the extended measurement noise; blending the parameters of the probability distributions of the states of at least some of the plurality of vehicles with corresponding portions of the updated augmented states to output blended parameters of the probability distributions of the states of at least some of the plurality of vehicles; the extended measurements are obtained by combining measurements of the satellite signals at the plurality of vehicles; The method of claim 1, wherein the extended measurement noise is obtained by combining measurement noises at the multiple vehicles.
Citation Information
Patent Citations
Sensor error estimating device, and observation device, method, and program
JP2008175721A
Navigation Satellite Wide-Lane Bias Determination System and Method
US20170269223A1
System and Method for GNSS Ambiguity Resolution
US20190302274A1
