Method for estimating a restricted fully symmetric polytope enclosing a state representing the motion of at least one mobile object controlled by a nonlinear model

Abstract

A method for estimating a restricted holosymmetric polytope surrounding a motion state of at least one moving target, the method comprising: a) in a first holosymmetric polytope (Z x (t k‑1 )) linearizes the nonlinear transformation model at the elements of, b) calculates the first fully symmetric polytope (Z x (t k‑1 )), c) calculating the second fully symmetric polytope containing all linear errors, d) propagating (102) the first fully symmetric polytope to obtain the first time (t k‑1 ) at the second moment (t k ) encloses the state of motion of the priori fully symmetric polytope (Z x (t k )), wherein the a priori fully symmetric polytope (Z x (t k )) is a linear projection of a hypercube, the hypercube being subject to at least one linear constraint, e) updating (104) the prior fully symmetric polytope (Z x (t k )) to obtain the value at the second moment (t k ) The posterior restricted fully symmetric polytope (Z') enclosing the motion state x (t k )).

Description

Technical Field

[0001] The present invention relates to the technical field of set-membership state estimation technology.

[0002] The invention deals with a method for estimating a constrained zonotope containing states representing the motion of at least one mobile object controlled by a nonlinear model. Background Art

[0003] Over the past few years, the level of autonomy of unmanned vehicles has increased considerably, resulting in ongoing efforts to integrate unmanned aircraft systems (UAS) into the national airspace and autonomous vehicles into general traffic. It has long been known that cooperative motion strategies can bring many benefits with great economic potential. Automated platooning of ground vehicles can increase highway capacity, avoid traffic congestion, and reduce the consumption of individual vehicles. Close formation flying has been shown to significantly increase the range of aircraft. The most prominent examples include range improvements, thanks to the use of wake vortex effects and automatic aerial refueling in close aircraft formations. In order to execute these cooperative motion strategies in a safe and efficient manner, the relative positions of the vehicles need to be estimated with decimeter or even centimeter accuracy.

[0004] For close formation flying, various differential positioning techniques have been proposed in the past, since independent positioning based on the Global Navigation Satellite System (GNSS) does not provide the required accuracy. Many sensor fusion approaches can be found in the literature over the past few decades, which employ inertial measurement units, differential GNSS with or without phase information, machine vision, or a combination of both. Recently, cheap, decimeter-accurate Ultra-Wideband (UWB) ranging sensors have been successfully demonstrated to improve the robustness of airborne Real Time Kinematics (RTK) solutions.

[0005] What these existing methods have in common is that they rely on probabilistic filtering techniques, such as the Unscented Kalman Filter (UKF) or the Extended Kalman Filter (EKF). It is well known that many practical nonlinear estimation problems violate the basic assumptions of both the UKF and the EKF, in particular the key assumption that the process model and the distribution of the uncertainty in the observation model are known. In practice, only an approximation of the stochastic nature of both sources of uncertainty is usually available. Despite this inherent and well-known problem, the EKF and the UKF have been shown to work well in practice, providing accurate average state estimates.

[0006] However, when it comes to safe cooperative motion of multiple agents in close proximity, consistent computation of filter state uncertainty is more important than accurate point estimates. Ensuring filter consistency is an open problem for nonlinear estimation problems when using an EKF or UKF due to the approximation involved (the linearization step performed by the unscented transform of the EKF and UKF). However, we believe that algorithms that can compute guaranteed bounds on vehicle states are a key element in further safely integrating autonomous vehicles into heterogeneous environments such as mixed air and road traffic.

[0007] In this regard, set-membership state estimation techniques offer an interesting alternative to the probabilistic framework. Set-membership estimators rely on set-valued process models under the assumption that the observations and process model uncertainties are contained in known compact sets (e.g., intervals). No assumptions need to be made about the distribution of errors in these sets. For nonlinear systems, the problem of accurate set membership estimation is often computationally intractable. Research aims to find feasible but tight outer approximations to the set operations involved. Over the past few decades, many algorithms have been proposed that employ set shapes such as ellipses or intervals. Common challenges faced by all set-membership estimators include finding a compromise between conservatism (over-approximation introduced by representing complex sets with simpler, computationally tractable sets) and computational effort.

[0008] Despite the advantages of set membership methods, their application to localization of dependent agents has received little attention. A set membership localization algorithm for cooperative ground vehicles based on the Set Inversion Via Interval Analysis (SIVIA) algorithm has been proposed. SIVIA and related nonlinear set inversion algorithms offer tight coverage 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 increases rapidly with the number of states, limiting their use beyond 2D ground vehicle localization applications.

[0009] Thanks to their constant complexity, ellipses offer good computational performance and have been successfully applied to localize dependent agents in simulation. Recently, fully symmetric polytopes have received increasing attention for guaranteed state estimation. Fully symmetric polytopes offer more flexibility than ellipses, with their complexity being a design parameter. An inherent problem with fully symmetric polytopes is their symmetry, which introduces additional conservatism when the actual state set exhibits strong asymmetries. A second problem is that the intersection of two fully symmetric polytopes does not always result in a fully symmetric polytope, so, as in the case of ellipses, an outer approximation to the intermediate set must be computed at each filter recursion, which may involve solving a convex optimization problem. Summary of the invention

[0010] The present invention aims to solve or at least alleviate these two deficiencies.

[0011] The first aspect of the present invention is the method as described in Technical Solution 1.

[0012] The priori fully symmetric polytopes used in the method as described in technical solution 1 are restricted fully symmetric polytopes. Such fully symmetric polytopes allow better approximation of asymmetric set shapes. As an additional advantage, they are closed under intersection and Minkowski and operations.

[0013] The method according to the first aspect of the invention may include the following optional features.

[0014] The linearized element of the nonlinear transformation model may be the center of the smallest interval containing the first restricted fully symmetric polytope.

[0015] The method may further include linearizing the nonlinear observation model at elements of the first restricted fully symmetric polytope or the prior fully symmetric polytope to generate a linear observation model, wherein the observation model used in step e) is the generated linear observation model.

[0016] The linearized element of the nonlinear observation model can be the center of the smallest interval that contains the prior fully symmetric polytope.

[0017] The method may further comprise:

[0018] · computing the observation linearization error at different elements of the first restricted fully symmetric polytope or the a priori fully symmetric polytope, where the observation linearization error at a given element of the first restricted fully symmetric polytope or the a priori fully symmetric polytope is the difference between the output obtained by applying the nonlinear observation model to the given element and the output obtained by applying the linear observation model to the given element,

[0019] · Compute a fourth restricted fully symmetric polytope containing all computed observation linearization errors, where the posterior restricted fully symmetric polytope depends on the fourth restricted fully symmetric polytope.

[0020] The observation data of the mobile target may be acquired at a third time different from the second time, and the method may further include:

[0021] expanding the fourth restricted holosymmetric polytope to obtain an expanded restricted holosymmetric polytope which forms a strip in the state space and takes into account all possible changes of the state of motion between the second moment and the third moment and, possibly, taking into account the noise of the measurements caused by the sensor,

[0022] The posterior restricted holosymmetric polytope is computed as the intersection between the prior restricted holosymmetric polytope and the enlarged restricted holosymmetric polytope.

[0023] The method may further comprise:

[0024] Determine the complexity level of a posteriori restricted holosymmetric polytopes,

