System and method for tracking the expansion state of a moving object using model geometry learning

Abstract

Provided herein is a system and method for tracking an expansion state of an object, the expansion state of the object including a kinematic state indicating the position of the object and an extended state indicating one or a combination of the size and orientation of the object. The system includes: at least one sensor configured to detect a scene including a moving object using one or more signal transmissions to generate one or more measurements of the object per transmission; and a processor configured to execute a probability filter that tracks a joint probability of the expansion state of the object estimated by a motion model of the object and a measurement model of the object, wherein the measurement model includes a centrally truncated distribution with a predetermined truncation interval. The system also includes an output interface configured to output the expansion state of the object.

Description

[0001] Related applications

[0002] This application is a continuation-in-part of pending application 16 / 789,931 filed on February 13, 2020. Technical Field

[0003] The present invention relates generally to automotive object tracking and, more particularly, to systems and methods for tracking an expanded state of an object. Background Art

[0004] The control system used by a vehicle (autonomous or semi-autonomous) predicts safe motion or paths for the vehicle in order to avoid collisions with obstacles such as other vehicles or pedestrians, and the control system also optimizes some criteria associated with the operation of the vehicle. The vehicle's sensors sense the surrounding environment such as road edges, pedestrians and other vehicles. Automotive radar has shown its role from existing ADAS (Advanced Driver Assistance Systems) to emerging autonomous driving assistance systems. Together with ultrasound, camera and LIDAR sensors, automotive radar assists the task of environmental sensing and understanding in all-weather conditions with affordable cost and scalable production. Specifically, automotive radar provides direct measurement of radial velocity, long operating range, small size in millimeter or sub-terahertz frequency bands, and high spatial resolution.

[0005] The control system of a vehicle tracks the object state of other vehicles (where the object state includes the kinematic state) based on automotive radar measurements to control the vehicle. Extended object tracking (EOT) with multiple measurements per scan has shown improved object tracking compared to traditional point object tracking that includes only one measurement per scan by enhancing the object state from only the kinematic state to both the kinematic state and the extended state. The extended state provides the size and orientation of the tracked object. To achieve this, the spatial distribution (i.e., how the automotive radar measurements are distributed around the object space) needs to be captured along with the sensor noise. Current methods include a framework of a fixed set of points on a rigid body, which requires non-scalable data association between the fixed set of points and the automotive radar detections even for single object tracking. Spatial models such as contour models and surface models bypass the cumbersome data association step.

[0006] For automotive radar measurements, the contour model reflects the distribution of measurements along the contour of an object (e.g., a rigid body), and the surface model assumes that the radar measurements are generated from the inner surface of a two-dimensional shape. Examples of contour models include simple rectangular shapes and more general star-convex shapes modeled by random hypersurface models or Gaussian process models. Compared to contour models that require more degrees of freedom to describe more complex shapes, surface models such as Gaussian-based ellipse and layered Gaussian-based ellipse models are computationally much simpler. However, the measurements of the object are affected by noise and only receive reflections from the surface of the object. Therefore, the above models are far from real-world automotive radar measurements, which are observed to be distributed around the edges or surfaces of rigid objects with a specific volume.

[0007] Therefore, there is a need for a system and method for tracking the kinematic and extension states of an object by capturing real-world automotive radar measurements. Summary of the Invention

[0008] Some embodiments are directed to a system and method for tracking the expansion state of an object. The expansion state of an object includes a kinematic state indicating the position of the object and an extension state indicating one or a combination of the size and orientation of the object. Some embodiments are based on the recognition that the expansion state of an object can be estimated using a centrally truncated distribution and a corresponding underlying untruncated Gaussian distribution.

[0009] Some embodiments are based on the goal of tracking objects using automotive radar measurements. To this end, in some embodiments, objects are tracked using point object tracking that receives a single measurement from the object per scan. Point object tracking only provides the kinematic state (position) of the object. In addition, objects are tracked using a probabilistic filter with a measurement model of the kinematic state distribution. Some embodiments are based on extended object tracking (EOT), in which multiple objects are tracked and multiple measurements per time step are generated corresponding to each object. The measurements are constructed around the object space. Extended object tracking provides both the kinematic state and the extended state (size and orientation) of the object. The kinematic state and the extended state are collectively referred to as the extended state. Objects are tracked using a probabilistic filter with a measurement model of the extended state distribution.

[0010] However, real-world automotive radar measurement distributions are complex, with multiple reflections from objects. This complexity complicates the design of measurement models. Consequently, conventional measurement models are only applicable to the kinematic regime, not the extended regime.

[0011] To this end, in some embodiments, spatial models such as silhouette models and surface models are used to capture real-world automotive radar measurements. However, such spatial models are inaccurate. Some embodiments are based on the recognition that real-world automotive radar measurements are distributed around the edges or surfaces of objects with a certain volume, which leads to surface volume models. Surface volume models are a balance between silhouette models and surface models, with more realistic features while keeping the EOT accurate. However, surface volume models are computationally complex. To this end, some embodiments are based on the goal of formulating a model with distribution density occurring at the ends rather than in the center, so that it resembles and captures real-world automotive radar measurements.

[0012] To achieve this, a central truncated distribution is iteratively estimated using a probability filter. The central truncated distribution is used to fit the measurements. The central truncated distribution is based on a truncation interval at the center and provides a smaller probability for measurements at the center of the central truncated distribution within the truncation interval and a larger probability for measurements outside the truncation interval. To this end, some embodiments are based on the recognition that the central truncated distribution can be used to represent real-world measurements.

[0013] A centered truncated distribution is a truncation of the underlying untruncated Gaussian distribution according to a truncation interval. The underlying Gaussian distribution is centered at the mean of the distribution, and the variance measures the spread and width of the distribution.

[0014] To this end, some embodiments are based on the goal of estimating a centrally truncated distribution of fitted measurements and subsequently the mean and variance of an underlying Gaussian distribution corresponding to the estimated centrally truncated distribution. Some embodiments are based on the recognition that the mean of the underlying Gaussian distribution indicates the position of the object in the expanded state, and the variance of the underlying Gaussian distribution indicates the size and orientation of the object in the expanded state. To this end, some embodiments are based on the recognition that using a pair of the centrally truncated distribution and the underlying Gaussian distribution, both the kinematic state and the expanded state of the object can be estimated. This simplifies the parameterization of tracking the expanded state. Furthermore, using a pair of centrally truncated and underlying Gaussian distributions reduces the dimensionality of the computation.

[0015] Some embodiments are based on the goal of removing noise from a measurement when the measurement is affected by noise. In some embodiments, the probability filter is configured to remove noise from the measurement before estimating the noise-free measurement source based on a centrally truncated Gaussian distribution, such that the probability filter generates a centrally truncated distribution that fits the noise-free measurement source. Furthermore, a truncation interval is determined for the noise-free measurement source. According to some embodiments, the measurement model is a hierarchical measurement model that defines, for each measurement, probability parameters for hidden measurements of the noise-free source.

[0016] To this end, some embodiments are based on the recognition that a hierarchical measurement model fundamentally captures not only the spatial distribution—that is, how the automotive radar measurements are spatially distributed around the object—but also the characteristics of sensor noise. Furthermore, a Bayesian EOT algorithm is formulated based on the hierarchical measurement model for both the kinematic and extended states. In some embodiments, the Bayesian EOT algorithm is developed based on the hierarchical measurement model by recursively predicting the extended state and updating the extended state and cutoff interval. The cutoff interval is also referred to as the cutoff boundary.

[0017] Some embodiments are based on another recognition that the boundary estimation in the state update step can be offloaded by utilizing a large-scale offline training dataset to learn the truncation boundaries and other parameters associated with the measurement model from the training data.

[0018] To this end, in some embodiments, truncation boundaries including values ​​for truncation intervals for truncating the probability distribution are stored in a memory (e.g., a memory operatively connected to a processor), the processor being configured to execute a probability filter that tracks the joint probability of the dilated states. In some implementations, the memory may store single and / or multiple combinations of values ​​for the truncation intervals that are applicable to different situations.

[0019] Additionally or alternatively, the memory may store other predetermined parameters of the measurement model. For example, in some embodiments, the memory stores structural geometric parameters of the measurement model learned from training data. Examples of structural geometric parameters include a cutoff boundary defining the extent of the cutoff interval and the orientation of the cutoff interval relative to the orientation of the object. Knowing the cutoff boundary reduces the computational complexity of EOT tracking.

[0020] Furthermore, in some embodiments, a vehicle model having an extended state with bounded uncertainty is used to determine control inputs for a controller of the vehicle, and the vehicle is controlled according to the control inputs. The vehicle model includes a motion model of an object subject to process noise and a measurement model of the object subject to measurement noise, such that one or a combination of the process noise and the measurement noise bounds an uncertainty in the extended state of the object.

[0021] Thus, one embodiment discloses a system for tracking an expanded state of an object including a kinematic state indicating a position of the object and an extended state indicating one or a combination of a size and an orientation of the object, the system comprising: at least one sensor configured to detect a scene including a moving object using one or more signal transmissions to generate one or more measurements of the object; a processor configured to execute a probability filter that tracks a joint probability of the expanded state of the object estimated by a motion model of the object and a measurement model of the object, wherein the measurement model comprises a central truncated distribution having a truncation interval that provides a smaller probability for measurements at a center of the central truncated distribution within the truncation interval and a larger probability for measurements outside the truncation interval, wherein the central truncated distribution is a truncation of an underlying untruncated Gaussian distribution according to the truncation interval, wherein the probability filter is configured to estimate the central truncated distribution of the fitted measurements and generate a mean and variance of the underlying Gaussian distribution corresponding to the central truncated distribution, such that the mean of the underlying Gaussian distribution indicates the position of the object in the expanded state and the variance of the underlying Gaussian distribution indicates the size and orientation of the object in the expanded state; and an output interface configured to output the expanded state of the object.

