System and method for controlling a device using a composite probability filter

Abstract

The disclosure discloses a system and method for controlling a device using a composite probabilistic filter. The method includes collecting a measurement sequence indicative of a state of the device at different control steps. Further, the method includes iteratively executing a composite probability filter configured to track the device state at each of the different control steps using the measurement sequence to produce a sequence of device states corresponding to the measurement sequence. Further, the method includes controlling the device using the tracked device state.

Description

Technical Field

[0001] The present disclosure relates generally to control and state tracking, and more particularly to systems and methods for controlling a device using a complex probability filter. Background Art

[0002] Various control methods use probabilistic filters to estimate the state of the controlled device. Probabilistic filters such as Kalman filters use a series of measurements observed over time that include statistical noise and other inaccuracies and produce estimates of unknown variables that are generally more accurate than those based on a single measurement alone.

[0003] The probability filter works by a two-stage process including a prediction phase and an update phase. For the prediction phase, the probability filter generates an estimate of the current state variable and its uncertainty. To this end, the probability filter uses a prediction model that is affected by process noise. An example of a prediction model is a model of the motion of a device under control. In addition, the result of the next measurement is observed (corrupted by some errors, including random noise), and the estimate of the current state variable is updated using a weighted average of the measured values, where more weight is given to the measured values ​​with greater certainty. To this end, the probability filter uses a measurement model that is affected by measurement noise. Both process noise and measurement noise can be represented by a probability density function (PDF) that shows the likelihood of possible changes in the estimated state and / or measured values.

[0004] Typically, process noise and 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 impractical to predetermine the measurement noise. For example, in a vehicle control application where a vehicle is controlled based on vehicle states tracked using measurements from a global navigation satellite system (GNSS), the GNSS measurements have different reliabilities at different times. For example, at different times, different constellations of satellites may be in the line of sight of the vehicle's GNSS receiver, resulting in variations in the measurement noise. Additionally or alternatively, satellite signals may be affected by multipath, adding significant noise to the GNSS measurements.

[0005] To this end, it is desirable to adjust the measurement noise based on the tracked vehicle state and control application. While measurement noise can be varied in probabilistic filters such as Kalman filters, the estimation of measurement noise in real-time applications is computationally challenging and may also cause the probabilistic filter to diverge. This challenge is due in part to the difficulty of analyzing the statistical properties of the measurements and the assumptions on which the probabilistic filter operates.

[0006] Therefore, there is a need for a probabilistic filter framework that can adapt to different measurement noises in a computationally efficient manner. Summary of the invention

[0007] Some embodiments are based on the recognition that the correctness of the measurement noise can be evaluated using internal variables and / or calculations of a probability filter similar to a Kalman filter. Specifically, the evaluation metric of the correctness of the measurement noise can be the likelihood that the measurement noise will associate a current measurement indicating a device state with a state predicted by a prediction model of the probability filter.

[0008] This correlation can be illustrated by the following example. A predicted state of a device (e.g., a predicted state of a vehicle) is transformed into a measurement domain (e.g., in a GNSS measurement space). For example, such a transformation can be performed using a model of the measurements. Next, the measurement noise is centered around the transformed state to produce a probability distribution of the expected measurement. In the case of a Gaussian probability distribution used by the probability filter, the mean of the Gaussian probability distribution of the expected measurement is determined by the state predicted by the Kalman filter, and the variance is one of the predetermined measurement noise. When a measurement is received for updating the predicted state, the measurement can be mapped to the probability distribution of the expected measurement to estimate the likelihood of the correlation sought.

[0009] In this way, the measurement noise is estimated using the internal variables and / or calculations of the probability filter without the need for additional statistical analysis of the measurements outside the performance of the probability filter. It should be noted that the above examples not only illustrate the principle of the correlation between the state estimate and the measured value, but can also be used to implement the estimation of such correlation. However, different embodiments may use different techniques to evaluate the correlation. For example, some embodiments use an evaluation of the Kalman gain determined by the Kalman filter to update the predicted state and the covariance of the predicted state, which is the mean square error (MSE) for an unbiased estimator. Such calculations are internal to the probability filter, i.e., they need to be calculated anyway to track the device state. Therefore, the calculation of these variables does not require additional resources.

[0010] However, some embodiments are based on the recognition that, while the use of internal variables and / or calculations of the probability filter can reduce the computational requirements of the evaluation of the correctness of a selected measurement noise, the evaluation itself may be corrupted by the internal performance of the probability filter. In other words, the correctness of the measurement noise is not necessarily a reflection of the true correctness of the measurement independent of the Kalman filter, but rather the correctness from the perspective of the probability filter itself. For this reason, using this evaluation to adjust for the measurement noise may be problematic, but using this evaluation to evaluate the performance of the probability filter under the selected measurement noise is not.

[0011] To address this problem, some embodiments use multiple probability filters with different measurement noises, and determine the device state as a weighted combination of the states estimated by the different filters, where the weight of each filter is derived from the corresponding evaluation of the likelihood of the measurement noise to associate the current measurement indicating the device state with the state predicted by the prediction model of the probability filter. 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 of tracking vehicle states based on satellite signals received from a global navigation satellite system (GNSS) are sensitive to satellite measurements with high qualitativeness, because otherwise the tracking methods cannot produce high-precision estimates. Other embodiments are based on the recognition that various sensors need to be combined to improve state tracking. The various sensors complement each other because such a combination is more likely to produce perceptible estimates under a wider variety of conditions. For example, one embodiment combines GNSS position measurements with camera relative position measurements and a road map to track vehicle states. Another embodiment combines a camera that measures the relative position of vehicles adjacent to the own vehicle and a lidar measurement that measures the relative position of vehicles adjacent to the own vehicle, because such measurements complement each other due to differences in the construction of the camera and the lidar, respectively.

[0013] Some embodiments utilize a probabilistic filter to fuse measurements and generate state estimates. The probabilistic filter requires a probabilistic description of the measurements, a measurement model. To this end, some embodiments recognize the need to determine measurement noise characteristics for the measurement model to be used in the probabilistic filter. Some embodiments are based on the understanding that in order to use the probabilistic measurement model in measurement updates, the accuracy of the sensor measurements needs to be determined simultaneously with tracking the vehicle state, because tracking is performed in real time and therefore the measurement model may not be set before executing the recursive probabilistic filter. Some embodiments utilize this understanding to adjust the measurement noise used in the probabilistic measurement model when the sensor measurements arrive at the composite probabilistic filter.

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

[0015] Some embodiments are based on the understanding that if a probability filter using a particular combination of measurement noise values ​​for the various sensors gives the best fit at the previous time step, it is likely to produce a good fit at the next time step as well. For example, if a GNSS position measurement was unreliable at the previous time step due to, for example, multipath in an urban environment, it is likely to continue to be affected by the multipath effect at the next time step as well. However, if the unreliability of the measurement is not due to the multipath effect but to some other unmodeled interference, then it still has a chance of producing a reliable measurement. Some embodiments use this understanding to integrate and weight multiple probability filters at each time step of the control, so that the probability filters are given different weights based on their weights at the previous time step.

[0016] Some embodiments are based on the understanding that the computational complexity of multiple probability filters grows unfavorably as the number of noise value hypotheses and the number of sensors increase. For example, one embodiment understands that using different possible noise values ​​can be interpreted as gridding the continuous space of possible noise values, making the number of possible combinations of noise values ​​exponentially complex, resulting in an exponential number of probability filters employed in the estimation.

[0017] Some embodiments recognize that in automotive applications, the computational power of automotive-grade electronic control units is limited, and using a large number of probability filters is computationally infeasible. One embodiment remedies this by selecting a subset of probability filters to be used in the estimation. For example, one embodiment selects probability filters corresponding to measurement noise values ​​close to those that represented the state well at previous time steps. Operating in this manner enables the subset of probability filters selected at each time step to follow the evolution of sensor quality.

[0018] Thus, one embodiment discloses a feedback controller for controlling the motion of the device based on the device state tracked using motion measurements indicative of the device state. The feedback controller comprises: at least one processor; and at least one memory storing instructions, which when executed by the at least one processor cause the feedback controller to: collect a sequence of measurements indicative of the device state at different control steps; iteratively execute a composite probability filter, the composite probability filter configured to track the device state at each of the different control steps using the sequence of measurements to produce a sequence of device states corresponding to the sequence of measurements. In order to perform iterations for a current control step using a current measurement, the composite probability filter is configured to: execute a plurality of probability filters parameterized on the device state to produce a plurality of estimates of the device state for the current control step, wherein each of the plurality of probability filters iteratively predicts the current state of the device using a prediction model subject to process noise, and updates the predicted current state based on the current measurement using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby causing changes in the plurality of estimates of the device state; estimate the likelihood of the corresponding measurement noise for each of the plurality of probability filters to associate the current measurement with the predicted current state; combine the plurality of estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and output the device state tracked by the composite probability filter for the current control step based on the weighted combination. The feedback controller is also configured to control the device using the state sequence tracked by the composite probability filter.