[0025] If the complexity level is greater than a threshold, a reduction process is applied to the posterior restricted fully symmetric polytope to obtain a posterior fully symmetric polytope with a lower complexity than that before the reduction and containing the posterior fully symmetric polytope.

[0026] The method may further include: when subsequently executing steps a) to e) defined in Technical Solution 1, using the a posteriori restricted fully symmetric polytope as the first restricted fully symmetric polytope and using the second moment as the first moment.

[0027] The method may further comprise:

[0028] Calculate the maximum time interval between the first moment and the moment when the sensor acquires the observation data,

[0029] If the maximum time interval is greater than the predetermined duration, applying the linear transformation model to the predetermined restricted holosymmetric polytope to generate a priori restricted holosymmetric polytope, otherwise using the predetermined restricted holosymmetric polytope as the priori restricted holosymmetric polytope.

[0030] The observation data may include at least one of the following data acquired by the GNSS receiver: a pseudorange, an acquired carrier phase, and a distance between a mobile target and another mobile target.

[0031] The motion data may include motion data of a moving object relative to another moving object.

[0032] The first restricted fully symmetric polytope can be calculated using a vector set inversion algorithm with interval analysis.

[0033] A second aspect of the present invention is a computer program product comprising instructions which, when executed by a computer, cause the computer to perform the method of the first aspect of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 schematically represents an apparatus for estimating a set of motion states surrounding a moving object;

[0035] Figure 2 The steps of an exemplary method for estimating a C-holosymmetric polytope surrounding a state x of a system are described;

[0036] Figure 3 illustrate Figure 2 The sequence of operations of the propagation steps of the method shown in;

[0037] Figure 4 is a graphical representation of the position p of the target device and the beacon and the distance d to the x-axis;

[0038] Figure 5 express Figure 2 Snapshots of the propagation, update, and reduction steps of the method shown in;

[0039] Figure 6 A graphical representation of the target device's true state trajectory;

[0040] Figure 7 A graphical representation of the CZESMF range for the target device;

[0041] Figure 8 A graphical representation of the location range of the target device;

[0042] Fig. 9 A graphical representation of the speed range of the target device;

[0043] Fig.10 a graphical representation of the location of another target device;

[0044] Fig.11 Indicates the range of subpaving obtained by VSIVIA;

[0045] Fig.12 Indicates for Fig.10 the obtained prior C-fully symmetric polytope of the target device together with the obtained posterior C-fully symmetric polytope and the reduced posterior C-fully symmetric polytope;

[0046] Fig.13 represents the a priori C-fully symmetric polytope and the posterior C-fully symmetric polytope with increased observation timing noise;

[0047] Fig.14 express Fig.10 The full state range trajectory of the target device;

[0048] Fig.15 Indicates that when the timing noise of the observation value increases Fig.10 The full state range trajectory of the target device;

[0049] Fig.16 Graphical representation of two radii measured for the distance linearization error intervals obtained by two different methods;

[0050] Fig.17 represents the positions of the other two target devices during the simulation;

[0051] Fig.18 represents the ensemble volume obtained by velocity norm prediction and by T3D prediction with GNSS updates or GNSS+UWB updates;

[0052] Fig.19 Represents the relative position set volume obtained using the CZESMF filter and using the IESMF filter. DETAILED DESCRIPTION

[0053] The following description is structured as follows: After defining the notation (Section 1) and reviewing the set-membership state estimation problem (Section 2), a computationally efficient state set estimation method based on restricted holosymmetric polytopes (C-holosymmetric polytopes) will be described (Section 3). Next, three practical applications of the method will be given as examples:

[0054] Simple 1D example (Part 4),

[0055] The slightly more complex 2D localization problem (Part 5),

[0056] · The 3D peer positioning benchmark problem using differential GNSS and peer ranging observations (Part 6).

[0057] 1. Symbols

[0058] To accommodate this complexity, we will use the familiar notation x(t l ). The discrete nature of the filter execution is captured by indexing in time. Specifically, t k Specifies the time at which the processing unit computes the prior and posterior state sets. For a vector x(t), time t represents the time at which the vector is valid.

[0059] Accordingly, the set Z(t k ) at time t k is valid, that is, if t = t k , then x(t)∈Z(t k ). If the effective time is the interval [t k ], then the set is valid for all times in this interval, that is, if t∈[t k ], then x(t)∈Z(t k ).

[0060] In addition, the matrix J nxm represents an n-row and m-column matrix with all elements equal to 1. The identity matrix of size n is represented by I n Indicates that the zero square matrix of size n consists of 0 n express.

[0061] The dot product of two vectors a and b is expressed as<a,b> .

[0062] The interval matrix is ​​given by Indicates that M , The elements of are the real lower and upper limits of the interval elements of [M]. For m = 1 and n > 1, we call it an interval vector, that is, [p]. The scalar interval is represented by Indicates that the lower and upper limits x , For an interval [M], M represents the real number of the interval, that is, M∈[M].

[0063] Assuming additional errors, it is 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 ​​given by When a term and an interval term appear in the same expression, such as [c] = a + [b], the term (a) is assumed to represent a degenerate interval ([a, a]), and all operations in the expression are interval operations.

[0064] Since C-holorsymmetric polytopes are not closed under nonlinear transformations, first-order Taylor approximations of the nonlinear system equations are used in the following, see Sections 3.2 and 3.3. To ensure that these approximations do not compromise the consistency of the resulting state range, the concept of linear inclusion is used, as is the case with other set estimation methods, see for example E. Scholte et al. "A nonlinear set-membership filter for online applications". A linear inclusion of a nonlinear function is a set-valued linear function that is guaranteed to contain the solution of the nonlinear function on some set of inputs. As an example, consider the function

[0065] y=h(x) (1)

[0066] Where x∈X, that is, x is contained in a compact set X. Linear function

[0067]

[0068] is a linear inclusion of Equation 1, with the condition

[0069] y∈y0+Hx+[∈] (3)

[0070] For all x∈X, the set-valued solution of the linear set-valued function Nonlinear solutions are always included.

[0071] 2. Problem Statement

[0072] The motion of a mobile object can be described by a motion state including motion data. The motion data may include, for example, the position of the object, the speed of the object, the acceleration of the object, other data or a combination thereof. When a mobile object moves, the motion state may change over time.

[0073] Hereinafter, for simplicity, the mobile object 1 may be referred to as a “system”, and the motion state may be referred to as a “state”.

[0074] refer to Figure 1 , a device 1 for estimating a set of motion states surrounding a mobile target includes a processing unit 2 and an observation unit 4.

[0075] The processing unit 2 is configured to execute a computer program comprising code instructions implementing the estimation method described below. The computer program will be referred to as a "CZESMF filter" (Extended Set Membership Filter of Restricted Fully Symmetric Polytopes) or "filter" hereinafter, because it actually acts like a filter.

[0076] The filter is configured to utilize a predetermined transformation model (hereinafter also referred to as a "propagation model" or a "dynamic model") and a predetermined observation model.

[0077] The processing unit 2 is configured to operate at different processing moments so as to be able to estimate different sets of motion states surrounding the mobile object at the different moments.

[0078] The processing unit 2 may be of any of the following types: one or more processors, one or more microprocessors, FPGA, ASIC, etc.

[0079] The observation unit 4 includes at least one sensor configured to observe the mobile target. Therefore, the observation unit 4 is configured to output observation value data related to the mobile target. The observation unit is also configured to operate at different observation times so that different observation values ​​can be obtained at the different times.

