TECHNICAL FIELD

The invention relates to the technical field of set-membership state estimation techniques.

The invention deals with a method of estimating a constrained zonotope containing a state representing motion of at least one mobile target governed by a nonlinear model.

BACKGROUND

Unmanned vehicles have seen a considerable rise in their level of autonomy over the last years, leading up to present on-going efforts to integrate unmanned aircraft systems (UAS) into the national airspaces and of autonomous cars into the general traffic. It has been known for quite some time that cooperative motion strategies can unlock a number of benefits with considerable economic potential. Automatic platooning of ground vehicles can increase highway capacity, avoid traffic jams and reduce consumption of individual vehicles. Tight formation flight has been demonstrated to enable significant aircraft range enhancements. The most prominent examples include range improvements thanks to the exploitation of wake vortex effects in tight aircraft formations and automatic in-air refueling. To execute these cooperative motion strategies in a safe and efficient manner, relative positions of vehicles need to be estimated with decimeter level, if not centimeter level accuracy.

For tight formation flight, as standalone positioning based on Global Navigation Satellite Systems (GNSS) does not provide the necessary accuracy, a variety of differential positioning techniques has been proposed in the past. Numerous sensor fusion approaches can be found in the literature of the last decades, employing inertial measurement units, differential GNSS with or without exploiting the phase information, machine vision or combinations of both. More recently, inexpensive, decimeter-precision Ultra-Wideband (UWB) ranging sensors have been successfully shown to improve the robustness of airborne Real Time Kinematics (RTK) solutions.

Common among these existing approaches is their reliance on probabilistic filtering techniques such as the Unscented (UKF) or the Extended Kalman Filter (EKF). As is well known, many practical nonlinear estimation problems violate the underlying assumption of both the UKF and the EKF, notably the key assumption of distributions of process model and observation model uncertainties being known. In reality, typically only approximations of the stochastic properties of both sources of uncertainty are available. In spite of this inherent and well-known issue, both EKF and UKF have been demonstrated to work well in practice, providing accurate mean state estimates.

However, when it comes to safe cooperative motion of multiple agents in close proximity, the consistent computation of filter state uncertainty is of even more crucial importance than accurate point estimates. Due to the approximations involved (the linearization steps performed by the EKF and the unscented transform of the UKF), guaranteeing filter consistency is an open problem for nonlinear estimation problems when employing the EKF or UKF. However, we consider algorithms that can compute guaranteed enclosures of vehicle states a key element for the further safe integration of autonomous vehicles into heterogeneous environments such as mixed air traffic and common road traffic.

Set-membership state estimation techniques offer an interesting alternative to probabilistic frameworks in this regard. Set-membership estimators rely on a set-valued process model, assuming that observation and process model uncertainties are contained in known compact sets, e.g. intervals. No assumptions about the error distribution within these sets are required. The exact set-membership estimation problem is usually computationally intractable for nonlinear systems. Research is directed at finding implementable but tight outer approximations of the involved set operations. Numerous algorithms have been proposed over the last decades, employing set shapes such as ellipsoids or intervals. The common challenge all set-membership estimators are facing consists of finding a tradeoff between conservatism - the degree of over-approximation introduced by representing complex sets by simpler, computationally tractable ones - and computational effort.

In spite of the merits of set-membership approaches, their application to relative agent localization has received little attention. A cooperative set-membership localization algorithm for ground vehicles based on the Set Inversion Via Interval Analysis algorithm (SIVIA) has been proposed. SIVIA and related nonlinear set inversion algorithms provide tight enclosures when applied to state estimation algorithms, with their degree of approximation only limited by available computing resources. However, the complexity of these algorithms increases however typically fast with the number of states, limiting their usefulness beyond 2D ground vehicle localization applications.

Ellipsoids provide good computational performance thanks to their constant complexity and have been successfully applied to relative agent localization in simulation. Recently, zonotopes have received increased attention for guaranteed state estimation. Zonotopes provide more flexibility than ellipsoids, their complexity being a design parameter. One inherent problem of zonotopes is their symmetry, which introduces additional conservatism whenever actual state sets exhibit strong asymmetries. A second problem is that the intersection of two zonotopes does not always result in a zonotope, thus, as it is the case for ellipsoids, outer approximations of intermediate sets must be computed at each filter recursion, potentially involving solving a convex optimization problem.

SUMMARY OF THE INVENTION

The invention aims at solving or at least mitigating both of these deficiencies.

A first aspect of the invention is the method of claim 1.

The a priori zonotope used in the method of claim 1 is a constrained zonotope. This class of zonotopes allows to better approximate asymmetrical set shapes. As an additional advantage, they are closed under intersection and closed under the Minkowski sum operation.

The method according to the first aspect of the invention may comprise the following optional features.

The element at which the nonlinear transition model is linearized may be a center of a smallest interval enclosure containing the first constrained zonotope.

The method may further comprise linearizing a nonlinear observation model at an element of the first constrained zonotope or the a priori zonotope, so as to produce a linear observation model, wherein the observation model used at step e) is the linear observation model produced.

The element at which the nonlinear observation model is linearized may be a center of a smallest interval enclosure containing the a priori zonotope.

The method may further comprise:

• computing observation linearization errors at different elements of the first constrained zonotope or the a priori zonotope, wherein an observation linearization error at a given element of the first constrained zonotope or the a priori zonotope is a difference between an output resulting from applying the nonlinear observation model to the given element and an output resulting from applying the linear observation model to the given element, • computing a fourth constrained zonotope that contains all the observation linearization errors computed, wherein the a posteriori constrained zonotope depends on the fourth constrained zonotope.

The observation data of the mobile target may have been acquired at a third moment different from the second moment, and the method may further comprises:

• inflating the fourth constrained zonotope so as to obtain an inflated constrained zonotope forming a strip in the state space and taking into account all possible variations of the motion state between the second moment and the third moment, and possibly taking into account measurement noise induced by the sensor,

• computing the a posteriori constrained zonotope as an intersection between the a priori constrained zonotope and the inflated constrained zonotope.

The method may further comprise

• determining a complexity level of the a posteriori constrained zonotope,

• if the complexity level is greater than a threshold, applying a reduction process to the a posteriori constrained zonotope) so as to obtain an a posteriori zonotope having a lower complexity than and containing the a posteriori zonotope before the reduction.

The method may further comprise using the a posteriori constrained zonotope as first constrained zonotope and using the second moment as first moment in a subsequent execution of steps a) to e) defined in claim 1.

The method may further comprise

• computing a maximum time interval between the first moment and a moment at which the observation data has been acquired by the sensor,