[0022] Thus, another embodiment discloses a method for tracking an expanded state of an object including a kinematic state indicating a position of the object and an extended state indicating one or a combination of a size and an orientation of the object, wherein the method uses a processor coupled to a memory storing executable instructions that, when executed by the processor, perform the steps of the method, the method comprising the steps of: detecting a scene including a moving object by at least one sensor using one or more signal transmissions to generate one or more measurements of the object in accordance with the transmissions; executing a probability filter that tracks a joint probability of the expanded state of the object estimated by a motion model of the object and a measurement model of the object, wherein the measurement model includes a central truncated distribution having a truncation interval that provides a smaller probability for measurements at a center of the central truncated distribution within the truncation interval and a larger probability for measurements outside the truncation interval, wherein the central truncated distribution is a truncation of an underlying untruncated Gaussian distribution according to the truncation interval, wherein the probability filter estimates the central truncated distribution of the fitted measurements and generates a mean and variance of the underlying Gaussian distribution corresponding to the central truncated distribution, such that the mean of the underlying Gaussian distribution indicates the position of the object in the expanded state and the variance of the underlying Gaussian distribution indicates the size and orientation of the object in the expanded state; and outputting the expanded state of the object via an output interface.

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

[0024] [Figure 1A ] Figure 1A A first schematic overview of some principles used by some embodiments for tracking the dilation state of an object is shown.

[0025] [ Figure 1B ] Figure 1B A second schematic overview is shown of some principles used by some embodiments for tracking the dilation state of an object.

[0026] [ Figure 1C ] Figure 1C A third schematic overview showing some principles used by some embodiments for tracking the dilation state of an object.

[0027] [ Figure 2 ] Figure 2 A block diagram illustrating a system for tracking the dilation state of an object, according to some implementations.

[0028] [ Figure 3A ] Figure 3A Schematic diagram illustrating the recursive computation of the posterior density of an object's dilated state using a recursive Bayesian filter in accordance with some embodiments.

[0029] [ Figure 3B ] Figure 3B Schematic diagram showing the recursive computation of the posterior density of an object's dilated state using recursive Bayesian filtering in accordance with some embodiments.

[0030] [ Figure 4 ] Figure 4 Shown are examples of hidden measurement source variables and distributions of observable measurements according to some embodiments.

[0031] [ Figure 5A ] Figure 5A Shown is an exemplary cutoff interval adaptation when the front or back of an object faces the sensor, according to some embodiments.

[0032] [ Figure 5B ] Figure 5B Shown is an exemplary adaptation of the cutoff interval when the object is oriented sideways relative to the sensor, according to some embodiments.

[0033] [ Figure 6 ] Figure 6 Schematic diagram illustrating the expansion state prediction step according to some embodiments.

[0034] [ Figure 7A ] Figure 7A Schematic diagram illustrating the steps of dilation state update according to some embodiments.

[0035] [ Figure 7B ] Figure 7BAn exemplary pseudo measurement according to some embodiments is shown.

[0036] [ Figure 7C ] Figure 7C Schematic diagram illustrating pseudo measurement generation, truncation boundary update, and dilation state update according to some embodiments.

[0037] [ Figure 8A ] Figure 8A A schematic diagram illustrating a truncation boundary update step according to some embodiments.

[0038] [ Figure 8B ] Figure 8B Filtered scan aggregation in an object coordinate system is shown in accordance with some embodiments.

[0039] [ Figure 9A ] Figure 9A Shown is a simulation of a scenario where an object moves for 90 time steps during a turn, in accordance with some embodiments.

[0040] [ Figure 9B ] Figure 9B A graph illustrating performance evaluation using an ideal measurement model according to some embodiments is shown.

[0041] [ Figure 9C ] Figure 9C is a table showing the root mean square error (RMSE) of the kinematic state and extension state estimation of the object corresponding to the ideal measurement model with the regular random matrix (RM) and the hierarchical truncated Gaussian random matrix (HTG-RM).

[0042] [ Figure 10A ] Figure 10A A graph illustrating performance evaluation under model mismatch according to some embodiments is shown.

[0043] [ Figure 10B ] Figure 10B is a table showing the RMSE of the kinematic state and extension state estimation of the object corresponding to RM and HTG-RM under model mismatch.

[0044] [ Figure 11A ] Figure 11A A schematic diagram of a vehicle including a controller in communication with a system employing the principles of some embodiments is shown.

[0045] [ Figure 11B ] Figure 11B Schematic diagram showing interaction between a controller and a controller of a vehicle according to some embodiments.

[0046] [ Figure 11C ] Figure 11C Schematic diagram showing an autonomous or semi-autonomous controlled vehicle using some embodiments to generate control inputs.

[0047] [ Figure 12 ] Figure 12 A flow chart illustrating a method for learning geometry-dependent structural model parameters according to some embodiments.

[0048] [ Figure 13A ] Figure 13A Schematic diagram illustrating the transformation of training data collected from different motions of different objects into a common unit coordinate system, according to some embodiments.

[0049] [ Figure 13B ] Figure 13B Schematic diagram illustrating the transformation of training data collected from different motions of different objects into a common unit coordinate system, according to some embodiments.

[0050] [ Figure 13C ] Figure 13C Schematic diagram illustrating the transformation of training data collected from different motions of different objects into a common unit coordinate system, according to some embodiments.

[0051] [ Figure 14 ] Figure 14 Schematic diagram showing the evolution of the formation of an HTG model according to some embodiments.

[0052] [ Figure 15A ] Figure 15A Schematic diagram showing the state of an object, an ego vehicle, and ego vehicle (VC), sensor (SC), and object (OC) coordinate systems used by some embodiments.

[0053] [ Figure 15B ] Figure 15B A flow chart illustrating an offline spatial model learning workflow for estimating parameters of a model for tracking a vehicle, according to some embodiments.

[0054] [ Figure 16A ] Figure 16A An example of learning a measurement model for different viewing angles is shown in accordance with some embodiments.

[0055] [ Figure 16B ] Figure 16B An example of learning a measurement model for different viewing angles is shown in accordance with some embodiments.

[0056] [ Figure 17 ] Figure 17 A block diagram illustrating a method of tracking an extension state of an object according to one embodiment.

[0057] [ Figure 18 ] Figure 18Block diagram illustrating random matrix (RM) updates of a dilated state of a moving object in accordance with some embodiments. DETAILED DESCRIPTION

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

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

[0060] Figure 1A 、 Figure 1B and Figure 1C A schematic overview of some of the principles used in some embodiments for tracking the extended state of an object is shown. A sensor 104 (e.g., an automotive radar) is used to track an object (e.g., a vehicle 106). In point object tracking 100, a single measurement 108 is received from vehicle 106 per scan. Point object tracking only provides the kinematic state (position) of vehicle 106. In addition, vehicle 106 is tracked using a probabilistic filter with a measurement model that has a distribution of the kinematic state. In extended object tracking (EOT) 102, multiple measurements 110 are received per scan. Multiple measurements 110 are spatially constructed around vehicle 106. Extended object tracking provides both the kinematic state and the extended state (size and orientation) of vehicle 106. The kinematic state and the extended state are collectively referred to as the extended state. Vehicle 106 is tracked using a probabilistic filter with a measurement model that has a distribution of the extended state. However, the distribution of real-world automotive radar measurements 112 indicates that the multiple reflections from vehicle 106 are complex. Due to this complexity, the design of the measurement model becomes complex. Therefore, conventional measurement models are only applicable to the kinematic state, not the extended state.

[0061] To this end, in some embodiments, spatial models 114 such as contour models 116 and surface models 118 are used to capture real-world automotive radar measurements 112. However, the above spatial models 114 are inaccurate. Some embodiments are based on the recognition that real-world automotive radar measurements are distributed around the edges or surfaces of an object (vehicle 106) having a specific volume, thereby generating a surface volume model 120. The surface volume model 120 strikes a balance between the contour model 116 and the surface model 118, having more realistic features while maintaining accurate EOT. However, the surface volume model 120 is computationally complex. To this end, some embodiments are based on the goal of formulating a model (as shown at 122) where the distribution density appears at the ends rather than the center of one or more dimensions, such that it resembles and captures real-world automotive radar measurements 112.

[0062] To achieve this, in some embodiments, a central truncated distribution 124 is estimated. Figure 1C A one-dimensional central truncated distribution 124 with appropriate scaling / normalization and the underlying untruncated Gaussian distribution 132 are shown. The central truncated distribution 124 is based on a truncation interval at the center 126. For example, the truncation interval is given by a < x < b. The central truncated distribution 124 is used to fit the measurements 110. Additionally, the central truncated distribution 124 provides a lower probability for measurements at the center of the central truncated distribution 126 (i.e., within the truncation interval) and a higher probability for measurements outside the truncation intervals 128, 130. To this end, some embodiments are based on the recognition that the central truncated distribution 124 can be used to represent real-world automotive radar measurements 112.