[0019] Thus, another embodiment discloses a method for controlling the motion of a device based on a state of the device tracked using measurements of the motion indicative of the state of the device. The method comprises the steps of collecting a sequence of measurements indicative of the state of the device at different control steps; and iteratively executing a composite probability filter configured to track the state of the device at each of the different control steps using the sequence of measurements to produce a sequence of device states corresponding to the sequence of measurements. In order to perform iterations for a current control step using a current measurement, the composite probability filter is configured to: execute a plurality of probability filters parameterized on the device state to produce a plurality of estimates of the device state for the current control step, wherein each of the plurality of probability filters iteratively predicts the current state of the device using a prediction model subject to process noise, and updates the predicted current state based on the current measurement using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby causing a change in the plurality of estimates of the device state; estimate the likelihood of the corresponding measurement noise for each of the plurality of probability filters to associate the current measurement with the predicted current state; combine the plurality of estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and output the device state tracked by the composite probability filter for the current control step based on the weighted combination. The method also includes controlling the device using the sequence of states tracked by the composite probability filter.

[0020] Thus, yet another embodiment discloses a non-transitory computer-readable storage medium on which a program executable by a processor is implemented to perform a method for controlling the motion of a device based on a state of the device tracked using measurements of the motion indicative of the state of the device. The method comprises the steps of collecting a sequence of measurements indicative of the state of the device at different control steps; iteratively executing a composite probability filter configured to track the state of the device at each of the different control steps using the sequence of measurements to produce a sequence of device states corresponding to the sequence of measurements. In order to perform iterations for a current control step using a current measurement, the composite probability filter is configured to: execute a plurality of probability filters parameterized on the device state to produce a plurality of estimates of the device state for the current control step, wherein each of the plurality of probability filters iteratively predicts the current state of the device using a prediction model subject to process noise, and updates the predicted current state based on the current measurement using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby causing a change in the plurality of estimates of the device state; estimate the likelihood of the corresponding measurement noise for each of the plurality of probability filters to associate the current measurement with the predicted current state; combine the plurality of estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and output the device state tracked by the composite probability filter for the current control step based on the weighted combination. The method also includes controlling the device using the sequence of states tracked by the composite probability filter.

[0021] Therefore, some embodiments disclose a controller for tracking a vehicle state based on measurements using a global navigation satellite system (GNSS) to control the vehicle movement. The controller includes: at least one processor; and at least one memory storing instructions, which, when executed by the at least one processor, cause the controller to: collect a GNSS measurement sequence indicating the state of the vehicle at different control steps; iteratively execute a composite probability filter, the composite probability filter being configured to track the vehicle state at the different control steps using the GNSS measurement sequence to generate the vehicle state sequence corresponding to the GNSS measurement sequence. In order to perform an iteration for a current control step using a current GNSS measurement, the composite probability filter is configured to: execute multiple probability filters having the same measurement model that associates the current GNSS measurement with a current estimate of the vehicle state to produce multiple estimates of the vehicle state for the current control step, wherein the multiple probability filters are affected by different measurement noise characteristics, thereby causing variations in the multiple estimates of the vehicle state; combine the multiple estimates of the vehicle state into a weighted combination, whose weights are derived from the likelihood of the current GNSS measurement according to different measurement noise centered on an estimate of the current GNSS measurement predicted by one or a combination of the multiple probability filters; and estimate the vehicle state tracked by the composite probability filter for the current control step based on the weighted combination. The controller is also configured to control the vehicle using the sequence of states estimated by the composite probability filter.

[0022] The presently disclosed embodiments will be further explained with reference to the accompanying drawings. The drawings shown are not necessarily drawn to scale, with emphasis instead generally being placed upon illustrating the principles of the presently disclosed embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] [ Figure 1A ]

[0024] Figure 1A A block diagram of a feedback controller for controlling motion of a device according to an embodiment of the present disclosure is shown.

[0025] [ Figure 1B ]

[0026] Figure 1B A schematic diagram for controlling the movement of a device according to some embodiments of the present disclosure is shown.

[0027] [ Figure 1C ]

[0028] Figure 1C An illustration of iterative state tracking by a probabilistic filter according to some embodiments of the present disclosure is shown.

[0029] [ Figure 1D ]

[0030] Figure 1D An illustration of an exemplary evaluation of correctness of measurement noise according to some embodiments of the present disclosure is shown.

[0031] [ Figure 1E ]

[0032] Figure 1E An illustration of an exemplary evaluation of correctness of measurement noise according to some embodiments of the present disclosure is shown.

[0033] [ Figure 1F ]

[0034] Figure 1F A schematic diagram for initializing multiple probability filters according to some embodiments of the present disclosure is illustrated.

[0035] [ Figure 1G ]

[0036] Figure 1G A schematic diagram for initializing multiple probability filters according to some other embodiments of the present disclosure is illustrated.

[0037] [ Figure 2A ]

[0038] Figure 2A A schematic diagram of a global navigation satellite system (GNSS) system is shown according to some embodiments of the present disclosure.

[0039] [ Figure 2B ]

[0040] Figure 2B An illustration of a scenario in which multipath interferes with a receiver's signal is shown according to some embodiments of the present disclosure.

[0041] [ Figure 3A ]

[0042] Figure 3A A block diagram of a method for estimating a vehicle state using GNSS measurements and controlling the vehicle based on the estimated state according to some embodiments of the present disclosure is shown.

[0043] [ Figure 3B ]

[0044] Figure 3B A block diagram illustrating the functions performed by a composite probability filter for iteration according to some embodiments of the present disclosure is shown.

[0045] [ Figure 3C ]

[0046] Figure 3C A block diagram of a weighted combination for producing a state estimate according to some embodiments of the present disclosure is shown.

[0047] [ Figure 3D ]

[0048] Figure 3D A block diagram for generating a weighted combination according to an alternative embodiment of the present disclosure is shown.

[0049] [ Figure 3E ]

[0050] Figure 3E A schematic diagram for determining a weighted combination using stored historical weights according to some embodiments of the present disclosure is shown.

[0051] [ Figure 4A ]

[0052] Figure 4A A schematic diagram for estimating a vehicle state using measurements from multiple sensors according to some embodiments of the present disclosure is shown.

[0053] [ Figure 4B ]

[0054] Figure 4B An illustration of different sensor measurements collected according to various embodiments of the present disclosure is shown.

[0055] [ Figure 4C ]

[0056] Figure 4C An illustration of a case in which there are estimated measurements to be compared with collected measurements by sensors according to various embodiments of the present disclosure is shown.

[0057] [ Figure 4D ]

[0058] Figure 4D An illustration of how to assign different weights to different estimation values ​​of a probability filter and form a composite distribution according to various embodiments of the present disclosure is shown.

[0059] [ Figure 4E ]

[0060] Figure 4E An illustration is shown of how different sensors according to various embodiments of the present disclosure obtain different weights and they are defined on different measurement spaces.

[0061] [ Figure 4F ]

[0062] Figure 4FAn illustration of how to convert measurement values ​​defined on different measurement spaces into the same measurement space to be compared with each other according to various embodiments of the present disclosure is shown.

[0063] [ Figure 5 ]

[0064] Figure 5 A block diagram for controlling a vehicle based on estimated states according to various embodiments of the present disclosure is shown.

[0065] [ Fig. 6A ]

[0066] Fig. 6A An illustration of a motion model employed in a probabilistic filter according to some embodiments of the present disclosure is shown.

[0067] [ Figure 6B ]

[0068] Figure 6B Illustrated is how camera measurements according to some embodiments of the present disclosure produce distances to road lane markings and an approximation of the road ahead.

[0069] [ Figure 7 ]

[0070] Figure 7 A block diagram of a controller for controlling vehicle motion based on vehicle states tracked using GNSS measurements, according to some embodiments of the present disclosure is shown.

[0071] [ Fig. 8A ]

[0072] Fig. 8A A schematic diagram of a vehicle including a controller for controlling movement of the vehicle according to some embodiments of the present disclosure is shown.

[0073] [ Figure 8B ]

[0074] Figure 8B A schematic diagram showing the interaction between a controller and a controller of a vehicle according to some embodiments of the present disclosure. DETAILED DESCRIPTION

[0075] In the following description, for the purpose of explanation, many 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 can be practiced without these specific details. In other cases, devices and methods are shown in block diagram form only to avoid obscuring the present disclosure.