• if the maximum time interval is greater than a predetermined duration, applying the linear transition model to the predetermined constrained zonotope so as to produce the a priori constrained zonotope, else using the predetermined constrained zonotope as the a priori constrained zonotope.

The observation data may comprise at least one of the following data acquired by a GNSS receiver: a pseudo-range, a carrier phase acquired, a range between the mobile target and another mobile target.

The motion data may include motion data of the mobile target relative to another mobile target. The first constrained zonotope may be computed using a Vector Set Inverter Via Interval Analysis algorithm.

A second aspect of the invention is a computer program product comprising instructions which, when the program is executed by a computer, cause the computer to carry out the method of the first aspect of the invention.

GENERAL DESCRIPTION OF THE DRAWINGS

Figure 1 schematically represents a device for estimating a set enclosing the motion state of a mobile target;

Figure 2 illustrates the steps of an example method for estimating a C-zonotope enclosing a state x of a system;

Figure 3 illustrates a sequence of operations of a propagation step of the method shown in Figure 2;

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

Figure 5 represents snapshots of the propagation, update and reduction steps of the method shown in Figure 2;

Figure 6 is a graphical representation of a true state trajectory of the target device;

Figure 7 is a graphical representation of a CZESMF enclosure of the target device;

Figure 8 is a graphical representation of a position interval enclosure of the target device;

Figure 9 is a graphical representation of a velocity interval enclosure of the target device;

Figure 10 is a graphical representation of the position of another target device;

Figure 1 1 represents an interval enclosure of a subpaving obtained by VSIVIA;

Figure 12 represents an obtained a priori C-zonotope along with an obtained a posteriori C-zonotope and a reduced a posteriori C-zonotope for the target device of Figure 10;

Figure 13 represents the a priori and a posteriori C-zonotopes in the case of an increased observation timing noise;

Figure 14 represents a full state enclosure trajectory for the target device of Figure 10; Figure 15 represents a full state enclosure trajectory for the target device of Figure 10 in the case of an increased observation timing noise;

Figure 16 is a graphical representation of two radii of range linearization error interval measurements obtained by two distinct approaches; Figure 17 represents the positions of two other target devices during a simulation;

Figure 18 represents set volumes obtained by speed norm prediction and by T3D prediction, using a GNSS update or a GNSS+UWB update;

Figure 19 represents relative position set volumes obtained using a CZESMF filter and using an IESMF filter.

DETAILED DESCRIPTION OF AT LEAST ONE EMBODIMENT

The following description is structured as follows: After defining notations (section 1 ) and recalling the problem of set-membership state estimation (section 2), a computationally efficient state set estimation method based on constrained zonotopes (C-zonotopes) will be described (section 3). Then three practical applications of the method will be given as examples:

• a simple 1 D example (section 4),

• a slightly more involved 2D localization problem (section 5),

• a 3D peer-to-peer localization benchmark problem using differential GNSS and peer-to-peer ranging observations (section 6).

1. Notations

To accommodate this complexity, the notation x(t ₍ ) familiar from continuous-time estimation theory will be used hereinafter. The discrete nature of filter execution is captured by indexing the time. Specifically, t _k designates the time for which a priori and a posteriori state sets are computed by the processing unit. For a vector x(t), the time t indicates the time of validity of this vector.

Correspondingly, a set Z(t _k ) is valid at time t _k , i.e. x(t) e Z(t _k ) if t = t _k . If the time of validity is an interval [t _k ] , the set is valid for all times in this interval, i.e. x(t) e Z(t _k ) if t e [t _k ] . Besides, a matrix J _nxm denotes a matrix of n rows and m columns where all elements are one. The identity matrix of size n is denoted by I _n and a square zero matrix of size n by

On ·

The dot product of two vectors a, b is denoted < a, b >.

Interval matrices are denoted by [M] = [M, M] where the elements of M_, M e M ^nxm are real lower and upper bounds, respectively, of the interval element of [M]. For m = 1 and n > 1 we speak of an interval vector, i.e. [p]. Scalar intervals are denoted by [x] = [x, x] where lower and upper bounds x, x e M. For an interval [M], M denotes a realization of the interval, i.e. M e [M].

Where additive errors are assumed, it can be more convenient to write interval matrices 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 rad(M) = M— mid(M). Wherever terms and interval terms appear in the same expression, such as [c] = a + [b], it is assumed that the terms (a) represent degenerate intervals ([a, a]) and all operations in the expression are interval operations.

As C-zonotopes are not closed under nonlinear transforms, first order Taylor approximations of 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 enclosures, the concept of linear inclusion is used, as do other set estimation approaches, see e.g. “A nonlinear set-membership filter for online applications”, by E. Scholte et al. A linear inclusion of a nonlinear function is a set-valued linear function that is guaranteed to contain the solution of the nonlinear function over a certain input set. As an example, consider the function y = Kx) (1 )

With x E X, i.e. x is contained in some compact set X. A linear function y = y ₀ + Hx + e (2) is a linear inclusion of equation 1 if y e y ₀ + Hx + [e] (3) for all x E X, i.e. the set-valued solution [y] of the linear set valued function always contains the nonlinear solution. 2. Problem statement

A motion of a mobile target can be described by a motion state comprising motion data. The motion data may for instance include: a position of the target, a speed of the target, an acceleration of the target, other data, or a combination thereof. The motion state evolves over time when the mobile target moves.

In the following, the mobile target 1 may be referred to as the“system”, and the motion state as the“state” for the sake of simplicity.

Referring to figure 1 , a device 1 for estimating a set enclosing the motion state of the mobile target comprises a processing unit 2 and an observation unit 4.

The processing unit 2 is configured to execute a computer program comprising code instructions implementing an estimation method that will be described hereinafter. This computer program will be referred to as the “CZESMF filter” (Constrained Zonotope Extended Set-membership Filter) or“the filter” hereinafter since it actually acts like a filter.

The filter is configured to use a predetermined transition model (also referred to as “propagation model” or“dynamics model” hereinafter) and a predetermined observation model.

The processing unit 2 is configured to operate at different processing moments, such that different sets enclosing the motion state of the mobile target at said different moments can be estimated.

The processing unit 2 can be of any type: one or more processor, one or more microprocessor, FPGA, ASIC, etc.

The observation unit 4 comprises at least one sensor configured to observe the mobile target. The observation unit 4 is therefore configured to output observation data related to the mobile target. The observation unit is also configured to operate at different observation moments, such that different observations can be acquired at said different moments.

The purpose of set-membership state estimation is computing a region X(t ) c M ⁿ in which the state x(t) of the system is guaranteed to reside at a time t.