[0063] This concept can be naturally extended to one or more dimensions. For example, a two-dimensional central truncated distribution of the length and width of an object and a three-dimensional central truncated distribution of the length, width, and height of an object. For a multi-dimensional central truncated distribution, the truncation region can be a more complex shape than a square or rectangle.

[0064] The central truncated distribution 124 is a truncation of the underlying untruncated Gaussian distribution 132 with appropriate normalization / scaling. The underlying untruncated Gaussian distribution 132 is obtained based on the central truncated distribution. The mean and variance of the central truncated distribution 124 are different from the mean 136 and variance 134 of the underlying untruncated Gaussian distribution 132. In some embodiments, the mean 136 and variance 134 of the underlying untruncated Gaussian distribution 132 can be derived from the mean and variance of the central truncated distribution 124. Therefore, some embodiments are based on the recognition that there is a mutual relationship between the central truncated distribution 124 and the underlying untruncated Gaussian distribution 132. To this end, in some embodiments, the mean 136 and variance 134 of the underlying untruncated Gaussian distribution 132 can be derived from the mean and variance of the central truncated distribution 124.

[0065] Some embodiments are based on the recognition that an underlying untruncated Gaussian distribution 132 can be used to estimate the expanded state of the vehicle 106. To this end, in some embodiments, a central truncated distribution 124 is fitted to the measurement source, and the mean 136 and variance 134 of the underlying untruncated Gaussian distribution 132 corresponding to the estimated central truncated distribution 124 are iteratively estimated by a probability filter. The underlying untruncated Gaussian distribution 132 is centered around the mean 136 of the distribution, and the variance 134 measures the spread, or width, of the distribution. The mean 136 of the underlying untruncated Gaussian distribution 132 indicates the position of the object in the expanded state, and the variance 134 of the underlying Gaussian distribution 132 indicates the size and orientation of the object in the expanded state.

[0066] To this end, some embodiments are based on the recognition that using a pair of a centrally truncated distribution and a corresponding underlying untruncated Gaussian distribution can track the dilation state of an object. Furthermore, this simplifies the parameterization of tracking the dilation state. Furthermore, using a pair of centrally truncated and underlying Gaussian distributions reduces the dimensionality of the computation because the underlying Gaussian distribution 132 is represented by fewer parameters than a more complex distribution representing the actual measurement.

[0067] Figure 2 A block diagram of a system 200 for tracking the expansion state of an object according to some embodiments is shown. The object can be a vehicle, such as (but not limited to) a car, bicycle, bus, or truck. In addition, the vehicle can be an autonomous or semi-autonomous vehicle. The expansion state includes a kinematic state indicating the position of the object and an extended state indicating the size and / or orientation of the object. According to some embodiments, the kinematic state corresponds to the motion parameters of the object, such as speed, acceleration, heading, and turn rate. In some other embodiments, the kinematic state corresponds to the position of the object with its motion parameters. The system 200 may include a sensor 202 or be operatively connected to a group of sensors to detect a scene using one or more signal transmissions to generate one or more measurements of the object per transmission. According to some embodiments, the sensor 202 may be an automotive radar. In some embodiments, the scene includes moving objects. In some other embodiments, the scene may include one or more objects, including both moving objects and stationary objects.

[0068] System 200 may have multiple interfaces that connect system 200 to other systems and devices. For example, a network interface controller (NIC) 214 is adapted to connect system 200 to a network 216 via bus 212, which connects system 200 to an operatively connected set of sensors. Through network 216 (wirelessly or via a wired connection), system 200 receives reflectance data from one or more signal transmissions to generate one or more measurements of the object per transmission. Additionally or alternatively, system 200 includes a control interface 228 configured to send control inputs to controller 222.

[0069] The system 200 includes a processor 204 configured to execute stored instructions and a memory 206 storing instructions executable by the processor 204. The processor 204 can be a single-core processor, a multi-core processor, a computing cluster, or any number of other configurations. The memory 206 can include random access memory (RAM), read-only memory (ROM), flash memory, or any other suitable memory system. The processor 204 is connected to one or more input devices and output devices via a bus 212. These instructions implement a method for tracking the expansion state of an object.

[0070] To this end, system 200 includes a motion model 208 and a measurement model 210. In some embodiments, system 200 includes a probability filter 224. Probability filter 224 is executed by processor 204. Probability filter 224 iteratively executes motion model 208 to predict the dilation state and executes measurement model 210 to update the dilation state of the object predicted by motion model 208. Execution of motion model 208 produces a prediction of the dilation state of the object subject to a fixed value of the object's size and varying the object's orientation, such that the object's size is updated only by measurement model 210, while the object's orientation is updated by both motion model 208 and measurement model 210.

[0071] Some embodiments are based on the recognition that the measurement model 210 includes a centrally truncated distribution 124 having a truncation interval. In some embodiments, the centrally truncated distribution 124 is a centrally truncated Gaussian distribution. In some other embodiments, the centrally truncated distribution 124 is a centrally truncated Student-t distribution. The centrally truncated distribution 124 is a truncation of an underlying untruncated Gaussian distribution according to the truncation interval. In some embodiments, a probability filter 224 is configured to estimate the centrally truncated distribution 124 that fits the measurement. The probability filter 224 executes the measurement model 210 to iteratively update the previous truncation interval determined during the previous iteration of the probability filter to generate a current truncation interval that fits the dilation state predicted by the motion model. Furthermore, the probability filter 224 updates the dilation state using the measurement model 210 having the centrally truncated Gaussian distribution for the current truncation interval. According to some embodiments, the probability filter 224 is one or a combination of a Bayesian filter, a Kalman filter, and a particle filter. A Bayesian filter is a general filter that can be used with different types of distributions. A Kalman filter operates efficiently with a Gaussian distribution. In some implementations, the processor 204 changes the cutoff interval for the sensor 202 in response to a change in the orientation of the object.

[0072] Furthermore, the probability filter 224 is configured to generate a mean and variance of an underlying Gaussian distribution corresponding to the estimated truncated central Gaussian distribution, such that the mean indicates the position of the object in the expanded state and the variance indicates the size and orientation of the object in the expanded state. To this end, some embodiments are based on the recognition that the expanded state of the object can be estimated using the estimated truncated central Gaussian distribution and the underlying Gaussian distribution. In some embodiments, execution of the measurement model 210 outputs a covariance matrix of the fitted measurements. The diagonal elements of the covariance matrix define the size of the object, and the off-diagonal elements of the covariance matrix define the orientation of the object.

[0073] Some embodiments are based on the goal of removing noise from a measurement when it is affected by noise. To this end, in some embodiments, probability filter 224 is configured to remove noise from the measurement before evaluating the likelihood of a noise-free measurement source based on a centrally truncated Gaussian distribution, such that probability filter 224 generates a centrally truncated distribution 124 that fits the noise-free measurement source. Furthermore, a truncation interval is determined for the noise-free measurement source. According to some embodiments, measurement model 210 is a hierarchical measurement model that defines, for each measurement, probability parameters for hidden measurements of noise-free sources.

[0074] System 200 includes a model of a vehicle 226, including a motion model 208 of an object subject to process noise and a measurement model 210 of the object subject to measurement noise, such that one or a combination of the process noise and the measurement noise bounds uncertainty in an expanded state of the object. System 200 is operatively connected to the vehicle. Processor 204 is configured to use the model of vehicle 226 having an expanded state with bounded uncertainty to determine control inputs for a controller 222 of the vehicle and to control the vehicle based on the control inputs.

[0075] In some embodiments, processor 204 is configured to execute a probability filter 224 that tracks a joint probability of the dilation state of the object estimated by the object's motion model 208 and the object's measurement model 210. The joint probability is a posterior probability conditioned on the dilation state estimated during a previous iteration of the probability filter 224. Furthermore, system 200 includes an output interface 220 to output the dilation state of the object. In some embodiments, output interface 220 is configured to output the dilation state of the object to controller 222.

[0076] Figure 3A Schematic diagram showing the recursive computation of the posterior density of the dilated state of an object using recursive Bayesian filtering according to some embodiments. The dilated (kinematic and stretch) state is defined as ξ k =[x k ,X k ], a random vector x k ∈R dx represents the kinematic state, Xk Represents the stretched state. For time step k, n is observed from the car radar sensor k Measurements

[0077] Assuming that the posterior density 302 is known at time k-1 to be p(ξ k-1 |Z 1:k-1 ), recursive Bayesian filtering begins 300. The posterior density is predicted by performing posterior density prediction 304. In some embodiments, the posterior density is predicted from the Chapman-Kolmogorov equation

[0078] p(ξ k-1 |Z 1:k-1 )=∫p(ξ k-1 |Z 1:k-1 )p(ξ k |ξ k-1 )dξ k-1 ...............(1)

[0079] The transfer density p(ξ k |ξ k-1 ) is given by the object motion model. In addition, the predicted posterior density is used to calculate the current measurement Z k 308 updates 306 to

[0080] p(ξ k |Z 1:k )∝p(ξ k |Z 1:k-1 )p(Z k |ξ k ).............(2)