[0076] As used in this specification and claims, the terms "for example," "for example," and "such as," and the verbs "comprise," "have," "include," and their other verb forms, when used in conjunction with a list of one or more components or other items, are each interpreted as open-ended, meaning that the list is not to be viewed as excluding other, additional components or items. The term "based on" means based, at least in part, on. Furthermore, it is to be understood that the phraseology and terminology employed herein are for descriptive purposes and should not be considered limiting. Any directions used in this specification are for convenience only and have no legal or limiting effect.

[0077] Figure 1A A block diagram of a feedback controller 100 for controlling the motion of a device according to an embodiment of the present disclosure is shown. The feedback controller 100 includes a processor 102 and a memory 104. The processor 102 can be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 104 can include a random access memory (RAM), a read-only memory (ROM), flash memory, or any other suitable memory system. Additionally, in some embodiments, the memory 104 can be implemented using a hard drive, an optical drive, a thumb drive, a drive array, or any combination thereof. In an embodiment, the memory 104 stores a compound probability filter 104a. The compound probability filter 104a is Figure 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, etc. The feedback controller 100 is configured to perform various functions to control the movement of the device, such as Figure 1B As explained in .

[0078] Figure 1B A schematic diagram for controlling the motion of a device according to some embodiments of the present disclosure is shown. The feedback controller 100 controls the device based on the device state tracked using measurements indicating the device state. For example, if the device is a vehicle, the device state may include one or a combination of the position of the vehicle, the heading angle of the vehicle, and the speed of the vehicle. In an embodiment, the measurement indicating the device state may correspond to a sensor measurement. In other words, the measurement indicating the device state may be obtained from one or more sensors. For example, a measurement indicating the vehicle speed may be obtained from a speed sensor of the vehicle.

[0079] The feedback controller 100 first collects 110 a measurement sequence indicating the state of the vehicle at different control steps. Furthermore, the feedback controller 100 iteratively executes 120a a composite probability filter 104a configured to track the state of the device at each different control step using the measurement sequence to produce a device state sequence 125a corresponding to the measurement sequence. Furthermore, the feedback controller 100 controls the device 130a using the state sequence 125a tracked by the composite probability filter 104a.

[0080] In order to perform iterations on the current control step using the current measurement, the composite probability filter 104a is configured to execute multiple probability filters 121a, 122a, and 123a that parameterize the device state to generate multiple estimates of the device state for the current control step. For example, filter 121a generates state 131a, filter 122a generates state 132a, and filter 123a generates state 133a.

[0081] Each of the plurality of probability filters 121 a to 123 a is a probability filter that iteratively predicts a current state of a device using a prediction model affected by process noise, and updates the predicted current state based on current measurement using a measurement model affected by measurement noise.

[0082] Figure 1C An illustration of iterative state tracking of a probability filter of some embodiments is shown. Starting from a state estimate 110b determined for a previous iteration, the probability 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 can be defined by parameters of a probability distribution. Upon receiving a current measurement 125b, the probability filter updates or corrects 135b the predicted current state 120b based on a measurement model that connects the measurement value with the predicted current state 120b and subject to measurement noise 130b to estimate a state 140b for the current iteration.

[0083] Due to the probabilistic nature of tracking, the selection of measurement noise 130b affects update 135b. To this end, the selection of measurement noise 130b affects the estimate of state 140b, and thus the correctness of the selection of measurement noise 130b can be beneficial to the operation of the probabilistic filter.

[0084] Some embodiments are based on the recognition that the correctness of the measurement noise 130b can be evaluated using internal variables and / or calculations of a probabilistic filter similar to a Kalman filter. Specifically, the evaluation metric of the correctness of the measurement noise 130b can be the likelihood that the measurement noise 130b will indicate that the current measurement of the device state is related to the state predicted by the prediction model.

[0085] Figure 1D and Figure 1E 1 shows an illustration of an exemplary evaluation of the correctness of measurement noise according to some embodiments of the present disclosure. The predicted current state 105d of the controlled device (e.g., the predicted state of the vehicle) is transformed into the domain of measurement (i.e., measurement space) 110d to obtain a transformed state 115d. An example of a transformed state is Figure 1E 140c shown in FIG. 140c. For example, such a transformation may be performed using a model of the measurements. Next, different measured noise characteristics (eg, noise covariance) are centered 120d on the transformed state 140c to estimate Figure 1E 110c and 120c as shown in . In an embodiment, each of the multiple probability filters is a Kalman filter having process noise and measurement noise defined by corresponding Gaussian probability distributions, so 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 produce the Gaussian probability distribution of the measurement noise.

[0086] Furthermore, when a current measurement 125d is received for updating the predicted current state 105d, the measurement value may be mapped 130c onto a probability distribution to estimate the likelihood of the sought correlation. Figure 1E As shown, the likelihood of measurement noise 120c is greater than the likelihood of measurement noise 110c at mapped measurement value 130c.

[0087] To this end, the composite probability filter 104a executes multiple probability filters 121a, 122a, and 123a with different measurement noises, resulting in a change in multiple estimates of the device state (e.g., states 131a, 132a, and 133a). Next, the composite probability filter 104a estimates the likelihood of the corresponding measurement noise for each of the multiple probability filters to relate the current measurement to the predicted current state, and combines 150 the multiple estimates of the device state into a weighted combination with normalized weights 141a, 142a, and 143a derived from the likelihoods estimated by the corresponding probability filters. Therefore, the device state tracked by the composite probability filter 104a for the current control step is based on the weighted combination 150a.

[0088] In this way, the measurement noise is evaluated using the internal variables and / or calculations of the probability filter without the need for additional statistical analysis of the measurements outside the performance of the probability filter. It should be noted that the above examples not only illustrate the principle of the correlation between state estimation and measurement, but can also be used to implement the estimation of correlation. However, different embodiments can use different techniques to evaluate the correlation. 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 the unbiased estimator. For example, an embodiment determines the Kalman gain of each Kalman filter in the composite probability filter, uses the Kalman gain to determine the covariance of the updated predicted current state and the updated state estimate, and determines the likelihood of the measurement noise based on the Kalman gain based on the updated state and covariance. Such calculations are inside the probability filter (i.e., calculations are required to track the device state anyway). Therefore, the calculation of these variables does not require additional resources.

[0089] However, some embodiments are based on the recognition that, while the use of internal variables and / or calculations of the probability filter can reduce the computational requirements of the evaluation of the correctness of the measurement noise, the evaluation itself is corrupted by the internal performance of the probability filter. In other words, the correctness of the measurement noise is not necessarily a reflection of the true correctness of the measurement independent of the probability filter, but rather a correctness from the perspective of the probability filter itself. For this reason, it may be problematic to use this evaluation to adapt the measurement noise, but rather to use this evaluation to evaluate the performance of the probability filter with the selected measurement noise.

[0090] To address this problem, some embodiments use multiple probability filters with different measurement noises, and determine the device state as a weighted combination of the states estimated by the multiple probability filters 121a to 123a, where the weight of each filter is derived from a corresponding evaluation of the likelihood of the measurement noise to relate the current measurement indicating the device state to the state predicted by the filter's prediction model. In this way, different measurement noises can be taken into account without the need to analyze the statistical properties of the measurements.

[0091] In some embodiments, at each time step, each of the plurality of probability filters 121a to 123a is initialized based on the weighted combination 125a. Figure 1F In one embodiment, the transformation state 140c is used to initialize each of the plurality of probability filters 121a to 123a and to update the predicted state of the probability filter differently by having different measurement noise covariances inside each probability filter. To this end, the estimate of the predicted current state transformed into the measurement space (i.e., the transformation state 140c) is common to the plurality of probability filters 121a to 123a.

[0092] In some other embodiments, multiple probability filters 121a to 123a are run in parallel using internal state estimates, wherein the weighted combination 125a is used only as the output of the composite probability filter 104a. Figure 1G , the first transformed state 151a of probability filter 121a is obtained by transforming the predicted current state of probability filter 121a into the measurement space. The second transformed state 152a of probability filter 122a is obtained by transforming the predicted current state of probability filter 122a into the measurement space. The third transformed state 153a of probability filter 123a is obtained by transforming the predicted current state of probability filter 123c into the measurement space. In the next iteration, multiple probability filters 121a to 123a use the first transformed state 151a, the second transformed state 152a and the third transformed state 153a to update their internal state estimates, respectively. In other words, for different probability filters, the estimates of the predicted current state transformed into the measurement space are different.

[0093] Some embodiments are based on the recognition that when each probability filter is initialized with the same transition state estimate in each control time step, a lack of diversity in the state estimates may result, since each probability filter has only one time step to diversify. Some other embodiments are based on the understanding that in order to make the multiple probability filters 121a to 123a operate independently of each other, exhaustion may result, meaning that after a few time steps, only one of the multiple probability filters 121a to 123a may have a non-zero weight.

[0094] To address the above issues, some embodiments initialize the state estimates together at each control time step by mixing the estimates, where the mixture of each probability filter is determined by the weights 141a, 142a and 143a and the relationship with other weights.