2. 1. State dynamics In discrete state estimation, it is often assumed that the system state dynamics are governed by a static model of the type (tfc-i)) + q(t _k - 1) (4) with the previous state x(t _fe _ i) e M ⁿ , the system input u^.i) e M ^p , the propagation model error q(t _k - ₁ ) e Q(t _fc _ _x ) c M ⁿ , and a constant sampling time T _s = t _k

In practice, irregular observation times lead to irregular sampling times, i.e. propagation horizons in the filter. Depending on the sampling time, different propagation models may yield state enclosures of differing tightness. A practical example from navigation is inertial dead-reckoning localization, where the double integration of observed accelerations yields very tight short-term position enclosures, but sensor biases lead to quadratic growth of position enclosures in the long term. In contrast, a simple velocity- norm bound model of vehicle dynamics (see section 5 for an example) leads to linear growth of position enclosures over time, thus possibly providing tighter enclosures for longer propagation horizons.

Another example is treated in section 6, where tight relative position propagations using time differential GNSS phase observations are available only at the GNSS receiver's sampling times, while in between GNSS observations only a crude velocity-norm bound propagation model is available.

We accommodate multiple propagation models by assuming a very general time-varying state dynamics model that is a nonlinear map l" x l ^p x l x l -» l" x(t _k ) = /(xCt _f e-i), uCt _f c-i), t _f e-i, t _k ) + q(t _f e-i) (5)

The arguments t _k and t _k-t indicate that the model performs a propagation of the system state over the time horizon from t _k-t to t _k as well as the time-varying nature of /(). Thanks to the propagation model being allowed to be time-varying, the filter can employ the least conservative one from a bank of available models at each propagation step 102 (see Fig 2).

2.2. Observations

In practice, observations made by the observation unit 4 are often nor perfectly synchronous nor perfectly periodic. As a further complication, the observation times may only be approximately known, e.g. due to lack of hard-real-time communication protocols. Observation time uncertainty leads to a subtle but serious issue not addressed by existing set-membership state estimation schemes. Propagated state enclosures are guaranteed to contain the state only at the selected propagation time t _k . We call this the time of validity of a given state enclosure Z _x (t _k ) . Since an observation corresponds to some unknown time within an interval, updating the a priori enclosure with this observation leads to a state enclosure that is not guaranteed to be valid at any time, even if t _k falls within the observation time interval. This is simply due to the fact that Z _x (t _k ) and the state space constraints corresponding to the observation do not concern the same point in time. Neglecting observation time uncertainty so leads to invalid state enclosures after only one update. A remedy for this issue is presented hereinafter.

The observations are assumed to be governed by the nonlinear map P x P x l -»

Where y(t ) e M ^m(i) is a vector of sensor observations, w(t) e W is the corresponding measurement error, t _y (t ) is the observed observation time stamp, w _t (t) e [w _t ] is the observation time error and t is the true observation time. The observation time stamp error w _t (t) covers not only timing jitter but also communication delays e.g. due to package encoding/decoding delay over a digital data link. All times are wallclock time.

Note that no assumptions are made about the measurement model error other than its being contained in some known set W(t). As such, it covers hardware noise, biases and modeling errors. Note also that both the observation model h(... ) and the size m(t) of the observation vector can be time-varying.

While y(t) denotes observations provided by the model, actual measurements received from sensors are denoted by z(t).

3. Extended set-membership filter based on C-zonotopes

The most popular computationally efficient set representations used in set state estimation are linear transformations of basic geometric entities, such as ellipsoids - transformed unit hyperballs - and zonotopes - transformed unit hypercubes. The inherent symmetry of both the hyperball and the hypercube limits both the ellipsoid's and the zonotope s ability to tightly approximate asymmetric or generally irregularly shaped sets. Further aggravating the situation, both are not closed under the intersection operation of the filter update. As a consequence, outer approximations of the result of each intersection need to be computed. Constrained zonotopes mitigate both of these issues. They are linear transformations of a constrained unit hypercube and as such can be asymmetric. They are closed under intersection, i.e. the intersection of two C-zonotopes is another C-zonotope, although of higher complexity.

A C-zonotope Z = {G, c, A, b} c M ⁿ is defined by its generator matrix G, its center c and the constraint parameters A, b of the unit hypercube, and describes the set :

Z = {ΰx + c I ||x|| _oo £ 1 _< Ax = b} (8)

Each constraint represents a plane intersecting the unit hypercube. The resulting intersection is then projected via the generator matrix G and finally translated by the center vector c.

C-zonotopes are an alternative parameterization of convex polytopes. Besides, C- zonotopes are a superset of regular zonotopes, as each C-zonotope without constraints is a regular zonotope. For briefness and in accordance with the existing literature, the C- zonotope representation of a convex polytope is denoted as G-rep. and their half-space representation as H-rep.

The interval enclosure of a C-zonotope Z is denoted by [Z] and the G-rep. of an interval [a] by Z([a]).

Referring to figure 2, a method for estimating a C-zonotope enclosing the state x of the system comprises the following main steps carried by out by the filter, based on a predetermined C-zonotope Z _x {t _k _^) enclosing the state at time

• a propagation step 102 wherein the predetermined C-zonotope is processed so as to obtain an a priori C-zonotope Zx(t _k ) enclosing the state at time t _k ;

• an update step 104 wherein the a priori constrained zonotope Zx(t _k ) is combined with observation data acquired by the observation unit 4 so as to obtain an a posteriori C-zonotope Z' _x (t _k ) enclosing the state at time t _k .

The a posteriori C-zonotope enclosing the state x at the time t _k and taking into account all observations up to and including t _k is denoted as Z' _x (t _k ) = {C _x (t _k ), c' _x (t _k ), A' _x (t _k ), b' _x (t _k )} (9)

While the a priori C-zonotope enclosing the state x at the time t _k , before performing the update, is denoted by

It will be detailed below that the propagation step 102 and the update step 104 may use a linear transition model and a linear observation model, respectively, said linear models being obtained by linearizing nonlinear models at specific points.

Moreover, the method may further comprise a reduction step 106 wherein the filter converts the a posteriori C-zonotope Z' _x (t _k ) into another C-zonotope Z _x (t _k ) of lower complexity but containing the C-zonotope Z’ _x (t _k ).

The propagation step 102, the update step 104 and the reduction step 106 are repeated recursively. More precisely, the method comprises multiple recursions, each recursion performing the propagation step 102, the update step 104 and the reduction step 106.

The very first recursion of the method is preceded by an initialization step 100 wherein the initial C-zonotope Z _x (t _k ) is determined.

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

3.1. Initialization

The initial state is known to be contained in some C-zonotope Z _x (t _k ), the first a priori state enclosure.

3.2. Propagation

At the propagation step 102, executed for k >1 , the filter computes the a priori state set Z _x (t _k ). It is guaranteed to contain the state at time t _k > t _k-1 given the preceding a posteriori state set _¾ .(t _fe-1 ).

