SYSTEM AND METHOD FOR CONTROLLING A DEVICE USING A COMPOSITE PROBABILITY FILTERS

Abstract

The present disclosure discloses a system and method for controlling a device using a composite probabilistic filter. The method includes collecting a series of measurements indicative of a state of the device at different control steps. The method further includes iteratively executing a composite probabilistic filter configured to track a state of the device at each of the different control steps using the series of measurements to generate a sequence of states of the device corresponding to the series of measurements. The method further includes controlling the device using the tracked states of the device.

Description

[Technical field]

[0001] FIELD OF THE DISCLOSURE This disclosure relates generally to control and state tracking, and more particularly to systems and methods for controlling devices using complex probabilistic filters. [Background technology]

[0002] Various control methods use stochastic filters to estimate the state of a device under control. Stochastic filters, such as the Kalman filter, use a series of measurements observed over time, which contain statistical noise and other imprecision, to generate estimates of unknown variables that tend to be more accurate than estimates based on a single measurement alone.

[0003] The stochastic filter works by a two-stage process including a prediction stage and an update stage. For the prediction stage, the stochastic filter generates estimates of the current state variables along with their uncertainties. To do so, the stochastic filter employs a prediction model subject to process noise. An example of a prediction model is a motion model of the device under control. Furthermore, the results of the next measurements (corrupted by some errors including random noise) are observed, and the estimates of the current state variables are updated using a weighted average of the measurements, giving more weight to the measurements with more certainty. To do so, the stochastic filter employs a measurement model subject to measurement noise. Both the process noise and the measurement noise can be represented by a probability density function (PDF) that indicates the likelihood of the occurrence of the variations in the estimated states and / or measurements.

[0004] Typically, the process noise and the measurement noise are predetermined to reflect an understanding of the uncertainty of the motion model and the reliability of the measurements. However, in many applications, it is not practical to predetermine the measurement noise. For example, in vehicle control applications where a vehicle is controlled based on the state of the vehicle tracked using Global Navigation Satellite System (GNSS) measurements, the GNSS measurements have different reliability at different times. For example, at different times, different constellations of satellites may be in line of sight of the vehicle's GNSS receiver, resulting in variations in the measurement noise. Additionally or alternatively, satellite signals may be subject to multipath that adds significant noise to the GNSS measurements.

[0005] To that end, it is desirable to adjust the measurement noise based on the state of the tracked vehicle and the control application. Although the measurement noise can be varied in a stochastic filter such as a Kalman filter, estimating the measurement noise in real-time applications can be computationally challenging and can also lead to divergence of the stochastic filter. This challenge stems in part from the difficulty of analyzing the statistical properties of the measurements and assumptions that underlie the operation of the stochastic filter.

[0006] Therefore, a probabilistic filter framework is needed that can be computationally efficiently adapted to different measurement noises.

[0007] Some embodiments are based on the recognition that the internal variables and / or calculations of a probabilistic filter, such as a Kalman filter, can be used to assess the accuracy of the measurement noise. In particular, a metric for the assessment of the accuracy of the measurement noise can be the likelihood of the measurement noise to correlate a current measurement indicative of the state of the device with a state predicted by a predictive model of the probabilistic filter.

[0008] This correlation can be illustrated by the following example: The predicted state of the device, for example the predicted state of a vehicle, is transformed into a domain of measurements, for example in the GNSS measurement space. For example, such a transformation can be performed using a model of the measurements. Then, the transformed state is taken as the center of the measurement noise to generate a probability distribution of the predicted measurements. In the case of a Gaussian probability distribution used by the probabilistic filter, the mean of the Gaussian probability distribution of the expected measurements is determined by the state predicted by the Kalman filter, and the variance is one of the given measurement noise. When measurements for updating the predicted state are received, the likelihood of the desired correlation can be estimated by mapping the measurements to the probability distribution of the expected measurements.

[0009] In this way, the measurement noise is evaluated using the internal variables and / or calculations of the stochastic filter without requiring additional statistical analysis of the measurements other than the performance of the stochastic filter. It should be noted that the above example not only illustrates the principle of correlation between state estimates and measurements, but can also be used to perform the estimation of this correlation. However, different embodiments can use different techniques to evaluate this correlation. For example, some embodiments use the estimate of the Kalman gain determined by the Kalman filter to update the predicted state and the covariance of the predicted state. This is the mean-square error (MSE) in the case of an unbiased estimator. Such calculations are performed inside the stochastic filter, i.e., calculated in any way to track the state of the device. Therefore, the calculation of these variables does not require additional resources.

[0010] However, some embodiments are based on the realization that, although the use of internal variables and / or calculations of the stochastic filter can reduce the computational requirements for the assessment of the accuracy of the selected measurement noise, the assessment itself is compromised by the internal performance of the stochastic filter. In other words, the accuracy of the measurement noise is not necessarily a true accuracy reflecting measurements that do not rely on the Kalman filter, but an accuracy from the point of view of the stochastic filter itself. Therefore, it may be problematic to use this assessment to fit the measurement noise, but rather to use this assessment to assess the performance of the stochastic filter with the selected measurement noise.

[0011] To address this issue, some embodiments use multiple probabilistic filters with different measurement noises, determine the state of the device as a weighted combination of the states estimated by the different filters with the weights of each filter derived from a corresponding estimate of the likelihood of the measurement noise, and correlate current measurements indicative of the state of the device with the states predicted by the predictive models of the probabilistic filters. In this way, different measurement noises can be taken into account without the need to analyze the statistical properties of the measurements.

[0012] Some embodiments are based on the recognition that current methods for tracking vehicle states based on satellite signals received from a Global Navigation Satellite System (GNSS) are sensitive to highly qualitative satellite measurements, because they are not otherwise able to produce highly accurate estimates. Other embodiments are based on the recognition that it is necessary to combine different sensors to improve state tracking, which complement each other, because such a combination is more likely to produce estimates that are detectable in a greater variety of conditions. For example, one embodiment combines GNSS position measurements with camera relative position measurements and a map of the road to track the state of the vehicle. Another embodiment combines a camera that measures the relative position of adjacent vehicles to the ego vehicle with a lidar measurement that measures the relative position of adjacent vehicles to the ego vehicle, because these measurements complement each other due to the different configurations of the camera and the lidar, respectively.

[0013] Some embodiments utilize a probabilistic filter to fuse measurements and generate a state estimate. The probabilistic filter requires a measurement model that is a probabilistic description of the measurements. To this end, some embodiments recognize that a measurement noise characteristic needs to be determined for the measurement model to be used in the probabilistic filter. Some embodiments are based on the understanding that in order to use a probabilistic measurement model during measurement updates, the accuracy of the sensor measurements needs to be determined simultaneously with tracking the vehicle's state, since tracking is performed in real time and the measurement model may not be set before the execution of the recursive probabilistic filter. Some embodiments utilize this understanding to adapt the measurement noise used in the probabilistic measurement model as the sensor measurements arrive at the composite probabilistic filter.

[0014] To that end, some embodiments jointly estimate the vehicle state and the measurement noises of different sensors to determine the measurement noise that best describes the vehicle state according to these measurements. For example, one embodiment associates a first noise value with the first sensor and a second noise value with the first sensor, and additionally associates the first noise value with the second sensor and the second noise value with the second sensor, and executes multiple probabilistic filters for each combination of noise value and sensor to determine which of them best describes the vehicle state. In other embodiments, the multiple probabilistic filters are combined into a composite probabilistic filter that weights the multiple probabilistic filters together as a weighted combination of the multiple filters.

[0015] Some embodiments are based on the understanding that if a probabilistic filter using a particular combination of noise values ​​for various sensors gave the best fit in the previous time step, it is likely to produce a good fit for the next time step as well. For example, if a GNSS position measurement was unreliable in the previous time step due to multipath, for example in an urban environment, it may be subject to multipath in the next time step as well. However, there is also an opportunity to produce reliable measurements if the unreliability of the measurements is not due to multipath effects but some other unmodeled disturbance. Some embodiments use this understanding to integrate and weight multiple probabilistic filters in each time step of the control, whereby the probabilistic filters are given different weights based on their weights in the previous time step.

[0016] Some embodiments are based on the understanding that the computational complexity of multiple probabilistic filters grows unfavorably with the number of noise value hypotheses and the number of sensors. For example, in one embodiment, it is understood that using a range of possible noise values ​​can be interpreted as gridding a continuous space of possible noise values ​​such that the number of possible combinations of noise values, and thus the number of probabilistic filters used in the estimation, is exponentially complex.

[0017] Some embodiments recognize that in automotive applications, the computational power of automotive-grade electronic control units is limited, and the use of a large number of probabilistic filters is not computationally feasible. One embodiment improves on this by selecting a subset of the probabilistic filters to be used for the estimation. For example, one embodiment selects the probabilistic filters that correspond to values ​​of the noise level that are close to being representative of the conditions at the previous time step. This allows the subset of probabilistic filters selected at each time step to follow the evolution of the sensor quality.