[0095] Some embodiments are based on the following recognition: Figure 1B The composite probability filter framework described in can be applied to global navigation satellite system (GNSS) measurements. For example, the composite probability filter 104a can be configured to track vehicle states at different control steps using GNSS measurements to generate a sequence of vehicle states corresponding to the GNSS measurements.

[0096] GNSS is a satellite system that can be used to determine the geographic location of a mobile receiver relative to the Earth. GNSS may include GPS, Galileo, Glonass, QZSS, and BeiDou. Figure 2A An example of GNSS is explained in detail in . Figure 2AA schematic diagram of a GNSS according to some embodiments is shown. For example, an Nth satellite 202 transmits 220 and 221 code and carrier phase measurements 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.

[0097] In various embodiments, GNSS receivers 230 and 231 may be of different types. Figure 2A In an exemplary embodiment, receiver 231 is a base station receiver whose location is known. For example, receiver 231 can be a receiver mounted on the ground. In contrast, receiver 230 is a mobile receiver configured to move. For example, receiver 230 is installed in a vehicle. In some embodiments, second receiver 231 is optional and can be used to remove or at least reduce uncertainties and errors due to various sources such as atmospheric effects and errors in the internal clocks of the receiver and satellites. In some embodiments, there are multiple GNSS receivers that receive code and carrier phase signals.

[0098] An object of some embodiments is to disclose a system and method for improving satellite-based vehicle state tracking, wherein the vehicle is equipped with a GNSS receiver. Another object is to provide such a system and method that uses asynchronous collaboration of information received from satellite signals. Yet another object of some embodiments is to provide such a system and method that is probabilistic, i.e., that takes into account probabilistic interference and error sources. Other embodiments are intended to track the vehicle state using different information from different sources and not rely solely on satellite signals. For example, in some embodiments, the vehicle state is tracked using a first moment and a second moment of a probability distribution of a GNSS signal received from a satellite using a GNSS receiver and a vehicle state received from a remote server using a radio frequency (RF) receiver.

[0099] In some scenarios (e.g., deep urban canyons), the satellite signals are subject to multiple distortions, resulting in information content in the code and carrier phase signals that makes it difficult to perform high-precision state estimation. Figure 2B An illustration of a scenario in which multipath interferes with the signals of receiver 201b is shown. Receiver 201b receives various signals 209b and 219b from satellites 210b and 220b. There are other satellites 230b and 240b transmitting signals 228b, 229b, 238b, 239b, but these signals are not transmitted directly to the receiver due to obstacles 270b (e.g., buildings in an urban area).

[0100] Previously, signal 238b sent from satellite 240b was not available, but suddenly satellite signal 239b arrives at the receiver after multipath 202b. This situation can severely impair the performance of the probability filter in tracking the vehicle state, because the probability filter locks to the wrong ambiguity estimate, resulting in a large estimation error.

[0101] In addition, some embodiments are based on the recognition that in order to perform certain advanced driver assistance system (ADAS) tasks such as adaptive cruise control or short-term lane changes, it is sufficient to know only the relative position and speed with respect to other vehicles, which can be measured by, for example, ultrasound, radar or cameras. However, in many applications involving more advanced ADAS and vehicles with autonomous driving (AD) capabilities, it is not enough to know only the relative position with respect to surrounding vehicles, which can be measured directly, but also state information with respect to objects that cannot even be seen from the AD-capable vehicle at a given time step must be known. For example, in route planning or multi-agent motion planning and coordination tasks, the control problem to be solved is how to optimally coordinate vehicles towards different goals, taking into account different road surface qualities, and each passenger of a given vehicle has different priorities, such as related to driving comfort and other performance indicators. Determining which timed path a specific vehicle should take depends on the position of the own vehicle, the timed paths of other vehicles, and the specific environmental settings of the road of interest.

[0102] Some embodiments are based on the recognition that, while cameras can be used to detect relative motion with the road and surrounding objects in the vicinity of an AD-capable vehicle, cameras cannot provide a global position of the vehicle. Additionally, while range sensors such as radar, lidar, and ultrasound can detect relative motion similar to cameras, they cannot be used as the sole sensor to globally locate a vehicle.

[0103] In addition, some embodiments are based on the recognition that GNSS is susceptible to various interferences and obstructions (e.g., from tall buildings in urban canyons). Therefore, some embodiments supplement GNSS with additional sensing. For example, GNSS combined with additional sensing such as cameras and lidar can achieve global positioning and positioning relative to maps and other objects, because GNSS provides global positioning and the additional sensing provides relative positioning. Therefore, in several positioning estimation methods, multiple sensors are used together to achieve vehicle positioning, with the goal of improving performance compared to what a single sensor can provide.

[0104] To this end, various methods fuse GNSS measurements and measurements of multiple sensors of different types indicating device states to estimate the vehicle state. The multiple sensors may include one or a combination of cameras, radars, lidars, etc. Additionally or alternatively, the multiple sensors may include one or a combination of cameras that produce color images, depth sensors that produce depth images, and roadside units (RSUs) that produce fused measurements of multiple remote sensors.

[0105] Using measurements from multiple sensors is beneficial. For example, different sensors can complement each other, e.g., GNSS is dependent on whether the environment is urban or rural, but not weather dependent, while cameras are independent of whether the environment is rural or urban, but are sensitive to weather conditions and road quality to detect lane markings.

[0106] Various estimation methods that fuse measurements from multiple sensors assume that the measurement noise of such sensors is determined in advance. However, in practice, measurement noise varies over time because it varies with driving conditions, the environment, and the filtering and computer vision algorithms that generate the sensor measurements. For example, a camera is used together with a computer vision algorithm to generate lane marking measurements for lanes adjacent to the lane in which the vehicle is traveling. Depending on weather conditions, road quality, and the specific computer vision algorithm used, 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 such detection results, the characteristics of the measurement noise will vary accordingly.

[0107] Therefore, it is necessary to track the vehicle state by fusing the measurements of multiple sensors while adapting to the changing measurement noise. According to some embodiments, the feedback controller 100 can be operably connected to multiple sensors using wired, wireless, or both communication links, and thus the feedback controller 100 can collect measurements of multiple sensors indicating the state of the device. In addition, the measurement model of the multiple probability filters 121a to 123a of the composite probability filter 104a fuses the measurements of the multiple sensors to generate a corresponding estimate of the vehicle state.

[0108] In the following part of the disclosure, a method for estimating a vehicle state using GNSS measurements and controlling the vehicle based on the estimated state is described.Next, a method for estimating a vehicle state using measurements of multiple sensors is described.

[0109] Figure 3AA block diagram of a method 300 for estimating a vehicle state using GNSS measurements and controlling a vehicle based on the estimated state according to some embodiments of the present disclosure is shown. As used herein, a vehicle can be any type of mobile entity 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 probability filter 104a to parameterize the vehicle state. At box 310, the method 300 includes collecting a GNSS measurement sequence indicating a vehicle state at different control steps. At box 320, the method 300 includes iteratively executing a composite probability filter 104a, the composite probability filter 104a being configured to track the vehicle state using the GNSS measurement sequence with different control step sizes to generate a vehicle state sequence corresponding to the GNSS measurement sequence. At box 330, the method 300 includes controlling the vehicle based on the tracked vehicle state.

[0110] To perform the current iteration of the control step using the current GNSS measurements, the composite probability filter 104a performs Figure 3B The functionality described in .

[0111] Figure 3B A block diagram of the functions performed by the composite probability filter 104a in an iteration according to some embodiments of the present disclosure is shown. At box 340, the composite probability filter 104a executes multiple probability filters with the same measurement model that associates the current GNSS measurement with the current estimate of the vehicle state to produce multiple estimates of the vehicle state for the current control step. The multiple probability filters are affected by different measurement noises that cause changes between the multiple estimates of the vehicle state. In some implementations, the multiple probability filters include a first probability filter and a second probability filter, wherein the first probability filter is configured to produce a first estimate of the vehicle state affected by a first measurement noise defined by a first probability density function (PDF), and the second probability filter is configured to produce a second estimate of the vehicle state affected by a second measurement noise defined by a second PDF different from the first PDF.

[0112] At block 350, the compound probability filter 104a combines the multiple estimates of the vehicle state into a weighted combination whose weights are derived from the likelihood of the current GNSS measurement based on different measurement noises centered around the estimate of the current GNSS measurement predicted by one or more of the multiple probability filters. At block 360, the compound probability filter 104a estimates the vehicle state tracked by the compound probability filter 104a for the current control step based on the weighted combination.

[0113] Figure 3CA block diagram for generating a weighted combination 350 according to some embodiments of the present disclosure is shown. At block 370a, the compound probability filter 104a determines an estimate of the current GNSS measurement by transforming a prediction of the current state of the vehicle from the state space to the measurement space. At block 370b, the compound probability filter 104a centers the first PDF and the second PDF on the estimate of the current GNSS measurement.