As a particularity of uncertain observations, the question of how to choose the time for which to compute the a priori C-zonotope arises. It can even lie in the future at the time when a filter recursion is started, to compensate for filter execution time. Depending on the execution time of the recursion (propagation and update), it can however make more sense to set t _k close to the observation time and perform a final propagation to compensate for execution time. Some options are a nominal, periodic observation time, if observations are nominally periodic, or the center of observation time intervals. As a beneficial effect of the first choice, filter results become periodic, hiding the observation time uncertainty inside the filter. Since C-zonotopes are not closed under nonlinear maps, we start, analogous to an Extended Kalman Filter (EKF), by forming a first order Taylor approximation of the state dynamics model equation (5) about an operating point u(£ _f c-i)· The centers of the interval enclosure of the preceding a posteriori state and input enclosures are chosen as operating point: x(t _k-t ) = mid([Z _x (t _k-1 ) ) (11 ) u(.t _k -i) = middZuit^)]) where Zu = {Gu, Cu, Au, bu} (12)

Since C-zonotope centers do not even necessarily lie within their C-zonotope, this choice tends to lead to smaller, but does not minimize linearization errors.

Linearization leads us to the familiar first order Taylor expansion x(tk) = f(x(t _k -i), u(tk-i), tk, tk _+i ) + q(t _k -i)

+ F(tk-i)(x(t _k -i) - x(tk-i)) + B(tk-i)(u(t _k -i) - u(tk-i))

+ q(t _k -i) + c ^tk-i) (13) Where the linearization error is (t _fe ) e [c (t _fe )] and with

Cf(tk-i)= f(x(t _k -i), fl(tk-i), tk, tk _+i ) - F(tk-i)x(tk-i) - B(tk-i)u(tk-i) (16)