[0081] in is the overall measurement likelihood, represents the individual spatial distribution. Therefore, an updated posterior density is obtained 312. Since the Bayesian filter is recursive 310, the above steps are performed iteratively until the termination condition is met. In each iteration 310, the posterior density in the next iteration is predicted based on the updated predicted density of the current iteration. For the state update step 306, it includes blocks for scanning aggregation of local measurements 316, truncation boundary updates 318 and dilated state updates 320. It should be noted that the local scanning aggregation step 316 is optional. In addition, if the truncation boundary is known or used to save computational complexity, the online truncation boundary update 318 can also be skipped. To this end, some embodiments are based on the goal of giving all past measurements Z up to time k using Bayesian filtering. 1:k＝ {Z1,···,Z k}, by recursively calculating the posterior density p(ξ k |Z 1:k ) to track the object. In addition, in some embodiments, the posterior density p(ξ k |Z 1:k) Estimate the expansion state ξ with corresponding uncertainty k .

[0082] Figure 3B Shows the truncation boundary according to some embodiments. Figure 3A In these embodiments, truncation boundaries 319, including values ​​for truncation intervals for truncating the probability distribution, are stored in a memory (e.g., memory 206 operatively connected to processor 204), which is configured to execute probability filter 224 that tracks the joint probability of the expanded states. In different embodiments, the memory may store single and / or multiple combinations of values ​​for the truncation intervals that are suitable for different situations.

[0083] Additionally or alternatively, the memory may store other predetermined parameters of the measurement model. For example, in some embodiments, the memory stores structural geometric parameters of the measurement model learned from training data. Examples of structural geometric parameters include a cutoff boundary defining the extent of the cutoff interval and the orientation of the cutoff interval relative to the orientation of the object. Knowing the cutoff boundary reduces the computational complexity of EOT tracking.

[0084] Hierarchical measurement model

[0085] The measurements 110 of the object are affected by noise and only receive reflections from the surface of the object. Therefore, executing a probabilistic filter on the noisy measurements may produce an incorrect estimate of the dilation state and, therefore, incorrect object tracking. To this end, a cutoff interval is determined for a noise-free measurement source (also referred to as a measurement source) such that the probabilistic filter generates a center-truncated Gaussian distribution that fits the noise-free measurement source. Some embodiments are based on the recognition that the probabilistic filter is configured to remove noise from the measurements before evaluating the likelihood of the noise-free measurement source based on the center-truncated Gaussian distribution.

[0086] In less noisy scenarios, some embodiments are based on the recognition that the probability filter is configured to evaluate the likelihood of noisy measurements according to a hierarchical center-truncated Gaussian distribution.

[0087] To remove noise from measurements, some embodiments include the implementation of a hierarchical measurement model that Include hidden measurement source variables In other words, according to some embodiments, the hierarchical measurement model provides the probability parameters of the hidden measurement without noise sources for each measurement. Therefore, mathematically defined as

[0088]

[0089] where H is the kinematic state x k The observation matrix of the position component (object center) is selected below, ρ is the scaling factor, D k Specify density support, is a normalization factor. In some embodiments, the hidden measurement source variable Also known as the noiseless measurement source variant. The probability filter replaces the measurement with the hidden measurement during its execution. Thus, the probability filter generates a centrally truncated Gaussian distribution that fits the noiseless measurement source. In some embodiments, the hidden measurement of each measured noiseless source is used to capture features with higher probability around the edges of an object.

[0090] Figure 4 Show hidden measurement source variables Example 400 where the density support is outside rectangle 404. Density support D k It can be any shape. For example, consider a rectangular shape 404 around the center of the object. The rectangular shape 404 consists of four boundaries Description. In some embodiments, the boundaries correspond to intervals with truncated centers. Truncated Gaussian probability density Centered on the coordinate origin (ρ=0.25, l=4.7, w=1.8, a1=b1=2.14 and a2=b2=0.75).

[0091] Noisy measurements are used to model sensor noise. In some embodiments, the noiseless measurement source is modified with sensor noise. By assuming a Gaussian distribution The conditional distribution of to capture sensor noise, where R k represents the noise covariance matrix.

[0092] To this end, given a hierarchical measurement model (3), some embodiments are based on the recognition that the resulting measurement likelihood (spatial distribution) can be calculated by integrating the measurement source variables

[0093]

[0094] The above measurement likelihood (4) leads to the following total measurement likelihood It can be used in (2) of the update step 306. Figure 4 The resulting measured likelihood 402 is shown where R k= diag([0.09, 0.09]). The hierarchical spatial distribution pushes the measurement likelihood away from the center, being highly similar to the distribution of real-world automotive radar measurements. Additionally, according to some embodiments, due to self-occlusion, the hierarchical truncation model flexibly describes partially observed radar measurements (e.g., only observing a rare part of a vehicle).

[0095] Figure 5A Illustrates an exemplary truncation interval adaptation when an object faces the sensor 500 frontally at 506 or rearwardly at 502 according to some embodiments. For example, consider an object being a vehicle (e.g., vehicle 106) of length "l" 504 and width w 510, and oriented such that its rear 502 faces the sensor 500. The sensor 500 obtains measurements corresponding to a local view of the vehicle (i.e., looking from behind). For example, the truncation interval is given by a < x < b. In some embodiments, the truncation interval includes a starting interval ending at infinity to reflect the orientation of the object relative to the sensor 500, e.g., a < x < ∞. In some other embodiments, the truncation interval includes a starting interval ending at a maximum value, e.g., a < x < n max For the above orientation (looking from behind), the truncation interval of the length 504 of the vehicle ends at infinity on the opposite side 506 (front side) of the vehicle relative to the sensor 500. Subsequently, the corresponding probability density 508 is determined. Alternatively, in some embodiments, the truncation interval of the length 504 of the vehicle ends at a maximum value on the opposite side 506 (front side) of the vehicle relative to the sensor 500, and the corresponding probability density can be determined.

[0096] Figure 5B Illustrates an exemplary truncation interval adaptation when an object is oriented laterally relative to the sensor 500 according to some embodiments. For example, consider an object being the same vehicle as contemplated in Figure 5A but oriented laterally relative to the sensor 500. The sensor 500 obtains measurements corresponding to a local view of the vehicle (i.e., looking from the side). In this case, the truncation interval of the width 510 of the vehicle ends at infinity on the opposite side 512 of the vehicle relative to the sensor. Subsequently, the corresponding probability density 514 is determined. Alternatively, in some embodiments, the truncation interval of the width 510 of the vehicle ends at a maximum value on the opposite side 512 of the vehicle relative to the sensor. For this, some embodiments are based on the recognition that the hierarchical truncation model not only flexibly describes fully observed radar measurements but also flexibly describes partially observed radar measurements.

[0097] Extended Object Tracking (EOT) algorithm

[0098] A Bayesian EOT algorithm is developed based on a hierarchical measurement model for both the kinematic state and the extension state. The Bayesian EOT algorithm is also known as a hierarchical truncated Gaussian random matrix (HTG-RM) algorithm. In some embodiments, the Bayesian EOT algorithm is developed based on a hierarchical measurement model by recursively predicting the expansion state and updating the expansion state and truncation intervals. The truncation intervals are also known as truncation boundaries. Similar to the regular random matrix based approach, it is assumed that both the predicted and updated expansion state densities share the factored form of the kinematic state and the extension state.

[0099] p(ξ k |Z 1:k*) ≈p(x k |Z 1:k* )p(X k |Z 1:k* )..............(5)

[0100] =N(x k ;m k|k* ,P k|k* )IW(X k ;v k|k* ,V k|k* ), where k * =k-1 for expansion state prediction, k * = k for extended state update. In other words, the kinematic state x k is a Gaussian distribution with a predicted / updated mean m k|k* and the covariance matrix P k|k* , and the stretch matrix X k is the inverse Wishart (IW) distribution with v k|k* Degrees of freedom and scaling matrix V k|k* Determine the parameters {m,P,v,V} of these associations for prediction k|k-1 and the parameters {m,P,v,V} used for updating k|k The Bayesian EOT algorithm involves recursively predicting the expansion state and updating the expansion state and truncation interval.

[0101] Forecast of expansionary state

[0102] Figure 6 Schematic diagram showing the expansion state prediction step 304 according to some embodiments. Given a motion model 600

[0103]

[0104] where g(·) is the kinematic motion model, Q k-1 is the process noise w k-1 The covariance matrix, E xrepresents the corresponding transformation matrix (e.g., identity matrix or rotation matrix, depending on x k-1 ). Assume that the posterior density p(ξ k-1 |Z 1:k-1 ) shares the same form of (5) and the transition probability of (6), which is determined by the processor 204 for the predicted state density p(ξ k |Z 1:k-1 )602 Calculate the associated parameters {m, P, v, V} k|k-1 . Predicted state density p(ξ k |Z 1:k-1 )'s associated parameters {m,P,v,V} k|k-1 Given as

[0105] m k|k-1 =g(m k-1|k-1 ),

[0106]

[0107]

[0108]

[0109] Where T s is the sampling time and τ is a maneuver-related constant. In some embodiments, the kinematic state prediction 603 in (7a) and (7b) follows the prediction steps of a standard Kalman filter (KF) if g is a linear model, or follows the extended KF when g is nonlinear. The extended state prediction 603 is given by (7c) and (7d). In some other embodiments, one of a Bayesian filter or a particle filter is used instead of a Kalman filter for the kinematic state and extended state prediction. To this end, some embodiments are based on the recognition that a probabilistic filter executed by the processor 204 iteratively executes the motion model to predict the expanded state.

[0110] Expansion status update

