This invention relates generally to distributed estimation systems (DESs), and more particularly to jointly tracking states of a plurality of moving devices wherein each of the moving devices is configured to transmit to the hybrid DES over a wireless communication channel
A Global Navigation Satellite System (GNSS) is a system of satellites that can be used for determining the geographic location of a mobile receiver with respect to the earth. Examples of a GNSS include GPS, Galileo, Glonass, QZSS, and BeiDou. Various global navigation satellite (GNS) correction systems are known that are configured for receiving GNSS signal data from the GNSS satellites, for processing these GNSS data, for calculating GNSS corrections from the GNSS data, and for providing these corrections to a mobile receiver, with the purpose of achieving quicker and more accurate calculation of the mobile receiver's geographic position.
Various position estimation methods are known wherein the position calculations are based on repeated measurement of the so-called pseudo-range and carrier phase observables by Earth-based GNSS receivers. The “pseudo-range” or “code” observable represents a difference between the transmit time of a GNSS satellite signal and the local receive time of this satellite signal, and hence includes the geometric distance covered by the satellite's radio signal. The measurement of the alignment between the carrier wave of the received GNSS satellite signal and a copy of such a signal generated inside the receiver provides another source of information for determining the apparent distance between the satellite and the receiver. The corresponding observable is called the “carrier phase”, which represents the integrated value of the Doppler frequency due to the relative motion of the transmitting satellite and the receiver.
Any pseudo-range observation comprises inevitable error contributions, among which are receiver and transmitter clock errors, as well as additional delays caused by the non-zero refractivity of the atmosphere, instrumental delays, multipath effects, and detector noise. Any carrier phase observation additionally comprises an unknown integer number of signal cycles, that is, an integer number of wavelengths, that have elapsed before a lock-in to this signal alignment has been obtained. This number is referred to as the “carrier phase ambiguity”. Usually, the observables are measured, i.e. sampled, by a receiver at discrete consecutive times. The index for the time at which an observable is measured is referred to as an “epoch”. The known position determination methods commonly involve a dynamic numerical value estimation and correction scheme for the distances and error components, based on measurements for the observables sampled at consecutive epochs.
When GNSS signals are continuously tracked and no loss-of-lock occurs, the integer ambiguities resolved at the beginning of a tracking phase can be kept for the entire GNSS positioning span. The ONSS satellite signals, however, may be occasionally shaded (e.g., due to buildings in “urban canyon” environments), or momentarily blocked (e.g.; when the receiver passes under a bridge or through a tunnel). Generally, in such cases, the integer ambiguity values are lost and must be re-determined. This process can take from a few seconds to several minutes. In fact, the presence of significant multipath errors or unmodeled systematic biases in one or more measurements of either pseudo-range or carrier phase may make it difficult with present commercial positioning systems to resolve the ambiguities. As the receiver separation (i.e., the distance between a reference receiver and a. mobile receiver whose position is being determined) increases, distance-dependent biases (e.g. orbit errors and ionospheric and tropospheric effects) grow, and, as a consequence, reliable ambiguity resolution (or re-initialization) becomes an even greater challenge. Furthermore, loss-of-lock can also occur due to a discontinuity in a receiver's continuous phase lock on a signal, which is referred to as a cycle slip, For instance, cycle slips can be caused by a. power loss, a. failure of the receiver software, or a malfunctioning satellite oscillator. In addition, cycle slip can be caused by changing ionospheric conditions,
GNSS enhancement refers to techniques used to improve the accuracy of positioning information provided by the Global Positioning System or other global navigation satellite systems in general, a network of satellites used for navigation. For example, some methods use differencing techniques based on differencing between satellites, differencing between receivers, differencing between epochs, and combination thereof. Single and double differences between satellites and the receivers reduce the error sources but do not eliminate them.
Consequently, there is a need to increase the accuracy of GNSS positioning. To address this problem, a number of different methods use the cooperation of multiple GNSS receivers to increase 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 a restriction on the cooperative enhancement of accuracy of GNSS positioning is not always practical.
Some embodiments are based on the realization that current methods of tracking of a state of a moving device, such as a vehicle, assume either an individual or centralized estimation based on internal modules of the vehicle or a distributed estimation that performs the state estimation in a tightly controlled and/or synchronized manner. Examples of such distributed estimation include decentralized systems that determine different aspects of the state tracking and estimate the state of the vehicle by reaching a consensus, imbalanced systems that track the state of the system independently while one type of tracking is dominant over the other, and distributed systems including multiple synchronized receivers preferably located at a fixed distance from each other.
Some embodiments appreciate the advantages of cooperative state tracking when internal modules of a moving vehicle use some additional information determined externally. However, some embodiments are based on the recognition that such external information is not always available. Hence, there is a need for cooperative, tracking, when the tracking is performed by the internal modules of the vehicle but can seamlessly integrate the external information when such information is available.
Some embodiments are based on the recognition that the cooperative estimation can either be performed at a central computing node, or the estimation can be performed completely decentralized, or it can be a distributed combined approach. Some embodiments are based on the recognition that different applications require different types of information, and furthermore, that even if an application at a particular time step can make optimal use of a type of information, such optimal type of information can change from a time step to a next time step.
To that end, some embodiments disclose a hybrid distributed estimation system (HDES) utilizing at least two types of information according to some embodiments. For example, the information utilized by the HDES includes estimates from the local filter executing in each moving device. Additionally or alternatively, the information utilized by the HDES includes measurements for each moving device, which can be obtained using sensors either physically or operatively connected to the moving device. In such a manner, the HDES can select the best information for the current state of the joint tracking. The best type of information to be used by the DES depends on numerous factors, including the type of environment, type of moving device, quality of measurements, quality of estimates, and how long the estimation in the DES and moving devices has been ongoing.
Other embodiments determine whether to execute the DES altogether or revert back to only using local estimates because in certain settings, the DES only makes estimation worse. For example, when the measurement noise between two sensors is correlated, meaning there is a relation between them, but the DES does not know about this, the DES may end up producing estimates with more uncertainty than a local estimator would provide. For example, for particular types of sensors, when the delay in transmitting some measurements to the DES is large in relation to the time the estimator has executed, using estimates
Therefore, some embodiments use these and/or other factors to determine which information to be used at particular time steps. Doing in such a manner makes it possible to come up with the best possible estimation at a given time. Additionally or alternatively, some embodiments acknowledge that when switching between the type of information, different DESs have to be used because it is impractical to provide a universal DES that can handle any type of measurement without major alterations. For example, one embodiment switches between a measurement-sharing probabilistic filter and an estimate-sharing consensus-based distributed Kalman filter (DKF).
Accordingly, one embodiment discloses a hybrid distributed estimation system (HDES) for jointly tracking states of a plurality of moving devices, wherein each of the moving devices is configured to transmit to the HDES over a wireless communication channel one or a combination of measurements indicative of a state of a moving device and an estimation of the state of the moving device derived from the measurements.
The HDES includes a memory configured to store a first distributed estimation system (DES) configured upon activation to jointly track the states of the moving devices based on the measurements of the states of the moving devices and a second DES configured upon activation to jointly track the states of the moving devices based on the estimations of the states of the moving devices; a receiver configured to receive over the communication channel multi-type information from the plurality of moving devices, wherein types of the information include one or a combination of a first type for the measurements of the states of the moving devices and a second type for the estimation of the states of the moving devices; a processor configured to select between the first type and the second type of information, activate the first DES or the second DES based on the selected type of the information, and jointly estimate the states of the moving devices using the activated DES; and transmitter configured to transmit to the moving devices over the communication channel at least one or a combination of the selected type of information and the jointly estimated states of the moving devices.
Another embodiment discloses a computer-implemented method for jointly tracking states of a plurality of moving devices, wherein the method uses a processor coupled to a memory storing a first distributed estimation system (DES) configured upon activation to jointly track the states of the moving devices based on the measurements of the states of the moving devices and a second DES configured upon activation to jointly track the states of the moving devices based on the estimations of the states of the moving devices, wherein the processor is coupled with stored instructions implementing the method, wherein the instructions, when executed by the processor carry steps of the method, including receiving over a communication channel multi-type information from the plurality of moving devices, wherein types of the information include one or a combination of a first type for the measurements of the states of the moving devices and a second type for estimation of the states of the moving devices derived from the measurements; selecting between the first type and the second type of information, activate the first DES or the second DES based on the selected type of the information, and jointly estimate the states of the moving devices using the activated DES; and transmitting to the moving devices over the communication channel at least one or a combination of the selected type of information and the jointly estimated states of the moving devices.
The KF starts with an initial knowledge 110b of the state, to determine a mean of the state and its variance 111b. The KF then predicts 120b the state and the variance to the next time step, using a model of the system, such as the motion model of the vehicle, to obtain an updated mean and variance 121b of the state. The KF then uses a measurement 130b in an update step 140b using the measurement model of the system, wherein the model relates the sensing device data 107, 117, 127, and 137, to determine an updated mean and variance 141b of the state. An output 150b is then obtained, and the procedure is repeated for the next time step 160b.
Some embodiments employ a probabilistic filter including various variants of KFs, e.g., extended KFs (EKFs), linear-regression KFs (LRKFs), such as the unscented KF (UKF). Even though there are multiple variants of the KF, they conceptually function as exemplified by
Some embodiments are based on the understanding that moving devices can either perform their estimation locally based on information from the sensors and information also from the surrounding moving devices. For example, device 110 can perform its estimation based on only sensors 105,115, 125, and 135, additionally or alternatively, it can include the information 131, 121, directly coining from the moving devices.
Some embodiments are based on the recognition that the cooperative estimation can either be performed at a central computing node, the estimation can be performed completely decentralized, or it can be a distributed combined approach.
Some embodiments are based on the recognition that different applications require different types of information, and furthermore, that even if an application at a particular time step can make optimal use of a type of information, such optimal type of information can change from a time step to a next time step.
In some embodiments, the information 225, 235, 245 that is sent to the DES is the estimate from the local filter executing in each moving device 220, 230, 240. In other embodiments, the information 225, 235, 245 that is sent is the measurement vector for each moving device, which has been obtained using sensors either physically or operatively connected to the moving device. The best type of information to be used by the DES depends on numerous factors, including the type of environment, type of moving device, quality of measurements, quality of estimates, and how long the estimation in the DES and moving devices has been ongoing. Other embodiments determine whether to execute the DES altogether, or revert back to only using local estimates, because in certain settings the DES only makes estimation worse.
For example, when the measurement noise between two sensors is correlated, meaning there is a relation between them, but the DES does not know about this, the DES may end up in producing estimates with more uncertainty than a local estimator would provide.
For example, for particular types of sensors, when the delay in transmitting some measurements to the DES is large in relation to the time the estimator has executed, using estimates may be preferable.
Therefore, some embodiments use these factors to determine which information to be used at particular time steps. Doing in such a manner makes it possible to come up with the best possible estimation at a given time.
Other embodiments acknowledge that when switching between the type of information, different DESs have to be used because it is impractical to provide a universal DES that can handle any type of measurement without major alterations. For example, one embodiment switches between a measurement-sharing probabilistic filter and an estimate-sharing consensus-based distributed Kalman filter (DKF).
Using the determined type of information 329a, the method then determines 330a the DES to be executed using the selected type of information. For example, in one embodiment the information is selected to be the measurements relating to the moving devices, and the DES is a measurement-sharing Kalman-type filter. In another embodiment, the information is the estimated state and the corresponding DES is a consensus-based distributed Kalman filter (DKF).
Using the determined DES 335a, the method executes 340a the DES to produce the estimated states 345a of the moving devices. Finally, the method transmits 350a the estimated states 345a to produce transmitted estimates 355a to each of the moving devices.
The HDES includes a memory 380 that stores 381 a first DES configured upon activation to jointly track the states of the moving devices based on measurements of the states of the moving devices. The memory 380 also stores 382 a second DES configured upon activation to jointly track the moving devices based on estimations of the states of the moving devices. For instance, in one embodiment the first DES is a measurement-sharing Kalman filter and the second DES is a consensus-based DKF. For instance, in one embodiment the first DES is a measurement-sharing Kalman filter and the second DES is a weighted DKF. In some embodiments, the first DES includes a model of the cross-correlation of the measurement noise of the measurements from the moving devices. In other embodiments, the cross-correlation is unknown and is estimated in the HDES based on the transmitted 339 noise and location of the sensors measuring the moving devices.
The memory 380 can also include 383 a probabilistic motion model relating a previous belief on the state of the vehicle with a prediction of the state of the moving devices according to the motion model and a probabilistic measurement model relating a belief on the state of the vehicle with a measurement of the state of each moving device. The memory also stores instructions 384 on how to determine which DES to execute. The memory additionally or alternative can store 385 the communication bandwidth needed for the first DES and the second DES as a function of the state dimension and measurement dimension.
The receiver 360 is configured to receive over the communication channel multi-type information from the plurality of moving devices, wherein types of the information include one or a combination of a first type for the measurements of the states of the moving devices and a second type for the estimation of the states of the moving devices.
The receiver 360 is operatively connected 350 to a processor 330 configured to select 331 the first type or the second type of the information. Based on the determined 331 type of information, the processor is furthermore configured to switch between activation and deactivation of the first DES and the second DES based on the selected type of information, and to execute the activated DES 332 to produce 333 estimates of the states of the moving devices.
The HDES 300 includes a transmitter 320 operatively connected 350 to the processor 330. The transmitter 320 is configured to submit 309 to the moving devices over the communication channel the selected 331 type of information and the jointly tracked states 333 of the moving devices estimated by the activated DES 332.
In some embodiments, the submitted tracked states include a first moment of the state of a moving device. In other embodiments, the information includes a first moment and a second moment of a moving device. In other embodiments, the information includes higher-order moments to form a general probability distribution of the state of a moving device. In other embodiments, the information includes data indicative of an estimation of the state of a moving device and the estimation noise, e.g., as samples, first and second moment, alternatively including higher-order moments. In yet 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 first and second moments, and the execution of such instructions, possibly coupled with a communication time between the remote server and the RF receiver. To this end, in some embodiments, information 309 includes a time stamp of the time the first and second moments were determined.
In some embodiments the first type of information includes a measurement and a measurement statistic of the expected distribution of the measurement, e.g., a noise covariance in the case of Gaussian assumed noise. In other embodiments the first type of information additionally includes information necessary to construct a measurement model, e.g., the location of the sensor measuring the moving device, the time of measurement. In some embodiments the second type of information include a mean estimate of the state and, additionally, the associated covariance. In other embodiments the second type additionally includes one or a combination of time of the estimation, higher order moments or samples of estimations of the states, e.g., as an output from a particle filter.
Some embodiments are based on the understanding that the determining to use the first type of information or the second type of information is not a one-time decision, and a first type of information that is the preferred choice at a particular time step may be the least preferred choice at a future time step, or vice versa.
For instance, if the cross-correlation between measurements across the moving devices is known to the HDES, it is fundamentally, from an information-theoretic viewpoint, better to use the first type of information than the second type of information, because the first type of information contains information of correlation between the moving devices that the second type of information cannot include.
For instance, if the cross-correlation between measurements across the moving devices is unknown to the HDES, it provides no extra information to use the first type of information over the second type of information. In addition, if using the first type of information without having the cross-correlation, such estimation may lead to wrong conclusions of the joint state estimates. Hence, it may be preferable to use the second type of information.
Some embodiments are based on the understanding that information relevant to determining the type of information to use varies over time. For instance, the cross-correlation between measurements can be unknown at certain time steps but later become known, which could warrant choosing the first type of information over the second type of information.
For instance, the number of moving devices included in the joint estimation can vary over time, and as a result, the information provided by the first type of information and the second type of information will both vary over time. In addition, the quality of the types of information varies over time, differently from each other, and as such, the type of information to choose varies over time.
As an exemplar application, consider the setting in
To that end, some embodiments track a variation of the quality of information over time and select between the first and the second DES by comparing the measure of the quality with a threshold. Examples of the measures of the quality of information include signal-to-noise ratio (SNR), presence, types, and the extent of multipath signals, confidence of measurements, and the like.
The determining 510a measurement model is based on a mathematical probabilistic representation of the relation between the state of the moving devices and the measurement. For instance, in one embodiment the measurement model is approximately linear according to
wherein is the Gaussian distribution,
is the matrix of vector measurements of the N moving devices,
is the corresponding measurement matrix, and
is the covariance matrix, wherein the off-diagonal blocks correspond to the cross-covariance and is either transmitted with the type of information or determined by the HDES according to other embodiments. In other embodiments, the measurement model is nonlinear according to y=h(x)+e, where h is the nonlinear relation. Some embodiments linearize h to get C, whereas other embodiments represent the model in its original nonlinear formulation. In some embodiments, the received information 315a includes information necessary to determine the measurement model, in other embodiments the information is already stored in the memory 385.
To determine whether the cross-covariance can be computed, the method checks the received information 315a for its content. For instance, in some embodiments, the positions of the sensors, together with nominal noise values, are needed to determine the cross-covariance. In other embodiments, the sensor locations are fixed and the cross-covariance can then be determined a priori and stored in the memory 380. In other embodiments, the sensors are attached to the moving devices and therefore the cross-covariance can be computed from the estimations of the state of the moving device.
Some embodiments determine the type of information based on one or a combination of a bandwidth of the communication channel and several of the moving devices, a correlation among the measurements collected by the moving devices, an expected difference between joint state estimations of the first DES and the second DES, and the communication delay in transmitting the first and second type of information to the HDES.
To determine 540a the performance gap 545a, different embodiments proceed in several ways.
If the devices are static 520b, the method proceeds with determining 530b a performance matrix 535b. In some embodiments, the performance matrix is determined as a combination of the measurement model, the joint measurement covariance without the cross-covariance inserted into the joint measurement covariance, and the joint measurement covariance with the cross-covariance inserted into the joint measurement covariance.
For instance, one embodiment determines the performance gap between a DES with a first type of information and a DES with a second type of information by determining the expected uncertainty, e.g., the covariance, of the DES estimates. For example, one embodiment determines the performance gap according to
where M+(CT
The performance gap Jgap is in general a matrix indicating the general performance gap between a DES using a first type of information with a DES using a second type of information. Some embodiments evaluate the performance gap by determining the trace of Jgap, which gives a way to evaluate the performance gap matrix. Other embodiments evaluate Jgap by determining the norm of Jgap, which is a second way to evaluate the performance gap matrix.
If the devices are not static 520b, the method instead proceeds, using the cross-covariance 535a inserted into the joint measurement covariance, with predicting 550b the state performance 555b of the first DES. Next, the method predicts 560b the state performance 565b of the state using the second DES. Using the determined performances, the performances are compared and subsequently the performance gap 575b is determined 570b.
In some embodiments, the performance is defined as the predicted second moment of the DES, i.e., the covariance of the estimated state of the DES. For instance, in one embodiment the Kalman filter recursions are used to recursively predict the covariance of the state estimate with 550b and without 570b the cross-covariance inserted into the joint measurement covariance according to Pk|k1=(CTR−1C+(Pk|k−11)−1)−1 for the first DES and Pk|k2=((CT
Using the determined performances for the respective DES, the method determines 579b the performance gap 575b. For instance, one embodiment determines the performance gap by comparing the mean-square errors assuming an unbiased estimator, which is directly related to the covariance of such unbiased estimator.
In some embodiments, the benefit 515c is a weighted combination of the performance gap 545a and communication bandwidth 505c. For instance, in settings where the communication resources are very high, the communication bandwidth 505c is getting a relatively small weight. In settings where the communication resources are low, the communication bandwidth 505c is getting a relatively large weight.
Other embodiments select the first type of information according to the criteria T[M]ij>K([M]ij+[N]ij), where [M]ij is the ijth element, N=(CTR−1C)−1, T the time step, and K the measurement delay, i.e., as a combination of the measurement model, measurement covariance, cross-covariance, the time the HDES has executed since initializing.
Some embodiments are based on the understanding that sometimes certain elements ij are of most importance, e.g., sometimes a particular state of a few particular moving devices is of interest, which gives a way to determine specific elements ij.
Other embodiments are based on the understanding that there sometimes may be multiple element combinations ij. In such a case, one embodiment evaluates T[M]ij>K([M]ij+[N]ij) for different element combinations and use a weighting between the element combinations to determine the selection of the type of information.
Referring to
The initialization 620a is done by setting the initial estimate of the first DES to the estimate of the second DES. For example, if the estimator is an estimator estimating the first two moments of the state, the most recent mean, and covariance 619a are used to initialize 620a the first DES. For example, if the HDES is estimated using a particle filter in both DES, the particles are set to align with each other.
In some embodiments the first DES and second DES use different types of estimators, e.g., the first DES estimates the first two moments and the second DES is a sampling-based estimator or vice versa. In such a case, the samples are used to determine the first two moments. Similarly, the samples can in its turn be sampled from the first two moments to initialize a sampling-based estimator.
Some embodiments implement the first DES as a KF. Other embodiments use an LRKF that works in the spirit of a KF. Yet other embodiments implement the first DES as a particle filter (PF).
Some embodiments apply a PF as a measurement sharing PF, wherein the measurement model include all measurements according to yk=h(xk)+ek, wherein yk={y1, yk2, . . . , ykN} and h(xk)={h1(xk), h2(xk), . . . , hN(xk)}, and wherein
is the covariance matrix of the zero-mean Gaussian distributed measurement noise ek, wherein the off-diagonal blocks correspond to the cross-covariance and is either transmitted with the type of information or determined by the HDES according to other embodiments. In other words, the PF uses the nonlinear measurement relation h(xk) directly as opposed to a KF that employs a linearization.
Using the measurement model, the PF approximates the posterior density as
where qki is the importance weight of the ith state trajectory x0:ki and δ(·) is the Dirac delta mass. The PF recursively estimates the posterior density by repeated application of Bayes' rule according to p(x0:k|y0:k)∝p(yk|x0:k, y0:k−1)p(xk|x0:k−1, y0:k−1)·p(x0:k−1|y0:k−1). To predict the state samples, the key design step is the proposal distribution π(·), which results in predicted state samples according to xk˜π(xk|x0:k−1, y0:k). In some embodiments, the proposal distribution is defined to predict state samples according to xki=f(xk−1i)+wk−1i, where wk−1i is sampled according to a predefined noise model wk−1i˜(0, Q).
Some embodiments realize that implementing such proposal distribution is uninformative. Instead, some embodiments use the conditional distribution as proposal distribution, π(xk|xk−1i, y0:k)=p(xk|xk−1i, y0:k). For instance, some embodiments approximate the measurement relation as linear and Gaussian for each particle,
p(xk|xk−1i, y0:k)=(xk; {circumflex over (x)}ki, Σki)
{circumflex over (x)}
k
i
=x
k−1
i
+K
k
i(yk−ŷk|k−1i),
K
k
i
=Q(Q+Ski)−1,
Σki=((Ski)−1+(Q)−1)−1,
but other approximations of the measurement relation can be used, resulting in other proposal distributions.
Yet other embodiments recognize that the motion model and measurement model is not linear in all states. For example, for a moving device, a possible state includes a position, heading, and velocity of the moving device. However, for a GNSS measurement the position is nonlinear in the measurement relation but the heading and velocity are not. Hence various embodiments employ marginalized PFs, which execute a PF for the nonlinear part of the state vector, and conditioned on the state trajectory, KFs are executed, one for each particle.
Various embodiments employ different motion models such that many states can be nonlinear. However, some embodiments recognize that some nonlinearities are severe, whereas other nonlinearities are mild. Consequently, one embodiment implements the PF as a marginalized particle filter where the severely nonlinear states are used in the PF and linear and mildly nonlinear states are approximated with an extended KF or LRKF.
Determining the sequence of probability distributions amounts to determining the distribution of probabilities such as those in
Some embodiments implement the second DES as a consensus DKF (CDKF), wherein the estimates are combined by one of several consensus protocols defined by a set of weights
For example, some embodiments use one of the three protocols
One embodiment implements the consensus DKF on information form with an information vector γk|k and information matrix Γk|k relating to the state mean estimate as {circumflex over (x)}k|ki=(Γk|kii)−1γk|kiand covariance as Pk|kii=(Γk|kii)−1. The reason for such implementation is that in information form, e.g., in the information form KF, N updates can be made by simply summing the information matrices and vectors. In one embodiment the CDKF is iterated at each time step, and the consensus protocols are implemented.
by having the nodes iterate their information vectors (and information matrices) with their neighbors over several steps according to
for some iterations over n.
Other embodiments implement the second DES as a fused DKF (FDKF), wherein the weights are determined based on a relative uncertainty of the estimates,
Yet other embodiments employ weighted DKFs to propagate the mean estimate, by fusing a set of weight matrices {Wij}i=1, j=1i=N, j=N subject to the constraints
In some embodiments, the weights are used to fuse the estimates similar to a CDKF, as a weighted combination of estimates, wherein the weights are optimizing the weighted posterior covariance of the estimation error.
In various embodiments, the GNSS receiver 930 and 931 can be of different types. For example, in exemplar embodiment of
In some embodiments, there are multiple mobile GNSS receivers that are jointly tracked by an HDES. The first type of information is the measurement information of code and carrier-phase measurements.
In some embodiments, the model of the motion of a receiver is a general-purpose kinematic constant-acceleration model with the state vector xk=[pr,kvr,kαr,k]T, where the three components are the position, velocity, and acceleration of the receiver. In some other embodiments, the time evolution ambiguity of propagation of the satellite signals is modeled as nk+1=nk+wn, k, wn,k˜(0, Qn), where nk+1 is the ambiguity and wn,k is the Gaussian process noise with covariance Qn.
In some embodiments, the ambiguity is included in the state of the vehicle. Other embodiments also include bias states capturing residual errors in the atmospheric delays, e.g., ionospheric delays. For receivers sufficiently close to each other, the ionospheric delays are the same, or very similar, for different vehicles. Some embodiments utilize this to relationships to resolve these delays and/or ambiguities.
Some embodiments capture the carrier and code signals in the measurement model yk=hk+λ
In some embodiments, the probabilistic filter uses the carrier phase single difference (SD) and/or double difference (DD) for estimating a state of the receiver indicating a position the receiver. 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 referred as the single difference (SD) in carrier phase. Alternatively, the SD can be defined as the difference between signals from two different satellites reaching a receiver. For example, the difference can come from a first and a second satellites when the first satellite is called the base satellite. For example, the difference between signal 910 from satellite 901 and signal 920 from satellite 902 is one SD signal, where satellite 901 is the base satellite. Using pairs of receivers, 931 and 930 in
In general, a GNSS can use multiple constellations at the same time to determine the receiver state. For example, GPS, Galileo, Glonass, and QZSS can be used concurrently. Satellite systems typically transmit information at up to three different frequency bands, and for each frequency band, each satellite transmits a code measurement and a carrier-phase measurement. These measurements can be combined as either single differenced or double differenced, wherein a single difference includes taking the difference between a reference satellite and other satellites, and wherein double differencing includes differencing also between the receiver of interest and a base receiver with known static location.
P
k
j=ρkj+c(δtr,k−δtkj)+Ikj+Tkj+ϵkj, (1)
Φkj=ρkj+c(δtr,k−δtkj)−Ikj+Tkj+λnj+ηkj, (2)
where pj is the code measurement ρj is the distance between the receiver and the j th satellite, c is the speed of light, δtr is the receiver clock bias, δtj is the satellite clock bias, Ij is the ionospheric delay, Tj is the tropospheric delay, Σj is the probabilistic code observation noise, Φj is the carrier-phase observation, λ is the carrier wavelength, nj is the integer ambiguity, and ηj is the probabilistic carrier observation noise.
In one embodiment, the original measurement model is transformed by utilizing a base receiver b mounted at a known location broadcasting to the original receiver r, most of the sources of error can be removed. For instance, one embodiment forms the difference between the two receivers 930 and 931 in
Additionally or alternatively, some embodiments are based on realization that ignoring state biases such as ionospheric errors can lead to slight inaccuracies of state estimation. This is because biases are usually removed by single or double differencing of GNSS measurements. This solution works well when a desired accuracy for position estimation of a vehicle is in the order of meters, but can be a problem when the desired accuracy is in the order of centimeters. To that end, some embodiments include state biases in the state of the vehicle and determine them as part of the state tracking provided by the probabilistic filter.
In this example, multiple vehicles 1000, 1010, 1020, are moving on a given freeway 1001. Each vehicle can make many motions. For example, the vehicles can stay on the same path 1050, 1090, 1080, or can change paths (or lanes) 1060, 1070. Each vehicle has its own sensing capabilities, e.g., Lidars, cameras, etc. Each vehicle has the possibility to transmit and receive 1030, 1040 information with its neighboring vehicles and/or can exchange information indirectly through other vehicles via a remote server. For example, the vehicles 1000 and 1080 can exchange information through a vehicle 1010. With this type of communication network, the information can be transmitted over a large portion of the freeway or highway 1001.
Some embodiments are configured to address the following scenario. For example, the vehicle 1020 wants to change its path and chooses option 1070 in its path planning However, at the same time vehicle 1010 also chooses to change its lane and wants to follow option 1060. In this case, the two vehicles might collide, or at best vehicle 1010 will have to execute and emergency brake to avoid colliding with vehicle 1020. This is where the present invention can help. To that end, some embodiments enable the vehicles to transmit not only what the vehicles sense at the present time instant t, but also, additionally or alternatively, transmit what the vehicles are planning to do at time T+δt.
In the example of
Additionally or alternatively, the motion of the vehicles can be jointly controlled by the remote server based on state estimations determined in a distributed manner. For example, in some embodiments, the multiple vehicles determined for joint state estimation are the vehicles that form and potentially can form a platoon of vehicles jointly controlled with shared control objective.
The NIC 1250 also includes a transmitter adapted to transmit the control commands to the controlled vehicles via the network 1290. To that end, the system 1200 includes an output interface, e.g., a control interface 1270, configured to submit the control commands 1275 to the controlled vehicles in the group of mixed-autonomy vehicles through the network 1290. In such a manner, the system 1200 can be arranged on a remote server in direct or indirect wireless communication with the mixed-automata vehicles.
The system 1200 can also include other types of input and output interfaces. For example, the system 1200 can include a human machine interface 1210. The human machine interface 1210 can connect the controller 1200 to a keyboard 1211 and pointing device 1212, wherein the pointing device 1212 can include a mouse, trackball, touchpad, joystick, pointing stick, stylus, or touchscreen, among others.
The system 1200 includes a processor 1220 configured to execute stored instructions, as well as a memory 1240 that stores instructions that are executable by the processor. The processor 1220 can be a single core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 1240 can include random access memory (RAM), read only memory (ROM), flash memory, or any other suitable memory machines. The processor 1220 can be connected through the bus 1206 to one or more input and output devices.
The processor 1220 is operatively connected to a memory storage 1230 storing the instructions as well as processing data used by the instructions. The storage 1230 can form a part of or be operatively connected to the memory 1040. For example, the memory can be configured to store an HDES with a first DES and second DES 1231 trained to track the augmented state of mixed-automata vehicles, transform the traffic state into target headways for the mixed-autonomy vehicles; and store a one or multiple models 1233 configured to explain the motion of the vehicles. For example, the models 1233 can include motion models, measurement models, traffic models, and the like.
The processor 1220 is configured to determine control commands for the controlled vehicles that indirectly control the uncontrolled vehicles as well. To that end, the processor is configured to execute a control generator 1232 to determine control commands based on the state of the vehicles. In some embodiments, the control generator 1232 uses a deep reinforcement learning (DRL) controller trained to generate control command from the augmented state for individual and/or a platoon of vehicles.
The vehicle can also include an engine 1306, which can be controlled by the controller 1302 or by other components of the vehicle 1301. The vehicle can also include one or more sensors 1304 to sense the surrounding environment. Examples of the sensors 1304 include distance range finders, radars, lidars, and cameras. The vehicle 1301 can also include one or more sensors 1305 to sense its current motion quantities and internal status. Examples of the sensors 1305 include global positioning system (GPS), accelerometers, inertial measurement units, gyroscopes, shaft rotational sensors, torque sensors, deflection sensors, pressure sensor, and flow sensors. The sensors provide information to the controller 1302. The vehicle can be equipped with a transceiver 1306 enabling communication capabilities of the controller 1302 through wired or wireless communication channels.
Further, based on the information transmitted by the drone 1400, a controller 1411 is configured to control motion of the drone 1400 by computing a motion plan for the drone 1400. The motion plan for the drone 1400 may comprise one or more trajectories to be traveled by the drone. In some embodiments, there are one or multiple devices (drones such as the drone 1400) whose motions are coordinated and controlled by the controller 1411. Controlling and coordinating the motion of the one or multiple devices corresponds to solving a mixed-integer optimization problem.
In different embodiments, the controller 1411 obtains parameters of the task from the drone 1400 and/or remote server (not shown). The parameters of the task include the state of the drone 1400, but may include more information. In some embodiments, the parameters may include one or a combination of an initial position of the drone 1400, a target position of the drone 1400, a geometrical configuration of one or multiple stationary obstacles defining at least a part of the constraint, geometrical configuration, and motion of moving obstacles defining at least a part of the constraint. The parameters are submitted to a motion planner to obtain an estimated motion trajectory for performing the task, where the motion planner is configured to output the estimated motion trajectory for performing the task.
In other embodiments at least one drone 1401c is wirelessly connected 1499c via a transmission/receiving interface to remote server 1440c. For instance, in one embodiment an HDES is located at 1440c, and the communication topology between the drones is part of the first and second type of information.
The above-described embodiments of the present invention can be implemented in any of numerous ways. For example, the embodiments may be implemented using hardware, software or a combination thereof. When implemented in software, the software code can be executed on any suitable processor or collection of processors, whether provided in a single computer or distributed among multiple computers. Such processors may be implemented as integrated circuits, with one or more processors in an integrated circuit component. Though, a processor may be implemented using circuitry in any suitable format.
Also, the various methods or processes outlined herein may be coded as software that is executable on one or more processors that employ any one of a variety of operating systems or platforms. Additionally, such software may be written using any of a number of suitable programming languages and/or programming or scripting tools, and also may be compiled as executable machine language code or intermediate code that is executed on a framework or virtual machine. Typically, the functionality of the program modules may be combined or distributed as desired in various embodiments.
Also, the embodiments of the invention may be embodied as a method, of which an example has been provided. The acts performed as part of the method may be ordered in any suitable way. Accordingly, embodiments may be constructed in which acts are performed in an order different than illustrated, which may include performing some acts concurrently, even though shown as sequential acts in illustrative embodiments.
Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention.