Method for estimating a constrained zonotope encompassing states describing the motion of at least one moving object governed by a nonlinear model
The use of constrained zonotopes in set membership estimation addresses the inconsistency issues of UKF and EKF by providing a guaranteed and computationally efficient method for accurate state estimation in autonomous vehicle systems, enhancing safety and precision in collaborative movements.
Patent Information
- Application Number
- JP2021566225
- Authority / Receiving Office
- JP · JP
- Patent Type
- Patents
- Current Assignee / Owner
- Priority Date
- 2019-05-07
- Filing Date
- 2020-05-07
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2040-05-07
AI Technical Summary
Existing state estimation methods for autonomous vehicles, such as the Unscented Kalman Filter (UKF) and Extended Kalman Filter (EKF), fail to guarantee filter consistency due to approximations in nonlinear estimation problems, especially when calculating filter state uncertainty, which is crucial for safe cooperative movement of multiple agents in close proximity.
A method using constrained zonotopes for set membership estimation, which are closed under crossover operations and allow for better approximation of asymmetric shapes, ensuring accurate and consistent state estimation by linearizing nonlinear models and incorporating observation data while managing computational complexity through simplification processes.
The method provides a guaranteed envelope of vehicle states with reduced conservatism and computational efficiency, enabling precise relative positioning for autonomous vehicles, even in heterogeneous environments, by using constrained zonotopes that are closed under Minkowski sum operations and allow for tighter state envelopment.
Smart Images