[0114] At block 370c, the compound probability filter 104a determines a first likelihood of the current GNSS measurement based on a first PDF centered on the estimate of the current GNSS measurement. At block 370d, the compound probability filter 104a determines a second likelihood of the current GNSS measurement based on a second PDF centered on the estimate of the current GNSS measurement. At block 370e, the compound probability 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.

[0115] At block 370f, the compound probability filter 104a determines a weighted combination based on the first estimate of the vehicle state weighted with the first weight and the second estimate of the vehicle state weighted with the second weight.

[0116] Figure 3D A block diagram for generating a weighted combination 350 according to an alternative embodiment of the present disclosure is shown. At block 380a, the composite probability filter 104a executes a first probability filter to predict a first current state of the vehicle based on an internal state of the first probability filter and updates the first current state of the vehicle. The first current state of the vehicle is updated by processing the current GNSS measurement according to the gain of the first probability filter using the measurement model to generate a first estimate of the vehicle state.

[0117] At block 380b, the composite probability filter 104a executes a second probability filter to predict a second current state of the vehicle based on the internal state of the second probability filter and updates the second current state of the vehicle. The second current state of the vehicle is updated by processing the current GNSS measurement according to the gain of the second probability filter using the measurement model to generate a second estimate of the vehicle state.

[0118] At block 380c, the composite probability filter 104a updates the internal state of the first probability filter and the internal state of the second probability filter based on a combination of the first estimate and the second estimate of the vehicle state produced by the first probability filter and the second probability filter.

[0119] In some embodiments, the composite probability filter stores historical weights of the first probability filter and the second probability filter determined for a plurality of previous control steps. In addition, the composite probability filter 104a updates the weight for weighting the first probability filter at the current control step based on the average value of the current weight (e.g., the first weight) and the historical weight of the first probability filter. Similarly, the composite probability filter 104a updates the weight for weighting the second probability filter at the current control step based on the average value of the current weight (e.g., the second weight) and the historical weight of the second probability filter.

[0120] Figure 3E A schematic diagram for determining a weighted combination using stored historical weights according to some embodiments of the present disclosure is shown. At box 390a, a state estimate determined by each probability filter is received. Next, at box 390b, the stored historical weights 390e and the assumption of the noise covariance are compared using the collected measurements 390d to determine the weights 390f. At box 390c, based on the determined weights 390f, the state estimates are combined into a weighted combination. The historical weights may be stored in the memory 104 of the feedback controller 100. In an alternative embodiment, the historical weights may be stored in an external memory and the feedback controller 100 retrieves the historical weights from the external memory.

[0121] Comparing state estimates and determining weights 390f can be performed in several ways. For example, in one embodiment, when the state estimate is inserted into a measurement model with an associated noise covariance assumption, the weight of each probability filter is determined by a combination of stored historical weights and a weighted difference between the collected measurements and the received state estimate. In another embodiment, when the state estimate is inserted into a measurement model with an associated noise covariance assumption, the weight of each probability filter is uniquely determined by the weighted difference between the collected measurements and the received state estimate. In yet another embodiment, when the state estimate is inserted into a measurement model with an associated noise covariance assumption and weights determined using a fixed number of previous time steps, the weights are determined based on the average of the current weighted differences between the collected measurements and the received state estimate. For example, one embodiment uses a sliding window of weights determined using previous time steps to determine the weights as a moving average. Operating in this manner provides a means for controlling the change of weights over consecutive time steps to ensure smooth estimation performance.

[0122] Figure 4AA schematic diagram for estimating a vehicle state using measurements from multiple sensors according to some embodiments of the present disclosure is shown. In some embodiments, a composite probability filter 400a parameterized on the vehicle state is used to estimate the vehicle state. The composite probability 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.

[0123] The composite probability filter 400a includes a plurality of probability filters. For example, the composite probability filter 400a includes a first probability filter 420a and a second probability filter 410a. The first probability filter 420a uses a first assumption of the noise covariance 407b of the first measurement value and a first assumption of the noise covariance 207b of the second measurement value to determine a first state estimate 425a. Similarly, the second probability filter 410a uses a second assumption of the noise covariance 408a of the first measurement value and a second assumption of the noise covariance 408a of the second measurement value to determine a second state estimate 415a. In an embodiment, the first assumption of the noise covariance of the first measurement value is different from the second assumption of the noise covariance of the first measurement value, and the first assumption of the noise covariance of the second measurement value is different from the second assumption of the noise covariance of the second measurement value, or both are different. In this case, the first noise distribution and the second noise distribution of any measurement value are different distributions in terms of their parameters, but represent the same entity. For example, the two distributions represent the measurement results of the camera, but have different noise values.

[0124] Next, using the first state estimate 425a of the first probability filter 420a and the second state estimate 415a from the second probability filter 410a, the vehicle state 435a is determined as a weighted combination 430a, wherein the weight 430a of the combination is determined online, offline, or a combination of both. In one embodiment, the vehicle state 435a is a weighted average of the first probability filter 420a and the second probability filter 410a. In another embodiment, the vehicle state 435a is the output of the probability filter with the highest weight.

[0125] In one embodiment, each probability filter is a linear regression Kalman filter that estimates the mean and covariance of the vehicle state. In another embodiment, the probability filter is a particle filter that outputs a sampled representation of the posterior distribution. Some embodiments employ probability filters including various variants of Kalman filters (KFs), such as extended KFs (EKFs), linear regression KFs (LRKFs) such as unscented KFs (UKFs).

[0126] Some embodiments are based on the understanding that if a probability filter using a particular combination of measurement noise values ​​for the various sensors gives the best fit at the previous time step, it is likely to produce a good fit at the next time step as well. For example, if a GNSS measurement was unreliable at the previous time step due to, for example, multipath in an urban environment, it is likely to continue to be affected by the multipath effect at the next time step as well. However, if the unreliability of the measurement is not due to the multipath effect but to some other unmodeled interference, it is also likely to produce a reliable measurement. Some embodiments use this understanding to integrate and weight multiple probability filters at each time step of the control, so that the probability filters are given different weights based on their weights at the previous time step.

[0127] Some embodiments are based on the understanding that the computational complexity of multiple probability filters grows unfavorably as the number of noise value hypotheses and the number of sensors increase. For example, one embodiment understands that using different possible noise values ​​can be interpreted as gridding the continuous space of possible noise values, so that the number of possible combinations of noise values ​​grows exponentially, resulting in an exponential increase in the number of probability filters employed in the estimation.

[0128] Some embodiments recognize that in automotive applications, the computational power of automotive-grade electronic control units is limited, and using a large number of probability filters is computationally infeasible. One embodiment remedies this by selecting a subset of probability filters to be used in the estimation. For example, one embodiment selects probability filters corresponding to measurement noise values ​​close to those that represented the state well at previous time steps. Operating in this manner enables the subset of probability filters selected at each time step to follow the evolution of sensor quality.

[0129] Figure 4B An illustration of different sensor measurements 409a collected according to various embodiments of the present disclosure is shown. For example, the first measurement of the collected measurements 409a is a camera measurement 410d that measures lane markings of a road together with a computer vision algorithm, and the second measurement is a lidar 420d that measures the distance to the environment (e.g., road barriers and other vehicles). Additionally or alternatively, the measurements 409a are a combination of measurements that provide measurements of the vehicle in its environment 430d, 440d, for example, weights are optimized based on the fit of the corresponding model to the measurements, and the environment is pre-mapped offline.

[0130] Some embodiments use a first assumption of the noise covariance of the measurements and a second assumption of the noise covariance of the measurements, wherein the first assumption is used by one of the probability filters within the composite filter and the second assumption is used by another of the probability filters.

[0131] Figure 4C An illustration of the situation where there is an estimated measurement 415e to be compared with a collected measurement 425e by a sensor is shown. An assumption of distribution 430e is used in one probability filter, where another probability filter uses an assumption of distribution 410e. Thus, for Figure 4C In the scenario in FIG. 4 , the assumed probability filter using distribution 430e accurately describes the collected measurements because at the value of collected measurement 425e, distribution 410e has a higher probability than distribution 430e.

[0132] Figure 4D An illustration of how to assign different weights to different estimated values ​​of a probability filter and form a composite distribution according to various embodiments of the present disclosure is shown. A first probability filter 420a estimates a state distribution 430f and a second probability filter 410a estimates a state distribution 410f. Weights 435f and 415f for each probability filter are determined. Composite distribution 420f is determined as a weighted combination of state distributions 430f and 410f.