[0080] The purpose of the set membership state estimation is to calculate the region where the state x(t) of the system is guaranteed to exist at time t.

[0081] 2.1. State Dynamics

[0082] In discrete state estimation, it is usually assumed that the system state dynamics are governed by a static model of the type

[0083] x(t k )=f(x(t k-1 ),u(t k-1 ))+q(t k-1 ) (4)

[0084] The model has a previous state System Input Propagation Model Error and a constant sampling time T s =t k -t k-1 .

[0085] In practice, irregular observation times yield irregular sampling times, i.e., propagation ranges in the filter. Depending on the sampling times, different propagation models can yield state ranges with different tightness. A practical example from navigation is inertial dead-reckoning localization, where the double integral of the observed acceleration yields a very tight short-term position range, but sensor biases lead to a long-term quadratic growth in the position range. In contrast, a simple velocity norm bound model of the vehicle dynamics (see Section 5 for an example) makes the position range grow linearly with time, and thus may provide a tighter range for longer propagation ranges.

[0086] Another example is dealt with in Section 6, where tight relative position propagation using time-differenced GNSS phase observations is only available at the sampling time of the GNSS receiver, while between GNSS observations only a coarse velocity norm bound propagation model is available.

[0087] We accommodate multiple propagation models by assuming a very general time-varying state dynamics model, which is a nonlinear mapping

[0088] x(t k )=f(x(t k-1 ),u(t k-1 ),t k-1 ,t k )+q(t k-1 ) (5)

[0089] Independent variable t k and t k-1 Indicates that the model is in t k-1 to k The propagation of the system state is performed within the time range of and the time-varying nature of f(). Thanks to allowing the propagation model to be time-varying, the filter can be at each propagation step 102 (see Figure 2 ) adopts the least conservative model among a set of available models.

[0090] 2.2. Observations

[0091] In practice, the observations made by the observation unit 4 are typically neither perfectly synchronous nor perfectly periodic. To complicate matters further, the observation value times may only be approximately known, for example due to the lack of a hard real-time communication protocol.

[0092] The uncertainty in the time of observations leads to a subtle but serious problem that has not been solved by existing set-membership state estimation schemes. The propagation state range is guaranteed to be only within the selected propagation time t k We call this the given state range Since the observed value corresponds to an unknown time in an interval, using this observed value to update the prior range will result in the state range being unable to be guaranteed to be valid at any time, even if t k This is also true for the observation time interval. This is simply because and the state space limits corresponding to the observations do not refer to the same time point. Therefore, ignoring the observation time uncertainty leads to an invalid state range after only one update. A remedy for this problem is presented below.

[0093] Assume that the observations are mapped by a nonlinear control

[0094]

[0095]

[0096] in, is the vector of sensor observations, w(t)∈W is the corresponding measurement error, is the observed observation timestamp, w t (t)∈[w t ] is the observation time error, and t is the true observation time. The observation timestamp error w t (t) covers not only timing jitter, but also communication delays due to, for example, packet encoding / decoding delays on digital data links. All times are wall clock times.

[0097] It should be noted that no assumptions are made about the measurement model error, other than that it is contained in some known set W(t). As such, it covers hardware noise, bias, and modeling errors. It should also be noted that both the observation model h(...) and the size of the observation vector m(t) can be time-varying.

[0098] While y(t) represents the observations provided by the model, the actual measurements received from the sensors are represented by z(t).

[0099] 3. Extended set membership filter based on C-fully symmetric polytopes

[0100] The most popular computationally efficient set representations used in set state estimation are linear transformations of basic geometric entities, such as ellipsoids-transformed unit hyperspheres and fully symmetric polytopes-transformed unit hypercubes. The inherent symmetries of hyperspheres and hypercubes limit the ability of ellipsoids and fully symmetric polytopes to closely approximate asymmetric or generally irregularly shaped sets. Further exacerbating the situation is that neither is closed under the intersection operation of filter update. Therefore, it is necessary to calculate the outer approximation of each intersection result. Restricted fully symmetric polytopes alleviate these two problems. It is a linear transformation of a restricted unit hypercube, and as such can be asymmetric. It is closed under intersection, i.e., the intersection of two C-fully symmetric polytopes is another C-fully symmetric polytope, but its complexity is higher.

[0101] C-Fully Symmetric Polytope It is defined by its generator matrix G, its center c, and the constraint parameters A,b of the unit hypercube and describes the set:

[0102] Z={Gξ+c|‖ξ‖ ∞ ≤1,Aξ=b} (8)

[0103] Each constraint represents a plane that intersects the unit hypercube. The resulting intersection is then projected through the generator matrix G and finally translated by the center vector c.

[0104] C-holosymmetric polytopes are an alternative parameterization of convex polytopes. Furthermore, C-holosymmetric polytopes are a superset of regular holosymmetric polytopes, since every unconstrained C-holosymmetric polytope is a regular holosymmetric polytope. For brevity and in accordance with existing literature, the C-holosymmetric polytope representation of a convex polytope is denoted G-rep. and its half-space representation is denoted H-rep.

[0105] The interval range of the C-holosymmetric polytope Z is marked by [Z], and the G-rep. of the interval [a] is marked by Z([a]).

[0106] refer to Figure 2 , a method for estimating a C-fully symmetric polytope surrounding a system state x comprises a filter based on k-1 The predetermined C-holosymmetric polytope Z in the enclosed state x (t k-1 ) performs the following main steps:

[0107] A propagation step 102, in which a predetermined C-fully symmetric polytope is processed to obtain k A priori C-holosymmetric polytopes in enclosing state

[0108] Update step 104, where the prior is restricted to a fully symmetric polytope Combined with the observation value data acquired by the observation unit 4 to obtain the k The Posterior C-Fully Symmetric Polytope Z' in Enclosing State x (t k ).

[0109] At time t k In the enclosing state x and consider up to and including t k The posterior C-fully symmetric polytope for all observations of is denoted by

[0110] Z′ x (t k )={G′ x (t k ),c′ x (t k ),A′ x (t k ),b′ x (t k )} (9)

[0111] Before the update is performed, at time t kThe prior C-holosymmetric polytope enclosing the state x is denoted by

[0112]

[0113] As will be described in detail below, the propagation step 102 and the updating step 104 may use a linear transformation model and a linear observation value model, respectively, wherein the linear model is obtained by linearizing the nonlinear model at a specific point.

[0114] In addition, the method may further include a reduction step 106, wherein the filter converts the posterior C-fully symmetric polytope Z' x (t k ) into another less complex but C-holosymmetric polytope Z' x (t k ) of the C-holosymmetric polytope Z x (t k ).

[0115] The propagation step 102, the updating step 104 and the reducing step 106 are repeated recursively. More precisely, the method comprises a plurality of recursions, each recursion performing the propagation step 102, the updating step 104 and the reducing step 106.

[0116] The first recursion of the method is preceded by an initialization step 100, in which an initial C-holosymmetric polytope is determined.

[0117] Steps 100, 102, 104 and 106 will now be described in detail.

[0118] 3.1. Initialization

[0119] It is known that the initial state is contained in some C-holosymmetric polytopes In, that is, in the range of the first prior state.

[0120] 3.2. Propagation

[0121] In the propagation step 102, for k>1 execution, the filter calculates the prior state set Given the aforementioned posterior state set Z x (t k-1 ), it is guaranteed that at time t k >t k-1 Contains status.