[0018] Accordingly, one embodiment discloses a feedback controller for controlling movement of a device based on a state of the device tracked using movement measurements indicative of the state of the device, the feedback controller including at least one processor and at least one memory having instructions stored thereon that, when executed by the at least one processor, cause the feedback controller to collect a series of measurements indicative of the state of the device at different control steps, and to iteratively execute a composite probabilistic filter configured to track the state of the device at each of the different control steps using the series of measurements to generate a sequence of the states of the device corresponding to the series of measurements. To perform an iteration for a current control step using current measurements, the composite stochastic filter is configured to execute a plurality of stochastic filters parameterized on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting the current state of the device using a prediction model subject to process noise and updating the predicted current state based on current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, the composite stochastic filter is further configured to estimate, for each of the plurality of stochastic filters, a likelihood of a corresponding measurement noise to correlate the current measurement with the predicted current state, combine the plurality of estimates of the state of the device in a weighted combination with a normalized weight derived from the likelihood estimated for a corresponding stochastic filter, and output the state of the device tracked by the composite stochastic filter for the current control step based on the weighted combination. The feedback controller is further configured to control the device using the sequence of states tracked by the composite stochastic filter.

[0019] Accordingly, another embodiment discloses a method for controlling a movement of a device based on a state of the device tracked using movement measurements indicative of the state of the device, the method comprising collecting a series of measurements indicative of the state of the device at different control steps, and iteratively executing a composite probabilistic filter configured to track the state of the device at each of the different control steps using the series of measurements to generate a sequence of the states of the device corresponding to the series of measurements. To perform an iteration for a current control step using current measurements, the composite stochastic filter is configured to execute a plurality of stochastic filters parameterized on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting a current state of the device using a prediction model subject to process noise and updating the predicted current state based on the current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, the composite stochastic filter is further configured to estimate, for each of the plurality of stochastic filters, a likelihood of a corresponding measurement noise to correlate the current measurements with the predicted current state, combine the plurality of estimates of the state of the device in a weighted combination with a normalized weight derived from the likelihood estimated for a corresponding stochastic filter, and output the state of the device tracked by the composite stochastic filter for the current control step based on the weighted combination. The method further includes controlling the device using the sequence of the states tracked by the composite stochastic filter.

[0020] Thus, yet another embodiment discloses a non-transitory computer readable storage medium having embodied thereon a program executable by a processor for performing a method for controlling a movement of a device based on a state of the device tracked using movement measurements indicative of the state of the device, the method comprising collecting a series of measurements indicative of the state of the device at different control steps, and iteratively executing a composite probabilistic filter configured to track the state of the device at each of the different control steps using the series of measurements to generate a sequence of states of the device corresponding to the series of measurements. To perform an iteration for a current control step using current measurements, the composite stochastic filter is configured to execute a plurality of stochastic filters parameterized on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting a current state of the device using a prediction model subject to process noise and updating the predicted current state based on the current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, the composite stochastic filter is further configured to estimate, for each of the plurality of stochastic filters, a likelihood of a corresponding measurement noise to correlate the current measurements with the predicted current state, combine the plurality of estimates of the state of the device in a weighted combination with a normalized weight derived from the likelihood estimated for a corresponding stochastic filter, and output the state of the device tracked by the composite stochastic filter for the current control step based on the weighted combination. The method further includes controlling the device using the sequence of the states tracked by the composite stochastic filter.

[0021] Accordingly, some embodiments disclose a controller for controlling movement of a vehicle based on a state of the vehicle tracked using Global Navigation Satellite System (GNSS) measurements, the controller including at least one processor and at least one memory having instructions stored thereon that, when executed by the at least one processor, cause the controller to collect a sequence of GNSS measurements indicative of the state of the vehicle at different control steps, and iteratively execute a composite probabilistic filter configured to track the state of the vehicle at the different control steps using the sequence of GNSS measurements to generate a sequence of states of the vehicle corresponding to the sequence of GNSS measurements. To perform an iteration for a current control step using current GNSS measurements, the composite stochastic filter is configured to execute a plurality of stochastic filters having the same measurement model relating the current GNSS measurements to a current estimate of the state of the vehicle to generate a plurality of estimates of the state of the vehicle for the current control step, the plurality of stochastic filters being subject to different measurement noises that introduce variation between the plurality of estimates of the state of the vehicle, the composite stochastic filter being further configured to combine the plurality of estimates of the state of the vehicle into a weighted combination according to the different measurement noises centered on an estimate of the current GNSS measurement predicted by one or a combination of the plurality of stochastic filters with a weight derived from the likelihood of the current GNSS measurement, and to estimate the state of the vehicle tracked by the composite stochastic filter for the current control step based on the weighted combination. The controller is further configured to control a vehicle using the sequence of the states estimated by the composite stochastic filter.

[0022] Embodiments of the present disclosure will now be further described with reference to the accompanying drawings, in which: The drawings shown are not necessarily to scale, with emphasis generally being placed upon illustrating the principles of embodiments of the present disclosure. [Brief description of the drawings]

[0023] [Figure 1A] FIG. 2 is a block diagram illustrating a feedback controller for controlling the movement of a device according to one embodiment of the present disclosure. [Figure 1B] FIG. 1 illustrates a schematic for controlling the movement of a device, according to some embodiments of the present disclosure. [Figure 1C] FIG. 2 illustrates iterative state tracking with a probabilistic filter, in accordance with some embodiments of the present disclosure. [Figure 1D] 1 illustrates an example evaluation of measurement noise accuracy, in accordance with some embodiments of the present disclosure. [Figure 1E] 1 illustrates an example evaluation of measurement noise accuracy, in accordance with some embodiments of the present disclosure. [Figure 1F] FIG. 2 illustrates a schematic for initialization of multiple probabilistic filters, in accordance with some embodiments of the present disclosure. [Figure 1G] FIG. 10 illustrates a schematic for initialization of multiple probabilistic filters, according to some other embodiments of the present disclosure. [Figure 2A] FIG. 1 is a schematic diagram illustrating a Global Navigation Satellite System (GNSS), in accordance with some embodiments of the present disclosure. [Figure 2B] FIG. 2 illustrates a scenario in which multipath disrupts a signal for a receiver, in accordance with some embodiments of the present disclosure. [Figure 3A] FIG. 1 is a block diagram illustrating a method for estimating a state of a vehicle using GNSS measurements and controlling the vehicle based on the estimated state, according to some embodiments of the disclosure. [Figure 3B] FIG. 2 is a block diagram illustrating functions performed by a composite probabilistic filter for iteration, according to some embodiments of the present disclosure. [Figure 3C] FIG. 2 is a block diagram for generating a weighted combination of state estimates according to some embodiments of the present disclosure. [Figure 3CCont] FIG. 3C is a block diagram for generating a weighted combination of state estimates according to some embodiments of the present disclosure. [Figure 3D]FIG. 13 is a block diagram for generating a weighted combination according to an alternative embodiment of the present disclosure. [Figure 3E] FIG. 1 illustrates a schematic for determining a weighted combination using stored historical weights, according to some embodiments of the present disclosure. [Figure 4A] FIG. 1 illustrates a schematic for estimating a vehicle state using measurements from multiple sensors, according to some embodiments of the present disclosure. [Figure 4B] 1A-1C are diagrams illustrating different sensor measurements collected according to various embodiments of the present disclosure. [Figure 4C] FIG. 1 illustrates a situation where there is an estimated measurement compared with a collected measurement of a sensor, according to various embodiments of the present disclosure. [Figure 4D] FIG. 13 illustrates a method for assigning different weights to different estimates of a probabilistic filter to form a composite distribution, according to various embodiments of the present disclosure. [Figure 4E] FIG. 13 illustrates how different sensors get different weightings and how different sensors are defined on different measurement spaces, according to various embodiments of the present disclosure. [Figure 4F] FIG. 1 illustrates how measurements defined on different measurement spaces are transformed to the same measurement space for comparison with each other, according to various embodiments of the present disclosure. [Diagram 5] FIG. 2 is a block diagram for controlling a vehicle based on estimated states according to various embodiments of the present disclosure. [Figure 6A] FIG. 2 illustrates a motion model employed in a probabilistic filter, according to some embodiments of the present disclosure. [Figure 6B] 1A-1C are diagrams illustrating how camera measurements generate distances to road lane markings and approach distances to the road ahead, according to some embodiments of the present disclosure. [Figure 7] FIG. 2 is a block diagram illustrating a controller for controlling vehicle movement based on a state of the vehicle tracked using GNSS measurements, according to some embodiments of the disclosure. [Figure 8A] FIG. 1 is a schematic diagram illustrating a vehicle including a controller for controlling the movement of the vehicle, according to some embodiments of the present disclosure. [Figure 8B] FIG. 2 is a schematic diagram illustrating an interaction between a controller and a vehicle controller according to some embodiments of the disclosure. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0024] In the following description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of the present disclosure. However, it will be apparent to those skilled in the art that the present disclosure may be practiced without these specific details. In other instances, devices and methods are shown only in block diagram form in order to avoid obscuring the present disclosure.

[0025] As used herein and in the claims, the words "for example," "for example," and "such as," as well as "comprising," "having," "including," and other verb forms thereof, when used in conjunction with a list of one or more components or other items, should be construed as open-ended. This means that the list should not be considered to exclude additional components or items. The term "based on" means based at least in part on. Furthermore, it should be understood that the terms and terminology used herein are for purposes of description and should not be considered as limiting. Any headings used herein are for convenience only and should not be construed as having any legal or limiting effect.