[0133] Figure 4E An illustration of how different sensors according to various embodiments of the present disclosure obtain different weights and are defined on different measurement spaces is shown. For example, some embodiments use a first assumption for a distribution 414g of a first measurement defined on a measurement space 417g, a second assumption for a distribution 415g, and a first assumption for a distribution 424g of a second measurement defined on a second measurement space 427g, a second assumption for a distribution 425g. In several embodiments, distributions 414g and 415g of the first measurement space 417g belong to the same measurement space 417g, while distributions 424g and 425g of the second measurement space 427g belong to the same measurement space 427g. Subsequently, 414g and 415g can be combined in different ways with 425g and 426g. 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. Since distributions 414g and 415g are defined on the same measurement space, they can be directly used for each filter in subsequent measurement updates.

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

[0135] Figure 4FAn illustration of 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 is shown. Some embodiments transform a distribution 415h of a first measurement using a transformation 416h, and transform a distribution 425h of a second measurement using a transformation 426h to produce a distribution 435h of the first measurement, a distribution 445h of the second measurement, wherein the distributions are defined on the same measurement space 446h, and can therefore be compared and weighted with each other.

[0136] Some embodiments employ different sensors by using a joint measurement model. Figure 4F , some embodiments model the joint measurement model as a joint Gaussian distribution with mean and covariance according to the following formula:

[0137]

[0138] Among them, the covariance elements R1, R 12 and R2 is the measurement noise covariance unique to each probability filter.

[0139] In some embodiments, the output estimate 435a of the vehicle state is used to control the vehicle. In one embodiment, a model predictive controller (MPC) is used to control the vehicle based on the output estimate 435a of the vehicle state.

[0140] Figure 5 A block diagram for controlling a vehicle based on an estimated state 521 according to various embodiments of the present disclosure is shown. Specifically, MPC computes a control solution (e.g., a solution vector 555) by solving an inequality-constrained optimization problem 550 in the form of an optimal control structured procedure at each control time step. The solution vector 555 includes a future optimal control input sequence within the system prediction time horizon. The control cost function 540, the optimal control data 545 of the equality and inequality constraints 530 in the optimization problem 550 depend on the dynamic model 525, the vehicle constraints 520, the current estimated state 521 of the vehicle, and the control command including the reference 505 and the confidence 506.

[0141] In some embodiments, the solution to the inequality constrained optimization problem 550 uses states and control values ​​510 over a prediction time horizon from a previous control time step that can be read from the memory of the MPC. This concept is known as warm-up or warm-start of the optimization algorithm, which greatly reduces the computational effort required by the MPC. In a similar manner, the corresponding solution vector 555 can be used to update and store a sequence 560 of optimal or suboptimal states and control values ​​for the next control time step.

[0142] In some embodiments of the present disclosure, the MPC adjusts one or more terms in the control cost function 540 according to the reference 505 and the corresponding confidence 506 calculated by the composite probability filter 104 a .

[0143] In an embodiment, the prediction model used by the probability filter to predict the current state of the vehicle (eg, current state 120b) may be a motion model of the vehicle state transitions affected by process noise. Fig. 6A An example of a motion model is described in .

[0144] Fig. 6A An illustration of a motion model employed in a probabilistic filter according to some embodiments of the present disclosure is shown. The motion model is a single track model (i.e., a bicycle model), which is shown to be effective in normal driving scenarios. In some embodiments, the state vector is x=(p X , p Y , v X , v Y , ψ) (i.e., the position, velocity, and heading rate of the vehicle). In some other embodiments, the time evolution of the vehicle is modeled by moment balance as

[0145] Where m is mass and I is inertia, and F with a subscript is the force acting on the vehicle. The resulting model is nonlinear, and in some embodiments, the model is concisely written as in That is, zero-mean Gaussian process noise.

[0146] In other embodiments, the single track model is kinematic (ie, it ignores moment balance). For example, in one embodiment, the kinematic single track model is:

[0147]

[0148] in, It's a status.

[0149] In addition, 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 measurements (such as GNSS measurements). Some embodiments model GNSS measurements as position measurements preprocessed by a probabilistic filter (e.g., a mixed integer Kalman filter or a particle filter). Other embodiments model camera measurements as distances to lane markings of the road and an approximation of the road ahead (e.g., as Figure 6B), where the camera measures the distance to lane markings in either lane 610b or measures lane markings as a polynomial representation of the road ahead 620b. Some other embodiments utilize a prior map of the road to correlate the relative position measurements given by the camera with the global position measurements given by the GNSS. Still other embodiments utilize an inertial measurement unit including an accelerometer and a gyroscope to further enhance the vehicle state estimate. Some embodiments are based on y k =h(x k ,u k )+e k , where e k The measurement model is modeled as a Gaussian distribution with a zero mean Gaussian of a block diagonal covariance matrix distribution, where the elements of the covariance matrix are assumed for each of the multiple probability filters 121a to 123a of the composite probability filter 104a. Some embodiments are based on the recognition that the first two moments (e.g., mean and variance) are usually good at describing a distribution even if the distribution itself is not Gaussian. In some embodiments, this identification is used to determine the covariance of the vehicle state.

[0150] Some embodiments use the measurement model to update the parameters of the probability distribution (i.e., mean and covariance). For example, one embodiment uses a Kalman filter (KF) to update the parameters of the probability distribution. Another embodiment recognizes that the motion model and the measurement model are nonlinear for some states of the vehicle. To this end, some embodiments use a nonlinear KF such as a linear regression KF to update the parameters of the probability distribution. The linear regression KF is a filter that determines the first moment and the second moment by including the following moment integrals:

[0151] and At a set of integration points Evaluate at .

[0152] Some embodiments approximate the posterior density of each probability filter as a Gaussian approximation Given the assumed Gaussian filtered posterior at time step k, the distribution at time step k+1 is then approximated as Gaussian, By directly computing the associated moment integral,

[0153]

[0154] Some embodiments assume that process noise enters in an additive manner, which significantly simplifies the calculations.

[0155] Some other embodiments utilize the LRKF to determine the associated moment integral, where a set of integration points is used. For example, some embodiments use integration points according to an unscented transformation, and other embodiments utilize the spherical cube rule. However, any set of integration schemes may be used.

[0156] In some embodiments, the composite probability filter 104a is implemented according to an interactive multiple model (IMM) framework. Using IMM, a method for combining a system of first and second measurements using different combinations of assumptions about the measurement noise covariance is implemented. At each time step k, the IMM assigns a weight qk to each model to reflect the probability of explaining the measurement value.

[0157] Some embodiments define a prediction model according to:

[0158]

[0159] Among them, the model parameters According to the transition probability Π∈[0,1] m×m Finite state Markov chain evolution of .

[0160] In some embodiments, for each possible Different assumptions about the measurement noise are created according to the following equations:

[0161] At each time step of estimation, IMM uses a transformation matrix to mix different m models,

[0162]

[0163] After such mixing, some embodiments run m probability filters to determine the vehicle state estimate. Subsequently, the posterior distribution is determined according to the following formula:

[0164]

[0165] That is, a weighted combination of each probability filter, where each is weighted according to how well it fits the measured value normalized with the assumed noise covariance,

[0166]

[0167] Some embodiments provide a controller for controlling vehicle motion based on vehicle states tracked using GNSS measurements. Such a controller is described below. Figure 7 Explained in detail.

[0168] Figure 7A block diagram of a controller 700 for controlling the motion of a vehicle 720a based on the state of the vehicle 720a tracked using GNSS measurements according to some embodiments of the present disclosure is shown. 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, and the memory 765a is used to store assumptions of the noise covariance 743a of the motion model 740a, the measurement model 741a, the constraints 742a, and the measurements of the vehicle 720a. The processor 760a can 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 can include a random access memory (RAM), a read-only memory (ROM), a flash memory, or any other suitable memory system.

[0169] The controller 700 executes Figure 2A The steps of the method described in the embodiment of the present invention are used 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.

[0170] Fig. 8A A schematic diagram of a vehicle 720a including a controller 700 according to some embodiments of the present disclosure is shown. As used herein, the vehicle 720a can be any type of wheeled vehicle such as a passenger car, a bus, or a rover. In addition, the vehicle 720a can be an automatic or semi-automatic vehicle. For example, some embodiments control the movement of the vehicle 720a. Examples of 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 can be controlled by a driver of the vehicle 720a.

[0171] The vehicle may also include an engine 806 that may be controlled by the controller 700 or by other components of the vehicle 720a. The vehicle 702a may also include one or more sensors 804 to sense the surrounding environment. Examples of sensor 804 include range finders, radars, laser radars, and cameras. The vehicle 720a may also include one or more sensors 805 to sense its current motion and internal state. Examples of sensor 805 include global positioning systems (GPS), accelerometers, inertial measurement units, gyroscopes, shaft rotation sensors, torque sensors, deflection sensors, pressure sensors, and flow sensors. The sensors provide information to the controller 700. The vehicle may be equipped with a transceiver 807 that enables the communication capabilities of the controller 700 through a wired or wireless communication channel.