[0122] Due to the peculiarities of uncertain observations, the question arises of how to choose the time to compute the a priori C-fully symmetric polytope. It is even possible to do so at the beginning of the filter recursion in the future to compensate for the filter execution time. However, according to the execution time of the recursion (propagation and update), it is not possible to choose the time to compute the a priori C-fully symmetric polytope. kIt might make more sense to set it close to the observation time and do a final propagation to compensate for the execution time.

[0123] Some options are the nominal periodic observation time (if the observations are nominally periodic) or the center of the observation time interval.With the benefit of the first option, the filter result becomes periodic, hiding the uncertainty of the observation time in the filter.

[0124] Since C-holorthosymmetric polytopes are not closed under nonlinear mappings, similar to the Extended Kalman Filter (EKF), we can form a The state dynamics model of Equation (5) starts with a first-order Taylor approximation. The range of the aforementioned posterior state and the center of the input range are selected as the operating point:

[0125]

[0126] Where Z u = {G u ,c u ,A u ,b u} (12)

[0127] Since the center of a C-holosymmetric polytope does not necessarily even lie inside its C-holosymmetric polytope, this option tends to result in smaller linearization error, but does not minimize the linearization error.

[0128] Linearization leads us to the familiar first-order Taylor expansion

[0129]

[0130] Among them, the linearization error is ∈ f (t k )∈[∈ f (t k )], and among them

[0131]

[0132]

[0133] The corresponding linear inclusion is given by