Figure 0007589950000143 
Figure 0007589950000144 
Figure 0007589950000145
Abstract
Description
[Technical field]
[0001] The present invention relates to the technical field of set membership state estimation techniques.
[0002] The present invention deals with a method for estimating a constrained zonotope that includes states describing the motion of at least one moving object governed by a nonlinear model. [Background technology]
[0003] Unmanned vehicles have seen a significant increase in their level of autonomy over the last few years, leading to ongoing efforts to integrate Unmanned Aircraft Systems (UAS) into airspace and autonomous vehicles into general traffic. For some time now, it has been known that cooperative movement strategies unlock several benefits with considerable economic value. Autonomous platooning of ground vehicles can increase highway traffic capacity, avoid traffic congestion, and reduce individual vehicle consumption. Tight formation flying has been demonstrated to enable significant aircraft range enhancements. The most prominent examples are range improvements through the use of wake vortex effects in tight aircraft formations and automatic air-to-air refueling. To execute these cooperative movement strategies safely and efficiently, the relative positions of the vehicles need to be estimated to decimeter, if not centimeter, level accuracy.
[0004] For tight formation flying, standalone positioning based on Global Navigation Satellite Systems (GNSS) does not provide the required accuracy, so various differential positioning techniques have been proposed. In the literature from the last decade, one can find many sensor fusion methods using inertial measurement units, differential GNSS with or without phase information, machine vision, or a combination of both. Recently, inexpensive decimeter-precision Ultra-Wideband (UWB) ranging sensors have been successfully shown to improve the robustness of airborne real-time kinematic (RTK) solutions.
[0005] Common among these existing approaches is the reliance on probabilistic filtering techniques such as the Unscented Kalman Filter (UKF) or Extended Kalman Filter (EKF). As is well known, many practical nonlinear estimation problems violate the underlying assumptions of both UKF and EKF, in particular the key assumption that the distributions of uncertainties in the process model and the observation model are known. In reality, usually only approximations of the probabilistic nature of both sources of uncertainty are available. Despite this inherent and well-known problem, both EKF and UKF have been demonstrated to work well in practice and provide accurate mean state estimates.
[0006] However, for safe cooperative motion of multiple agents in close proximity, consistent computation of filter state uncertainties is much more important than accurate point estimation. Due to the approximations involved (linearization steps performed by EKF and unscented transformations of UKF), guaranteeing filter consistency is an open problem for nonlinear estimation problems when using EKF or UKF. However, we consider an algorithm that can compute guaranteed envelopes of vehicle states, which is a key element for further safe integration of autonomous vehicles into heterogeneous environments such as mixed air traffic and common road traffic.
[0007] Set membership state estimation techniques offer an interesting alternative to probabilistic frameworks in this regard. Set membership estimators rely on set-valued process models, assuming that the uncertainties of the observations and the process model are contained in known compact sets, i.e. intervals. No assumptions are required about the error distribution within these sets. Exact set membership estimation problems are usually computationally intractable for nonlinear systems. Research has been done on the feasibility of implementing the set operations involved. Most of the work has been directed at finding a tight outer approximation. Over the last decade, many algorithms have been proposed that exploit set shapes such as ellipsoids or intervals. A common problem faced by all set membership estimators consists of finding a trade-off between conservatism - the degree of overapproximation introduced by representing a complex set in a simpler, more computationally tractable way - and the computational effort.
[0008] Despite the merits of set membership methods, their application to agent relative localization has received little attention. A ground vehicle collaborative set membership localization algorithm based on the Set Inversion Via Interval Analysis algorithm (SIVIA) has been proposed. SIVIA and related nonlinear set inversion algorithms provide tight envelopes when applied to state estimation algorithms, with the degree of approximation limited only by the available computational resources. However, the complexity of these algorithms typically grows exponentially with the number of states, limiting their usefulness beyond 2D ground vehicle localization applications.
[0009] Ellipsoids offer good computational performance with constant complexity and have been successfully applied to agent relative localization in simulations. In recent years, zonotopes have received increasing attention for guaranteed state estimation. Zonotopes offer greater flexibility than ellipsoids, and complexity is a design parameter. One issue inherent to zonotopes is symmetry, which introduces additional conservatism whenever the real state set exhibits strong asymmetry. A second issue is that the intersection of two zonotopes does not always result in a zonotope, and therefore, as in the case of ellipsoids, an outer approximation of the intermediate set must be computed at each filter recursion, which potentially involves solving a convex optimization problem. Summary of the Invention [Problem to be solved by the invention]
[0010] The present invention aims to overcome or at least mitigate both of these drawbacks. [Means for solving the problem]
[0011] A first aspect of the present invention is a method according to claim 1.
[0012] The a priori zonotopes used in the method of claim 1 are constrained zonotopes. This class of zonotopes can better approximate asymmetric collection shapes. As an added advantage, this class of zonotopes is closed under intersection and closed under Minkowski addition.
[0013] The method according to the first aspect of the invention may include the following optional features.
[0014] The element at which the nonlinear transition model is linearized may be the center of a minimum interval encircling the first constrained zonotope.
[0015] The method may further include linearizing the nonlinear observation model in an element of the first constrained zonotope or the pre-constrained zonotope, thereby generating a linear observation model, and the observation model used in step e) is the generated linear observation model.
[0016] The element around which the nonlinear observation model is linearized may be the center of the smallest interval encircling the a priori zonotope.
[0017] The method comprises: calculating an observation linearization error at different elements of the first constrained zonotope or the a priori zonotope, where the observation linearization error at a given element of the first constrained zonotope or the a priori zonotope is the difference between an output generated as a result of applying the nonlinear observation model to the given element and an output generated as a result of applying the linear observation model to the given element; calculating a fourth constrained zonotope that includes all of the calculated observation linearization errors, where the a posteriori constrained zonotope depends on the fourth constrained zonotope; It may further include.
[0018] The observational data of the moving object may be acquired at a third instant in time different from the second instant in time, and the method further comprising: dilating the fourth constrained zonotope, thereby obtaining an expanded constrained zonotope forming a strip in the state space, taking into account all possible variations in the state of motion between the second and third instants of time and possibly also the measurement noise induced by the sensor; Compute the post-constrained zonotope as the intersection of the pre-constrained zonotope and the expansion-constrained zonotope; It may further include.
[0019] The method comprises: Identifying the complexity of the post-constrained zonotope; applying a reduction process to the posterior constrained zonotope if the complexity is greater than a threshold, thereby obtaining a posterior zonotope that has a lower complexity than the posterior zonotope before reduction and that contains the posterior zonotope before reduction; It may further include.
[0020] The method may further comprise using the post-constrained zonotope as the first constrained zonotope and using the second instant as the first instant in a subsequent execution of steps a) to e) defined in claim 1.
[0021] The method comprises: Calculating a maximum time interval between a first instant and an instant at which the observation data is acquired by the sensor; applying a linear transition model to the pre-determined constrained zonotope if the maximum time interval is greater than a predetermined duration, thereby generating a pre-constrained zonotope, and otherwise using the pre-determined constrained zonotope as the pre-constrained zonotope; It may further include.
[0022] The observation data may include at least one of the following data acquired by the GNSS receiver: pseudorange, acquired carrier phase, range between the moving object and another moving object.
[0023] The motion data may include motion data of a moving object relative to another moving object.
[0024] The first constrained zonotope may be computed using a vector set inverter algorithm via interval analysis.
[0025] A second aspect of the present invention is a computer program executed by a computer. and a computer program comprising instructions for causing a computer to carry out the method of the first aspect of the present invention. [Brief description of the drawings]
[0026] [Figure 1] 1 illustrates a schematic representation of a device for estimating an envelope of motion states of a moving object. [Diagram 2] 1 illustrates steps of an example method for estimating a C zonotope surrounding a state x of a system. [Diagram 3] 3 illustrates a sequence of operations for the propagation step of the method illustrated in FIG. 2. [Figure 4] 1 is a graphical representation of the target device and beacon position p and distance d to the x-axis. [Diagram 5] 3 shows snapshots of the propagation, update and simplification steps of the method shown in FIG. 2. [Figure 6] 1 is a graphical representation of the true state trajectory of a subject device. [Figure 7] 1 is a graphical representation of the CZESMF encapsulation of a subject device. [Figure 8] 1 is a graphical representation of a location interval perimeter of a target device. [Figure 9] 1 is a graphical representation of a speed interval envelope for a subject device. [Figure 10] 1 is a graphical representation of the location of another target device. [Figure 11] 1 shows the section encapsulation of subpaving obtained by VSIVIA. [Figure 12]The pre-C zonotope is shown along with the acquired and reduced posterior C zonotopes of the target device in FIG. [Figure 13] Pre- and post-C zonotopes are shown when the observation timing noise increases. [Figure 14] 11 shows the complete state encircling trajectory of the subject device of FIG. [Figure 15] 11 illustrates the complete state envelopment trajectory of the subject device of FIG. 10 as observed timing noise increases. [Figure 16] 1 is a graphical representation of two radii of range linearization error interval measurements obtained by two separate techniques. [Figure 17] Represents the positions of two other target devices during the simulation. [Figure 18] 13 represents the ensemble volume obtained by velocity norm prediction and T3D prediction using GNSS updates or GNSS+UWB updates. [Figure 19] 13 shows the relative position set volume obtained using the CZESMF filter and the IESMF filter. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS
[0027] The following description has the following structure: after defining the notation (section 1) and recalling the problem of set membership state estimation (section 2), we describe a computationally efficient state set estimation method based on constrained zonotopes (C-zonotopes) (section 3). Then, three practical applications of the method are given as examples: A simple 1D example (section 4), The slightly more involved 2D localization problem (Section 5), · The 3D peer-to-peer localization benchmark problem using differential GNSS and peer-to-peer ranging observations (Section 6).
[0028] 1. Notations To deal with this complexity, we use the familiar notation x(t l ) is used below. The discreteness of the filter execution is captured by indexing time. Specifically, t k denotes the time at which the a priori and a posteriori state sets are computed by the processing unit. For a vector x(t), the time t denotes the validity time of this vector.
[0029] Correspondingly, the set Z(t k ) at time t k is valid at t=t k If x(t)∈Z(t k ) The effective time is in the interval [t k ], the set is valid for all times in this interval, i.e., t∈[t k ], then x(t)∈Z(t k ).
[0030] In addition, the matrix J nxm denotes a matrix with n rows and m columns whose elements are all 1. The identity matrix of size n is I n A zero square matrix of size n is denoted by 0. n As shown in the figure.
[0031] The dot product of two vectors a and b is<a,b> It is shown as:
[0032] The interval matrix is
number
number
number
number
[0033] When additive error is assumed, it may be more convenient to write the interval matrix in center-range notation as [M]=mid(M)+[-rad(M),rad(M)], where mid(M) is the center point of [M] and the interval radius matrix is
number
[0034] Since the C-zonotope is non-closed under nonlinear transformations, first-order Taylor approximations of the nonlinear system equations are used below: see sections 3.2 and 3.3. To ensure that these approximations do not destroy the consistency of the resulting state envelope, the notion of linear containment is used, as in other set estimation techniques; see for example "A nonlinear set-membership filter for online applications" by E. Scholte et al. A linear containment of a nonlinear function is a set-valued linear function that is guaranteed to contain the solution of the nonlinear function over a particular set of inputs. As an example, we consider a function where x∈X, i.e., x is contained in some compact set X.
number
number
number
number
[0035] 2.Problem statement The motion of a moving object (target) can be described by a motion state that includes motion data. The motion data may include, for example, the target's position, the target's velocity, the target's acceleration, other data, or a combination thereof. The motion state evolves over time as the moving object moves.
[0036] Hereinafter, for the sake of brevity, the moving object 1 may be referred to as the "system" and the motion state may be referred to as the "state".
[0037] With reference to FIG. 1, a device 1 for estimating a set encompassing a motion state of a moving object comprises a processing unit 2 and an observation unit 4 .
[0038] The processing unit 2 is configured to execute a computer program comprising code instructions implementing the estimation method described below, which is hereafter referred to as the "CZESMF Filter" (Constrained Zonotope Extended Set-membership Filter) or "filter" since it actually acts like a filter.
[0039] The filter is configured to use a given transition model (hereinafter sometimes referred to as a "propagation model" or "dynamics model") and a given observation model.
[0040] The processing unit 2 is configured to operate at different processing instants so as to be able to estimate different sets encompassing the motion states of a moving object at said different instants.
[0041] The processing unit 2 can be of any type: one or more processors, one or more microprocessors, an FPGA, an ASIC, etc.
[0042] The observation unit 4 comprises at least one sensor configured to observe the moving object. The observation unit 4 is therefore configured to output observation data related to the moving object. The observation unit is also configured to operate at different observation instants such that different observations can be obtained at said different instants.
[0043] The goal of set membership state estimation is to find a region where the state x(t) of the system is guaranteed to exist at time t.
number
[0044] 2.1. State dynamics In discrete state estimation, the system state dynamics are often expressed as a function of the prior state
number
number
number
number
[0045] In practice, irregular observation times result in irregular sampling times, i.e. irregular propagation horizons in the filters. Depending on the sampling times, different propagation models may generate state envelopes with different tightness. A practical example from navigation is inertial dead reckoning localization, where double integration of the observed accelerations produces a very tight short-term position envelope, but in the long term, sensor biases produce a quadratic growth of the position envelope. Conversely, a simple velocity-norm bounded model of vehicle dynamics (see e.g. Section 5) produces a linear growth of the position envelope over time, and thus in some cases a longer propagation horizon provides a tighter envelope.
[0046] Another example is treated in Section 6, where dense relative position propagation using time-differentiated GNSS phase observations is only available at the sampling time of the GNSS receiver, while only a coarse velocity-norm bounded propagation model is available between GNSS observations.
[0047] In the present invention, a nonlinear mapping
number
number
[0048] Argument t k and t k-1 The model is,t k-1 From k We perform the propagation of the system state over a time horizon up to and show the time-variance of f(). Allowing the propagation model to be time-varying allows the filter to use the least conservative of the set of models available at each propagation step 102 (see FIG. 2).
[0049] 2.2. Observations In practice, the observations made by the observation units 4 are often not perfectly synchronized and not perfectly periodic. A further complication is that the observation times are only roughly known, e.g. due to the lack of hard real-time communication protocols.
[0050] The uncertainty in the observation time, although subtle, poses a serious problem that is not addressed by existing set membership state estimation schemes. The propagation state envelope is k This is called the valid time of a given state envelope.
number
number
[0051] Observation is a nonlinear mapping
number
number
number
[0052] Note that no assumptions are made about the measurement model error other than that it is contained in some known set W(t). Thus, the observation timestamp error encompasses hardware noise, bias, and modeling errors. Note also that the observation model h(...) and the size of the observation vector m(t) can be time-varying.
[0053] Let y(t) denote the observations provided by the model, while the actual measurements received from the sensors are denoted by z(t).
[0054] 3.Extended set-membership filter based on C-zonotopes The most popular and computationally efficient set representations used in set state estimation are linear transformations of basic geometric entities such as the hypersphere, which transforms an ellipsoid, and the hypercube, which transforms a zonotope. The inherent symmetries of both the hypersphere and the hypercube allow for dense approximations of asymmetric or generally irregularly shaped sets. This limits the capabilities of ellipsoids and zonotopes. To make the situation worse, both are non-closed under the crossover operation of the filter update. As a result, it is necessary to compute an outer approximation of the result of each crossover. Constrained zonotopes alleviate both of these problems. A constrained zonotope is a linear transformation of the constrained unit hypercube, and therefore can be asymmetric. It is closed under crossover, i.e. the crossover of two C zonotopes is another C zonotope, but with higher complexity.
[0055] C Zonotope
number
number
[0056] Each constraint represents a plane that intersects the unit hypercube. The resulting intersections are then projected through a generator matrix G and finally translated by a center vector c.
[0057] A C-zonotope is an alternative parameterization of a convex polyhedron. In addition, every unconstrained C-zonotope is a regular zonotope, so a C-zonotope is a superset of regular zonotopes. For brevity, and in accordance with the existing literature, the C-zonotope representation of a convex polyhedron is denoted as G-rep, and its half-space representation is denoted as H-rep.
[0058] The interval envelopment of a C-zonotope Z is denoted by [Z], and the G-rep of interval [a] is denoted by Z([a]).
[0059] Referring to FIG. 2, a method for estimating the C-zonotope that encompasses a system state x is k-1 A given C-zonotope Z that encompasses states in x (t k-1 ), the main steps performed by the filter include: A given C xotope is processed and reaches a time t k A priori C-zonotope that encompasses states in
number
number
[0060] Time t k surrounds state x in t k Including t k The posterior C zonotope, taking into account all observations up to
number
[0061] On the other hand, before the update, at time t k The prior C-zonotope that encloses state x in
number
[0062] As will be detailed below, the propagation step 102 and the update step 104 may use a linear transition model and a linear observation model, respectively, which are obtained by linearizing a non-linear model at a particular point.
[0063] Further, the method further comprises the step of: x (t k ) to the less complex C-zonotope Z' x (t k ) another C zonotope Z x (t k )
[0064] The propagation step 102, the update step 104, and the reduction step 106 are repeated recursively. More precisely, the method includes multiple recursions, each of which performs the propagation step 102, the update step 104, and the reduction step 106.
[0065] The first recursion of the method involves the initial C zonotope
number
[0066] Steps 100, 102, 104, and 106 will now be described in more detail.
[0067] 3.1.Initialization The initial state is some C-zonotope that is the first pre-state encirclement.
number
[0068] 3.2. Propagation In the propagation step 102, which is performed if k>1, the filter is propagated to the pre-state set
number
[0069] The peculiarity of uncertain observations is the question of how to choose the time to compute the prior C-zonotope. When the filter recursion starts in the future, it is even possible to compensate for the filter execution time. However, depending on the execution time of the recursion (propagation and update), the time t k It may be wise to set,close to the observation time and perform a final propagation to,compensate for the execution time.
[0070] Some options are the nominally periodic observation time or the center of the observation time interval if the observations are nominally periodic. An advantageous effect of the first option is that the filter result is periodic, hiding the observation time uncertainty inside the filter.
[0071] Since the C-zonotope is non-closed under a nonlinear mapping, we use the extended Kalman filter (E As with KF), the point of action
number
number
[0072] This choice leads to a smaller linearization error because the center of the C zonotope does not even have to be within the C zonotope, but it does not minimize the linearization error.
[0073] The linearization is done using the familiar first-order Taylor expansion
number
number
[0074] The corresponding linear inclusion is
number
number
[0075] The matrix multiplication and summation operations in Equation 18 are set operations on the C zonotope defined in "Constrained zonotopes: A new tool for set-based estimation and fault detection" by JK Scott et al. The linearization error and model uncertainty of the nonlinear state dynamics model are Z q (t k-1 ) and Z u (t k-1 ) is the C-zonotope encapsulation of any uncertain input. The propagation uncertainty term q(t k ) actually corresponds to the process noise of the EKF, and Czonotope Z q (t k ) corresponds to the process noise covariance matrix Q of the EKF.
[0076] [ε f (t k )] is Czonotope Z x (t k-1 ) can be viewed as a collection of transition linearization errors at different elements of Z x (t k-1 The transition linearization error at a given element of is the difference between the output produced by applying a nonlinear transition model to the given element and the output produced by applying a linear transition model to that given element.
[0077] In addition, the previously mentioned formula Z q (t k-1 )+Z([ε f (t k-1 )])+Z([c f (t k-1 ]) can be considered as a C zonotope that contains all the linearization errors.
number
[0078] Conditional propagation In the recursive filtering approach, the objective of the propagation step 102 is to obtain the state envelope that is valid at the time - or here near the time - when the observation about the system state is made.
[0079] There are two situations in which the method does not need to perform propagation, reducing filter execution time.
[0080] The first situation occurs when an update 104 is performed over a series of synchronous vector observations, in which case the posterior C-zonotope resulting from an update 104 with a first vector element can be reused as the a priori C-zonotope for subsequent sequential updates with the remaining elements of the observation vector.
[0081] In addition, if observations arrive close in time, instead of performing propagation, we can further expand the observation interval (see Section 3.3.1) to allow the validity time of the observation to extend beyond the validity time of the previous posterior C-zonotope (for added conservatism). To accommodate this situation, the threshold
number
[0082] Algorithm 1 below shows how conditional propagation can be implemented in practice: (see also Figure 3).
number
[0083] 3.3.Update To perform the update, the nonlinear observation equation is linearized to form its linear containment. It is guaranteed to be valid over the a priori state envelope by evaluating the remainder (i.e., the linearization error) by interval arithmetic (IA). Each element of the observation vector then defines a strip in the state space. The a priori C-zonotope can be crossed with all these strips at once or sequentially. Popular aggregation forms such as ellipsoids and zonotopes are non-closed under crossing with strips and therefore require outer approximation of each crossover result, making sequential crossover suboptimal. On the other hand, when using sequential updates, re-linearization of the observation equation after each crossover can lead to much tighter updates due to tight linearization error bounds. These two opposing effects depend on the degree of nonlinearity of the observations involved and are therefore application dependent.
[0084] A unique feature of C-zonotopes is that they are closed under intersection with the strip. Thus, updating with the entire observation vector at once or sequentially updating with each scalar element are equivalent operations. As a result, by using C-zonotopes, we can benefit from both: the simplicity of sequential updates without losing intersection optimality, and the compacting effect of relinearization.
[0085] Thus, without loss of generality, a scalar observation is considered as follows: a vector of synchronous observations is converted into a sequence of scalar observations with identical timestamps before performing the filter recursion. Thus, as an example, a simpler scalar observation formula:
number
number
[0086] Assuming interval value error, time t y,k A first-order approximation of the nonlinear observation map valid in is of the form:
number
number
number
number
[0087] In fact, a set containing the linear observation equation and the error of this linear equation for all states in the a priori C-zonotope is now calculated. This error has several components: D(t k )Z u (t k ): the effect of system inputs, e.g., control inputs generated by the autopilot or external perturbations. Czonotope Z u includes all possible inputs or perturbations. Z[ε h (t k )]: the linearization error interval transformed into a C-zonotope that contains all the observation errors at the different elements of the a priori zonotope. The observation linearization error at a given element of the a priori zonotope is the difference between the output produced by applying the nonlinear observation model to the given element and the output produced by applying the linear observation model to that given element. ·Z([c h (t k)): The C zonotope representing all constant terms in the measurement noise interval of the linearized observation equation and the original nonlinear observation equation.
[0088] To perform the update, two sets are crossed. The first set is given by the a priori C-zonotope (Eq. 18). The second set (see Eq. 35) is defined by the observations. In our case, the observations z(t y,k ) is included in the right hand side of Equation 26, so y(t y,k ) to z(t y,k ) to form formula 35. In Raman observations, Equation 35 represents a strip in state space, see Figure 3. Clearly, the second set is not a C-zonotope, since it is not closed (the strip extends to infinity). However, its intersection with the prior C-zonotope is a C-zonotope (posterior C-zonotope).
[0089] Note that although the system equations that are linearized are approximations, the corresponding linear inclusion is guaranteed to contain the corresponding nonlinear solution.
[0090] 3.3.1. Observation interval inflation The true observation time is known to have bounded uncertainty, i.e., in the interval [t y,k ]. In other words, the observation is valid somewhere within this time interval. The validity times of the observation and the prior C-zonotope are not guaranteed to match, so updates may not be performed.
[0091] A simple way to remedy this situation is to use the observation error interval
number
number
number
number
number
number
[0092] The main purpose of the dilation step is to solve the following problem: the valid time of the observation is not guaranteed to match the valid time of the a priori C zonotope. The basic idea to solve this is to dilate the observation error interval to ensure that its valid time includes the valid time of the a priori C zonotope. This means that the observation time
number
[0093] All known terms are lumped together into one inflated measurement error interval,
number
number
[0094] In addition, the linearization error ε h (t k) is the inflated prior state set
number
number
number
[0095] Scalar observation z(t y,k ) is used,
number
number
number
number
[0096] 3.4.Reduction As can be seen from Eqs. 36-41, each update is a function of the complexity of the state Czonotope - i.e., the number of generators n g and the number of constraints, n c-. To control the computational complexity and memory footprint, the filter x (t k ) complexity,
number
[0097] Algorithm 2 below shows how the reduction step 106 can be implemented in practice.
number
[0098] Algorithms 3 and 4 below show the different steps performed by the filter according to one embodiment.
number
number
[0099] 4.1D application In this first example, we consider a target device (or agent) whose motion is restricted to the x-axis of a Cartesian system. See Figure 4. We estimate the target device's position p and velocity v based solely on range observations to the beacons. A beacon located off the x-axis by a distance d makes the observation equation nonlinear, and therefore requires a nonlinear estimator such as CZESMF. In this minimal example, there is no uncertainty in the observation time.
[0100] State dynamics x=(pν) T teeth,
number
[0101] Range observation is
number
[0102] 4.1.Initialization First, the position and velocity of the object are
number
[0103] 4.2. Propagation The target acceleration is
number
number
[0104] Since the inclusion formula 46 is already linear, the pre-C zonotope is
number
[0105] 4.3.Update The observation time has no uncertainty in this example, so t k =t y,k , i.e., we propagate the state envelope to the valid time of the observation. From Equation 43, we obtain the linear envelope
number
number
number
[0106] When computing the bounds on the linearization error, the common approach of evaluating the Lagrange residuals using interval arithmetic often leads to overly conservative bounds due to dependencies. h Explicitly evaluating can provide a tighter bound, even tighter if monotonicity is exploited. The explicit formula for the linearization error is
number
[0107] To check for monotonicity, the derivative
number
[0108] Pre-C Zonotope
number
number
[0109] The update is then performed according to Equation 36.
[0110] 4.4.Simulations Snapshots of the propagation step 102, update step 104, and reduction step 106 are displayed in Figure 5. Figures 6 and 7 show the true state trajectory and CZESMF envelope, respectively. Figures 8 and 9 show the corresponding position and velocity interval envelopes, respectively. After each update, the state C-zonotope becomes
number
[0111] 5.2D localization application In this example, consider a target device moving in a field, with two static ranging beacons (see Fig. 10). This could be an automated lawnmower that needs to avoid cutting flowers in a designated zone and stay within the perimeter of the field, or an automated lift truck in a modern warehouse.
[0112] Target position x=(x1x2) T teeth
number
number
number
[0113] The target device is periodically
number
number
[0114] Each beacon i k Distance measurement observations to
number
[0115] These two observations are nominally synchronized at a frequency of 1 Hz. The imperfect synchronization of the ranging sensor with the wall clock time introduces a bounded timing jitter w t (t)∈[-10 -2 10 -2 ]s, i.e., a pair of ranging observations arrives approximately every second, but not exactly every second.
[0116] To obtain the periodic state envelope, the filter recursion is triggered at the upper bound of the observation time interval, i.e., (t1, t2, t3, t4, ...) = (0.01s, 0.01s, 1.01s, 1.01s, ...).
[0117] 5.1.Initialization (Update) The target device is initially inside the field. This allows the computation of a C-zonotope corresponding to an interval covering the field. However, this very conservative initial envelope leads to a very conservative filter solution. Therefore, a tighter envelope of the initial state is computed using VSIVIA. This one time step is not computationally expensive in two dimensions (233 processed boxes at 0.8s and ε=0.1m). For estimation problems with higher order states, it may be preferable to couple VSIVIA with constraint propagation to compute a sufficiently tight initial state envelope in an acceptable time.
[0118] The C-zonotopes (denoted as "solution" and "undefined" in FIG. 11) corresponding to the interval encapsulation of the subpaving obtained by VSIVIA are used to initialize the filters.
[0119] 5.2. Propagation In the state, the state dynamics is already linear. Therefore, the propagation is
number
[0120] The state dynamics uncertainty sphere defined by Eq. 58 uncertainty ball) is Z s To obtain (tk-1), we use the C-zonotope for external approximation (see Appendix A below). An example of the resulting pre-C-zonotope is shown in Figure 12.
number
[0121] 5.3.Update This corresponds to the range beacon i k This simplifies the process of performing each update in the nominal system. The pre-C zonotope in the beacon system is
number
number
[0122] To further simplify it,
number
number
number
[0123] From Equation 67, linear inclusion
number
number
number
[0124] 5.3.1. Observation interval inflation Before compensating for timing jitter, the state increment interval [Δx(t k )]
number
number
number
[0125] Using Equations 36-41, the post-C zonotope in the beacon system
number
number
[0126] Figure 12 shows the obtained posterior C xnotope and the reduced posterior C xnotope at t9. Figure 13 shows the results obtained with increasing timing noise values. Note that the posterior C xnotope becomes larger with increasing observation interval expansion.
[0127] The full state enveloping orbit is shown in Figure 14, and in Figure 15 we show the case where the observation timing noise is increased. After each update, the state C zonotope becomes
number
[0128] 6. Application to 3D peer-to-peer localization In this example, the state of interest is the relative position between two agents, such as drones flying in a tight formation. Many aerospace applications require air-to-air refueling, energy-efficient flight Formation flying and relative position assurance of small satellite constellations are required. In this example, raw GNSS code and carrier phase observations as well as UWB peer-to-peer ranging observations are available. Both types of sensors are inexpensive, have very low mass - tens of grams - and are readily available as commercial off-the-shelf components (COTS). We show how CZESMF can be applied to this localization problem of high practical relevance and evaluate its maintainability in a realistic simulation benchmark in comparison with existing constellation membership state estimation methods.
[0129] In what follows, we omit the time index whenever possible to clearly improve readability.
[0130] 6.1.Initialization Due to the high degree of linearity of the GNSS code dual phase difference observations, the filter can simply be initialized with a very conservative guess about the initial relative position.
[0131] 6.2. Propagation A set with guaranteed relative position x is propagated between two vehicles A and B. x=p B -p A
[0132] Moderately conservative ensemble-valued dynamic vehicle models are rarely available, due in large part to the lack of established ensemble-theoretic system identification methods. Thus, a vehicle-independent propagation model is highly desirable. Ensemble membership inertial navigation is one option that is worthy of further study, but is not explored further here. Instead, we rely on time-differentiated GNSS carrier double phase difference observations, when available, and a rough velocity norm constrained model in the absence of GNSS observations. Standalone time-differentiated carrier phase observations were originally received and deployed for precise flight trajectory reconstruction. Building on that, time-differentiated double phase difference (T3D) observations were first recently proposed for tight ensemble-valued relative trajectory tracking. With millimeter-level carrier noise, a very narrow envelope of position increments between GNSS observations can be computed. When applied to relative position estimation, the propagation state set is less conservative, leading to a smaller linearization error set in the update step, ultimately resulting in a less conservative posterior state set.
[0133] 6.2.1.T3D observations Forming a single difference of the carrier phase observations largely eliminates atmospheric and satellite clock errors.
[0134] The single difference carrier phase observations of two receivers A and B and satellite S are
number
[0135] d s =p A -p S and p B =p A Use +x to make it more compact
number
[0136] The differential receiver clock error is eliminated by doubly differencing with respect to the reference satellite R.
number
[0137] Therefore, the double-difference carrier-phase observation is a nonlinear function of the relative positions of receivers A and B and the vector between the absolute receiver position and the satellite orbital position.
[0138] Assuming that the receiver maintains phase lock over two subsequent epochs, the ambiguity is a constant that is eliminated by forming the difference of two subsequent double-difference observations, the T3D observation
number
[0139] Below, position increment Δx=x(t k )-x(t k-1 We show how we can calculate the C zonotope that encompasses
[0140] Directly linearizing (using Eq. 78) and computing the interval hull of the Laplace residual using interval arithmetic as proposed in "A nonlinear set-membership filter for online applications" by E. Scholte et al. leads to severe overapproximations due to heavy dependencies.
[0141] This problem is overcome by linearizing the single difference equation 76 instead.
[0142] The Lagrange residuals are then evaluated using IA, and the resulting intervals are the hardware error intervals ε ΔΦ will be added.
[0143] To simplify the computation of the necessary Jacobians and Hessians, Eq. 76 can be written as
number
number
number
[0144] Next, the Jacobian and Hessian of Equation 81 are
number
[0145] Therefore, the interval bounds for the observation model errors due to linearization and parametric uncertainties (uncertain orbital positions of the satellites) are:
number
[0146] Proceed to form linear single difference observations.
number
[0147] If the separation is small, [R ΔΦ ] is typically very narrow, on the order of millimeters, so it linearizes to about x=0 and takes the more compact form familiar from the RTK literature.
number
number
number
number
[0148] Observation time t k-1 and t k The difference between two successive carrier phase observations, each having Then, the ambiguity constant term disappears. Δx(t k )=x(t k )-x(t k-1 ), T3D observations can:
number
number
[0149] Here, the geometric matrix
number
number
number
[0150] T3D Observation
number
number
[0151] Next, the corresponding C xotope ZΔx(t k ) was disclosed in "Constrained zonotopes: A new tool for set-based estimation and fault detection" by JK Scott et al. This is given by Theorem 1, which states that F(t k-1 )=I3, B(t k-1 )=I3,Z Δx (t k ), Z q (t k-1 ) = 0.
[0152] 6.2.2. Velocity-norm propagation In this application, the vehicle-to-vehicle ranging observations are not synchronized with the GNSS observations and arrive at a higher rate. Before updating with the ranging observations, we use a coarse model based on norm-bounded relative velocities to perform the necessary propagation. The propagation horizon t k -t k-1 Then, the norm bound ν max is the waveform r=ν max (t k -t k-1 ) to define a symmetric C-zonotope Z of user-defined complexity. S We approximate this sphere by. Then, the propagation is k-1 )=I3, B(t k-1 )=I3,Z u (t k-1 )=0, Z q (t k-1 )=Z S This is done by setting
[0153] 6.3.Update 6.3.3.GNSS update GNSS code phase single difference observation,
number
[0154] d s =p A -p S and p B =p A Use +x to make it more compact
number
[0155] The differential receiver clock error is eliminated by doubly differencing with respect to the reference satellite R.
number
[0156] Thus, just like the carrier phase double difference, the double difference code phase observation is a nonlinear function of the relative positions of A and B and the vector between the absolute vehicle position and the satellite orbital position. Following the same approach as the carrier phase observation, we form a linear inclusion of the code phase double difference,
number
[0157] 6.3.2.Ranging Update Vehicle-to-vehicle distance measurement observations are
number
number
[0158] Below, r A , r B is small, i.e., the ranging antenna is close to the GNSS antenna, the interval boundaries of the additive effect on the ranging observations are identified, and the combined range error w r Assume that it is summarized as:
number
[0159] To linearize Equation 107, we first write the range observations as
number
[0160] The first interval Taylor expansion of Equation 108 is then carried out in the same manner as in the GNSS carrier phase observation.
number
number
[0161] Geometrically, this linearized range observation is the gradient vector
number
[0162] Applying the common technique of evaluating the Lagrange remainder terms in Equation 109 using interval arithmetic provides a large overapproximation due to multiple dependencies in x.
[0163] The wider the linearization error bound, the less likely it is that the linearized observations can contribute to the shrinkage of the guaranteed state set.
[0164] Below, we show that we can exploit monotonicity to obtain tighter bounds by explicitly evaluating the error between the nonlinear observation equation 107 and its linearization.
[0165] The explicit linearization error bounds are
number
[0166] To check the monotonicity of Equation 111 with respect to [x], the interval-valued gradient
number
[0167] Geometrically, this is the difference between two unit vectors. Hence, equation (111) is piecewise monotonic, since the signs of the elements of (112) are constant over the subinterval of [x].
number
[0168] It is noted that the general approach yields interval radii up to four times larger than the piecewise monotonic approach, which more closely approximates the sampled interval.
[0169] 6.4. Simulations The filters were evaluated on a benchmark UAS operation, consisting of two UAS: leader and follower. The follower changes its relative position from right to left on the x-axis in a circular orbit, see Figure 17. The relative positions of both agents are tracked at 1 Hz. The simulated GNSS and ranging observations are contaminated with recorded observation errors. For the GNSS observations, these are obtained from zero-baseline measurements of two low-cost single-frequency GPS receivers. The ranging observation errors are obtained from two static UWB ranging beacons placed at a known distance.
[0170] 6.4.1. Set Volumes In all three situations, the CZESMF filter provides smaller ensemble volumes than both IESMF (iteratively expanded set membership filter) and VSIVIA (a vector implementation of the SIVIA algorithm). We also evaluated the benefit of using T3D observations for propagation compared to the coarse velocity norm boundary model. As Figure 18 shows, the C zonotope volume can be made almost an order of magnitude smaller by using carrier phase information for the propagation step.
[0171] 6.5. Sensitivity to satellite constellations The achievable size and shape of the relative position envelope depends on the baseline satellite geometry, which is captured by the familiar Dilution of Precision (DOP) accuracy measure. We are interested to know whether the relative conservatism of the set membership filters we have developed is robust with respect to satellite constellation variations. Simulations of benchmark operations were performed over a set of randomly selected GPS orbit records. Satellite positions were calculated from spline approximations of the first two minutes of each orbit record. Figure 19 shows the resulting relative position set volumes of CZESMF and IESMF normalized with the subpaving volume obtained by VSIVIA. After the initial transition, CZESMF shows lower conservatism than VSIVIA over all simulations and very little variation with respect to GNSS orbits. IESMF is much more conservative and has a similarly low sensitivity to satellite orbits. These simulations provide some confidence that the observed superior tightness of the envelope calculated by CZESMF can be expected to carry over to an online implementation of the filter.
[0172] 7. Conclusion The ensemble state estimation method described above for nonlinear systems, based on C-zonotoe robustness, outperforms the VSIVIA-based nonlinear offline ensemble estimator and the recent fast ellipsoidal ensemble state estimator in simulation situations (with respect to the ensemble volume).
[0173] The proposed filter can be applied to a wide variety of estimation problems, including Simultaneous Localization And Mapping (SLAM), an exciting perspective given the current interest in safe visual localization of road vehicles.
[0174] Appendix A: Approximation of two-dimensional or three-dimensional spheres using the C-zonotope To approximate a sphere of radius r centered on c, we compute the approximation in H-rep and transform it to a C zonotope.
[0175] θ is the zenith angle, φ is the azimuth angle, and l is the complexity parameter.
number
number
[0176] Each of these normal vectors defines a half-space. Then, using the method given in "Constrained zonotopes: A new tool for set-based estimation and fault detection" by J.K. Scott et al., we can easily write the corresponding polytope in H-rep:
number
[0177] In two dimensions, the same technique works by solving for a set of angles θ = (θ1, θ2, . . . ,θ l )
Claims
1. 1. A method for estimating a constrained zonotope encompassing a motion state of at least one moving object, comprising the steps of: a) The first moment (t k-1 A first constrained zonotope (Z x (t k-1 )) a linearization of the nonlinear transition model at the elements of (a) to (c), thereby generating a linear transition model; b) the first constrained zonotope (Z x (t k-1 ) calculating a transition linearization error at different elements of the first constrained zonotope, where the transition linearization error at a given element of the first constrained zonotope is the difference between an output generated by applying the nonlinear transition model to the given element and an output generated by applying the linear transition model to the given element; c) calculating a second constrained zonotope that includes all of the transition linearization errors; d) propagating (102) the first constrained zonotope, whereby at the first instant (t k-1 At the second instant (t k ) a pre-constrained zonotope that encompasses the motion state [0010] and the propagation of the first constrained zonotope is - applying the linear transition model to the first constrained zonotope to generate a third constrained zonotope; - Propagating (102), which includes summing the second constrained zonotope and the third constrained zonotope; e) the pre-constrained zonotope [0025] thereby updating (104) said second instant (t k ) the post-constrained zonotope (Z′ x (t k) and updating the pre-constrained zonotope includes applying an observation model to the pre-constrained zonotope and to observation data of the moving object acquired by at least one sensor (104); The method includes:
2. The method of claim 1 , wherein the element at which the nonlinear transition model is linearized is the center of a minimum interval enclosure that contains the first constrained zonotope.
3. 3. The method of claim 1 or 2, further comprising linearizing a nonlinear observation model in the first constrained zonotope or an element of the pre-constrained zonotope, thereby generating a linear observation model, and the observation model used in step e) is the generated linear observation model.
4. The method of claim 3 , wherein the element at which the nonlinear observation model is linearized is the center of a minimal interval enclosure that contains the pre-constrained zonotope.
5. - calculating an observation linearization error at different elements of the first constrained zonotope or the pre-constrained zonotope, where the observation linearization error at a given element of the first constrained zonotope or the pre-constrained zonotope is the difference between the output generated as a result of applying the non-linear observation model to the given element and the output generated as a result of applying the linear observation model to the given element; A fourth constrained zonotope (Z([ε h (t k ) )])) and - dilating the fourth constrained zonotope (Z([ε h (t k )])), thereby obtaining an expanded constrained zonotope (Z([ε ′ ])) forming a strip in state space, taking into account all possible variations of the state of motion between the second and third instants of time and possibly also the measurement noise induced by the sensor, the observation data of the moving object having been obtained at a third instant of time different from the second instant of time; - the pre-constrained zonotope [0030] and computing the post-constrained zonotope (Z'x(tk)) as the intersection of Z([ε']) with the expansion-constrained zonotope (Z([ε'])); The method of claim 3 or 4, further comprising:
6. - the a posteriori constrained zonotope (Z' x (t k )) to determine the complexity of the If the complexity is greater than a threshold, the reduction process is performed on the posterior constrained zonotope (Z' x (t k ) to obtain the posterior zonotope (Z′) before the reduction. x (t k )) and the posterior zonotope (Z') before the reduction x (t k )) including the post-zonotope (Z x (t k ) to obtain and apply for the The method of any one of claims 1 to 5, further comprising:
7. The posterior constrained zonotope (Z′ x (t k 7. The method according to claim 1, further comprising using the second instant as the first instant in a subsequent execution of steps a) to e).
8. - calculating the maximum time interval between said first instant and the instant at which said observation data is acquired by said sensor; - applying the linear transition model to a pre-determined constrained zonotope if the maximum time interval is greater than a pre-defined duration, thereby generating the pre-constrained zonotope, and otherwise using the pre-determined constrained zonotope as the pre-constrained zonotope; The method of any one of claims 1 to 7, further comprising:
9. The method according to any one of claims 1 to 8, wherein the observation data includes at least one of the following data acquired by a GNSS receiver: pseudorange, acquired carrier phase, and range between the moving object and another moving object.
10. A method according to any preceding claim, wherein the movement data comprises movement data of the moving object relative to another moving object.
11. The method of any one of claims 1 to 10, wherein the first constrained zonotope is computed using a vector set inverter algorithm via interval analysis.
12. A computer program comprising instructions for causing the computer to carry out a method according to any one of claims 1 to 11 when executed by a computer.
Citation Information
Patent Citations
Airbag device
JP2007091181A
Saddle-riding type vehicle
JP2010058763A