[0026] FIG. 1A illustrates a block diagram of a feedback controller 100 for controlling a movement of a device according to one embodiment of the present disclosure. The feedback controller 100 includes a processor 102 and a memory 104. The processor 102 may be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 104 may include a random access memory (RAM), a read only memory (ROM), a flash memory, or any other suitable memory system. Additionally, in some embodiments, the memory 104 may be implemented using a hard drive, an optical drive, a thumb drive, an array of drives, or any combination thereof. In an embodiment, the memory 104a stores a composite probabilistic filter 104a. The composite probabilistic filter 104a is described in detail in FIG. 1B. In an embodiment, the feedback controller 100 may be communicatively coupled to a device. The device may be a vehicle, a robot, a drone, or the like. The feedback controller 100 is configured to perform various functions for controlling the movement of the device as described in FIG. 1B.

[0027] FIG. 1B shows a schematic diagram for controlling the movement of a device according to some embodiments of the present disclosure. The feedback controller 100 controls the device based on a state of the device that is tracked using measurements indicative of the state of the device. For example, if the device is a vehicle, the state of the device may include one or a combination of the following: a position of the vehicle, a heading angle of the vehicle, and a speed of the vehicle. In an embodiment, the measurements indicative of the state of the device may correspond to sensor measurements. In other words, the measurements indicative of the state of the device may be obtained from one or more sensors. For example, the measurements indicative of the speed of the vehicle may be obtained from a speed sensor of the vehicle.

[0028] The feedback controller 100 first collects (110a) a series of measurements indicative of a state of the device at different control steps. The feedback controller 100 then iteratively executes (120a) a composite stochastic filter 104a configured to track a state of the device at each of the different control steps using the series of measurements to generate a sequence 125a of states of the device corresponding to the series of measurements. The feedback controller 100 then controls (130a) the device using the sequence 125a of states tracked by the composite stochastic filter 104a.

[0029] To perform an iteration for a current control step using current measurements, composite stochastic filter 104a is configured to execute multiple stochastic filters 121a, 122a, and 123a parameterized on the state of the device to generate multiple estimates of the state of the device for the current control step. For example, filter 121a generates state 131a, filter 122a generates state 132a, and filter 123a generates state 133a.

[0030] Each of the multiple stochastic filters 121a to 123a is a stochastic filter that iteratively predicts the current state of the device using a prediction model subject to process noise, and updates the predicted current state based on current measurement values ​​using a measurement model subject to measurement noise.

[0031] 1C shows an illustration of iterative state tracking by a probabilistic filter of some embodiments. Starting from a state estimate 110b determined for a previous iteration, the probabilistic filter predicts 115b a current state 120b of the device for the current iteration using a prediction model subject to process noise, such that the current state 120b is definable by parameters of a probabilistic distribution. Upon receiving a current measurement 125b, the probabilistic filter updates or corrects 135b the predicted current state 120b according to a measurement model relating the measurement to the predicted current state 120b and subject to measurement noise 130b to estimate a state 140b for the current iteration.

[0032] Due to the probabilistic nature of tracking, the selection of measurement noise 130b affects updates 135b, which in turn affects the estimation of state 140b, and thus the operation of the probabilistic filter may benefit from the accurate selection of measurement noise 130b.

[0033] Some embodiments are based on the recognition that internal variables and / or calculations of a probabilistic filter, such as a Kalman filter, can be used to assess the accuracy of the measurement noise 130b. In particular, a metric for assessing the accuracy of the measurement noise 130b can be the likelihood of the measurement noise 130b to correlate a current measurement indicative of the state of the device with a state predicted by a predictive model.

[0034] 1D and 1E show diagrams of an example assessment of the accuracy of measurement noise according to some embodiments of the present disclosure. A predicted current state 105d of a controlled device, e.g., a predicted state of a vehicle, is transformed 110d into the domain of measurements (i.e., measurement space) to obtain a transformed state 115d. An example of a transformed state is 140c shown in FIG. 1E. Such a transformation can be performed, for example, using a model of the measurements. Different measurement noise features, e.g., noise covariance, are then centered 120d on the transformed state 140c to estimate the likelihood of different measurement noises 110c and 120c shown in FIG. 1E. In an embodiment, each of the multiple probabilistic filters is a Kalman filter with process noise and measurement noise defined by corresponding Gaussian probability distributions, such that the estimate of the predicted current state transformed into the measurement space is the mean of the Gaussian probability distribution, and the different measurement noise covariances result in Gaussian probability distributions of the measurement noise.

[0035] Furthermore, when a current measurement 125d for updating the predicted current state 105d is received, the measurement may be mapped 130c onto a probability distribution to estimate the likelihood of the sought correlation. For example, as can be seen in FIG. 1E, the likelihood of the measurement noise 120c is greater than the likelihood of the measurement noise 110c on the mapped measurement 130c.

[0036] To that end, the composite stochastic filter 104a implements multiple stochastic filters 121a, 122a, and 123a with different measurement noises that introduce variance into multiple estimates of the device's state, e.g., state 131a, state 132a, and state 133a. The composite stochastic filter 104a then estimates the likelihood of the corresponding measurement noise for each of the multiple stochastic filters to correlate the current measurement with the predicted current state, and combines 150a the multiple estimates of the device's state into a weighted combination with normalized weights 141a, 142a, and 143a derived from the estimated likelihoods for the corresponding stochastic filters. Thus, the device's state tracked by the composite stochastic filter 104a for the current control step is based on the weighted combination 150a.

[0037] In this way, the measurement noise is evaluated using the internal variables and / or calculations of the stochastic filters without requiring additional statistical analysis of the measurements other than the performance of the stochastic filters. It should be noted that the above example not only illustrates the principle of correlation between state estimates and measurements, but can also be used to perform the estimation of the correlation. However, different techniques can be used to evaluate the correlation for different embodiments. For example, some embodiments use the evaluation of the Kalman gain determined by the Kalman filter to update the predicted current state and the covariance of the state estimate, which is the mean-square error (MSE) for an unbiased estimator. For example, one embodiment determines the Kalman gain for each Kalman filter in the composite stochastic filter, uses the Kalman gain to determine the updated predicted current state and the updated covariance of the state estimate, and determines the likelihood of the measurement noise based on the updated state and covariance based on the Kalman gain. Such calculations are performed inside the stochastic filters, i.e., are calculated in any way to track the state of the device. Therefore, the calculation of these variables does not require additional resources.

[0038] However, some embodiments are based on the recognition that although the computational requirements for the assessment of the accuracy of the measurement noise can be reduced by using internal variables and / or calculations of the stochastic filter, the assessment itself is compromised by the internal performance of the stochastic filter. In other words, the accuracy of the measurement noise is not necessarily a true accuracy reflecting a measurement independent of the stochastic filter, but an accuracy from the point of view of the stochastic filter itself. Therefore, it may be problematic to use this assessment to fit the measurement noise, but rather to use this assessment to assess the performance of the stochastic filter with the selected measurement noise.

[0039] To address this issue, some embodiments use multiple probabilistic filters with different measurement noises to determine the state of the device as a weighted combination of the state estimated by the multiple probabilistic filters 121a-123a and the weights of each filter derived from a corresponding evaluation of the likelihood of the measurement noise, to correlate current measurements indicative of the state of the device with the state predicted by the predictive models of the filters. In this way, different measurement noises can be taken into account without the need to analyze the statistical properties of the measurements.

[0040] In some embodiments, at each time step, each of the plurality of stochastic filters 121a-123a is initialized based on the weighted combination 125a. For example, referring to FIG. 1F, in one embodiment, the transformed state 140c is used to initialize each of the plurality of stochastic filters 121a-123a, and the predicted states of the stochastic filters are updated differently by having different measurement noise covariances inside each stochastic filter. Therefore, an estimate of the predicted current state (i.e., the transformed state 140c) transformed into the measurement space is common to the plurality of stochastic filters 121a-123a.

[0041] In some other embodiments, the multiple stochastic filters 121a-123a are run in parallel using the internal state estimates, and the weighted combination 125a is used only as the output of the composite stochastic filter 104a. For example, referring to FIG. 1G, the first transformed state 151a is obtained for the stochastic filter 121a by transforming the predicted current state of the stochastic filter 121a into the measurement space. The second transformed state 152a is obtained for the stochastic filter 122a by transforming the predicted current state of the stochastic filter 122a into the measurement space. The third transformed state 153a is obtained for the stochastic filter 123a by transforming the predicted current state of the stochastic filter 123c into the measurement space. Each of the multiple stochastic filters 121a-123a uses the first transformed state 151a, the second transformed state 152a, and the third transformed state 153a to update their own internal state estimates during the next iteration. In other words, the predicted current state estimates transformed into the measurement space differ for each probability filter.

[0042] Some embodiments are based on the realization that if the transformed state estimates are the same, initializing each stochastic filter at each time step of the control may cause a lack of divergence of the state estimates because each stochastic filter has only one time step to diverge. Some other embodiments are based on the understanding that operating multiple stochastic filters 121a-123a independently of each other may cause starvation, meaning that after a few time steps, only one of the multiple stochastic filters 121a-123a may have a non-zero weight.