[0172] Figure 8BA schematic diagram of the interaction between the controller 700 and the controller 820 of the vehicle 720a according to some embodiments of the present disclosure is shown. 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 rotation and acceleration of the vehicle 720a. In this 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 high-level controller (e.g., a lane keeping assist controller 835 that further processes the control input of the controller 700). In both cases, 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 brake of the vehicle 720a to control the movement of the vehicle 720a.

[0173] 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 input to controller 820 based on the estimated state of vehicle 720a in order to control the movement of vehicle 720a.

[0174] Some embodiments are based on the understanding that the accuracy of a sensor can change over time, either smoothly or suddenly. Other embodiments recognize that in their standard formulations, probabilistic filters such as KFs and LRKFs require a priori set noise covariances for the measurements used to update the state estimate, where the noise covariance has the effect of normalizing and weighting the importance of each measurement. Some other embodiments are based on the recognition that noise-adaptive KFs (e.g., using variational Bayesian methods for noise adaptation) do not have convergence guarantees and perform well when small changes are made to the a priori set noise covariance. However, for example, in the case of outliers in GPS measurements, this approach is prone to error because outliers tend to occur from one time step to another, and known noise-adaptive methods have problems correctly adapting to such large changes in noise covariance in real time. For example, variational Bayesian methods for noise estimation depend on a tuning factor (e.g., a forgetting factor) that determines the rate at which changes in the state estimate may occur. This forgetting factor is designed to assume slowly changing states. Therefore, rapid changes in situations such as GNSS measurement reliability cannot be handled well in this approach. This is in contrast to the method disclosed in the present disclosure, which converges quickly because the probability of each probability filter is determined using a likelihood that incorporates an assumption of the measurement noise covariance into the likelihood.

[0175] This description provides only exemplary embodiments and is not intended to limit the scope, applicability or configuration of the present disclosure. Instead, the following description of the exemplary embodiments will provide a description of implementations for implementing one or more exemplary embodiments for those skilled in the art. It is contemplated that various changes may be made in the functions and arrangements of the elements without departing from the spirit and scope of the subject matter as set forth in the appended claims.

[0176] Specific details are given in the following description to provide a thorough understanding of the embodiments. However, it will be appreciated by those of ordinary skill in the art that the embodiments may be practiced without these specific details. For example, the systems, processes, and other elements in the disclosed subject matter may be shown as components in block diagram form to avoid obscuring the embodiments with unnecessary details. In other cases, known processes, structures, and techniques may be shown without unnecessary details to avoid obscuring the embodiments. In addition, similar reference numerals and names indicate similar elements in the various drawings.

[0177] In addition, each embodiment can be described as a process depicted as a flow chart, flow diagram, data flow diagram, structure diagram or block diagram. Although a flow chart can describe an operation as a sequential process, many operations can be performed in parallel or simultaneously. In addition, the order of the operations can be rearranged. The process can terminate when its operation is completed, but may have additional steps not discussed or included in the figure. In addition, not all operations in any particularly described process can occur in all embodiments. A process can correspond to a method, function, process, subroutine, subprogram, etc. When a process corresponds to a function, the termination of the function can correspond to the function returning to the calling function or main function.

[0178] In addition, embodiments of the disclosed subject matter may be implemented at least in part manually or automatically. Manual or automatic implementation may be performed or at least assisted by the use of a machine, hardware, software, firmware, middleware, microcode, hardware description language, or any combination thereof. When implemented in software, firmware, middleware, or microcode, program code or code segments for performing the necessary tasks may be stored in a machine-readable medium. A processor may perform the necessary tasks.

[0179] The various methods or processes outlined herein may be encoded as software executable on one or more processors employing any of a variety of operating systems or platforms. Additionally, such software may be written using any of a number of suitable programming languages ​​and / or programming or scripting tools, and may also be compiled into executable machine language code or intermediate code executed on a framework or virtual machine. Typically, the functions of program modules may be combined or distributed as desired in various embodiments.

[0180] Embodiments of the present disclosure may be implemented as methods in which examples have been provided. The actions performed as part of the method may be ordered in any suitable manner. Thus, embodiments may be constructed in which the actions are performed in a different order than shown, which may include performing some actions simultaneously, even though shown as sequential actions in illustrative embodiments.

[0181] In addition, the embodiments of the present disclosure and the functional operations described in this specification can be implemented in digital electronic circuits, tangibly embodied computer software or firmware, computer hardware including the structures disclosed in this specification and their structural equivalents, or a combination of one or more of them. In addition, some embodiments of the present disclosure can be implemented as one or more computer programs (i.e., one or more modules of computer program instructions encoded on a tangible non-temporary program carrier for execution by a data processing device or for controlling the operation of a data processing device). In addition, program instructions can be encoded on an artificially generated propagation signal (e.g., a machine-generated electrical, optical or electromagnetic signal) that is generated to encode information for transmission to a suitable receiver device for execution by a data processing device. The computer storage medium can be a machine-readable storage device, a machine-readable storage substrate, a random or serial access memory device, or a combination of one or more of them.

[0182] According to an embodiment of the present disclosure, the term "data processing device" may cover all types of devices, apparatuses and machines for processing data, including examples of programmable processors, computers or multiple processors or computers. The device may include a dedicated logic circuit (e.g., an FPGA (field programmable gate array) or an ASIC (application-specific integrated circuit)). In addition to hardware, the device may also include code that creates an execution environment for the computer program in question, for example, code that constitutes processor firmware, a protocol stack, a database management system, an operating system, or a combination of one or more of them.

[0183] A computer program (which may also be referred to 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 languages, or declarative or procedural languages, and may be deployed in any form, including 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 a portion of a file that holds other programs or data, for example, in one or more scripts in a markup language document, in a single file dedicated to the program in question, or in multiple coordinated files (e.g., files storing one or more modules, subroutines, or portions of code).

[0184] The computer program may be deployed to be executed on one computer or on multiple computers located at one site or distributed on multiple sites and interconnected by a communication network. Computers suitable for executing computer programs include, for example, central processing units that may be based on general or special purpose microprocessors or on both, as well as any other kind of microprocessor. Typically, the central processing unit will receive instructions and data from a read-only memory or a random access memory or from both. The basic elements of a computer are a central processing unit for implementing or executing instructions and one or more memory devices for storing instructions and data.

[0185] Typically, a computer will also include or be operably connected to receive data from or transfer data to, or both, one or more mass storage devices (e.g., magnetic, magneto-optical, or optical disks) for storing data. However, a computer need not have such devices. In addition, a computer may 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 (e.g., a universal serial bus (USB) flash drive), to name a few.

[0186] To provide interaction with a user, embodiments of the subject matter described in this specification may be implemented on a computer having a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user, and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices may also be used to provide interaction with the user; for example, the feedback provided to the user may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user may be received in any form, including acoustic, voice, or tactile input. In addition, the computer may interact with the user 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 a user's client device in response to a request received from the web browser.

[0187] Implementations of the subject matter described in this specification 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 with a graphical user interface or a web browser through which a user can interact with implementations of the subject matter described in this specification), or any combination of one or more such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include local area networks ("LANs") and wide area networks ("WANs"), such as the Internet.

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

[0189] Although the present disclosure has been described with reference to certain preferred embodiments, it is to be understood that various other adaptations and modifications may be made within the spirit and scope of the present disclosure. Therefore, the aspects of the appended claims cover all such changes and modifications that fall within the true spirit and scope of the present disclosure.

Claims

1. A feedback controller for controlling motion of a device based on a state of the device tracked using motion measurements indicative of the state of the device, the feedback controller comprising: at least one processor; and at least one memory storing instructions that, when executed by the at least one processor, cause the feedback controller to: collecting a sequence of measurements indicative of states of the device at different control steps; iteratively executing a composite probability filter, the composite probability filter being configured to track the device state at each of the different control steps using the measurement sequence to produce a sequence of device states corresponding to the measurement sequence, wherein, to perform an iteration for a current control step using a current measurement, the composite probability filter is configured to: executing a plurality of probability filters parameterized on the device state to generate a plurality of estimates of the device state for a current control step, wherein each of the plurality of probability filters iteratively predicts a current state of the device using a prediction model subject to process noise and updates the predicted current state based on current measurements using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby resulting in variations in the plurality of estimates of the device state; estimating, for each of the plurality of probability filters, a likelihood of a corresponding measurement noise to associate the current measurement with the predicted current state; combining the multiple estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and outputting the device state tracked by the complex probability filter for a current control step based on the weighted combination; and The device is controlled using the sequence of states tracked by the complex probability filter.

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

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