[0134] x(t k )∈F(t k-1 )Z x (t k-1 )+B(t k-1 )Z u (t k-1 )+Zq (t k-1 )+Z([ε f (t k-1 )])+Z([c f (t k-1 )](17)

[0135] Then, from time t k-1 To time t k The propagation execution is

[0136]

[0137] The matrix multiplication and summation operations in Equation 18 are set operations on C-zonotops defined by J. K. Scott et al. in “Constrained zonotopes: A new tool for set-based estimation and fault detection”. q (t k-1 ) encompasses the linearization error and model uncertainty of the nonlinear state dynamic model, and Z u (t k-1 ) is the C-totally symmetric polytope range of any uncertain input. The propagation uncertainty term q(t k ) actually corresponds to the process noise of the EKF, while the C-fully symmetric polytope Z q (t k ) corresponds to the process noise covariance matrix Q of the EKF.

[0138] [∈ f (t k )] can be regarded as a C-holosymmetric polytope Z x (t k-1 ) at different elements of a set of conversion linearization errors. x (t k-1 ) is the difference between the output obtained by applying the nonlinear transformation model to the given element and the output obtained by applying the linear transformation model to the given element.

[0139] In addition, the above expression Z q (t k-1 )+Z([ε f (t k-1 )])+Z([c f (t k-1 )] can be viewed as a C-fully symmetric polytope that contains all linearization errors. In addition, the expression It can be viewed as applying the linear transformation model to Zx (t k-1 ) The posterior holosymmetric polytope is the sum of two expressions.

[0140] 3.2.1. Conditional Propagation

[0141] In the recursive filtering scheme, the purpose of the propagation step 102 is to obtain a state range that is valid at or close to the time when the system state is observed.

[0142] In this method, there are two cases where propagation does not need to be performed, thereby reducing the filter running time.

[0143] The first case occurs when an update 104 is performed on a sequence of synchrophasor observations. In this case, the posterior C-full-symmetric polytope resulting from the update 104 using the first vector elements can be reused as the prior C-full-symmetric polytope for subsequent sequential updates using the remaining elements of the observation vector.

[0144] Furthermore, when observations are close in time, instead of performing propagation, in order to further expand the interval of observations (see Section 3.3.1), the validity period of the observations is extended so that it is acceptable (in terms of increased conservatism) to exceed the validity period of the aforementioned posterior C-full symmetric polytope. To accommodate this, the filter uses a threshold Before each propagation, the filter determines whether the propagation needs to be performed.

[0145] Algorithm 1 below illustrates how conditional propagation can be implemented in practice: (See also Figure 3 ):

[0146]

[0147] 3.3. Update

[0148] In order to perform the update, the nonlinear observation equation is linearized to form its linear inclusion. The remainder (i.e., linearization error) is evaluated by interval arithmetic (IA), which can be guaranteed to be valid on the prior state range. Then, each element of the observation vector defines a strip in the state space. The prior C-full symmetric polytope can intersect with all these strips simultaneously or sequentially. As the popular set form of the ellipsoid and the full symmetric polytope is not closed at the intersection with the strip, an outer approximation of each intersection result is required, which makes the sequential intersection not optimal. On the other hand, with sequential updates, re-linearizing the observation equation after each intersection may result in significantly tighter updates due to the tightening of the linearization error interval. The results of these two opposite effects depend on the degree of nonlinearity of the observations involved, and are application-dependent as such.

[0149] As a unique feature of C-holosymmetric polytopes, they are closed at intersections with strips. Therefore, updating the entire observation vector with each scalar element simultaneously or sequentially are equivalent operations. Therefore, in the case of C-holosymmetric polytopes, we benefit from the simplicity of sequential updates without losing intersection optimality, and the tightening effect of relinearization.

[0150] Therefore, without loss of generality, scalar observations are considered in the following. Before running the filter recursion, the vector of synchronized observations is converted into a series of scalar observations with the same timestamp. Therefore, in the following, the simpler scalar observation equation is used as an example:

[0151]

[0152] Among them, t y,k is the true observation time of observation k, and is the time of the apparent observation value affected by the timing error.

[0153] Assuming interval error, at time t y,k The first-order approximation of the nonlinear observation mapping that is effective at is in the form of

[0154]

[0155] in

[0156]

[0157]

[0158]

[0159]

[0160] Among them, ∈ h (t k )=∈ h,x (t k )+∈ h,u (t k )∈[∈ h (t k )] is the prior state and input Z u It should be noted that t k is the effective time of the a priori C-holosymmetric polytope. The corresponding linear inclusion is given by

[0161] y(t k )∈H(t k )Z x (t k )+B(t k-1 )Z u (t k )+Z q (t k )+Z([ε h (t k )])+Z([c h (t k )] (26)

[0162] In practice, a linear observation equation and a set of errors containing this linear equation for all a priori C-holistic symmetric polytope states are computed here. This error has several components:

[0163] ·D(t k )Z u (t k ): The effect of system inputs, such as control inputs generated by the autopilot or external disturbances. C-Fully Symmetric Polytope Z u Include all possible inputs or perturbations.

[0164] ·Z[∈ h (t k )]: converted into a linearized error interval of a C-fully symmetric polytope. This fully symmetric polytope contains all the observation errors at different elements of the prior fully symmetric polytope. The observation linearization error at a given element of the prior fully symmetric polytope is the difference between the output obtained by applying the nonlinear observation model to the given element and the output obtained by applying the linear observation model to the given element.

[0165] ·Z([c h (t k)]): A C-holosymmetric polytope representing all constant terms of the linearized observation equations and the measurement noise intervals of the original nonlinear observation equations.

[0166] To perform the update, two sets are intersected. The first set is given by the prior C-fully symmetric polytope (Eq. 18). The second set (see Eq. 35) is defined by the observations. Since we know that the observation z(t y,k ) is included on the right side of Equation 26, so we can replace y(t y,k ) is replaced by z(t y,k ), to form Equation 35. Next, for scalar observations, Equation 35 represents a strip in state space, see Figure 3 Obviously, the second set is not a C-holosymmetric polytope, because it is not closed (the strips extend infinitely). However, its intersection with the a priori C-holosymmetric polytope is a C-holosymmetric polytope (the a posteriori C-holosymmetric polytope).

[0167] It should be noted that although the linearization of the system equations is an approximation step, the corresponding linear inclusion is guaranteed to include the corresponding nonlinear solution.

[0168] 3.3.1. Expansion of the observation range

[0169] Recall that the true observation time is known to have bounded uncertainty, that is, it is known to be in the interval [t y,k ]. In other words, the observation is valid somewhere within this time interval. Since there is no guarantee that the observation is valid for the same time as the a priori C-holosymmetric polytope, the update may not be performed.

[0170] As a simple way to remedy this situation, enlarge the error margin of the observed value Starting from the linear inclusion equation 26, the time between the effective time of the prior C-holosymmetric polytope and the observed value is bounded by the following interval

[0171] [Δt k ] = t k -[t y,k ] (27)

[0172] Among them, the state increment interval

[0173] [Δx(t k )] (28)

[0174] satisfy

[0175]

[0176] if

[0177] t∈t k +[t y,k ] (30)

[0178] We can compute the extended linear inclusion

[0179] y(t y,k )∈H(t k )Z x (t k )+D(t k )Z u (t k )+Z q (t k )+Z([ε h (t k )])+Z([c h (t k )]+Z(H(t k )[Δx]) (31)

[0180] The main goal of the expansion step is to solve the following problem: the effective time of the observation cannot be guaranteed to be consistent with the effective time of the prior C-holistic symmetric polytope. The basic idea to solve it is to expand the error interval of the observation to ensure that its effective time includes the effective time of the prior C-holistic symmetric polytope. This is done by calculating the difference between the observation time and the The upper limit of the state change (Equation 28) between the effective time (Equation 27) is obtained. Next, all known terms are grouped together into an expanded observation error interval, resulting in Equation 32, which can be used by the filter in the update step 104 (Equation 36).

[0181] All known terms are grouped into an expanded observation error interval, which simplifies to

[0182] y(t y,k )∈H(t k )Z x (t k )+Z([ε′]) (32)

[0183] Where, ([ε′]) = D(t k )[Z u (t k )]+[ε h (t k )]+[c h (t k )]+H(t k )[Δx])

[0184] It should be noted that now we need to expand the prior state set Calculate the linearization error ∈ h (tk ). For implementation, it can be in the range Upper evaluation ∈ h (t k ) to be able to use interval arithmetic. Due to the increased complexity of C-holosymmetric polytopes involved in sum operations, this is usually preferable to choosing the interval alternative. Faster.

[0185] Next, for the scalar observation z(t y,k ), we get

[0186] z(t y,k )∈H(t k )Z x (t k )+Z([ε′]) (33)

[0187] and

[0188] H(t k )Z x (t k )∈z(t y,k )-Z([ε′]) (34)

[0189] ∈Z([ε]) (35)

[0190] Where [∈] = z - [∈′], and where Z([∈]) = {rad([∈]), mid([∈]), 0, 0} is the G-rep. of the interval [∈].

[0191]

[0192] in

[0193]

[0194]

[0195]

[0196]

[0197] For an overview of propagation and update timing , see Figure 3 .

[0198] 3.4. Reduction

[0199] From equations 36 to 41, we can see that each update increases the complexity of the state C - the fully symmetric polytope - that is, the number of generators n g and the number of constraints n c In order to control the computational complexity and memory usage, if Zx (t k ) is more complex than The filter performs a reduction after updating if the selected complexity threshold is defined by .

[0200] Algorithm 2 below illustrates how the reduction step 106 can be implemented in practice:

[0201]

[0202] Algorithms 3, 4 below illustrate the different steps performed by the filter according to an embodiment.

[0203]

[0204]

[0205] 4.1D Applications

[0206] In this first example, we consider a target device (or agent) whose motion is constrained along the x-axis of a Cartesian framework, see Figure 4 We estimate its position p and velocity v based only on the distance observations to the beacon. A beacon positioned at a distance d off the x-axis makes the observation equation nonlinear and as such requires a nonlinear estimator such as CZESMF. In this minimal example, there is no uncertainty in the observation time.

[0207] State x = (pv) T The dynamics of is governed by the following equation

[0208]

[0209] Among them, T s =t k -t k-1 And a(t) is the acceleration.

[0210] The distance observation is governed by the following equation

[0211]

[0212] Among them, w k ∈[w]=[-0.224,0.168]m.

[0213] 4.1. Initialization

[0214] We assume that the initial target position and velocity are constrained to

[0215]

[0216] 4.2. Propagation

[0217] The target acceleration is and T s =t k -t k-1 In the case of limit, x(t k ) is given by

[0218]

[0219] Among them, Z q (t k-1 )=QZ([a]).

[0220] Since equation 46 is already linear, we directly obtain the following prior C-holosymmetric polytope:

[0221]

[0222] 4.3. Update

[0223] Since there is no uncertainty in the observation time in this example, t k =t y，k , which is the effective time for us to propagate the state range to the observation value. From Equation 43, the following linear inclusion is formed

[0224] r(t k )∈H(t k )Z x (t k )+Z([w])+Z([∈ h ])+C h (t k ) (48)

[0225] in

[0226]

[0227] and

[0228]

[0229] When computing bounds for the linearization error, the general approach of evaluating the Lagrange residue via interval arithmetic often produces overly conservative bounds due to dependencies. h This provides a tighter range, especially when monotonicity is exploited. The explicit expression for the linearization error is

[0230]

[0231] To check monotonicity, consider the derivative

[0232]

[0233] In order to ∈ h We only need to evaluate its vertex and possible inflection points given by

[0234]

[0235]

[0236] The update is then performed according to Equation 36.

[0237] 4.4. Simulation

[0238] Figure 5 A snapshot of the propagation step 102, the update step 104, and the reduction step 106 is shown in FIG. Figure 6 and Figure 7 The true state trajectory and CZESMF range are shown respectively. Figure 8 and Fig. 9 The corresponding position and velocity ranges are shown respectively. After each update, the state C-full symmetric polytope has been reduced to

[0239] 5.2D positioning application

[0240] In this example, we consider a target device moving in a field with two static ranging beacons (see Fig.10 ). The target device may be an autonomous lawn mower that needs to avoid cutting flowers in a designated area and avoid leaving the perimeter of the field, or the target device may be an autonomous forklift in a modern warehouse.

[0241] The target position x = (x1 x2) T Governed by the following equation:

[0242] x(t k )=x(t k-1 )+q(t k-1 ) (57)

[0243] in

[0244]

[0245] in, is the maximum velocity of the target. This velocity norm constraint is a conservative but easy to obtain dynamic model if the internal dynamics of the moving target device are not better understood.

[0246] The target device periodically receives ranging observations of two beacons at uncertain locations.

[0247]

[0248]

[0249] in

[0250]

[0251] For each beacon i k The range observation value of is controlled by the following equation

[0252]

[0253] The two observations are nominally synchronized at 1 Hz. The imperfect synchronization of the ranging sensor with the wall clock time results in a bounded timing jitter w t (t)∈[-10 -2 10 -2 ]s. That is, a pair of ranging observations arrive approximately (but not exactly) one second apart.

[0254] In order to obtain the periodic state range, the filter recursion is triggered at the upper limit of the observation time interval, that is, (t1, t2, t3, t4, ...) = (0.01s, 0.01s, 1.01s, 1.01s, ...).

[0255] 5.1. Initialization

[0256] The target device is initially inside the field. This allows computing a C-fully symmetric polytope corresponding to an interval covering the field. However, this extremely conservative initial range leads to an extremely conservative filter solution. Therefore, VSIVIA is used to compute a tighter range of the initial state. This one-time step is not very computationally expensive in two dimensions (0.8s and 233 processing frames with ∈=0.1m). For estimation problems with higher state dimensions, it is best to couple VSIVIA with constraint propagation to compute a sufficiently tight range of initial states in an acceptable time.

[0257] The range of sub-layouts obtained by VSIVIA (in Fig.11 The C-fully symmetric polytopes corresponding to the two groups (denoted as “solution” and “undefined” in ) are used to initialize the filter.

[0258] 5.2. Propagation

[0259] The state dynamics are already linear in the state. Therefore, the propagation can be performed directly via the following equation

[0260]

[0261] The state dynamic uncertainty sphere defined by Equation 58 is approximated externally by a C-holosymmetric polytope (see Appendix A below) to obtain Z s (tk-1). An example of the resulting a priori C-holosymmetric polytope is shown in Fig.12 It should be noted that How does it contain the true position at t9? Note also that t9 corresponds to the first propagation update sequence performed close to t=4s, since the two ranging observations come to about 1 Hz and each observation increases the filter recursion index k.

[0262] 5.3. Update

[0263] It simplifies the corresponding ranging beacon i k Each update process is performed in a nominal frame of . The prior C-fully symmetric polytope in the beacon frame is given by the following equation

[0264]

[0265] Then, each ranging observation is controlled by the following equation

[0266] r(t k )=||x(t k )-∈ b ||+w r，k (65)

[0267] To simplify further, we absorb the small beacon uncertainty into the observation error w by the following equation r，k middle

[0268] ||x||-||∈ b ||≥||x-∈ b ||≤||x||+||∈ b || (66)

[0269] Thus,

[0270] r(t k )=||x(t k )||+w′ r，k (67)

[0271] Among them, w′ r，k ∈[w′ r，n ]=[w r ]+[-rad([∈ b ]), rad([∈ b ])].

[0272] From Equation 67 we form the following linear inclusion

[0273] r(t k )∈H(tk )Z x (t k )+Z([w′ r ])+Z([∈ h ])+c h (t k ) (68)

[0274] in

[0275]

[0276] and

[0277]

[0278] 5.3.1. Expansion of the observation range

[0279] Before compensating for timing jitter, from the velocity norm bounds Get the state increment interval [Δx(t k )]

[0280]

[0281] And the expanded observation includes the calculation as follows

[0282]

[0283] The a posteriori C-fully symmetric polytope in the beacon frame is calculated using equations 36 to 41. Afterwards, the a posteriori C-holosymmetric polytope is translated back to the navigation frame

[0284]

[0285] Fig.12 The posterior C-holosymmetric polytope obtained at t9 and the reduced posterior C-holosymmetric polytope are shown. Fig.13 Results obtained with increasing values ​​of the timing noise are shown. Note that the posterior C-holosymmetric polytope is larger due to the increased enlargement of the observation interval.

[0286] The full state range trajectory is displayed in Fig.14 in Fig.15 The increased observation timing noise is shown in . After each update, the state C-holosymmetric polytope has been reduced to By setting Δt to 0.1s, we ensure that propagation is performed only once for each pair of ranging observations, thus reducing the filter running time (conditional propagation).

[0287] 6. Application to 3D peer positioning

[0288] In this example, the state of interest is the relative position between two agents, such as drones flying in close formation. Many aerospace applications require guaranteed relative positioning, such as aerial refueling, energy-efficient formation flying, and nanosatellite swarms. In this example, raw GNSS code and carrier phase observations are obtained, as well as UWB peer-to-peer ranging observations. Both sensors are inexpensive, very low mass - tens of grams - and readily available as Commercial-Off-The-Shelf (COTS) components. We show how CZESMF can be applied to this highly practically relevant localization problem and compare it to existing ensemble state estimation schemes, evaluating its conservativeness on a realistic simulation benchmark.

[0289] In the following time, remove indexes wherever possible to improve readability.

[0290] 6.1. Initialization

[0291] Due to the high linearity of the GNSS code phase double-difference observations, the filter can be initialized with only a very conservative guess about the initial relative position.

[0292] 6.2. Propagation

[0293] We propagate the guaranteed set of relative positions x between two vehicles A and B

[0294] x=p B -p A

[0295] Moderately conservative set-valued dynamic vehicle models are rarely available, mainly due to the lack of established set-theoretic system identification methods. Therefore, vehicle-independent propagation models are highly desirable. Ensemble inertial navigation is an option that deserves further investigation, but is not explored further here. Instead, we rely on time-differenced GNSS carrier phase double difference observations (when available) and a coarse velocity norm constraint model when GNSS observations are sometimes not available. Received independent time-differenced carrier phase observations were initially introduced for accurate flight trajectory reconstruction. Building on this, Time-Differenced Double Phase Difference (T3D) observations were recently first proposed for tightly set-valued correlated trajectory tracking. Millimeter-level carrier noise allows computing an extremely narrow range of position increments between GNSS observations. In the case of relative position estimation, the propagated state set is less conservative, resulting in a smaller set of linearization errors in the update step and ultimately a less conservative posterior state set.

[0296] 6.2.1.T3D Observations

[0297] Single-differencing the carrier phase observations removes most of the atmospheric errors and satellite clock errors.

[0298] Next, the single-difference carrier phase observations of the two receivers A and B and the satellite S are given by

[0299] ΔΦ(p A , p B , p S )=||p A -p S ||-||p B -p S ||+ΔNλ+∈ ΔΦ +ΔTc (75)

[0300] Among them, ∈ ΔΦ Capturing the residuals caused by hardware noise and multipath, ΔT is the differential receiver clock error, c is the speed of light, λ is the carrier wavelength, and ΔN is the differential carrier period ambiguity.

[0301] When using d S =p A -p S and p B =p A +x, we can write it more compactly as

[0302] ΔΦ(x,d S )=||d S ||-||d S +x||+ΔNλ+∈ ΔΦ +ΔTc (76)

[0303] Eliminate differential receiver clock errors by forming a double difference relative to a reference satellite R

[0304]

[0305] Therefore, the double-difference carrier phase observation is a non-linear function of the relative positions of receivers A and B and the vector between the absolute receiver position and the satellite orbit position.

[0306] Assuming the receiver maintains phase lock over two subsequent periods, the ambiguity is a constant that is eliminated by forming the difference of two subsequent double-difference observations, forming the T3D observation

[0307]

[0308] Next we will show how we calculate the enclosing position increment Δx = x(t k )-x(t k-1 )'s C-holosymmetric polytope.

[0309] Direct linearization (using Eq. 78) and computing the interval range of the Laplace residue using interval arithmetic as proposed by E. Scholte et al. in "A nonlinear set-membership filter for online applications" will produce very large over-approximations due to the high dependence.

[0310] This problem can be overcome by linearizing the single-difference equation 76 instead.

[0311] The Lagrangian remainder is then evaluated using IA and the resulting interval is added to the hardware error interval ∈ ΔΦ .

[0312] To simplify the necessary calculations of the Jacobian and Hessian, it is convenient to write Equation 76 as

[0313]

[0314] The enhanced state vector is

[0315]

[0316] and

[0317] E1=[I303], E2=[I3I3], s=[1 -1] (83)

[0318] Then, the Jacobian and Hessian of Equation 81 can be conveniently expressed as

[0319]

[0320]

[0321] Therefore, an interval bound on the model error in the observations due to linearization and parameter uncertainty (uncertain orbital position of the satellite) can be obtained by evaluating the following equation

[0322]

[0323] We proceed to form linear single-difference observations

[0324] ΔΦ=ΔΦ(mid([d S ]), mid([x])) (88)

[0325] +pdvΔΦx(x-mid([x]))+R ΔΦ +∈ ΔΦ +ΔNλ+ΔTc (89)

[0326] Due to [R ΔΦ ] is usually very narrow, on the order of millimeters, and for small spacings, we can linearize x = 0 and have the following more compact form

[0327] ΔΦ=H S x+∈′ ΔΦ +ΔNλ+ΔTc (90)

[0328] It is common in RTK literature, where ∈′ ΔΦ =∈ ΔΦ +R ΔΦ +pdvΔΦx(x-mid([x])). Then we can form the linear double difference observation model without dependencies

[0329]

[0330] in, The observation error and the linearization error are now lumped together.

[0331] Take the observation time as t k-1 and t k The difference between two consecutive carrier phase observations eliminates the constant ambiguity term. k )=x(t k )-x(t k-1 ), the T3D observation is given by

[0332]

[0333] in

[0334]

[0335]

[0336] Here we exploit the fact that due to the large distance between the receiver and the satellite, the geometric matrix The change between two consecutive time steps is small, resulting in a very narrow increment term We have an observation that is linear in the incremental displacement between two time steps. We use this observation to calculate a very accurate bound on the incremental displacement as follows. By grouping the terms together, we can write Equation 96 as the following linear inclusion

[0337]

[0338] T3D Observations Constrain Δx to a polytope P, whose half-space representation is given by

[0339]

[0340] PΔx≤k (101)

[0341] Next, the corresponding C-holosymmetric polytope Z Δx (t k ) is given by Theorem 1 published by J.K. Scott et al. in “Constrained zonotopes: a new tool for set-based estimation and fault detection”. By setting F(t k-1 )=I3、B(t k-1 )=I3、Z Δx (t k ), Z q (t k-1 )=0 This corresponding C-fully symmetric polytope is used in filter propagation.

[0342] 6.2.2. Velocity norm propagation

[0343] In this application example, the inter-vehicle ranging observations are not synchronized with the GNSS observations and arrive at a higher rate. In order to perform the necessary propagation before updating with the ranging observations, a coarse model based on norm-bounded relative velocity is used. For the propagation line-of-sight t k -t k-1 , norm bound v max Define the radius r = v max (t k -t k-1 ) of a sphere. Through the symmetric C-holosymmetric polytope Z with user-defined complexity s To approximate the sphere, we then perform the propagation by setting

[0344] F(t k-1 )=I3、B(t k-1 )=I3、Z u (t k-1 )=0、Z q (t k-1 )=Z S .

[0345] 6.3. Update

[0346] 6.3.3.GNSS Update

[0347] The GNSS code phase single-difference observation is governed by the following equation

[0348] Δρ(pA , p J , p S )=||p A -p S ||-||p B -p S ||+∈ Δρ +ΔTc (102)

[0349] Among them, ∈ Δρ captures the residual caused by hardware noise and residual atmospheric error, ΔT is the differential receiver clock error, and c is the speed of light.

[0350] In using d S =p A -p S and p B =p A +x, we can write it more compactly as

[0351] Δρ(x,d S )=||d S ||-||d S +x||+∈ Δρ +ΔTc (103)

[0352] Eliminate differential receiver clock errors by forming a double difference relative to a reference satellite R

[0353]

[0354] Similar to the carrier phase double difference, the double difference code phase observation is therefore a nonlinear function of the relative position of A and B and the vector between the absolute vehicle position and the satellite orbit position. We follow the same strategy as for the carrier phase observation to form a linear inclusion of the code phase double difference, yielding

[0355]

[0356] It allows updating the state range.

[0357] 6.3.2. Ranging Update

[0358] The range observation between vehicles is given by the following equation

[0359]

[0360] Among them, the position of the UWB ranging antenna center relative to the GNSS antenna center in the corresponding main frame is r A 、r B , the noise of the device hardware measurement value is Since the postures of the two UASs R A , RB The uncertainty of the antenna position r A 、r B , Equation 106 is affected by parameter uncertainties.

[0361] In the following, it is assumed that r A 、r B is small, i.e. the ranging antenna is close to the GNSS antenna and its additional effect on the range observation has been bounded and incorporated into the combined range error w r middle:

[0362] r=||x||+w r (107)

[0363] To linearize Equation 107, we first write the distance observation as

[0364] r=(x T x) 1 / 2 +w r (108)

[0365] The first-order interval Taylor expansion of Equation 108 is the same as the GNSS carrier phase observation

[0366]

[0367] Among them, Jacobi and Hesse are

[0368]

[0369]

[0370] Geometrically, this linearized distance observation corresponds to the projection on the gradient vector The linear range corresponds to the relative position vector on the sphere. While a good approximation at large distances, as vehicles get closer to each other, the increasing curvature of the sphere introduces larger linearization errors, making tight range more important.

[0371] Due to the multiple dependencies in x, the general method of evaluating the Lagrangian residue of Eq. 109 using interval arithmetic applications provides a large over-approximation.

[0372] The wider the linearization error interval, the less likely it is that the linearized observations will help shrink the guaranteed set of states.

[0373] In the following, it is shown that by explicitly evaluating the error between the nonlinear observation equation 107 and its linearization, exploiting monotonicity, tighter bounds can be obtained.

[0374] The explicit linearization error interval is given by

[0375]

[0376] To verify the monotonicity of Equation 111 with respect to [x], consider the interval-valued gradient

[0377]

[0378]

[0379] Geometrically, this is the difference between two unit vectors. Expression (111) is therefore piecewise monotonic, since the signs of the elements of (112) are constant over subintervals of [x]. Since The number of these subintervals is at most 2 3 , which corresponds to a maximum of 27 unique vertices. By evaluating Eq. (111) for each of these vertices, we can calculate [∈ r ]. To quantify the benefit of exploiting piecewise monotonicity, we use two methods to compute the intervals and compare their corresponding radii. Fig.16 This measure is shown during a benchmark task (see Section 6.4), along with the inner approximation of the interval obtained by evaluating Eq. (111) for a large number of samples drawn from the a priori state C-fully symmetric polytope interval range.

[0380] It should be noted that the general scheme produces an interval radius four times larger than the scheme exploiting piecewise monotonicity, which in turn is very close to the interval obtained by sampling.

[0381] 6.4. Simulation

[0382] We evaluate the filter on a benchmark UAS maneuver. It consists of two UASs: a leader and a follower. The follower changes its relative position from the right to the left on the x-axis in a circular trajectory, see Fig.17 The relative positions of the two agents are tracked at 1 Hz. The simulated GNSS and ranging observations are intermixed with the recorded observation errors. For the GNSS observations, these errors 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 known distances.

[0383] 6.4.1. Collective volume

[0384] In all three scenarios, the CZESMF filter provides smaller set volumes than the Iterated Extended Set-membership Filter (IESMF) and the Vector implementation of SIVIA (VSIVIA). We also evaluate the benefit of using T3D observations for propagation compared to a coarse velocity norm bound model. Fig.18 As shown, by using the carrier phase information in the propagation step, the volume of the C-fully symmetric polytope can be reduced by almost an order of magnitude.

[0385] 6.5. Sensitivity to satellite clustering

[0386] The achievable size and shape of the relative position range depends on the geometry of the baseline satellites, as captured by the familiar Dilution of Precision (DOP) accuracy measure. Therefore, it is of interest to understand whether the relative conservatism of the proposed set membership filter is robust to changes in the satellite constellation. A baseline maneuver simulation is performed on a set of randomly selected GPS orbit records. Satellite positions are calculated from a spline fit over the first two minutes of each orbit record. Fig.19 The resulting relative position aggregate volumes of CZESMF and IESMF normalized by the subtiled volumes obtained by VSIVIA are shown. After the initial conversion, CZESMF exhibits less conservatism than VSIVIA in all simulations, and changes very little relative to the GNSS orbits. IESMF is significantly more conservative, with similarly low sensitivity to satellite orbits. These simulations provide some confidence that the observed excellent tightness of the ranges calculated by CZESMF is expected to carry over to the online implementation of the filter.

[0387] 7. Conclusion

[0388] The above-mentioned ensemble state estimation method for nonlinear systems based on C-holosymmetric polytopes significantly outperforms (in terms of ensemble volume) the VSIVIA-based nonlinear offline ensemble estimator and the recent fast ellipsoid ensemble state estimator in simulation scenarios.

[0389] The proposed filter can be applied to a variety of estimation problems, including simultaneous localization and mapping (SLAM), which is an exciting prospect given the current focus on safe visual localization of road vehicles.

[0390] Appendix A: Approximating a 2D or 3D Sphere with C-totally Symmetric Polytopes

[0391] To approximate a sphere of radius r centered at c, we compute the approximation using H-rep and convert it into a C-holosymmetric polytope.

[0392] Given in the interval The uniformly distributed angle is θ=(θ1,θ2,…,θ l ) and the angle uniformly distributed in [0,2π] is φ=(φ1,φ2,…,φ l ), where θ is the zenith angle, φ is the azimuth angle, and l is the complexity parameter. For each angle combination (θ i ,φ j ) Calculate the corresponding normal vector

[0393]

[0394] Each of these normal vectors defines a half-space. The corresponding polytope can then be easily written using H-rep

[0395]

[0396] And it is converted into G-rep. using the method given by J.K. Scott et al. in "Constrained zonotopes: A new tool for set-based estimation and fault detection".

[0397] In two dimensions, for a single set of angles θ=(θ1,θ2,…,θ l ) performs the same method.

Claims

1. A method for estimating a restricted fully symmetric polytope surrounding a motion state of at least one moving target, the method comprising: a) At the first moment (t k-1 ) encloses the first restricted fully symmetric polytope (Z x (t k-1 )) to linearize the nonlinear transformation model at the elements of , thereby producing a linear transformation model, b) Calculate the first restricted fully symmetric polytope (Z x (t k-1 )), wherein the conversion linearization error at a given element of the first restricted fully symmetric polytope is the difference between the output obtained by applying the nonlinear conversion model to the given element and the output obtained by applying the linear conversion model to the given element, c) computing a second restricted fully symmetric polytope that contains all linearization errors, d) Propagate (102) the first restricted fully symmetric polytope to obtain at the first moment (t k-1 ) at the second moment (t k ) A priori restricted fully symmetric polytopes enclosing the motion state The a priori restricted fully symmetric polytope is a linear projection of a hypercube subject to at least one linear constraint, and wherein the propagation of the first restricted holosymmetric polytope comprises: - applying a linear transformation model to the first restricted holosymmetric polytope to produce a third restricted holosymmetric polytope, and - sum the second restricted holosymmetric polytope and the third restricted holosymmetric polytope, e) Update (104) the prior restricted fully symmetric polytope To obtain the second moment (t k ) The posterior restricted fully symmetric polytope (Z') enclosing the motion state x (t k )), wherein updating the a priori restricted fully symmetric polytope includes applying an observation model to the a priori restricted fully symmetric polytope and observation data of the moving target acquired by at least one sensor.

2. The method according to claim 1, wherein: The linearized element of the nonlinear transformation model is the center of the smallest interval containing the first restricted fully symmetric polytope.

3. The method according to claim 1 further includes linearizing the nonlinear observation model at the elements of the first restricted fully symmetric polytope or the prior fully symmetric polytope to generate a linear observation model, wherein the observation model used in step e) is the generated linear observation model.