[0043] To solve the above problems, some embodiments initialize the state estimates together at each time step of control by mixing the estimates, where the mixture of each probabilistic filter is determined by weights 141a, 142a, and 143a and their relationships with the other weights.

[0044] Some embodiments are based on the realization that the composite probabilistic filter framework illustrated in Figure 1B can be applied to Global Navigation Satellite System (GNSS) measurements. For example, the composite probabilistic filter 104a can be configured to track vehicle states at different control steps using the GNSS measurements to generate a sequence of vehicle states corresponding to the GNSS measurements.

[0045] GNSS is a system of satellites that can be used to determine the geographical position of a mobile receiver relative to the Earth. GNSS may include GPS, Galileo, Glonass, QZSS, and BeiDou. An example of a GNSS is detailed in FIG. 2A. FIG. 2A shows a schematic diagram of a GNSS according to some embodiments. For example, an Nth satellite 202 transmits code and carrier phase measurements 220 and 221 to a set of receivers 230 and 231. For example, receiver 230 is positioned to receive signals 210, 220 from N satellites 201, 203, 204, and 202. Similarly, receiver 231 is positioned to receive signals 221 and 211 from N satellites 201, 203, 204, and 202.

[0046] In various embodiments, the GNSS receivers 230 and 231 may be of different types. For example, in the exemplary embodiment of FIG. 2A, the receiver 231 is a base station receiver whose position is known. For example, the receiver 231 may be a ground-mounted receiver. In contrast, the receiver 230 is a mobile receiver configured to move. For example, the receiver 230 is mounted on a vehicle. In some implementations, the second receiver 231 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.

[0047] 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., takes into account stochastic disturbances and sources of errors). An object of other embodiments is to track the state of the vehicle using different information from different sources, rather than just relying 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 probabilistic distribution of the state of the vehicle received from a remote server using a radio frequency (RF) receiver.

[0048] In certain scenarios, such as between tall buildings, there are multiple distortions of the satellite signals, making it difficult to perform high-precision state estimation due to the information content in the code and carrier phase signals. For example, FIG. 2B illustrates a scenario in which multipath obstructs the signals for receiver 201b. Receiver 201b receives various signals 209b and 219b from satellites 210b and 220b. There are other satellites 230b and 240b that transmit signals 228b, 229b, 238b, 239b, but due to the presence of obstacles 270b, such as buildings in urban areas, these signals are not transmitted directly to the receiver.

[0049] Until then, the signal 238b transmitted from the satellite 240b was unavailable, but suddenly, the satellite signal 239b reaches the receiver after multipath 202b. Such a scenario can seriously impair the performance of the stochastic filter in tracking the vehicle state, because the stochastic filter will track the wrong ambiguous estimates, causing large estimation errors.

[0050] In addition, some embodiments are based on the realization that it is sufficient to know the relative position and speed with respect to other vehicles, which can be measured, for example, by ultrasound, radar or camera, to perform certain advanced driver-assistance system (ADAS) tasks, such as adaptive cruise control or short-term lane change. However, in many applications involving vehicles with more advanced ADAS and autonomous driving (AD) capabilities, it is not enough to know the relative position with respect to surrounding vehicles, which can be measured directly, but it is also important to know state information for objects that are not visible to the vehicle with AD capabilities at a given time step. For example, in route planning or multi-agent motion planning and cooperation tasks, the control problem to be solved is to optimally coordinate the vehicle towards different goals for each passenger of a given vehicle, who may have different priorities related to, for example, driver comfort and other performance metrics, on different road surface qualities. Identifying which timed path a particular vehicle should take depends on the position of the ego vehicle, the timed paths of other vehicles, and the specific environmental settings of the road in question.

[0051] Some embodiments are based on the recognition that, although cameras can be used to detect the relative motion of a vehicle with respect to the road and surrounding objects in its immediate vicinity with AD capabilities, cameras cannot provide global positioning of the vehicle. Also, distance sensors such as radar, lidar and ultrasonic can detect relative motion similarly to cameras, but cannot be used as the only sensor to globally position a vehicle.

[0052] Furthermore, some embodiments are based on the recognition that GNSS is prone to various obstructions and occlusions, for example from tall buildings in urban buildings. Therefore, some embodiments complement GNSS with additional sensing. For example, GNSS combined with additional sensing, such as cameras and lidar, can achieve global positioning, even with respect to maps and other objects, since GNSS provides global positioning and the additional sensing provides relative positioning. For this reason, some localization methods use multiple sensors together to determine the location of the vehicle, aiming to improve the performance with respect to what the sensor alone can provide.

[0053] To this end, various methods fuse GNSS measurements with measurements of different types of sensors indicative of the state of the device to estimate the state of the vehicle. The sensors may include one or a combination of cameras, radar, lidar, etc. Additionally or alternatively, the sensors may include one or a combination of cameras that generate color images, depth sensors that generate depth images, and roadside units (RSUs) that generate fusion measurements of multiple remote sensors.

[0054] Using measurements from multiple sensors can be beneficial: different sensors can complement each other, e.g. GNSS depends on whether the environment is urban or rural but is independent of the weather, and cameras do not depend on whether the environment is rural or urban but are influenced by weather conditions and road quality for detecting lane markings.

[0055] Various estimation methods for fusing measurements of multiple sensors assume that the measurement noise of such sensors is determined a priori. However, in reality, the measurement noise varies over time, as it varies depending on the driving conditions, the environment, and the filtering and computer vision algorithms that generated such sensor measurements. For example, a camera is used in conjunction with a computer vision algorithm to generate lane marking measurements of several lanes adjacent to the lane in which the vehicle is traveling. Depending on the weather conditions, road quality, and the particular computer vision algorithm used, the lane markings are sometimes detected accurately, sometimes detected with errors, and sometimes other parts of the road, such as cracks in the road, are detected as lane markings. Depending on the detection results, the characteristics of the measurement noise will change.

[0056] Therefore, there is a need to track the state of the vehicle by fusing the measurements of multiple sensors while adapting to the changing measurement noise. According to some embodiments, the feedback controller 100 may be operatively connected to multiple sensors using a wired communication link, a wireless communication link, or a wired and wireless communication link, such that the feedback controller 100 can collect the measurements of multiple sensors indicative of the state of the device. Furthermore, the measurement models of the multiple probabilistic filters 121a-123a of the composite probabilistic filter 104a fuse the measurements of the multiple sensors to generate a corresponding estimate of the state of the vehicle.

[0057] In the remainder of this disclosure, a method for estimating a vehicle state using GNSS measurements and controlling the vehicle based on the estimated state is described. Then, a method for estimating a vehicle state using measurements from multiple sensors is described.

[0058] FIG. 3A illustrates a block diagram of a method 300 for estimating a state of a vehicle using GNSS measurements and controlling the vehicle based on the estimated state, according to some embodiments of the disclosure. As used herein, a vehicle may be any type of moving element, such as a passenger car, a tractor trailer, a bus, a drone, or a mobile robot. In some embodiments, the method 300 uses a composite probabilistic filter 104a parameterized for the state of the vehicle. In block 310, the method 300 includes collecting a series of GNSS measurements indicative of a state of the vehicle at different control steps. In block 320, the method 300 includes iteratively executing the composite probabilistic filter 104a configured to track the state of the vehicle at different control steps using the series of GNSS measurements to generate a sequence of states of the vehicle corresponding to the series of GNSS measurements. In block 330, the method 300 includes controlling the vehicle based on the tracked state of the vehicle.

[0059] To perform the iterations for the current control step using the current GNSS measurements, the composite probabilistic filter 104a performs the function illustrated in FIG. 3B.

[0060] 3B illustrates a block diagram of functions performed by the composite stochastic filter 104a during iteration, according to some embodiments of the present disclosure. In block 340, the composite stochastic filter 104a executes multiple stochastic filters having the same measurement model that relates current GNSS measurements to a current estimate of the vehicle's state to generate multiple estimates of the vehicle's state for the current control step. The multiple stochastic filters are subject to different measurement noises that result in variability among the multiple estimates of the vehicle's state. In some implementations, the multiple stochastic filters include a first stochastic filter configured to generate a first estimate of the vehicle's state subject to a first measurement noise defined by a first probability density function (PDF) and a second stochastic filter configured to generate a second estimate of the vehicle's state subject to a second measurement noise defined by a second PDF different from the first PDF.

[0061] In block 350, the multiple estimates of the vehicle state are combined into a weighted combination with weights derived from the likelihood of the current GNSS measurement according to different measurement noises around the estimate of the current GNSS measurement predicted by one or a combination of the multiple probabilistic filters. In block 360, the composite probabilistic filter 104a estimates the state of the vehicle tracked by the composite probabilistic filter 104a for the current control step based on the weighted combination.

[0062] 3C illustrates a block diagram for generating the weighted combination 350 according to some embodiments of the present disclosure. In block 370a, the composite probabilistic filter 104a determines an estimate of the current GNSS measurement by transforming a prediction of the current state of the vehicle from state space to measurement space. In block 370b, the composite probabilistic filter 104a centers the estimate of the current GNSS measurement on the first PDF and the second PDF.