4. The feedback controller according to claim 2, wherein: Each of the multiple probability filters is a Kalman filter whose process noise and measurement noise are defined by corresponding Gaussian probability distributions, such that the mean of the Gaussian probability distribution is the estimate of the predicted current state transformed into the measurement space, and different measurement noise covariances produce the Gaussian probability distribution of the measurement noise.

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

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

7. The feedback controller according to claim 4, wherein: The complex probability filter is configured to estimate the likelihood of correlation of different measurement noises based on a value of a gain of a corresponding Kalman filter used to update the predicted current state based on the current measurement.

8. The feedback controller according to claim 7, wherein: The gain is the Kalman gain.

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

10. The feedback controller according to claim 1, wherein: The feedback controller is operably connected to multiple sensors of different types using at least one of: a wired link, a wireless link, or a combination thereof to collect measurement values ​​of the multiple sensors indicative of the device state, and wherein the measurement model of the multiple probability filters fuses the measurement values ​​of the multiple sensors to produce a corresponding estimate of the device state.

11. A controller for tracking a vehicle state to control the movement of the vehicle based on measurements using a global navigation satellite system (GNSS), the controller comprising: at least one processor; and at least one memory storing instructions that, when executed by the at least one processor, cause the controller to: collecting a sequence of GNSS measurements indicative of a state of the vehicle at different control steps; iteratively executing a composite probability filter, the composite probability filter being configured to track the vehicle state at the different control steps using the GNSS measurement sequence to produce a sequence of vehicle states corresponding to the GNSS measurement sequence, wherein, to perform an iteration for a current control step using a current GNSS measurement, the composite probability filter is configured to: executing a plurality of probability filters having the same measurement model relating the current GNSS measurement to a current estimate of the vehicle state to produce a plurality of estimates of the vehicle state for a current control step, wherein the plurality of probability filters are affected by different measurement noise characteristics resulting in variations among the plurality of estimates of the vehicle state; combining the multiple estimates of the vehicle state into a weighted combination whose weights are derived from the likelihood of the current GNSS measurement according to different measurement noises centered around an estimate of the current GNSS measurement predicted by one or a combination of the multiple probability filters; and estimating the vehicle state tracked by the complex probability filter for the current control step based on the weighted combination; and The vehicle is controlled using the state sequence estimated by the complex probability filter.

12. The controller according to claim 11, wherein: The plurality of probability filters include a first probability filter configured to generate a first estimate of the vehicle state affected by a first measurement noise defined by a first probability density function PDF, and a second probability filter configured to generate a second estimate of the vehicle state affected by a second measurement noise defined by a second PDF different from the first PDF, wherein, in order to generate the weighted combination, the composite probability filter is configured to: determining the estimate of the current GNSS measurement 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 around an estimate of the current GNSS measurement; determining a first likelihood of the current GNSS measurement based on the first PDF centered at an estimate of the current GNSS measurement; determining a second likelihood of the current GNSS measurement based on the second PDF centered at an 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 vehicle state and a second weight for weighting the second estimate of the vehicle state; and The weighted combination is determined based on the first estimate of the vehicle state weighted with the first weight and the second estimate of the vehicle state weighted with the second weight.

13. The controller according to claim 11, wherein: The plurality of probability filters include a first probability filter configured to generate a first estimate of the vehicle state affected by a first measurement noise defined by a first probability density function PDF, and a second probability filter configured to generate a second estimate of the vehicle state affected by a second measurement noise defined by a second PDF different from the first PDF, wherein, in order to generate the weighted combination, the composite probability filter is configured to: executing the first probability filter to predict a first current state of the vehicle based on an internal state of the first probability filter, and updating the first current state of the vehicle using the measurement model, the measurement model processing the current GNSS measurements according to a gain of the first probability filter to produce the first estimate of the vehicle state; executing the second probability filter to predict a second current state of the vehicle based on an internal state of the second probability filter, and updating the second current state of the vehicle using the measurement model, the measurement model processing the current GNSS measurements according to a gain of the second probability filter to produce the second estimate of the vehicle state; and An internal state of the first probability filter and an internal state of the second probability filter are updated based on a combination of the first estimate and the second estimate of the vehicle state produced by the first probability filter and the second probability filter.

14. The controller according to claim 11, wherein: The plurality of probability filters comprises a first probability filter configured to generate a first estimate of the vehicle state affected by a first measurement noise defined by a first probability density function PDF, wherein, to generate a weight for scaling the first estimate of the vehicle state, the first probability filter is configured to: predicting a current state of the vehicle using a vehicle motion model affected by process noise based on a previous control command applied to the vehicle having a previous state at a previous control step; transforming a predicted current state of the vehicle into a measurement space to produce the estimate of the current GNSS measurement; centering the first PDF around the estimate of the current GNSS measurement; and A weight for weighting the first estimate of the vehicle state is determined based on a likelihood of the current GNSS measurement according to the first PDF centered about the estimate of the current GNSS measurement.

15. The controller according to 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, wherein the composite probability filter stores historical weights of the first probability filter determined for multiple previous control steps, and updates the weight used to weight the first probability filter at the current control step based on an average of the current weight and the historical weights.

16. The controller according to claim 11, wherein: The controller is operably connected to a plurality of sensors using at least one of: a wired link, a wireless link, or a combination thereof to collect measurements from the plurality of sensors indicative of the vehicle state, wherein the plurality of sensors includes a GNSS receiver that collects the GNSS measurements, and wherein the measurement model of the plurality of probability filters fuses the measurements from the plurality of sensors to produce corresponding estimates of the vehicle state.

17. The controller according to claim 16, wherein: In addition to the GNSS receiver, the multiple sensors include one or a combination of a camera that produces a color image, a depth sensor that produces a depth image, and a roadside unit RSU that produces fused measurements of multiple remote sensors.

18. The controller according to claim 17, wherein: The multiple sensors include a first sensor that collects a first measurement value and a second sensor that collects a second measurement value, wherein each of the multiple probability filters tracks the vehicle state based on the first measurement value and the second measurement value, wherein the multiple probability filters include a first probability filter that tracks the vehicle state using a first assumption of the noise covariance of the first measurement value and a first assumption of the noise covariance of the second measurement value, and a second probability filter that tracks the vehicle state using a second assumption of the noise covariance of the first measurement value and a second assumption of the noise covariance of the second measurement value, wherein the first assumption of the noise covariance of the first measurement value is different from the second assumption of the noise covariance of the first measurement value, the first assumption of the noise covariance of the second measurement value is different from the second assumption of the noise covariance of the second measurement value, or both are different.

19. A method for controlling the motion of a device based on a state of the device tracked using measurements of motion indicative of the state of the device, the method comprising the steps of: collecting a sequence of measurements indicative of the state of the device at different control steps; iteratively executing a composite probability filter, the composite probability filter being configured to track the device state at each of the different control steps using the measurement sequence to produce a sequence of device states corresponding to the measurement sequence, wherein, to perform an iteration for a current control step using a current measurement, the composite probability filter is configured to: executing a plurality of probability filters parameterized on the device state to generate a plurality of estimates of the device state for a current control step, wherein each of the plurality of probability filters iteratively predicts a current state of the device using a prediction model subject to process noise and updates the predicted current state based on current measurements using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby resulting in variations in the plurality of estimates of the device state; estimating, for each of the plurality of probability filters, a likelihood of corresponding measurement noise to associate a current measurement with a predicted current state; combining the multiple estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and outputting the device state tracked by the complex probability filter for a current control step based on the weighted combination; and The device is controlled using the sequence of states tracked by the complex probability filter.

20. A non-transitory computer readable storage medium having embodied thereon a program executable by a processor to perform a method for controlling motion of a device based on a state of the device tracked using measurements of motion indicative of the state of the device, the method comprising the steps of: collecting a sequence of measurements indicative of the state of the device at different control steps; iteratively executing a composite probability filter, the composite probability filter being configured to track the device state at each of the different control steps using the measurement sequence to produce a sequence of device states corresponding to the measurement sequence, wherein, to perform an iteration for a current control step using a current measurement, the composite probability filter is configured to: executing a plurality of probability filters parameterized on the device state to generate a plurality of estimates of the device state for a current control step, wherein each of the plurality of probability filters iteratively predicts a current state of the device using a prediction model subject to process noise and updates the predicted current state based on current measurements using a measurement model subject to measurement noise, wherein different probability filters in the plurality of probability filters have different measurement noises, thereby resulting in variations in the plurality of estimates of the device state; estimating, for each of the plurality of probability filters, a likelihood of corresponding measurement noise to associate a current measurement with a predicted current state; combining the multiple estimates of the device state into a weighted combination having normalized weights derived from the likelihoods estimated for the corresponding probability filters; and outputting the device state tracked by the complex probability filter for a current control step based on the weighted combination; and The device is controlled using the sequence of states tracked by the complex probability filter.

Images