[0111] Figure 7A A schematic diagram illustrating an expansion state update step 318 according to some embodiments is shown. Local observation measurements 322 are obtained after optional scan aggregation 316 and truncation boundary update 318. Based on the surface volume distribution of the automotive radar measurements, two sufficient statistics, namely, the sample mean and the sample spread (variance), can be calculated using a regularized random matrix method. However, using the regularized random matrix method to calculate the sample mean and variance may produce biased estimates of the object's kinematic state and expansion state.

[0112] To correct for these biases, pseudo measurements are developed. Furthermore, in some embodiments, pseudo measurements are used to calculate sample means and variances. In some embodiments, the processor 204 generates 702 pseudo measurements using a hierarchical measurement model. The pseudo measurements are used to calculate sample means and variances. Figure 7B Example pseudo measurements 714 according to some embodiments are shown. The dashed squares represent the truncation boundaries updated according to step 318. Circles 712 represent (possibly aggregated) local observed measurements in object coordinates, and dark dots 714 represent pseudo measurements. The ratio of the number of observed measurements 712 to pseudo measurements 714 is determined by the normalization factor of the truncated Gaussian distribution. k Observational measurements Corresponding to TN Measurement source variable for the distribution

[0113] Assume n c pseudo-measurements From the complementary measure likelihood

[0114]

[0115] Corresponding pseudo measurement source variable Among them D k ∪D c =R 2 If the number of pseudo samples satisfies The ratio of the source variables is

[0116]

[0117] can be viewed as coming from an underlying Gaussian distribution Therefore, the corresponding joint measurement Is from the distribution As a result of the underlying Gaussian distribution, the kinematic state x k and stretched state X k Can be achieved through The sample mean and variance give the first- and second-order sufficient statistics to capture 706.

[0118]

[0119]

[0120] in and

[0121] To this end, some embodiments are based on the goal of calculating the sample mean and variance. To calculate the sample mean and variance 706, the processor 204 generates a pseudo measurement. As from of the sample, then The object state {x k|k ,X k|k} and truncation boundaries From the previous iteration step. In some embodiments, to avoid and The synthetic sampling of and The sample mean of and its second-order moment Using two sufficient statistics, the processor 204 calculates 704 the density p(ξ k |Z k )'s associated parameters {m,P,v,V} k|k The updated density p(ξ k |Z k )'s associated parameters {m,P,v,V} k|k Given as

[0122] m k|k =m k|k-1 +Kε,..........(12a)

[0123] P k|k =P k|k-1 -KH P k|k-1 ,............(12b)

[0124] v k|k =v k|k-1 +n k ,..........(12c)

[0125]

[0126] Where K = P k|k-1 HS -1 , and Similar to the prediction step, the kinematic state x k The update step 708 is a Kalman filter-like update in (12a) and (12b). In addition, the stretch state update 708 in (12c) and (12d) requires two matrices.

[0127]

[0128]

[0129] They are respectively relative to the center of mass of the joint measurement and predicted measurement H m k|k-1(via ε) and joint measurement (via ) is expanded in proportion.

[0130] Figure 7C shows pseudo measurement generation, truncation boundary B according to some embodiments k Schematic diagram of updating and expanding state. Get the initial kinematic state x k , stretch state X k and the cutoff boundary B k Given the observed measurements 716 and the predicted kinematic state x k|k-1 , stretch state X k|k-1 and the cutoff boundary B k|k-1 , pseudo measurements 718 are generated by the processor 204 in iteration 0. Furthermore, in iteration 0, pseudo measurements 718 are used to estimate the kinematic state, dilation state, and truncation boundary, which are represented as and For this, iteration 0 produces and

[0131] In the next iteration (i.e., iteration 1), based on the previous iteration estimate and and observed measurements 716, pseudo measurements 720 are generated by the processor 204. In addition, the pseudo measurements 720 are used by the processor 204 to estimate the kinematic state, the dilation state, and the truncation boundary, which are represented as and Thus, the state and truncation boundaries estimated in iteration 0 are updated. Similarly, in iteration 2, pseudo measurements are generated by processor 204 and and Updated to and (Not shown in the figure.) The processor 204 performs iterations until a termination condition is satisfied. The termination condition may be predefined. In some embodiments, the termination condition is satisfied when the number of iterations is greater than a threshold.

[0132] Truncation boundary update