[0063] In block 370c, the composite stochastic filter 104a determines a first likelihood of the current GNSS measurement according to a first PDF centered on the estimate of the current GNSS measurement. In block 370d, the composite stochastic filter 104a determines a second likelihood of the current GNSS measurement according to a second PDF centered on the estimate of the current GNSS measurement. In block 370e, the composite stochastic filter 104a normalizes the first likelihood and the second likelihood to determine a first weight for weighting the first estimate of the vehicle state and a second weight for weighting the second estimate of the vehicle state.

[0064] In block 370f, the composite probabilistic filter 104a determines a weighted combination based on the first estimate of the vehicle's state weighted with the first weight and the second estimate of the vehicle's state weighted with the second weight.

[0065] 3D shows a block diagram for generating the weighted combination 350 according to an alternative embodiment of the present disclosure. In block 380a, the composite probabilistic filter 104a executes a first probabilistic filter to predict a first current state of the vehicle based on an internal state of the first probabilistic filter and updates the first current state of the vehicle. The first current state of the vehicle is updated using a measurement model that processes the current GNSS measurements according to the gain of the first probabilistic filter to generate a first estimate of the state of the vehicle.

[0066] At block 380b, the composite probabilistic filter 104a executes a second probabilistic filter to predict a second current state of the vehicle based on an internal state of the second probabilistic filter and updates the second current state of the vehicle using a measurement model that processes the current GNSS measurements according to a gain of the second probabilistic filter to generate a second estimate of the vehicle state.

[0067] In block 380c, the composite probabilistic filter 104a updates the internal state of the first stochastic filter and the internal state of the second stochastic filter based on a combination of the first and second estimates of the vehicle state produced by the first and second stochastic filters.

[0068] In some embodiments, the composite probabilistic filter stores the historical weights of the first and second probabilistic filters determined for some previous control steps. Furthermore, the composite probabilistic filter 104a updates the weight for weighting the first stochastic filter in the current control step based on the average of the current weight (e.g., the first weight) and the historical weight of the first stochastic filter. Similarly, the composite probabilistic filter 104a updates the weight for weighting the second stochastic filter in the current control step based on the average of the current weight (e.g., the second weight) and the historical weight of the second stochastic filter.

[0069] FIG. 3E shows a schematic diagram for determining a weighted combination using stored history weights according to some embodiments of the present disclosure. In block 390a, state estimates determined by each probabilistic filter are received. Then, in block 390b, collected measurements 390d are used to compare stored history weights 390e with a noise covariance hypothesis to determine weights 390f. In block 390c, based on the determined weights 390f, the state estimates are combined as a weighted combination. The history weights may be stored in memory 104 of the feedback controller 100. In an alternative embodiment, the history weights may be stored in an external memory, and the feedback controller 100 retrieves the history weights from the external memory.

[0070] The comparing of state estimates and determining the weights 390f can be performed in several ways. For example, in one embodiment, the weights for each probability filter are determined by a combination of stored historical weights and weighted differences of collected measurements and received state estimates when inserting the state estimates into a measurement model with an associated hypothesis of noise covariance. In another embodiment, the weights for each probability filter are determined uniquely by weighted differences of collected measurements and received state estimates when inserting the state estimates into a measurement model with an associated hypothesis of noise covariance. In yet another embodiment, the weights are determined based on an average of current weighted differences of collected measurements and received state estimates when inserting the state estimates into a measurement model with an associated hypothesis of noise covariance and weights determined using a certain number of previous time steps. For example, one embodiment determines the weights as moving averages using a sliding window of weights determined using previous time steps. This provides a means of controlling the variation of the weights over successive time steps to ensure smooth estimation performance.

[0071] 4A shows a schematic diagram for estimating a vehicle state using measurements of multiple sensors, according to some embodiments of the present disclosure. In some embodiments, a composite probabilistic filter 400a parameterized for the vehicle state is used to estimate the vehicle state. The composite probabilistic filter 400a estimates the vehicle state based on collected measurements 409a. For example, the collected measurements 409a include a first measurement from a first sensor and a second measurement from a second sensor.

[0072] The composite stochastic filter 400a includes multiple stochastic filters. For example, the composite stochastic filter 400a includes a first stochastic filter 420a and a second stochastic filter 410a. The first stochastic filter 420a uses a first hypothesis 407b of the noise covariance of the first measurement and a first hypothesis 207b of the noise covariance of the second measurement to determine a first state estimate 425a. Similarly, the second stochastic filter 410a uses a second hypothesis 408a of the noise covariance of the first measurement and a second hypothesis 408a of the noise covariance of the second measurement to determine a second state estimate 415a. In an embodiment, the first hypothesis of the noise covariance of the first measurement is different from the second hypothesis of the noise covariance of the first measurement, the first hypothesis of the noise covariance of the second measurement is different from the second hypothesis of the noise covariance of the second measurement, or both. In such a case, the first noise distribution and the second noise distribution of any measurement are different distributions with respect to that parameter but represent the same entity, e.g., both distributions represent camera measurements but with different noise values.

[0073] The first state estimate 425a of the first probabilistic filter 420a and the second state estimate 415a from the second probabilistic filter 410a are then used to determine the state of the vehicle 435a as a weighted combination 430a, where the combination weighting 430a is determined as online, offline, or a combination thereof. In one embodiment, the state of the vehicle 435a is the weighted average of the first probabilistic filter 420a and the second probabilistic filter 410a. In another embodiment, the state of the vehicle 435a is the output of the probabilistic filter with the highest weight.

[0074] In one embodiment, each stochastic filter is a linear regression Kalman filter that estimates the mean and covariance of the vehicle state. In another embodiment, the stochastic filters are particle filters that output a sampled representation of the posterior distribution. Some embodiments use stochastic filters including various variants of the Kalman filter (KF), such as linear regression KFs (LRKFs), e.g., extended KFs (EKFs), unscented KFs (UKFs), etc.

[0075] Some embodiments are based on the understanding that if a probabilistic filter using a particular combination of measurement noise values ​​for various sensors provided the best fit in the previous time step, it is likely to provide a good fit for the next time step as well. For example, if a GNSS measurement was unreliable in the previous time step due to multipath, for example in an urban environment, it may also be subject to multipath in the next time step. However, there is also an opportunity to generate reliable measurements if the unreliability of the measurement is not due to multipath effects, but some other unmodeled disturbance. Some embodiments use this understanding to integrate and weight multiple probabilistic filters in each time step of the control, whereby the probabilistic filters are given different weights based on their weights in the previous time step.

[0076] Some embodiments are based on the understanding that the computational complexity of multiple probabilistic filters undesirably grows with the number of noise value hypotheses and the number of sensors. For example, in one embodiment, it is understood that using different possible noise values ​​can be interpreted as gridding a continuous space of possible noise values, such that the number of possible combinations of noise values, and therefore the number of probabilistic filters used in the estimation, becomes exponentially complex.

[0077] Some embodiments recognize that in automotive applications, the computational power of automotive-grade electronic control units is limited, and the use of a large number of probabilistic filters is not computationally feasible. One embodiment improves on this by selecting a subset of the probabilistic filters to be used for the estimation. For example, one embodiment selects the probabilistic filters that correspond to values ​​of the measurement noise that are close to being representative of the conditions at the previous time step. This allows the subset of probabilistic filters selected at each time step to follow the evolution of the sensor quality.

[0078] 4B shows a diagram of different collected sensor measurements 409a according to various embodiments of the present disclosure. For example, a first measurement of the collected measurements 409a is a camera measurement 410d that measures lane markings on the road together with a computer vision algorithm, and a second measurement is a lidar 420d that measures the environment, e.g., road barriers and distances to other vehicles. Additionally or alternatively, the measurements 409a are a combination of measurements that provide a measurement of the vehicle in its environment 430d, 440d, where the environment has been mapped a priori offline, e.g., by optimizing weights according to fitting a corresponding model with the measurements.

[0079] Some embodiments use a first hypothesis for the noise covariance of the measurements and a second hypothesis for the noise covariance of the measurements, where the first hypothesis is used by one of the probabilistic filters and the second hypothesis is used by another of the probabilistic filters in the composite filter.

[0080] 4C illustrates a situation where there is an estimated measurement 415e compared with a collected measurement 425e of a sensor. One probabilistic filter uses the hypotheses of distribution 430e, and another uses the hypotheses of distribution 410e. As a result, in the scenario of FIG. 4C, the probabilistic filter using the hypotheses of distribution 430e accurately describes the collected measurement because the probability of the value of collected measurement 425e is higher for distribution 410e than for distribution 430e.

[0081] 4D illustrates a method of assigning different weights to different estimates of the probabilistic filters to form a composite distribution, according to various embodiments of the present disclosure. A first probabilistic filter 420a estimates a state distribution 430f, and a second probabilistic filter 410a estimates a state distribution 410f. Weights 435f and 415f for each probabilistic filter are determined. A composite distribution 420f is determined as a weighted combination of the state distributions 430f and 410f.