4. The method according to claim 3, wherein: The linearized element of the nonlinear observation model is the center of the smallest interval containing the prior fully symmetric polytope.

5. The method according to claim 3, further comprising: - calculating the observation linearization error at different elements of the first restricted fully symmetric polytope or the a priori fully symmetric polytope, wherein the observation linearization error at a given element of the first restricted fully symmetric polytope or the a priori fully symmetric polytope is the difference between the output obtained by applying the nonlinear observation model to the given element and the output obtained by applying the linear observation model to the given element, - Compute the fourth restricted fully symmetric polytope (Z([∈ h (t k )])), wherein the posterior restricted fully symmetric polytope (Z' x (t k )) depends on the fourth restricted holosymmetric polytope.

6. The method according to claim 1, wherein: Acquiring observation data of the mobile target at a third time different from the second time, and wherein the method further comprises: - Make the fourth restricted fully symmetric polytope (Z([∈ h (t k )])) to obtain an enlarged restricted fully symmetric polytope (Z([∈'])) which forms a strip in the state space and takes into account all possible changes of the state of motion between the second moment and the third moment and possibly taking into account the measurement value noise caused by the sensor, -The posterior restricted fully symmetric polytope (Z' x (t k )) is calculated as a priori restricted fully symmetric polytope ) and the enlarged restricted holosymmetric polytope (Z([∈'])).

7. The method according to claim 1, further comprising: -Determine the a posteriori restricted fully symmetric polytope (Z' x (t k ))'s complexity level, - If the complexity level is greater than a threshold, the reduction process is applied to the posterior restricted fully symmetric polytope (Z' x (t k )) to obtain a polytope with lower complexity than before the reduction and containing the posterior fully symmetric polytope (Z' x (t k )) of the posterior holosymmetric polytope (Z x (t k )).

8. The method according to claim 1 further comprises, when subsequently performing steps a) to e), converting the posterior restricted fully symmetric polytope (Z') x (t k )) is used as the first restricted fully symmetric polytope and the second moment is used as the first moment.

9. The method according to claim 1, further comprising - calculate the maximum time interval between the first moment and the moment when the sensor acquires the observation data, -If the maximum time interval is greater than the predetermined duration, applying the linear transformation model to the predetermined restricted holosymmetric polytope to generate the a priori restricted holosymmetric polytope, otherwise using the predetermined restricted holosymmetric polytope as the a priori restricted holosymmetric polytope.

10. The method according to claim 1, wherein: The observation data includes at least one of the following data acquired by a GNSS receiver: a pseudorange, an acquired carrier phase, and a distance between a mobile target and another mobile target.

11. The method according to claim 1, wherein: The motion state includes motion data of a moving object relative to another moving object.

12. The method according to claim 1, wherein: The first restricted fully symmetric polytope is computed using a vector set inversion algorithm with interval analysis.

13. A computer program product comprising instructions which, when the program is executed by a computer, cause the computer to perform the method according to any one of claims 1 to 12.

Images