[0133] Figure 8A Schematic diagram showing the coordinate transformation 322 from ego vehicle coordinates to object coordinates, the sweep aggregation 316 and the online truncation boundary update step 318 according to some embodiments. k|k =[x k|k ,X k|k ], by making the measurement likelihood Maximize to update the truncation boundary B k , where the individual measurement likelihood Depend on Figure 4 The likelihood described in the description of is given by (4). The measured likelihood is defined as four boundaries 404 (e.g., B k Specifically, at iteration t, the measurement at time step k in global coordinates is converted to a local measurement in the object coordinate (OC) system by processor 204 using the updated state estimate from iteration (t-1). k Define density support D k Using the scanned aggregated local measurements 316 from time step k-L+1 to time step k, the maximum likelihood (ML) estimate 318 used by the processor 204 is used to update the B k To this end, the truncation boundary is updated, thereby obtaining an updated truncation boundary. In addition, in some embodiments, the kinematic state and the extension state are updated by the processor 204 using the HTG-RM using the updated truncation boundary and the measurement at time step k. Some embodiments are based on the recognition that the truncation boundary can be accurately estimated using filtered scan aggregate measurements from past scans to update the truncation boundary.

[0134] Figure 8B Filtered sweep aggregation 316 in the object coordinate system according to some embodiments is shown. Filtered sweep aggregation is beneficial when automotive radar measurements are sparse and partially visible due to self-occlusion. Measurements 808 of the object 810 are obtained at time step k-1 in the global coordinate system. Similarly, measurements 812, 814 are obtained in the global coordinate system at timestamps k and k+1, respectively. Furthermore, some embodiments are based on the recognition that given a measurement in the global coordinate system (GC) (denoted by z) and the mean m of the object's kinematic state, the corresponding measurement in OC 816 at iteration t can be obtained as

[0135]

[0136] Where M is the kinematic mean of the available objects m (t-1) Constructed rotation matrix. In addition, All local measurements of the t-th iteration are grouped. At the last T-th iteration, the corresponding local measurements are retained for the scan aggregation. Specifically,

[0137]

[0138] and represents the filtered scan aggregated measurement from time step k. With a sliding window size L, the filtered scan aggregated measurement set is represented as

[0139] Filtered Scanning Aggregation Measurements and new local measurements Grouped into ML estimate of the cutoff bound at iteration t Given by the following formula

[0140]

[0141] in is of the form (4), which involves the normalization factor and cross-sectional area D k , as The ML estimates of the four truncation boundaries require computing integrals over D, and directly solving (16) may be computationally demanding for online updates. To this end, the processor 204 uses an expectation-maximization algorithm to divide the scan aggregate measurements into four clusters, which effectively decomposes the joint ML boundary update into at most four decoupled ML estimates of the truncation boundaries. The updates of the other three truncation boundaries can be similarly implemented by the processor 204. In the following, the notation of the iteration index t is omitted for simplicity. It should be noted that the truncation boundary can be set to +∞ when its corresponding measurement set is empty. Let f(y1)=TN(0,Λ 1,1 ,b k,1 ) and f(r1)=(0,R 1,1 ) respectively represent the density support {y|y>b k,1} is a univariate truncated Gaussian distribution with zero mean and variance R 1,1 The probability density function (PDF) of the Gaussian distribution is . Using the convolution formula, the density of z1=y1+r1 is given by

[0142]

[0143] where φ(·) represents the cumulative density function (CDF) of the standard Gaussian distribution, is the transformation object stretching matrix in OC, ζ 1,1 =Λ 1,1 +R 1,1 The decomposed ML estimator then measures the set The likelihood of maximizing is

[0144]

[0145] where z1 is given by the x-coordinate of z. This is equivalent to minimizing the following cost function

[0146]

[0147] This can be solved efficiently using standard root-finding algorithms (eg, Halley's method).

[0148] Figure 9AA simulation of a scenario in which an object moves for 90 time steps during a turn according to some embodiments is shown. The center of rotation of the object coincides with the physical center of the object. In some embodiments, the object is a rectangular object, for example, 4.7 meters long and 1.8 meters wide. The kinematic state of the object is defined as x k =[p k ,v k ,θ k ,ω k ] T ∈R 5 , with a two-dimensional position p k ∈R 2 , extreme speed v k , heading / orientation θ k and the turning rate ω k The stretched state of the object is determined by the position p k The symmetric positive definite covariance matrix X of k Since the object is a rigid body, its width (w) and length (l) are estimated using the eigenvalue decomposition of the estimated stretched state:

[0149] X k =M(θ k )diag([l 2 / 4,w 2 / 4])M T (θ k ),

[0150] Where M(θ k) is the object orientation (θ k ) is the identity matrix of the function.

[0151] In some embodiments, a nearly constant turn rate and extreme velocity motion model is used, where the sampling time T s = 1s and the standard polar and angular acceleration noise are and The transformation function E(·) is a rotation matrix that depends on the turning rate, i.e., E(x k )=M(ω k ). In addition, the process of obtaining simulation trajectory.

[0152] Figure 9B A performance evaluation graph using an idealized measurement model is shown, according to some embodiments. Consider the ideal case where the automotive radar measurements follow a hierarchical truncated Gaussian model during the simulation trajectory. Furthermore, in some embodiments, the number of measurements at each time step is derived from a Poisson distribution with mean 8. Figure 9ATwo snapshots of synthetic automotive radar measurements around an object 900 are shown, which are a good indicator of real-world radar measurements. As can be seen, most of these radar measurements appear to be around the object edge 902. From 100 Monte Carlo runs, the object tracking performance is compared between a regular random matrix (denoted as RM) and a layered truncated Gaussian random matrix (referred to as HTG-RM) algorithm. Figure 9B The object tracking performance in terms of localization error (position), object length and width error for a total of 90 time steps using the ideal measurement model is shown. Figure 9B It is clear that the HTG-RM algorithm outperforms the regularized random matrix method in several aspects. Specifically, the HTG-RM algorithm provides more consistent estimates of the object's length and width over time. In addition, the corresponding root mean square error (RMSE) of the object's kinematic state and extension state estimates are analyzed.

[0153] Figure 9C Table 1 shows the RMSE of the kinematic and extension state estimates of an object using the ideal measurement model, RM, and HTG-RM. The root mean square error of the kinematic and extension state estimates of the object under HTG-RM is significantly smaller than that under RM. Specifically, HTG-RM produces significantly smaller RMSE in the estimates of the object's length, l, and width, w, compared to RM.

[0154] Figure 10A Figure 1 shows the performance evaluation under model mismatch according to some embodiments. In real time, no measurement model can perfectly describe real-world automotive radar measurements. To this end, some embodiments are based on the recognition that to evaluate the robustness of the HTG-RM algorithm under model mismatch, a variational radar model of 50 Gaussian mixture components learned from aggregated real-world automotive radar measurements is fit to the Figure 9A Radar measurements are generated during the process. Figure 10A The object tracking performance in terms of localization error (position), object length and width error for a total of 90 time steps under model mismatch is shown.

[0155] from Figure 10A Obviously, the HTG-RM algorithm still outperforms the conventional RM method. Figure 9B The performance of HTG-RM degrades slightly compared to the ideal measurement model in

[15] , demonstrating the robustness of the HTG-RM algorithm on different surface volume measurement models. This is further confirmed by comparing the RMSE of the kinematic and extension state estimates of RM with the RMSE of the kinematic and extension state estimates of HTG-RM.

[0156] Figure 10Bis a table showing the RMSE of the kinematic state and extension state estimation of the object corresponding to RM and HTG-RM under model mismatch. Compared with the kinematic state and extension state estimation of the object under RM, the RMSE of the kinematic state and extension state estimation of the object under HTG-RM is significantly smaller. It is obvious from the table that, in particular, the RMSE of the object length and width estimation under HTG-RM is significantly smaller than that under RM.

[0157] Figure 11A A schematic diagram of a vehicle 1100 is shown, which includes a controller 1102 in communication with a system 200 employing the principles of some embodiments. Vehicle 1100 can be any type of wheeled vehicle, such as a passenger car, bus, or rover. In addition, vehicle 1100 can be an autonomous or semi-autonomous vehicle. For example, some embodiments control the movement of vehicle 1100. Examples of movement include lateral movement of the vehicle controlled by a steering system 1104 of vehicle 1100. In one embodiment, steering system 1104 is controlled by controller 1102. Additionally or alternatively, steering system 1104 can be controlled by a driver of vehicle 1100.

[0158] In some embodiments, the vehicle may include an engine 1110, which may be controlled by the controller 1102 or by other components of the vehicle 1100. In some embodiments, the vehicle may include an electric motor in place of the engine 1110, and may be controlled by the controller 1102 or by other components of the vehicle 1100. The vehicle may also include one or more sensors 1106 to sense the surrounding environment. Examples of sensors 1106 include rangefinders such as radar. In some embodiments, the vehicle 1100 includes one or more sensors 1108 to sense its current motion parameters and internal state. Examples of one or more sensors 1108 include a global positioning system (GPS), an accelerometer, an inertial measurement unit, a gyroscope, an axis rotation sensor, a torque sensor, a deflection sensor, a pressure sensor, and a flow sensor. The sensors provide information to the controller 1102. The vehicle may be equipped with a transceiver 1110, which enables the controller 1102 to communicate with the system 200 of some embodiments via a wired or wireless communication channel. For example, through transceiver 1110 , controller 1102 receives control input from system 200 .

[0159] Figure 11BA schematic diagram illustrating the interaction between controller 1102 and controller 1112 of vehicle 1100 according to some embodiments. For example, in some embodiments, controller 1102 of vehicle 1100 includes steering control 1114 and brake / throttle controller 1116, which control the rotation and acceleration of vehicle 1100. In this case, controller 1102 outputs control commands to controllers 1114 and 1116 based on control inputs to control the kinematic state of the vehicle. In some embodiments, controller 1112 also includes a higher-level controller, such as lane keeping assist controller 1118, which further processes the control commands from controller 1102. In both cases, controller 1112 utilizes the output of controller 1102 (i.e., control commands) to control at least one actuator of the vehicle (e.g., the vehicle's steering wheel and / or brakes) to control the vehicle's motion.

[0160] Figure 11C A schematic diagram of an autonomous or semi-autonomous controlled vehicle 1120 using some embodiments to generate control inputs is shown. The controlled vehicle 1120 can be equipped with the system 200. In some embodiments, the controlled vehicle 1120 tracks various obstacles 1122 and then generates control inputs based on the obstacle tracking. In some embodiments, the control inputs include commands specifying values ​​for one or a combination of the steering angle and rotational speed of the vehicle's wheels, and measurements include values ​​for one or a combination of the vehicle's rotation rate and the vehicle's acceleration.

[0161] The generated control inputs are intended to keep the controlled vehicle 1120 within certain boundaries of the road 1124 and to avoid other uncontrolled vehicles (i.e., obstacles 1122 to the controlled vehicle 1120). For example, based on the control inputs, the autonomous or semi-autonomous controlled vehicle 1120 may overtake another vehicle, for example, on the left or right, or instead stay behind another vehicle within its current lane on the road 1124.

[0162] Use machine learning to estimate the geometric parameters of the measurement model using an offline training dataset

[0163] Some embodiments are based on an understanding of the complexity of the spatial characteristics of real-world automotive radar measurements. This complexity is partially alleviated by introducing a hierarchical truncated Gaussian (HTG) measurement model, which uses a truncated Gaussian distribution to represent the complexity of the measurement. In fact, the HTG measurement model can appropriately approximate the spatial distribution of real-world automotive radar measurements on the vehicle. It can also be integrated into RM-based methods through a modified state update step and online truncation boundary estimation for full view measurement and / or partial view measurement due to object self-occlusion. However, some embodiments are based on another realization: the boundary estimation in the state update step can be offloaded by utilizing a large-scale offline training dataset so that the truncation boundaries and other parameters associated with the HTG measurement model can be learned from the training data.

[0164] Figure 12 A flow chart illustrating a method for learning geometrically dependent structural model parameters according to some embodiments is shown. Examples of structural geometric parameters include one or a combination of the truncation boundary of the HTG measurement model, its orientation, and a scaling factor. In some implementations, state-dependent parameters such as the kinematic mean and the stretch covariance matrix in the measurement likelihood are not part of the learned parameters.

[0165] In these embodiments, the measurement model includes a hierarchical truncated Gaussian model with structural geometric parameters learned from training data. The training data 1210 includes different measurements of different motions of different objects. To resolve inconsistencies in the training data, machine learning is used to learn 1230 the structural geometric parameters by transforming 1220 the training data 1210 into a common coordinate system. In this way, embodiments overcome the problem of lack of training data for machine learning.

[0166] Figure 13A 、 Figure 13B and Figure 13C Schematic diagram illustrating the transformation of training data collected from different motions of different objects into a common unit coordinate system, in accordance with some embodiments. Measurements collected from tracking different trajectories 1310 and 1315 are transformed into object-centered (OC) coordinate systems 1320 and 1325. The transformed measurements are then aggregated 1330. In some implementations, measurements are collected for motions of similar types of objects (e.g., motions from similar classes of vehicles). For example, for each trajectory, an embodiment transforms measurements from each time step from global coordinates (GC) to object-centered (OC) coordinates and aggregates OC measurements from all trajectories of similarly sized vehicles (e.g., cars).

[0167] Next, if Figure 13BAs shown, embodiments transform the aggregated OC 1330 measurements to a unit coordinate (UC) system 1340. In some implementations, transforming the UC system is performed by various normalization techniques that allow machine learning using the transformed training data. Figure 13C An example 1350 of aggregated measurements from a real-world dataset (nuScenes) in a UC system used by some embodiments is shown.

[0168] As mentioned above, some embodiments focus on the estimation of EOT in a probabilistic manner. k =(x k ,X k ), where x k represents the kinematic state, X k Is a symmetric positive definite matrix, representing the extended state. The kinematic state x k =[p k,x ,p k,y ,φ k ,v k ,ω k ] T Including p k,x and p k,y Given the position and heading φ of the object center k , speed v k and yaw angular velocity ω k For an object with a rectangular shape (e.g., a vehicle), its length l and width w can be obtained from X k The characteristic decomposition of is obtained.

[0169] Example probability EOT estimate at each time step k receives n k Measurements The goal of object tracking is to give all measurements Z up to and including the current time step k 1:k ={Z1,…,Z k}, recursively calculate the object state p(ξ using Bayesian estimation k |Z 1:k ). Then we can get the posterior density p(ξ k |Z 1:k ) extracts the object state ξ with the corresponding uncertainty measure k .

[0170] The posterior density p(ξ) at a given time step k-1 k-1 |Z 1:k-1 ) and the transfer density p(ξ k |ξ k-1 ), the predicted density is given by the prediction step

[0171] p(ξ k |Z 1:k-1)=∫p(ξ k-1 |Z 1:k-1 )p(ξ k |ξ k-1 )dξ k-1 . (twenty one)

[0172] Then measure Z using Bayes' rule k To update the density,

[0173] p(ξ k |Z 1:k )∝p(ξ k |Z 1:k-1 )p(Z k |ξ k ), (twenty two)

[0174] in is the joint measurement likelihood, where Some implementations approximate the prediction and posterior state densities so that they all have the same functional form, which allows the prediction and update functions to be used recursively.

[0175] Hierarchical truncated model

[0176] Some embodiments use a hierarchical truncated Gaussian (HTG) spatial distribution to model noisy automotive radar measurements. Some embodiments learn this distribution from offline training data. These embodiments are based on the understanding that measurements are affected by noise. Therefore, measurements can be represented as including a measurement source modified by noise. This representation allows the structural geometric parameters of the hierarchical truncated Gaussian model to be learned from the measurement source after noise removal.

[0177] Figure 14 Schematic diagram showing the evolution of the formation of an HTG model according to some embodiments. For example, each detection point z 1410 is modeled as a noisy measurement of a noise-free measurement source y. The distribution p(z|y) that models the sensor noise is a Gaussian N(z;y,R), where R is the measurement noise covariance matrix. The distribution p(y|ξ) that models the spatial characteristics of the measurement source is a truncated Gaussian of the form

[0178]

[0179] where ρ is the scaling factor, D is the density support, and 1 D (·) is the indicator function of D, c D is a normalization factor such that (23) integrates to 1. Note that for simplicity we use the general D and c D To symbolically represent the cross-sectional area and normalization constant involved in the equation, but D and c D May change from equation to equation.

[0180] The cutoff area 1420 may be completely specified by the object center h(x), the cutoff boundaries B = [a1, a2, b1, b2], and the orientation θ relative to the horizontal axis. One or more cutoff boundaries may be set to infinity to model partial view measurements due to self-occlusions. Additionally, it is noted that in some implementations, the orientation of the cutoff area need not be aligned with the object orientation. This results in better modeling of features where wheels and wheel covers are typical measurement sources. An illustrative example of p(y|ξ) is described by the cutoff 1420. The resulting measurement space distribution may be computed by marginalizing the measurement sources,

[0181]

[0182] For a given truncated Gaussian p(y|ξ) in 1420 , the resulting spatial distribution p(z|ξ) 1430 is defined by a given noise covariance matrix R .

[0183] Learning a hierarchical truncated measurement model

[0184] In some embodiments, the object state may be used as ground truth, and the training data used to learn the model includes a set of N two-dimensional cumulative data points in a global coordinate system. It is generated by HTG with given parameter values. The parameters of the model are unknown, so the goal is to estimate the parameters given the available data. The maximum likelihood (ML) estimate can be obtained by comparing the training data Z GC The parameters conditioned on it are obtained by maximizing the joint measurement likelihood.

[0185] Figure 15A Schematic illustration of object states, ego vehicle, and ego vehicle (VC), sensor (SC), and object (OC) coordinate systems used by some embodiments is shown.

[0186] Figure 15B A flowchart illustrating an offline spatial model learning workflow for estimating parameters of a model for tracking a vehicle, according to some embodiments. To obtain a hierarchical truncated model that well describes the spatial characteristics of automotive radar detections from vehicles, embodiments collect data samples 1510 to account for vehicles of different sizes and with different poses. To avoid collecting data in such a complex space, one embodiment applies a dimensionality reduction technique. Specifically, radar detections in the global ego vehicle coordinate system are first transformed to the object coordinate system to obtain 1520. Then, the normalized unit coordinate (UC) system The measurement in 1530 is given by

[0187] z UC =[2 / l,2 / w]zOC , (25)

[0188] After dimensionality reduction, the locations of all vehicle detections are transformed into a normalized object coordinate system, which is independent of object states, e.g., position, length, and width.

[0189] Next, an embodiment performs an offline ML estimation of the spatial model parameters 1540. The ML estimation of the coordinate transformed training data is given by

[0190]

[0191] where I2 is the identity matrix. Computing (26) involves evaluating the convolution of a binary truncated Gaussian and a binary Gaussian, which is generally intractable. To obtain a tractable solution to (26), some embodiments use the noise covariance R in normalized object coordinates of the form OC

[0192] R OC =M(θ)diag([r1,r2])M(θ) T , (27)

[0193] where M(θ) is the clockwise rotation matrix. Then obtaining the ML estimate of (26) is equivalent to obtaining

[0194]

[0195] The embodiment further measures Z UC Transform to the new coordinate system so that the orientation of the cut-off areas is now aligned with the axes and the transformed noise covariance is a diagonal matrix.

[0196] With the new parameterization, the above cost function for ML estimation can be calculated as the product of two unary Gaussians, and thus decoupled. Specifically, Equation (28) can be rewritten by applying the convolution formula as

[0197]

[0198]

[0199] So that ρ∈(0,1],θ∈[-π / 2,π / 2],{B,r1,r2}∈[0,∞),

[0200] in

[0201]

[0202] c D =1-((Φ(b1ρ -1 / 2 )-Φ(-a1ρ -1 / 2 ))×(Φ(b2ρ -1 / 2)-Φ(-a2ρ -1 / 2 ))),

[0203]

[0204] β(r)=r 1 / 2 ρ 1 / 2 (r+ρ) -1 ,

[0205]

[0206] where Φ(·) denotes the cumulative density function (CDF) of the standard normal distribution. ML estimates for all or some of the unknown parameters 1550 may be found, for example, using coordinate descent optimization, which iteratively determines a coordinate (here one of the unknown parameters) and minimizes the cost function of (29) at the selected coordinate while fixing all other coordinates with appropriate constraints (e.g., that the bound B is non-negative).

[0207] Dependence of the measurement model on the viewing angle

[0208] Automotive radar measurements may be reflected from parts of vehicles that are within the field of view of the radar sensor. To account for this self-occlusion feature, some embodiments divide the training dataset into groups based on the field of view annotated in the training dataset.

[0209] φ'=φ SC -atan2(p y,SC ,p x,SC ), (30)

[0210] where φ SC and [p x,SC ,p y,SC ] T denote the object orientation and center position in the sensor coordinate system, respectively. Then, a conditional measurement model for the field of view angle can be learned using the above ML estimation. As used herein, the field of view angle in the sensor coordinate (SC) system is the relative angle of the object with respect to the sensor itself. As an illustrative example, Figure 15A The field of view angle in the SC system is given by the angle α. For offline learning, since we transform the automotive radar measurements to the unit coordinate (UC) system, the field of view angle in the UC system is the angle in the OC system The difference from the angle α in the SC system.

[0211] Figure 16A and Figure 16BAn example of learning measurement models for different view angles according to some embodiments is shown. For example, model 1620 is learned from measurement 1610, while model 1640 is learned from measurement 1630. Measurements 1610 and 1630 are partial view measurements corresponding to different types of occlusions (e.g., annotated by different view angles).

[0212] In this way, different values ​​of the cutoff interval are predetermined for different types of occlusion of the object relative to the sensor, which allows the processor to update the selection of a current value of the cutoff interval in response to detecting a change in the occlusion type.

[0213] Figure 17 A block diagram of a method for tracking the extended state of an object according to one embodiment is shown. In this embodiment, a memory stores 1710 multiple learned spatial models and associated parameters from 1550 for different view angles learned from training data. Within the current iteration of the probability filter execution, the embodiment converts 1740 the object orientation angle (an element in the extended state) to the corresponding view angle and selects 1720 the learned spatial model from the memory for the updated view angle. Together with the new measured and predicted extended state, the embodiment uses the selected spatial model to calculate the likelihood function and update 1730 the extended state. For the next iteration, the object orientation angle in the extended state is converted again 1740 and the above process is repeated.

[0214] Random Matrix Methods for Learning Hierarchical Truncated Models

[0215] In various embodiments, as described above, the probabilistic filter iteratively uses the motion model to predict the dilation state and uses the measurement model to update the dilation state predicted by the motion model using a random matrix (RM) update of the dilation state of the object. Additionally or alternatively, some embodiments modify the RM method using a learned hierarchical truncation model. The modified RM method replaces the online boundary update with a learned spatial model. Specifically, the modified RM uses a new RM state update step. It is assumed that both the predicted density and the posterior density have the following factorized form

[0216]

[0217] Where k'∈{k-1,k}. Kinematic state x k is a product with mean m k|k' and the covariance matrix P k|k' Gaussian distribution, and the stretch matrix X k is with ν k|k' Degrees of freedom and scaling matrix V k|k' The inverse Wishart distribution of .

[0218] Figure 18Block diagram showing random matrix (RM) updates of the expanded state of a moving object according to some embodiments. During the prediction step 1804, the state transition density is approximated as the product of a Gaussian distribution and a Wishart distribution

[0219]

[0220] where g(·) represents the (nonlinear) motion model, Q represents the process noise covariance, and E x represents the transformation matrix, usually the rotation matrix depending on the kinematic state x. Given the state transition density (32) and the posterior density p(ξ) in (31), k-1 |Z 1:k-1 ), p(ξ k |Z 1:k-1 )’s predicted parameters {m,P,v,V} k|k-1 for

[0221] m k|k-1 =g(m k-1|k-1 ), (33)

[0222]

[0223]

[0224]

[0225] in T s is the sampling time and τ is a maneuver-related constant. The kinematic state predictions in (33) and (34) follow the prediction steps of the (nonlinear) Kalman filter, while the extension state predictions are given by (35) and (36).

[0226] Since the learned HTG measurement model 1819 depends on the object state, the object state is updated recursively 1820. The complete state update step for the learned hierarchical truncated model iteratively runs 1830 on two building blocks: 1) selecting the learned hierarchical truncated model conditioned on the view angle, and 2) updating the object state using the transformed measurement statistics until the convergence criteria are met. From one iteration to the next, a refined HTG model is obtained by making the object state estimate more accurate.