[0082] 4E illustrates how different sensors obtain different weights and are defined on different measurement spaces according to various embodiments of the present disclosure. For example, some embodiments use a first hypothesis of distribution 414g for a first measurement defined on a measurement space 417g, a second hypothesis of distribution 415g, and a first hypothesis of distribution 424g for a second measurement defined on a second measurement space 427g, a second hypothesis of distribution 425g. In some embodiments, distributions 414g and 415g for the first measurement space 417g belong to the same measurement space 417g, and distributions 424g and 425g for the second measurement space 427g belong to the same measurement space 427g. Then, 414g and 415g can be combined with 425g and 426g in different ways. For example, combination 414g and 425g can be used in one probability filter, while combination 414g and 424g can be used in another probability filter. Because distributions 414g and 415g are defined over the same measurement space, they can be used directly in the individual filters for subsequent measurement updates.

[0083] One embodiment recognizes that because the first measurement is defined on a measurement space 417g and the second measurement is defined on a second measurement space 427g, they cannot be directly compared.

[0084] 4F illustrates how measurements defined on different measurement spaces are transformed into the same measurement space for comparison with each other, according to various embodiments of the present disclosure. Some embodiments transform a first measurement distribution 415h with a transform 416h and a second measurement distribution 425h with a transform 426h, resulting in a first measurement distribution 435h and a second measurement distribution 445h. In this case, these distributions can be compared and weighted with each other because they are defined on the same measurement space 446h.

[0085]

number

[0086] In some embodiments, the output estimates 435a for the vehicle's state are used to control the vehicle. In one embodiment, a model-predictive controller (MPC) is used to control the vehicle based on the output estimates 435a for the vehicle's state.

[0087] 5 illustrates a block diagram for controlling a vehicle based on estimated states 521, according to various embodiments of the present disclosure. Specifically, the MPC calculates a control solution, e.g., a solution vector 555, by solving 550 an inequality-constrained optimization problem in the form of an optimal control structuring program 550 at each control time step. The solution vector 555 includes a sequence of future optimal control inputs over the forecast time horizon of the system. The optimal control data 545 for the control cost function 540, equality and inequality constraints 530 in the optimization problem 550 depends on the dynamic model 525, the vehicle constraints 520, the current estimated state of the vehicle 521, and the control commands including criteria 505 and confidence 506.

[0088] In some embodiments, the solution of the inequality constrained optimization problem 550 uses state and control values ​​over the forecast time range from the previous control time step 510, which can be read from the memory of the MPC. This concept is referred to as a warm or hot start of the optimization algorithm and greatly reduces the required computational effort of the MPC. Similarly, the corresponding solution vector 555 can be used to update and store a set of optimal or suboptimal state and control values ​​for the next control time step 560.

[0089] In some embodiments of the present disclosure, the MPC adapts one or more terms in a control cost function 540 to a criterion 505 and corresponding confidence 506 calculated by the composite probabilistic filter 104a.

[0090] In one embodiment, the predictive model used by the probabilistic filter to predict the current state of the vehicle (e.g., current state 120b) may be a kinetic model of the vehicle's state transitions subject to process noise. An example of a kinetic model is described below in FIG. 6A.

[0091]

number

[0092]

number

[0093] Furthermore, the state predicted by the motion model (e.g., state z) is updated using the measurement model. The measurement model used in the probabilistic filter models the measurements (e.g., GNSS measurements). Some embodiments model the GNSS measurements as position measurements that are preprocessed by a probabilistic filter, e.g., a mixed integer Kalman filter or a particle filter. Other embodiments model the camera measurements as distances to lane markings of the road and approach distances to the road ahead, e.g., as shown in FIG. 6B. In this case, the camera measures the distance 610b to lane markings, either lanes or lane markings, as a polynomial 620b of the road ahead. Some other embodiments leverage a previous map of the road to relate the relative position measurements provided by the camera to the global position measurements provided by the GNSS. Further embodiments leverage an inertial measurement unit, including an accelerometer and a gyro, to further improve the vehicle state estimation. Some embodiments model the y k =h(x k ,u k )+e k We model the measurement model as a Gaussian distributed according to k is a zero-mean Gaussian distributed with a block-diagonal covariance matrix, where the elements of the covariance matrix are assumed for each of the multiple stochastic filters 121a-123a of the composite stochastic filter 104a. Some embodiments are based on the realization that even if the underlying distribution is not Gaussian, typically the first two moments (e.g., mean and variance) adequately describe the distribution. In some embodiments, this realization is exploited in determining the covariance of the vehicle states.

[0094]

number

[0095]

number

[0096] Some embodiments assume that process noise is introduced additively, which significantly simplifies the calculations.

[0097] Some other embodiments utilize the LRKF to find the relevant moment integrals, where a set of integration points is used. For example, some embodiments use integration points according to an unscented transformation, while other embodiments utilize spherical cubature rules. However, any set of integration schemes can be used.

[0098] In some embodiments, the composite probabilistic filter 104a is implemented according to an interacting multiple model (IMM) framework. Using the IMM allows a systematic way to incorporate the first and second measurements with different combinations of hypotheses on the noise covariance of the measurements. At each time step k, the IMM assigns a weight q to each model that reflects the probability of explaining the measurement. k Assign.

[0099]

number

[0100]

number

[0101]

number

[0102]

number

[0103] Some embodiments provide a controller for controlling the movement of a vehicle based on a state of the vehicle tracked using GNSS measurements, such a controller is described in more detail below in FIG.

[0104] FIG. 7 illustrates a block diagram of a controller 700 for controlling the movement of a vehicle 720a based on a state of the vehicle 720a tracked using GNSS measurements, according to some embodiments of the disclosure. The controller 700 is communicatively coupled to the vehicle 720a. The controller 700 includes a computer, for example in the form of a single central processing unit (CPU) or multiple CPU processors 760a connected to a memory 765a for storing a motion model 740a, a measurement model 741a, constraints 742a, and a hypothesis of a noise covariance 743a of measurements of the vehicle 720a. The processor 760a may be a single-core microprocessor, a multi-core processor, a computing cluster, a network of multiple connected processors, or any number of other configurations. The memory 765a may include a random access memory (RAM), a read-only memory (ROM), a flash memory, or any other suitable memory system.

[0105] The controller 700 performs the steps of the method described in Figure 2A to estimate the state of the vehicle 720a. Based on the estimated state of the vehicle 720a, the controller 700 generates a control input 711a. The controller 700 controls the vehicle 720a based on the control input 711a.

[0106] FIG. 8A illustrates a schematic diagram of a vehicle 720a including a controller 700 according to some embodiments of the present disclosure. As used herein, the vehicle 720a may be any type of wheeled vehicle, such as a car, a bus, or a rover. Also, the vehicle 720a may be an autonomous vehicle or a semi-autonomous vehicle. For example, some embodiments control the movement of the vehicle 720a. Examples of the movement include lateral movement of the vehicle controlled by a steering system 803 of the vehicle 720a. In one embodiment, the steering system 803 is controlled by the controller 700. Additionally or alternatively, the steering system 803 may be controlled by a driver of the vehicle 720a.

[0107] The vehicle may also include an engine 806 that may be controlled by the controller 700 or other components of the vehicle 720a. The vehicle 702a may also include one or more sensors 804 for sensing the surrounding environment. Examples of the sensors 804 include range finders, radar, lidar, and cameras. The vehicle 720a may also include one or more sensors 805 for sensing its current momentum and internal conditions. Examples of the sensors 805 include a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, an axial rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors provide information to the controller 700. The vehicle may be equipped with a transceiver 807 that enables communication capabilities of the controller 700 through wired or wireless communication channels.

[0108] FIG. 8B shows a schematic diagram of the interaction between the controller 700 and the controller 820 of the vehicle 720a, according to some embodiments of the disclosure. For example, in some embodiments, the controller 820 of the vehicle 720a is a steering controller 825 and a brake / throttle controller 830 that control the turning and acceleration of the vehicle 720a. In such a case, the controller 700 outputs control inputs to the controllers 825 and 830 to control the state of the vehicle 720a. The controller 820 may also include a higher level controller, for example, a lane keeping assist controller 835, which further processes the control inputs of the controller 700. In either case, the controller 820 uses the output of the controller 700 to control at least one actuator of the vehicle, such as the steering wheel and / or brakes of the vehicle 720a, to control the movement of the vehicle 720a.

[0109] Alternatively, in some embodiments, feedback controller 100 may be used in place of controller 700 to control vehicle 720a. Feedback controller 100 also provides control inputs to controller 820 based on the estimated state of vehicle 720a to control the movement of vehicle 720a.

[0110] Some embodiments are based on the realization that the accuracy of a sensor can change smoothly and rapidly over time. Other embodiments recognize that probabilistic filters such as KFs and LRKFs, in their standard formulation, require an a priori set noise covariance of the measurements used to update the state estimate, which has the effect of normalizing and weighting together the importance of each measurement. Some other embodiments are based on the realization that KFs that adapt noise, for example using a variational Bayesian method for noise adaptation, do not have a convergence guarantee and work well when changing the a priori set noise covariance slightly. However, such methods are prone to errors, for example in the case of outliers in GPS measurements, because outliers tend to occur from one time step to another, and it is well known that noise adaptation methods involve problems with accurately adapting such large changes to the noise covariance in real time. For example, variational Bayesian methods for noise estimation rely on an adjustment factor, e.g., a forgetting factor, that determines the rate at which changes in the state estimate can occur. Such a forgetting factor is designed assuming slowly changing conditions. Thus, rapid changes, such as those in GNSS measurement reliability, are not well handled by such approaches. This is in contrast to the fast-converging method disclosed in this disclosure, since the probabilities for each probability filter are determined using likelihoods that have assumptions about the measurement noise covariance built into the likelihoods.