The corresponding linear inclusion being given by x(tk) e F(tk-i )Zx(tk-i ) + B(tk-i )Zu(tk-i ) + Z _q (tk-1 ) + Z([ _£/ (tk-i )] ) + Z([Cf (tk-i )] (17) a propagation from time t _k-t to time t _k is then performed as

Z _x (t _k ) = F(tk-i)Zx(t _k -i) + B(tk-i)Z _u (tk-i) + Z _q (tk-i) + Z([C (t _k -i)]) + Z([c _f (t _k -i)] (18) The matrix multiplication and sum operations in equation 18 are the set operations on C- zonotopes defined in“Constrained zonotopes: A new tool for set-based estimation and fault detection”, by J.K. Scott et al. The linearization error and the model uncertainty of the nonlinear state dynamics model are enclosed by (¾_ _] ) and _u (t _fe-1 ) is a C-zonotope enclosure of any uncertain input. The propagation uncertainty term q(t _k ) actually corresponds to the process noise of an EKF, and the C-zonotope Z _q (t _k ) corresponds to the process noise covariance matrix Q of an EKF.

[e (i _fe )] can be regarded as a set of transition linearization errors at different elements of C-zonotope Z _x {t _k _^). A transition linearization error at a given element of Z _x {t _k _^) is a difference between an output resulting from applying the nonlinear transition model to the given element and an output resulting from applying the linear transition model to the given element.

Besides, the expression (¾_ _] ) + Z([e^ (¾_ _! )]) + Z([<y (¾_ _! )] above mentioned may be regarded as a C-zonotope that contains all the linearization errors. Moreover, the expression F(t _fe-1 ) _¾ .(i _fe-1 ) + F(t _fei ) _u (i _fe-1 ) can be regarded as a C-zonotope resulting from applying the linear transition model to Z _x {t _k _ ). The a posteriori zonotope is the sum of both expressions.

3.2. 1. Conditional propagation In recursive filtering schemes, the purpose of the propagation step 102 is to obtain a state enclosure that is valid at the time - or in our case close to - the time when observation about the system's state are made.

In the present method, there are two situations where a propagation does not need to be performed, reducing filter run time. The first situation occurs when performing updates 104 over a sequence of synchronous vector observations. In this case, the a posteriori C-zonotope resulting from the update 104 with the first vector element can be re-used as a priori C-zonotope for the following sequential updates with the remaining elements of the observation vector.

Besides, when observations arrive close in time, instead of performing a propagation, it can be acceptable (in terms of added conservatism), to inflate the observation interval (see section 3.3.1 ) more, to extend the observation's time of validity over that of the preceding a posteriori C-zonotope. To accommodate this condition, a threshold At ³ 0 is used by the filter. Before each propagation, the filter determines whether a propagation needs to be performed.

Algorithm 1 below illustrates how a conditional propagation can be implemented in practice: (see also figure 3):

Algorithm 1: conditionalPropagatioe

Input

Output: performPropagation

1 Test for closeness

2 if sup then

3 performPropagation = false

4 else

s performPropagation = true

> Test for observation synchronicity

then

agation = false

3.3. Update

To perform the update, the nonlinear observation equation is linearized to form a linear inclusion of it. By evaluating the remainder (i.e. the linearization error) by Interval Arithmetic (IA), it is guaranteed to be valid over the a priori state enclosure. Each element of the observation vector then defines a strip in state space. The a priori C- zonotope can either be intersected with all these strips at once or sequentially. Popular set formalisms such as ellipsoids and zonotopes are not closed under intersection with a strip, and as such require an outer approximation of each intersection result, which renders the sequential intersection suboptimal. On the other hand, with sequential updates, re-linearization of the observation equation after each intersection can lead to significantly tighter updates due to tightened linearization error intervals. The result of these two opposing effects depends on the degree of nonlinearity of the observations involved and as such is application-dependent. As an unique feature of C-zonotopes, they are closed under intersection with a strip. In consequence, updating with the whole observation vector at once or sequentially with each scalar element are equivalent operations. As a consequence, using C-zonotopes we can benefit from both effects: the simplicity of sequential updates without loss of intersection optimality and the tightening effect of re-linearization.

Without loss of generality, scalar observations are therefore considered in the following. Vectors of synchronous observations are converted into a sequence of scalar observations with identical timestamps before running the filter recursion. Thus, in the following, the simpler scalar observation equation is taken as an example:

Where t _{y k} is the true observation time of observation k and t _{t k} is the apparent observation time subject to timing error.

Assuming interval-valued errors, a first-order approximation of the nonlinear observation map valid at time t _{y k} has the form

+ H(tk)(x(t _k ) - x(t _k -i)) + D(tk)(u(t _k ) - fl(tk))

+ w(tk) + £ _ft f(tk) (20)

= H(tk)x(t _k ) + D(tk)u(t _k ) + f _ft f(tk) + Ch(tk) (21 )

With x(tk) = mid([ x(tk)]) (23) u(tk) = mid([ u(tk)]) (24)

Ch(tk) = h(x(t _{x fe} ), u(t _{X k} ), t _{X k} ) - H(tk)x(t _k -i) - D(tk)u(t _k ) + w(tk) (25) where e _h (t _k ) = e _{h x} (t _k ) + e _{h u} (t _k ) e [e _h (t _k )] is the linearization error over the a priori states Z _x and inputs Z _u . Note that t _fe is the time of validity of the a priori C-zonotope. The corresponding linear inclusion is then given by y(t _k ) e H(t _k )Z _x (t _k ) + B(t _k-i )Z _u (t _k ) + Z _q (t _k ) + Z([e _/i (ti _< )]) + Z([C _h (t _k )] (26) In fact, a linear observation equation and a set that contains the error of this linear equation for all states that are in the a priori C-zonotope are computed here. This error has multiple components:

D(t _k )Z _u (t _k ): The impact of system inputs, e.g. control inputs generated by an autopilot or external perturbations. The C-zonotope Z _u contains all possible inputs or perturbations.

Z[e _h (t _k )]: The linearization error interval converted into a C-zonotope. This zonotope contains all observations errors at different elements of the a priori zonotope. An observation linearization error at a given element of the a priori zonotope is a difference between an output resulting from applying the nonlinear observation model to the given element and an output resulting from applying the linear observation model to the given element.

Z([c _h (t _k )]): a C-zonotope that represents all the constant terms of the linearized observation equation and the measurement noise interval of the original, nonlinear observation equation. To perform the update two sets are intersected. The first one is given by the a priori C- zonotope (equation 18). The second one (see equation 35) is defined by the observation. Since we know that the observation z(t _{y k} ) is contained in the right side of equation 26, we can replace y(t _{y fe} ) by z(t _{y k} ) in equation 26 to form equation 35. For a scalar observation, equation 35 then represents a strip in state space, see figure 3. Obviously, the second set is not a C-zonotope, since it is not closed (the strip runs infinitely). However, its intersection with the a priori C-zonotope is a C-zonotope (the a posteriori C-zonotope).

Note that while the linearized system equations are an approximation, the corresponding linear inclusions are guaranteed to contain the corresponding nonlinear solutions. 3.3.1. Observation interval inflation

Recall that the true observation time is known with bounded uncertainty, i.e. is known to fall within an interval [t _{y k} ]. In other words, the observation is valid somewhere within this time interval. Since the times of validity of the observations and the a priori C- zonotope are not guaranteed to coincide, the update may not be performed.

As a simple approach to remedy this situation, the observation error interval [ e _tk ] is inflated, extending an observation's time of validity. Starting from the linear inclusion equation 26, the time between the time of validity of the a priori C-zonotope and the observation is bounded by the interval

[At _f c]— t _k [ty. _f c] (27)

With a state increment interval

[A (t _f e)] (28) that satisfies

if t E t _k + [ t _{y k} ] (30) we can compute an extended linear inclusion y(t _y,k ) e H(t _k )Z _x (t _k ) + D(t _k )Z _u (t _k ) + Z _q (t _k ) + Z([£ _ft (t _k )])

₊ Z([c _h (t _k )] - Z(H(t _k )[Ax]) (31 )

The main objective of the inflation step is to solve the following problem: the time of validity of the observations is not guaranteed to coincide with the time of validity of the a priori C-zonotope. The basic idea to solve it is to inflate the observation error interval to ensure that its time of validity includes the time of validity of the a priori C-zonotope. This is obtained by computing an upper bound of the state variation (equation 28) between the observation time and the time of validity (equation 27) of Z _x (t _k ). Then all known terms are grouped together into one inflated observation error interval leading to equation 32, which can be used by the filter in the update step 104 (equation 36).

All known terms are lumped together into one inflated observation error interval, simplifying to y(t _y,k ) 6 H(t _k )Zx(t _k ) - Z([e']) (32) where ([e']) = D(t _k )[Z _u (t _k )] + [s _h (b,)] + [c _h (t _k )] + H(t _k )[Ax]) Note that the linearization error e _h (t _k ) now needs to be computed over the inflated a priori state set Z _x (t _k ) + Z[Ax(t _k )] . For implementation, e _h (t _k ) can be evaluated over the interval enclosure [Z _x \ + [Dc] to be able to use interval arithmetic. This is generally faster than choosing the alternative interval Z _x + Z\Ax\ due to the increase in C-zonotope complexity involved in the C-zonotope sum operation.

With the scalar observation z(t _{y k} ) we then have z(t _y,f e) e H(t _k )Z _x (t _k ) - Z([e']) (33) and

H(t _k )Z _x (t _k ) e z(t _y,k ) - Z([e']) (34) e Z([e]) (35) with [e] = z— |V] and where Z([e ]) = { rad([e]), mid([e ]), 0,0} is the G-rep. of the interval [el

Z’ _x (t _k ) = _x (t _k ) n _H Z([e]) (36)

= {G’ _x (t _k ), c’ _x (t _k ), A’ _x (t _k ), b’ _x (t _k )} (37)

With

G’ _x (t _k )= [G _x (t _k ) 0] (38) c’ _x (t _k ) = c _x (t _k ) (39)

See figure 3 for an overview of the propagation and update timing.

3.4. Reduction As can be seen from equations 36-41 , each update increases the complexity - i.e. the number of generators n _g and the number of constraints n _c - of the state C-zonotope. To control computational complexity and memory footprint, the filter performs a reduction after the update if the complexity of Z _x (t _k ) exceeds a certain chosen complexity threshold defined b n _g , n _c .

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

Algorithm 2: reduce

2 Reduce Z (t _k ) to Z _x( t _k ) by reduction algorithms

given in [1 , 25]

3 else

4 ¾4) = Z _x '(t _k )

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

Algorithm 3: This loop runs until the filter stops exe cuting

Compute Z _{A (} 6 ) with VSIVIA or from a priori knowledge

k = 1

Loop

Wait for incoming observation z e K ^m

Select propagation time t _k

i <— 1 to m do

if k == 1 then

Z = Z _x (h)

else

Z = Z _x (t _k - 1 )

Z _x (t _k ) = czesmf_recursion(Z, ¾·)

k += 1

Algorithm 4; czesmf recursion

Input

Output: Z _x (t _k )

if conditionalPropagation(At, {t _y ^ ¾_i ) then

Compute linear inclusion eq. 26

Compute a priori Z _x (t _k ) with eq. IS

else

Z _x (t _k ) = Z _x (t _k - ₁ )

Compute linear inclusion eq. 26

Compute inflated observation error interval with eq. 31 Compute a posteriori Z' (¾) with eq. 36

Reduce: Z _x (t _k ) = reduce(

4. 1 D application

In this first example, we consider a target device (or agent) whose motion is confined to the x axis of a Cartesian frame, see figure 4. We estimate its position p and velocity v based only on range observations to a beacon. The beacon being positioned off the x axis by the distance d renders the observation equation nonlinear and as such requires a nonlinear estimator such as the CZESMF. There is no uncertainty in observation times in this minimal example.

The dynamics of the state x = (p v) ^T are governed by

wherein T _s = t _k — t _k-t and a(t) is the acceleration.

Range observations are governed by r(ty,k) = + w _k (43) with w _k e [w] = [—0.224, 0.168] m. 4. 1. Initialization

We assume that initially the target's position and velocity are confined to

4.2. Propagation

With target accelerations being bounded by a(t) e [a] = [-0.4, 0.4] d T _s = t _k — t _k-t an inclusion for x(t _fe ) is given by

with z _¾ (t _fc _i) = QZ([a]) -

Since the inclusion equation 46 is already linear, we obtain the a priori C-zonotope directly as

4.3. Update

Since observations times are without uncertainty in this example, t _k = t _{y fe} , i.e. we propagate the state enclosure to the observations' time of validity. From equation 43 is formed the linear inclusion with

and

When computing an enclosure of the linearization error, the generic approach of evaluating the Lagrange remainder by interval arithmetic often leads to overly conservative enclosures due to dependencies. Evaluating e _h explicitly can provide tighter enclosures, even more so when exploiting monotonicity. An explicit expression of the linearization error is To check for monotonicity, consider the derivative To compute a tight inclusion of e _h over the interval enclosure of the a priori C-zonotope Zx(t _k ), we then only need to evaluate its vertices as well as the possible inflection point given by

(55)

(56)

The update is then performed according to equation 36.

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 the CZESMF enclosures, respectively. Figures 8 and 9 show the corresponding position and velocity interval enclosures, respectively. After each update, state C-zonotopes have been reduced to n _g = 6, n _g = 2.

5. 2D localization application

In this example we consider a target device moving on a field with two static ranging beacons (see figure 10). This could be an automatic lawnmower required to avoid cutting down flowers within specified zones and to avoid leaving the perimeter of the field or an automatic lift truck in a modern warehouse.

The target's position x = ( _x x ₂ ) ^T is governed by x(¾) = X(¾— i ) + q(i _f c-i) (57) where q(4)ll < v(4 _+i - ¾) (58) with the target's maximum velocity v. This velocity norm constraint is a conservative but easily obtainable dynamic model for a mobile target device if better knowledge of its inner dynamics is not available.

The target device regularly receives ranging observations to both beacons situated at the uncertain positions with

v . , .

The ranging observation to each beacon i _k is governed by

These two observations are nominally synchronous at a frequency of 1 Hz. Imperfect synchronization of the ranging sensor with wall clock time causes the bounded timing jitter w _t (t) e [-10 ^-2 1C) ^-2 ] s. That is to say, a pair of ranging observations arrives approximately every second, but not exactly. To obtain periodic state enclosures, the filter recursion is triggered at the upper bound of the observation time intervals, i.e. (t _1; t ₂ , t ₃ , t ₄ , ... ) = (0.01 s, 0.01 s, 1.01 s, 1.01 s, ... ).

5.1. Initialization

The target device is initially inside the field. This allows the computation of a C-zonotope that corresponds to an interval covering the field. This extremely conservative initial enclosure leads however to extremely conservative filter solutions. A tighter enclosure of the initial state is therefore computed with VSIVIA. This one-time step is not very computationally expensive in two dimensions (0.8 s and 233 processed boxes with e = 0.1 m). For estimation problems in higher state dimensions, it might be preferable to couple VSIVIA with constraint propagation to compute a sufficiently tight initial state enclosure in an acceptable time.

The C-zonotope corresponding to the interval enclosure of the subpaving (denoted as “solution” and“undefined” in figure 1 1 ) obtained by VSIVIA is used to initialize the filter.

5.2. Propagation The state dynamics are already linear in the state. The propagation can thus be directly performed by

¾4·) = ¾( -i) + ¾(4-i ) (63)

The state dynamics uncertainty ball defined by equation 58 is outer approximated by a C-zonotope (see Appendix A below) to obtain Z _s (tk-1 ) . An example of the resulting a priori C-zonotope is displayed in figure 12. Note how Z _x (t ₉ ) contains the true position at t ₉ . Note also that t ₉ corresponds to the first propagation-update sequence performed close to t = 4 s, since two ranging observations arrive at approximately 1 Hz and every observation increases the filter recursion index k.

5.3. Update

It simplifies things to perform each update in the nominal frame of the corresponding ranging beacon i _k . The a priori C-zonotope in the beacon frame being given by each ranging observation is then governed by r( ) = ||x(4) -€ _f e|j + w _rJc (65)

To simplify further, we absorb the small beacon uncertainty into the observation error w _r,k by virtue of leading to r(¾.) = ||x(4)|| + w' _rk (67)

with w' _{r fe} e [wr _, n] = K] + [ ^~ rad([e _b ]), rad([e _b ])] .

From equation 67 we form the linear inclusion

with and c¾(4) HWI - H(4)X( ) (70)

5.3. 1. Observation interval inflation Before compensating for timing jitter, the state increment interval [ x(t _k )] is obtained from the velocity norm bound v

[Dc] = 2 vrad([w _t ]) (71 )

[-1-1] and the inflated observation inclusion is computed as follows

After computing the a posteriori C-zonotope * (t _fe ) in the beacon frame with eqs. 36-41 , the a posteriori C-zonotope is translated back to the navigation frame

¾4-) ⁼ Z*(4) + p¾ (73)

Figure 12 shows the obtained a posteriori C-zonotope at t ₉ , as well as the reduced a posteriori C-zonotope. Figure 13 shows the obtained results with increased timing noise value. Note the larger a posteriori C-zonotope due to increased observation interval inflation.

The full state enclosure trajectory is displayed in figure 14, and in figure 15 for increased observation timing noise. After each update, state C-zonotopes have been reduced to fi _g = 15, n _c = 1. By setting At to 0.1 s, it is ensured that a propagation is only performed once for every pair of ranging observations, thus reducing filter run time (conditional propagation).

6. Application to 3D peer-to-peer localization In this example, the state of interest is the relative position between two agents, such as unmanned aircraft in close formation flight. Numerous aerospace applications require guaranteed relative positioning, such as aerial refueling, energy efficient formation flight and nanosatellite swarms. In this example, raw GNSS code and carrier phase observations as well as UWB peer-to-peer ranging observations are obtained. Both kinds of sensors are inexpensive, of very low mass - tens of grams - and readily available as Commercial-Off- The-Shelf (COTS) components. We show how the CZESMF can be applied to this localization problem of high practical relevance and evaluate its conservatism in a realistic simulation benchmark in comparison to existing set-membership state estimation schemes.

In the following time indices are dropped wherever unambiguously possible to improve readability.

6.1. Initialization

Due to the high degree of linearity of the GNSS code phase double difference observations, the filter can simply be initialized with a very conservative guess about the initial relative position.

6.2. Propagation

We propagate the guaranteed relative position x set between two vehicles A and B

X = PB ^~ PA

Set-valued dynamic vehicle models of moderate conservatism are rarely available, mostly due to the absence of established set-theoretic system identification methods. A vehicle- agnostic propagation model is therefore highly desirable. Set-membership inertial navigation is one option worthy of further research, but not further explored here. We instead rely on time-differenced GNSS carrier-phase double difference observations, when available, and a rough velocity norm constraint model at times without GNSS observations. Standalone time-differenced carrier phase observations have received originally introduced for precise flight trajectory reconstruction. Building thereon, Time- Differenced Double phase Difference (T3D) observations have first been proposed recently for tight set-valued relative trajectory tracking. The mi Hi meter- level carrier noise allows to compute very narrow enclosures of position increments between GNSS observations. Applied to relative position estimation, propagated state sets are less conservative, leading to smaller linearization error sets in the update step and finally to less conservative a posteriori state sets.

6.2. 1. T3D observations Forming single differences of carrier phase observations removes the majority of atmospheric errors and the satellite clock error.

Single difference carrier phase observations for two receivers A and B and a satellite S are then given by DF(RA, R*, R5 ) = ||RL -R5 IHIR5 -R5 || +DLM+€LF +D7U (75) where e _DF captures residual errors due to hardware noise and multipath, AT is the differential receiver clock error, c the speed of light, l is the carrier wavelength and AN the differential carrier cycle ambiguity.

Using d _s = p _A — ps and p _B = p _A + x we write more compactly DF(c, d ₅ ) = |jd _s II - ||¾ + x|| + ANA + ¾ _f + AT c (76)

The differential receiver clock error is removed by forming double differences with respect to a reference satellite R nF(c, s , An = s I)— j|d ₅ + x|j - Jjd^U + ||d^ + x| + €< < + vN A (77)

Double difference carrier phase observations are thus a nonlinear function of the relative position of receivers A and B and the vectors between absolute receiver positions and satellite orbit positions.

Assuming that the receiver maintains phase lock over two subsequent epochs, the ambiguities are a constant that is eliminated by forming the difference of two subsequent double difference observations, forming the T3D observation DnF(¾) =nF(¾) - nF(¾_i) (78)

In the following we show how we can compute a C-zonotope that encloses the position increment Ax = x(t _k ) - x(t _fe-1 ).

Linearizing (using equation 78) directly and computing an interval enclosure of the Laplace remainder using interval arithmetic as proposed in“A nonlinear set-membership filter for online applications”, by E. Scholte et al. yields very large over-approximation due to heavy dependencies.

This issue is overcome by instead linearizing the single difference equation 76.

The Lagrange remainder is then evaluated using IA and the resulting interval is added to the hardware error interval e _Df . To simplify the computation of the necessary Jacobian and Hessian it is convenient to write equation 76 as

with the augmented state vector z =(dj x ^T ) ^T (82) and

The Jacobian and Hessian of equation 81 can then conveniently be formed as

Interval bounds on the observation model error due to linearization and parametric uncertainty (uncertain orbit position of the satellite) can thus be obtained by evaluating

We proceed to forming the linear single difference observation

DF =A<D(mid([d _Jy ]), mid([x])) (88)

+ pdvA Fc(c— mid([x])) + ¾ + ¾f + ANA + AT c

(89) As [K _DF ] is generally very narrow, of the order of millimeters, for small separations we can linearize about x = 0 and have the more compact form

DF =¾x + _F + ANA + ATc (90) familiar from the RTK literature, with e' _f = e _F + K _DF + rάnAFc(c— mid([x])). We can then form a linear double difference observation model that is free of dependencies

YF =(¾ - H _s )x + F (91 )

=Hg ^c + F (92) where e' _nf now lumps together observation and linearization errors.

Taking the difference of two consecutive carrier phase observations with observation times t _fc _i and t _k respectively, the constant ambiguity terms are eliminated. Introducing Ax(t _k ) = (t _f e) - x(t _fe-1 ), the T3D observation is given by

DUF =H _V (4)X(4) - HV(4-I)X( - I ) + ¾v _® (4) (93)

=HV(¾)X(4) - (Hv(¾) + DH _n (¾)) ^c (4-i) + ¾v (4)

(94)

=H _n (4)D ^c (4) - DH _n ( ) ^c ( -i ) + ¾v _<i» (4- ) (95)

=H _n ( )D ^c ( ) + v _® (¾) (96) with n _F <4)€[ UF (¾)] (97)

[ nf(¾)ί =[^nf( )1 - [£gf( -i)] - [DHg( )][ ^c (¾_i)] (98)

We exploit here the fact that the geometry matrix H _v changes very little between two consecutive time steps due to the large distance between receiver and satellites, leading to a very narrow incremental term AH _v (j). We obtain an observation that is linear in the incremental displacement between two time steps. We take advantage of this observation to compute very accurate enclosures of the incremental displacement as follows. Lumping together terms, we write equation 96 as the linear inclusion D¥F e H _v Ax + [e] (99)

The T3D observation D\7F constrains Ax to a polytope P whose half-space representation is given by

RDc < k (101)

The corresponding C-zonotope Z _Ax (t _k ) is then given by Theorem 1 disclosed in “Constrained zonotopes: a new tool for set-based estimation and fault detection”, by J.K. Scott et al. This is used in the filter propagation by setting T(ί _¾-! ) = I ₃ , b(ί _¾-! ) = I ₃ ,

6.2.2. Velocity-norm propagation

In this application example, inter-vehicle ranging observations are not synchronized with GNSS observations and arrive at a higher rate. To perform the necessary propagation before updating with a ranging observation, we use a crude model based on norm- bounded relative velocity. For a propagation horizon t _k — t _k-t the norm-bound v _max defines a sphere with radius r = v _max (t _k - t _fc _ i). We outer-approximate this sphere by a symmetric C-zonotope, Z ^s of user-defined complexity. The propagation is then performed by setting

F(t _k -P) = I ₃ , B(t _k -P) = I ₃ , Z _u (t _k -P) = 0, Z _q (t _k -P) = Z ^s .

6.3. Update

6.3.3. GNSS update

GNSS code phase single difference observations are governed by

Ap(p _A , PJ, PS ) = UP A - Ps II - lips where e _p captures residual errors due to hardware noise and residual atmospheric errors, AT is the differential receiver clock error and c is the speed of light.

Using d _s = p _A — ps and p _B = p _A + x we write more compactly

Dr(c, d _s ) = ||ds II - lids + x|| + e _Ap + DG c (103)

The differential receiver clock error is removed by forming double differences with respect to a reference satellite R Just like carrier phase double differences, double difference code phase observations are thus a nonlinear function of the relative position of A and B and the vectors between absolute vehicle positions and satellite orbit positions. We follow the same strategy as for carrier phase observations to form a linear inclusion for code phase double differences, leading to

Vp e H _v ¾ + Z([ _p ]) (105) allowing for an update of the state enclosure.

6.3.2. Ranging update

The inter-vehicle ranging observations are given by with the positions r _A , r _B of the UWB ranging antenna centers with respect to the GNSS antenna centers in the respective body frame and the device hardware measurement noise w _r e [w _r ] . Equation 106 is subject to parametric uncertainties due to uncertainty in the attitudes R ^A ,R ^B of both UAS as well as uncertain antenna positions r _A , r _B . In the following we assume that r _A , r _B are small, i.e. the ranging antennas are close to the GNSS antennas, and that interval bounds of their additive effect on the range observation have been determined and lumped into a combined range error w _r : r = ||x|| + w _t r (107)

To linearize equation 107, we first write range observations as r =(x ^r x) ^1/2 + w _r (108)

The first order interval Taylor expansion of equation 108 follows in the same vein as for GNSS carrier phase observations

with the Jacobian and Hessian

(1 10)

Geometrically, this linearized range observation corresponds to the relative position dv

vector projected on the gradient vector— , while the nonlinear slant range corresponds to a sphere. Although a good approximation at large distances, the increasing curvature of the sphere introduces larger linearization errors as vehicles get closer to one another, rendering tight enclosures even more crucial.

Applying the generic approach of evaluating the Lagrange remainder term of equation 109 using Interval Arithmetic provides large over-approximation due to the multiple dependencies in x.

The wider the linearization error interval, the less likely it is that the linearized observations can contribute to contracting the guaranteed state set.

In the following we show that by explicitly evaluating the error between the nonlinear observation equation 107 and its linearization, exploiting monotonicity, tighter bounds can be obtained.

The explicit linearization error interval is given by dr

[<¾ ] =||[x]II - ||mid([x])|| -— ([x] - mid([x]))

rfx

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

lllxlll ||mid([x])||

Geometrically, this is the difference between two unit vectors. The expression (111 ) is thus piecewise monotonic, since the signs of the elements of (112) are constant over subintervals of [x] . Since x e l ³ , the number of these subintervals is at most 2 ³ , corresponding to at most 27 unique vertices. By evaluating equation (111 ) for each of these vertices, an exact enclosure of [ e _r ] can be computed. To quantify the benefit of exploiting piecewise monotonicity, we compute the interval with both approaches and compare their respective radii. Figure 16 displays this measure over the course of a benchmark mission (see section 6.4) along with an inner approximation of the interval obtained by evaluating equation (111 ) for a large number of samples drawn from the a priori state C-zonotope interval enclosure. Note that the generic scheme yields interval radii up to four times as large as the scheme exploiting piecewise monotonicity, which in turn closely approximates the interval obtained by sampling.

6.4. Simulations

We evaluated the filter on a benchmark UAS maneuver. It consists of two UAS, a leader and a follower. The follower changes its relative position from the right to the left of the x axis on a circular trajectory, see figure 17. We track the relative position of both agents at 1 Hz. Simulated GNSS and ranging observations are contaminated with recorded observation errors. For GNSS observations, these are obtained from zero baseline measurements of two low-cost single frequency GPS receivers. Ranging observations errors are obtained form two static UWB ranging beacons placed at a known distance.

6.4. 1. Set volumes

In all three scenarios, the CZESMF filter provides smaller set volumes than both the IESMF (Iterated Extended Set-membership Filter) and VSIVIA (Vector implementation of SIVIA algorithm). We also evaluated the benefit of using T3D observations for propagation, compared to the coarse velocity norm-bound model. As figure 18 shows, C-zonotope volumes can be reduced by almost an order of magnitude by using carrier phase information in the propagation step.

6.5. Sensitivity to satellite constellations

The achievable size and shape of relative position enclosures depend on the baseline- satellite geometry, as captured by the familiar Dilution of Precision (DOP) accuracy measure. It is thus of interest to know whether the relative conservativeness of the presented set-membership filters is robust with respect to variation in satellite constellations. We performed simulations of the benchmark maneuver over a set of randomly selected GPS orbit records. Satellite positions have been computed from a spline fit of the first two minutes of each orbit record. Figure 19 shows the resulting relative position set volumes of the CZESMF and the IESMF normalized with the subpaving volume obtained by VSIVIA. After an initial transition, the CZESMF shows less conservatism than VSIVIA over all simulations and very little variations with respect to GNSS orbits. The IESMF is significantly more conservative, with similarly low sensitivity to satellite orbits. These simulations provide some confidence that the observed superior tightness of the enclosures computed by the CZESMF can be expected to carry over to an online implementation of the filter. 7. Conclusion

The set state estimation method described above for nonlinear systems based on C- zonotopes robustly outperforms (in terms of set volumes) a nonlinear offline set estimator based on VSIVIA as well as a recent fast ellipsoidal set state estimator in a simulation scenario.

The proposed filter can be applied to a wide variety of estimation problems, including Simultaneous Localization And Mapping (SLAM), an exciting perspective in light of the current interest in safe visual localization of road vehicles.

Appendix A: Approximating a 2-dimensional or 3-dimensional ball with a C-zonotope To approximate a ball of radius r centered at c, we compute an approximation in H-rep and convert it to a C-zonotope.

Given a vector of angles Q = (9 _lt q ₂ , - , qi) evenly spaced in the interval [ ^— f -f] and ø (0i, 0 ₂ - - ø;) evenly spaced on [0, 2p] wherein Q is the zenith angle and ø is the azimuth angle and l is a complexity parameter, we compute for each combination of angles (q _ί; 0 _; ) a corresponding normal vector

Each of these normal vectors define a half-space. We can then readily write the corresponding polytope in H-rep

and convert it to G-rep. using the method given in“Constrained zonotopes: A new tool for set-based estimation and fault detection”, by J.K. Scott et al.

In two dimensions, the same approach is performed over a single set of angles Q = (0 _! , q ₂ , evenly spaced on [0, 2p] .