[0227] Specifically, at iteration t, some embodiments first use the updated object state estimated at iteration (t-1) to calculate the horizon angle (10), and then select the learned HTG measurement model. The RM method uses the statistics (mean and spread) of the Gaussian distribution of measurements to update the predicted state density. In order to integrate the HTG measurement model into the state update step of RM, an effective way is to construct the Gaussian measurement statistics using the HTG distribution of measurements.

[0228] The process is given as follows. The implementation method first calculates the density support of the truncated Gaussian distribution Next, the implementation uses the The learned sensor noise covariance R OC The pseudo-measurement statistics calculated are transformed from the normalized object coordinate system to the global coordinate system. Embodiments may then take a weighted sum of the transformed pseudo-measurement mean / spread and the received measurement mean / spread to obtain the transformed Gaussian measurement mean and expand Given the learned HTG model and the predicted density p(ξ k |Z 1:k-1 ), determine the updated parameters {m,P,v,V} of the posterior density k|k is given as:

[0229] m k|k =m k|k-1 +Kε, (37)

[0230] P k|k =P k|k-1 -KHP k|k-1 , (38)

[0231]

[0232]

[0233] in

[0234]

[0235]

[0236]

[0237]

[0238] At the first iteration, we can use the state estimate predicted at time step k by setting To initialize the algorithm.