[0111] The following description provides exemplary embodiments only and is not intended to limit the scope, applicability, or configuration of the present disclosure. Rather, the following description of exemplary embodiments will provide those skilled in the art with an enabling description for implementing one or more exemplary embodiments. It is contemplated that various changes may be made in the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter as set forth in the appended claims.

[0112] In the following description, specific details are given to enable a thorough understanding of the embodiments. However, those skilled in the art will appreciate that the embodiments may be practiced without these specific details. For example, systems, processes and other elements in the disclosed subject matter may be shown as components in block diagram form in order to avoid obscuring the embodiments in unnecessary detail. In other instances, well-known processes, structures and techniques may be shown without unnecessary detail in order to avoid obscuring the embodiments. Additionally, like reference numbers and names in the various drawings refer to like elements.

[0113] Also, particular embodiments may be described as a process that is depicted as a flowchart, a flow diagram, a data flow diagram, a structure diagram, or a block diagram. Although a flowchart may describe operations as a sequential process, many of the operations may be performed in parallel or simultaneously. In addition, the order of operations may be rearranged. A process may be terminated when its operations are completed, but may include additional steps not described or included in the diagram. Moreover, not all operations in any process specifically described may be performed in all embodiments. A process may correspond to a method, a function, a procedure, a subroutine, a subprogram, or the like. When a process corresponds to a function, the end of the function may correspond to a return of the function to the calling function or to the main function.

[0114] Furthermore, embodiments of the disclosed subject matter may be implemented, at least in part, either manually or automatically. Manual or automatic implementations may be performed or at least assisted by the use of machines, hardware, software, firmware, middleware, microcode, hardware description languages, or any combination thereof. When implemented in software, firmware, middleware, or microcode, program code or code segments to perform the necessary tasks may be stored in a machine-readable medium. A processor may perform the necessary tasks.

[0115] 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.

[0116] The embodiments of the present disclosure may be embodied as methods, which are provided as examples. The operations performed as part of the method may be ordered in any suitable manner. Thus, while shown as sequential operations in the exemplary embodiments, embodiments may be constructed in which operations are performed in a different order than that shown, including performing some operations simultaneously.

[0117] Furthermore, the embodiments of the present disclosure and the functional operations described herein can be realized in digital electronic circuitry, in tangibly implemented computer software or firmware, in computer hardware including the structures disclosed herein and their structural equivalents, or in a combination of one or more of them. Furthermore, some embodiments of the present disclosure can be realized as one or more computer programs, i.e., as one or more modules of computer program instructions encoded on a tangible, non-transitory program carrier for execution by or for controlling the operation of a data processing apparatus. Still further, the program instructions can be encoded in an artificially generated propagated signal, for example, a machine-generated electrical, optical, or electromagnetic signal. The propagated signal is generated to encode information that is transmitted to a suitable receiving device for execution by a data processing apparatus. The computer storage medium may be a machine-readable storage device, a machine-readable storage substrate, a random access memory device, or a serial access memory device, or one or more combinations thereof.

[0118] According to an embodiment of the present disclosure, the term "data processing apparatus" may encompass all kinds of apparatus, devices and machines for processing data, including, by way of example, a programmable processor, computer, or multiple processors or computers. The apparatus may include special purpose logic circuitry, such as a field programmable gate array (FPGA) or an application specific integrated circuit (ASIC). In addition to hardware, the apparatus may also include code that creates an execution environment for the computer program, such as code that constitutes a processor firmware, a protocol stack, a database management system, an operating system, or one or more combinations thereof.

[0119] A computer program (which may be called or described as a program, software, software application, module, software module, script, or code) may be written in any form of programming language, including compiled or interpreted, or declarative or procedural, and may be deployed in any form, such as a stand-alone program or as a module, component, subroutine, or other unit suitable for use in a computing environment. A computer program may, but need not, correspond to a file in a file system. A program may be stored in part of a file that holds other programs or data, for example in one or more scripts stored in a markup language document, in a single file dedicated to the program in question, or in a number of coordinated files, for example in a file that stores one or more modules, subprograms, or portions of code.

[0120] A computer program can be deployed to be executed on one computer or on multiple computers located at one site or distributed at multiple sites and interconnected by a communication network. A computer suitable for executing a computer program can be based, by way of example, on a general-purpose or special-purpose microprocessor or both, or any other type of central processing unit. In general, the central processing unit will receive instructions and data from a read-only memory or a random access memory or both. The essential elements of a computer are a central processing unit for carrying out or executing instructions, and one or more memory devices for storing instructions and data.

[0121] Typically, a computer also includes one or more mass storage devices, such as magnetic, magneto-optical, or optical disks, for storing data, or is operatively coupled to such disks to receive data from or transfer data to, or both. However, a computer need not have such devices. Furthermore, a computer can be embedded in another device, such as a mobile phone, a personal digital assistant (PDA), a mobile audio or video player, a game console, a Global Positioning System (GPS) receiver, or a portable storage device, such as a universal serial bus (USB) flash drive, to name a few.

[0122] To facilitate user interaction, embodiments of the subject matter described herein can be implemented on a computer having a display device, such as a cathode ray tube (CRT) or liquid crystal display (LCD) monitor, for displaying information to the user, and a keyboard and pointing device, such as a mouse or trackball, for enabling the user to provide input to the computer. Other types of devices may also be used to facilitate user interaction. For example, feedback provided to the user may be any form of sensory feedback, such as visual feedback, auditory feedback, or tactile feedback, and input from the user may be received in any form, including acoustic, speech, or tactile input. Additionally, the computer may facilitate user interaction by sending documents to and receiving documents from a device used by the user, for example, by sending a web page to a web browser on the user's client device in response to a request received from the web browser.

[0123] An embodiment of the subject matter described herein may be implemented in a computing system that includes a back-end component, e.g., a data server, or includes a middleware component, e.g., an application server, or includes a front-end component, e.g., a client computer having a graphical user interface or web browser that allows a user to interact with an implementation of the subject matter described herein, or includes any combination of one or more such back-end, middleware, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication, e.g., a communications network. Examples of communications networks include local area networks (LANs) and wide area networks (WANs), e.g., the Internet.

[0124] A computing system may include clients and servers. Clients and servers are generally remote from each other and typically interact through a communication network. The relationship of client and server arises by virtue of computer programs running on the respective computers and having a mutual relationship between client and server.

[0125] Although the present disclosure has been described with reference to certain preferred embodiments, it should be understood that various other adaptations and modifications can be made within the spirit and scope of the disclosure. It is therefore the object of the appended claims to cover all such variations and modifications as fall within the true spirit and scope of the present disclosure.

Claims

1. 1. A feedback controller for controlling a movement of a device based on a state of the device tracked using measurements of the movement indicative of the state of the device, the feedback controller comprising: at least one processor; and at least one memory having instructions stored thereon, the instructions, when executed by the at least one processor, causing the feedback controller to: collecting a series of measurements indicative of said state of said device at different control steps; iteratively executing a composite stochastic filter configured to track the states of the device at each of the different control steps using the series of measurements to generate a sequence of states of the device corresponding to the series of measurements, and to perform an iteration for a current control step using a current measurement, the composite stochastic filter comprising: and configured to execute a plurality of parameterized stochastic filters on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting a current state of the device using a prediction model subject to process noise and updating the predicted current state based on the current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, and the composite stochastic filter further comprises: for each of the plurality of probabilistic filters, estimating a likelihood of a corresponding measurement noise to correlate the current measurement with the predicted current state; combining the estimates of the states of the device into a weighted combination with normalized weights derived from the likelihoods estimated for corresponding probabilistic filters; based on the weighted combination, outputting the state of the device tracked by the composite probabilistic filter for the current control step, the instructions further including: A feedback controller that controls the device using the sequence of states tracked by the composite probabilistic filter.

2. 2. The feedback controller of claim 1, wherein the composite probabilistic filter is configured to estimate a likelihood of correlation of different measurement noises as a likelihood of the current measurement value according to the different measurement noises centered on the predicted current state estimate transformed into a measurement space.

3. The feedback controller of claim 2 , wherein the predicted current state is transformed to the measurement space using a measurement model.

4. 3. The feedback controller of claim 2, wherein each of the plurality of probabilistic filters is a Kalman filter with the process noise and the measurement noise defined by a corresponding Gaussian probability distribution, such that a mean of the Gaussian probability distribution is an estimate of the predicted current state transformed into the measurement space, and different measurement noise covariances result in the Gaussian probability distribution of the measurement noise.

5. The feedback controller of claim 4 , wherein the estimate of the predicted current state transformed into the measurement space is common to all of the plurality of probabilistic filters.

6. The feedback controller of claim 4 , wherein the estimate of the predicted current state transformed into the measurement space is different for different probabilistic filters.

7. 5. The feedback controller of claim 4, wherein the composite probabilistic filter is configured to estimate the likelihood of correlation of different measurement noises based on values ​​of gains of corresponding Kalman filters used to update the predicted current state based on the current measurements.

8. The feedback controller of claim 7 , wherein the gain is a Kalman gain.

9. The feedback controller of claim 1 , wherein the device is a vehicle tracked using Global Navigation Satellite System (GNSS) measurements, and the measurements include GNSS measurements.

10. 2. The feedback controller of claim 1, wherein the feedback controller is operatively connected to a plurality of sensors of different types using at least one of a wired link, a wireless link, or a combination thereof to collect the plurality of sensor measurements indicative of the state of the device, and the measurement model of the plurality of probabilistic filters fuses the measurements of the plurality of sensors to generate a corresponding estimate of the state of the device.

11. 1. A controller for controlling movement of a vehicle based on a state of the vehicle tracked using Global Navigation Satellite System (GNSS) measurements, the controller comprising: at least one processor; and at least one memory having instructions stored thereon, the instructions, when executed by the at least one processor, causing the controller to: collecting a series of GNSS measurements indicative of the state of the vehicle at different control steps; iteratively executing a composite probabilistic filter configured to track the states of the vehicle at the different control steps using the sequence of GNSS measurements to generate a sequence of states of the vehicle corresponding to the sequence of GNSS measurement values, and to perform an iteration for a current control step using a current GNSS measurement value, the composite probabilistic filter comprising: and configured to execute a plurality of probabilistic filters having an identical measurement model relating the current GNSS measurements to a current estimate of the state of the vehicle to generate a plurality of estimates of the state of the vehicle for the current control step, the plurality of probabilistic filters being subject to different measurement noise characteristics that introduce variability between the plurality of estimates of the state of the vehicle, the composite probabilistic filter further comprising: combining the estimates of the state of the vehicle according to different measurement noises centered on the estimate of the current GNSS measurement predicted by one or a combination of the plurality of probabilistic filters into a weighted combination with weights derived from the likelihood of the current GNSS measurement; and configured to estimate the state of the vehicle tracked by the composite probabilistic filter for the current control step based on the weighted combination, the instructions further including: A controller that controls the vehicle using the sequence of states estimated by the composite probabilistic filter.

12. The plurality of probabilistic filters includes a first stochastic filter configured to generate a first estimate of the state of the vehicle subject to a first measurement noise defined by a first probability density function (PDF) and a second stochastic filter configured to generate a second estimate of the state of the vehicle subject to a second measurement noise defined by a second PDF different from the first PDF, and for generating the weighted combination, the composite stochastic filter is configured to: determining the estimate of the current GNSS measurements by transforming a prediction of the current state of the vehicle from a state space to a measurement space; centering the first PDF and the second PDF with the estimate of the current GNSS measurement; determining a first likelihood of the current GNSS measurement according to the first PDF centered on the estimate of the current GNSS measurement; determining a second likelihood of the current GNSS measurement according to the second PDF centered on the estimate of the current GNSS measurement; normalizing the first likelihood and the second likelihood to determine a first weight for weighting the first estimate of the state of the vehicle and a second weight for weighting the second estimate of the state of the vehicle; 12. The controller of claim 11, configured to determine the weighted combination based on the first estimate of the state of the vehicle weighted with the first weight and the second estimate of the state of the vehicle weighted with the second weight.

13. The plurality of probabilistic filters includes a first stochastic filter configured to generate a first estimate of the state of the vehicle subject to a first measurement noise defined by a first probability density function (PDF) and a second stochastic filter configured to generate a second estimate of the state of the vehicle subject to a second measurement noise defined by a second PDF different from the first PDF, and for generating the weighted combination, the composite stochastic filter is configured to: Executing the first probabilistic filter to predict a first current state of the vehicle based on an internal state of the first probabilistic filter, updating the first current state of the vehicle using the measurement model, and processing the current GNSS measurements according to a gain of the first probabilistic filter to generate the first estimate of the state of the vehicle; Executing the second probabilistic filter to predict a second current state of the vehicle based on an internal state of the second probabilistic filter, updating the second current state of the vehicle using the measurement model, and processing the current GNSS measurements according to gains of the second probabilistic filter to generate the second estimate of the state of the vehicle; 12. The controller of claim 11, configured to update the internal state of the first probabilistic filter and the internal state of the second probabilistic filter based on a combination of the first and second estimates for the state of the vehicle produced by the first and second probabilistic filters.

14. The plurality of probabilistic filters includes a first probabilistic filter configured to generate a first estimate of the state of the vehicle subject to a first measurement noise defined by a first probability density function (PDF), and for generating weights for scaling the first estimate of the state of the vehicle, the first probabilistic filter is configured to: predicting a current state of the vehicle using a motion model of the vehicle subject to process noise based on a previous control command applied to the vehicle having a previous state in a previous control step; Transforming the predicted current state of the vehicle into the measurement space to generate the estimates of the current GNSS measurements; centering the first PDF on the estimate of the current GNSS measurements; 12. The controller of claim 11, further comprising: determining a weight for weighting the first estimate of the state of the vehicle based on a likelihood of the current GNSS measurement according to the first PDF centered on the estimate of the current GNSS measurement.

15. 15. The controller of claim 14, wherein the weight determined for the current control step based on the likelihood of the current GNSS measurement according to the first PDF is a current weight, and the composite probabilistic filter stores historical weights of the first probabilistic filter determined for several previous control steps, and updates a weight for weighting the first probabilistic filter in the current control step based on an average of the current weight and the historical weight.

16. 12. The controller of claim 11, wherein the controller is operatively connected to a plurality of sensors using at least one of a wired link, a wireless link, or a combination thereof to collect a plurality of sensor measurements indicative of the state of the vehicle, the plurality of sensors including a GNSS receiver that collects the GNSS measurements, and the measurement model of the plurality of probabilistic filters fuses the measurements of the plurality of sensors to generate a corresponding estimate of the state of the vehicle.

17. 17. The controller of claim 16, wherein in addition to the GNSS receiver, the multiple sensors include one or a combination of a camera that generates color images, a depth sensor that generates depth images, and a road side unit (RSU) that generates fusion measurements of multiple remote sensors.

18. 18. The controller of claim 17, wherein the plurality of sensors includes a first sensor that collects a first measurement and a second sensor that collects a second measurement, each of the plurality of stochastic filters tracks the state of the vehicle based on the first measurement and the second measurement, the plurality of stochastic filters including a first hypothesis of a noise covariance of the first measurement and a first hypothesis of a noise covariance of the second measurement, and a second hypothesis of a noise covariance of the first measurement and a second hypothesis of a noise covariance of the second measurement, wherein the first hypothesis of a noise covariance of the first measurement differs from the second hypothesis of a noise covariance of the first measurement, the first hypothesis of a noise covariance of the second measurement differs from the second hypothesis of a noise covariance of the second measurement, or both.

19. 1. A method for controlling a movement of a device based on a state of the device tracked using measurements of the movement indicative of the state of the device, comprising: collecting a series of measurements indicative of the state of the device at different control steps; and iteratively executing a composite stochastic filter configured to track the states of the device at each of the different control steps using the sequence of measurements to generate a sequence of states of the device corresponding to the sequence of measurements, wherein to perform an iteration for a current control step using a current measurement, the composite stochastic filter: and configured to execute a plurality of parameterized stochastic filters on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting a current state of the device using a prediction model subject to process noise and updating the predicted current state based on the current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, and the composite stochastic filter further comprises: for each of the plurality of probabilistic filters, estimating a likelihood of a corresponding measurement noise to correlate the current measurement with the predicted current state; combining the estimates of the states of the device into a weighted combination with normalized weights derived from the likelihoods estimated for corresponding probabilistic filters; and outputting the state of the device tracked by the composite probabilistic filter for the current control step based on the weighted combination, the method further comprising: controlling the device using the sequence of states tracked by the composite probabilistic filter.

20. A non-transitory computer readable storage medium having embodied thereon a program executable by a processor for performing a method for controlling a movement of a device based on a state of the device tracked using measurements of the movement indicative of the state of the device, the method comprising: collecting a series of measurements indicative of the state of the device at different control steps; and iteratively executing a composite stochastic filter configured to track the states of the device at each of the different control steps using the sequence of measurements to generate a sequence of states of the device corresponding to the sequence of measurements, wherein to perform an iteration for a current control step using a current measurement, the composite stochastic filter: and configured to execute a plurality of parameterized stochastic filters on the state of the device to generate a plurality of estimates of the state of the device for the current control step, each of the plurality of stochastic filters iteratively predicting a current state of the device using a prediction model subject to process noise and updating the predicted current state based on the current measurements using a measurement model subject to measurement noise, different stochastic filters of the plurality of stochastic filters having different measurement noises that introduce variation into the plurality of estimates of the state of the device, and the composite stochastic filter further comprises: for each of the plurality of probabilistic filters, estimating a likelihood of a corresponding measurement noise to correlate the current measurement with the predicted current state; combining the estimates of the states of the device into a weighted combination with normalized weights derived from the likelihoods estimated for corresponding probabilistic filters; and outputting the state of the device tracked by the composite probabilistic filter for the current control step based on the weighted combination, the method further comprising:

23. A non-transitory computer-readable storage medium comprising: controlling the device using the sequence of states tracked by the composite probabilistic filter.

Images