[0239] Compared to the previous update step in the unmodified RM, the update step of the learned HTG model directly uses the offline learned model parameters B, θ, ρ, and R. This makes the new update step computationally simpler because it skips the jump boundary update and converges faster. By incorporating the learned measurement model into the update step, the proposed method also exhibits improved EOT capabilities with sparse measurements.

[0240] The following 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 exemplary embodiments will provide those skilled in the art with a feasible description for implementing one or more exemplary embodiments. It is contemplated that various changes may be made to the function and arrangement of elements without departing from the spirit and scope of the disclosed subject matter set forth in the appended claims.

[0241] Specific details are given in the following description to provide a thorough understanding of the embodiments. However, it will be understood by those skilled in the art that the embodiments may be practiced without these specific details. For example, the systems, processes, and other elements of the disclosed subject matter may be shown as components in block diagram form to avoid obscuring the embodiments in terms of unnecessary details. In other cases, well-known processes, structures, and techniques may be shown without unnecessary details to avoid obscuring the embodiments. In addition, similar numbers and designations in the various figures indicate similar elements.

[0242] In addition, various embodiments may be described as processes, which may be depicted as flow charts, data flow diagrams, structure diagrams, or block diagrams. Although a flow chart may describe operations as a sequential process, many operations may be performed in parallel or simultaneously. In addition, the order of operations may be rearranged. A process may terminate when its operations are completed, but may have additional steps not discussed or included in the accompanying drawings. Furthermore, not all operations in any particular described process may appear in all embodiments. A process may correspond to a method, function, procedure, subroutine, subprogram, etc. When a process corresponds to a function, the termination of the function may correspond to the function returning to the calling function or the main function.

[0243] Furthermore, 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 that perform the desired tasks may be stored in a machine-readable medium. A processor may perform the desired tasks.

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

[0245] The embodiments of the present disclosure may be embodied as a method, an example of which has 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 an order different from that shown, which may include performing some actions simultaneously, although shown as sequential actions in the illustrative embodiments.

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

Claims

1. A system for tracking an expansion state of a moving object, the expansion state of the moving object comprising a kinematic state indicating a position of the object and an extended state indicating one or a combination of a size and an orientation of the object, the system comprising: at least one sensor configured to detect a scene including a moving object using a plurality of signal transmissions to generate a plurality of measurements of the object per transmission; a memory configured to store a value of a truncation interval for truncating a probability distribution; a processor configured to execute a probability filter that tracks a joint probability of the expanded state of the object estimated by a motion model of the object and a measurement model of the object, wherein the measurement model includes a central truncated distribution with a truncation interval, providing a smaller probability for the measurement at the center of the central truncated distribution within the truncation interval and a larger probability for the measurement outside the truncation interval, wherein the central truncated distribution is a truncation of an underlying untruncated distribution according to the truncation interval, wherein the probability filter is configured to estimate the central truncated distribution that fits the measurement and generate a mean and variance of the underlying untruncated distribution corresponding to the central truncated distribution, such that the mean of the underlying untruncated distribution indicates the position of the object in the expanded state and the variance of the underlying untruncated distribution indicates the size and orientation of the object in the expanded state; and An output interface is configured to output the expansion state of the object.

2. The system according to claim 1, wherein: The measurement model comprises a hierarchical truncated Gaussian model with structural geometric parameters learned from training data, wherein the structural geometric parameters include the truncation interval, wherein the probability filter is one or a combination of a Bayesian filter, a Kalman filter, and a particle filter, and wherein the joint probability is a posterior probability conditioned on the dilated state estimated during a previous iteration of the probability filter.

3. The system according to claim 2, wherein: The training data comprises different measurements of different motions of different objects, and wherein the structural geometric parameters are learned using machine learning by transforming the training data into a common unit coordinate system.

4. The system according to claim 2, wherein: The structural geometric parameters learned from the training data include truncation boundaries defining the extension of the truncation interval, and the orientation of the truncation interval relative to the orientation of the object.

5. The system according to claim 4, wherein: The measurements are affected by noise such that the measurements comprise a measurement source modified with noise, and wherein the structural geometric parameters of the hierarchical truncated Gaussian model are learned from the measurement source after removing the noise; or Wherein, the noise is defined by a noise covariance matrix learned from the training data, so that the hierarchical truncated Gaussian model includes a central truncated distribution expansion according to the noise covariance matrix.

6. The system according to claim 3, wherein: The memory stores in the training data a plurality of measurement models with different values ​​of the truncation interval learned for different view angles, and wherein, within a current iteration of execution of the probability filter, the processor selects from the memory a current measurement model with a current value of the truncation interval based on the measurements collected during a previous iteration of the probability filter, and updates the dilated state with the current measurement model of the central truncated distribution with the current value of the truncation interval.

7. The system according to claim 6, wherein: predetermining the different values ​​of the cutoff interval for different types of occlusion of the object relative to the sensor, such that the processor updates selection of the current value of the cutoff interval in response to detecting a change in the type of occlusion; or At least one truncation interval stored in the memory is a start interval ending in infinity.

8. The system according to claim 3, wherein: The motion model predicts the expanded state of the object subject to a fixed value of the object's size and a changing orientation of the object, such that the size of the object is updated only by the measurement model, while the orientation of the object is updated by both the motion model and the measurement model.

9. The system according to claim 1, wherein: Execution of the measurement model outputs a covariance matrix that fits the measurements, wherein diagonal elements of the covariance matrix define dimensions of the object, and wherein off-diagonal elements of the covariance matrix define orientations of the object; or Wherein, the underlying untruncated distribution is a Gaussian distribution and the center-truncated distribution is a center-truncated Gaussian distribution.

10. The system according to claim 1, wherein: The probabilistic filter iteratively uses the motion model to predict the dilated state and uses the measurement model to update the dilated state predicted by the motion model with a random matrix-based update of the dilated state of the object.

11. The system according to claim 10, wherein: The random matrix-based updating iteratively updates the dilated state of the object to converge to a statistic of the measured distribution.

12. The system of claim 1, wherein: The measurements are subject to noise defined by a noise covariance matrix, wherein the cutoff interval is determined for a measurement source without noise, and wherein the probability filter is configured to remove the noise from the measurements before evaluating the likelihood of a noise-free source for the measurements based on the central cutoff distribution, such that the probability filter generates the central cutoff distribution that fits the noise-free source for the measurements.

13. The system of claim 1, wherein: The measurement is affected by noise, wherein the truncation interval is determined directly for the measurement, and wherein the probability filter is configured to evaluate the likelihood of the measurement based on a stratified central truncated distribution such that the probability filter generates the stratified central truncated distribution that fits the measurement.

14. The system of claim 1, wherein: The object is a vehicle, and wherein the updating of the extended state comprises a scanned aggregation of measurements transformed from a global coordinate system to a local coordinate system of the vehicle.

15. The system of claim 14, wherein: The memory is configured to store a model of the vehicle, the model of the vehicle comprising the motion model of the object affected by process noise and the measurement model of the object affected by measurement noise, such that a combination of the process noise and the measurement noise bounds an uncertainty of the expanded state of the vehicle; and wherein the processor is configured to determine a control input of a controller of the vehicle using the model of the vehicle of the expanded state with bounded uncertainty, and to control the vehicle according to the control input.

Images