METHOD OF ESTIMATING A CONSTRAINED ZONOTOPE ENCLOSING A STATE REPRESENTING MOTION OF AT LEAST ONE MOBILE TARGET GOVERNED BY A NONLINEAR MODEL

Abstract

A method for estimating a constrained zonotope enclosing a motion state of at least one mobile target, the method including: a) linearizing a nonlinear transition model at an element of a first zonotope (Z.sub.x(t.sub.k−1)), b) computing transition linearization errors at different elements of the first zonotope (Z.sub.x(t.sub.k−1)), c) computing a second zonotope that contains all linearization errors, d) propagating (102) the first zonotope so as to obtain an a priori zonotope ({circumflex over (Z)}.sub.x(t.sub.k)) enclosing the motion state at a second moment (t.sub.k) after the first moment (t.sub.k−1), wherein the a priori zonotope ({circumflex over (Z)}.sub.x(t.sub.k)) is a linear projection of a hypercube, the hypercube being subject to at least one linear constraint, e) updating (104) the a priori zonotope ({circumflex over (Z)}.sub.x(t.sub.k)) so as to obtain an a posteriori constrained zonotope (Z′.sub.x(t.sub.k)) enclosing the motion state at the second moment (t.sub.k).

Claims

1. Method comprising: a) linearizing a nonlinear transition model at an element of a first constrained zonotope enclosing a motion state of at least one mobile target at a first moment, so as to produce a linear transition model, b) computing transition linearization errors at different elements of the first constrained zonotope, wherein the transition linearization errors comprise a transition linearization error at a given element of the first constrained zonotope being 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, c) computing a second constrained zonotope that contains all the linearization errors, d) propagating the first constrained zonotope so as to obtain an a priori constrained zonotope enclosing the motion state at a second moment after the first moment, wherein the a priori constrained zonotope is a linear projection of a hypercube, the hypercube being subject to at least one linear constraint, and wherein propagating the first constrained zonotope comprises: applying the linear transition model to the first constrained zonotope so as to produce a third constrained zonotope, and summing the second constrained zonotope and the third constrained zonotope, e) updating the a priori constrained zonotope so as to obtain an a posteriori constrained zonotope enclosing the motion state at the second moment, wherein updating the a priori constrained zonotope comprises applying an observation model to the a priori constrained zonotope and to observation data of the mobile target acquired by at least one sensor.

2. Method of claim 1, wherein the element at which the nonlinear transition model is linearized is a center of a smallest interval enclosure containing the first constrained zonotope.

3. Method of claim 1, further comprising 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.

4. Method of claim 3, wherein the element at which the nonlinear observation model is linearized is a center of a smallest interval enclosure containing the a priori zonotope.

5. Method of claim 3 or claim 4, further comprising: computing observation linearization errors at different elements of the first constrained zonotope or the a priori zonotope, wherein the observation linearization errors comprise an observation linearization error at a given element of the first constrained zonotope or the a priori zonotope being 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, wherein the a posteriori constrained zonotope depends on the fourth constrained zonotope.

6. Method according to claim 1, wherein the observation data of the mobile target has been acquired at a third moment different from the second moment, and wherein the method further comprises: inflating the fourth constrained zonotope so as to obtain an inflated constrained zonotope forming a strip in a space of the motion state and taking into account all possible variations of the motion state between the second moment and the third moment, and 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.

7. Method of claim 1, further comprising: 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 before the reduction and containing the a posteriori zonotope.

8. Method of claim 1, further comprising repeating steps a) to e), wherein said repeating comprises using the a posteriori constrained zonotope as first constrained zonotope and using the second moment as first moment.

9. Method of claim 1, further comprising 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.

10. Method of claim 1, wherein the observation data comprises 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.

11. Method of claim 1, wherein the motion data includes motion data of the mobile target relative to another mobile target.

12. Method of claim 1, wherein the first constrained zonotope is computed using a Vector Set Inverter Via Interval Analysis algorithm.

13. A non-transitory computer-readable medium comprising code instructions for causing a computer to perform the method as claimed in claim 1.

Description

BRIEF DESCRIPTION OF THE FIGURES

[0035] FIG. 1 schematically represents a device for estimating a set enclosing the motion state of a mobile target;

[0036] FIG. 2 illustrates the steps of an example method for estimating a C-zonotope enclosing a state x of a system;

[0037] FIG. 3 illustrates a sequence of operations of a propagation step of the method shown in FIG. 2;

[0038] FIG. 4 is a graphical representation of the position p and distance d to an x-axis for a target device and for a beacon;

[0039] FIG. 5 represents snapshots of the propagation, update and reduction steps of the method shown in FIG. 2;

[0040] FIG. 6 is a graphical representation of a true state trajectory of the target device;

[0041] FIG. 7 is a graphical representation of a CZESMF enclosure of the target device;

[0042] FIG. 8 is a graphical representation of a position interval enclosure of the target device;

[0043] FIG. 9 is a graphical representation of a velocity interval enclosure of the target device;

[0044] FIG. 10 is a graphical representation of the position of another target device;

[0045] FIG. 11 represents an interval enclosure of a subpaving obtained by VSIVIA;

[0046] FIG. 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 FIG. 10;

[0047] FIG. 13 represents the a priori and a posteriori C-zonotopes in the case of an increased observation timing noise;

[0048] FIG. 14 represents a full state enclosure trajectory for the target device of FIG. 10;

[0049] FIG. 15 represents a full state enclosure trajectory for the target device of FIG. 10 in the case of an increased observation timing noise;

[0050] FIG. 16 is a graphical representation of two radii of range linearization error interval measurements obtained by two distinct approaches;

[0051] FIG. 17 represents the positions of two other target devices during a simulation;

[0052] FIG. 18 represents set volumes obtained by speed norm prediction and by T3D prediction, using a GNSS update or a GNSS+UWB update;

[0053] FIG. 19 represents relative position set volumes obtained using a CZESMF filter and using an IESMF filter.

DETAILED DESCRIPTION

[0054] 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: [0055] a simple 1D example (section 4), [0056] a slightly more involved 2D localization problem (section 5), [0057] a 3D peer-to-peer localization benchmark problem using differential GNSS and peer-to-peer ranging observations (section 6).

1. Notations

[0058] To accommodate this complexity, the notation x(t.sub.l) familiar from continuous-time estimation theory will be used hereinafter. The discrete nature of filter execution is captured by indexing the time. Specifically, t.sub.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.

[0059] Correspondingly, a set Z(t.sub.k) is valid at time t.sub.k, i.e. x(t)∈Z(t.sub.k) if t=t.sub.k. If the time of validity is an interval [t.sub.k], the set is valid for all times in this interval, i.e. x(t)∈Z(t.sub.k) if t∈[t.sub.k].

[0060] Besides, a matrix J.sub.n×m denotes a matrix of n rows and m columns where all elements are one. The identity matrix of size n is denoted by I.sub.n and a square zero matrix of size n by 0.sub.n.

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

[0062] Interval matrices are denoted by [M]=[M, M] where the elements of M,M∈.sup.n×m 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∈. For an interval [M], M denotes a realization of the interval, i.e. M∈[M].

[0063] 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.

[0064] 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=h(x)  (1)

[0065] With x∈X, i.e. x is contained in some compact set X. A linear function

ŷ=y.sub.0+Hx+ϵ  (2)

[0066] is a linear inclusion of equation 1 if

y∈y.sub.0+Hx+[ϵ]  (3)

[0067] for all x∈X, i.e. the set-valued solution [ŷ] of the linear set valued function always contains the nonlinear solution.

2. Problem Statement

[0068] 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.

[0069] 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.

[0070] Referring to FIG. 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.

[0071] 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.

[0072] 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.

[0073] 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.

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

[0075] 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.

[0076] The purpose of set-membership state estimation is computing a region X(t)⊂.sup.n in which the state x(t) of the system is guaranteed to reside at a time t.

2.1. State Dynamics

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

x(t.sub.k)=f(x(t.sub.k−1), u(t.sub.k−1))+q(t.sub.k−1)  (4)

[0078] with the previous state x(t.sub.k−1)∈.sup.n, the system input u(t.sub.k−1)∈.sup.p, the propagation model error q(t.sub.k−1)∈(t.sub.k−1).Math..sup.n, and a constant sampling time T.sub.S=t.sub.k−.sub.k−1.

[0079] 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.

[0080] 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.

[0081] We accommodate multiple propagation models by assuming a very general time-varying state dynamics model that is a nonlinear map .sup.n×.sup.p××.fwdarw..sup.n

x(t.sub.k)=f(x(t.sub.k−1),u(t.sub.k−1),t.sub.k−1,t.sub.k)+q(t.sub.k−)  (5)

[0082] The arguments t.sub.k and t.sub.k−1 indicate that the model performs a propagation of the system state over the time horizon from t.sub.k−1 to t.sub.k as well as the time-varying nature of f( ). 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

[0083] 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.

[0084] 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.sub.k. We call this the time of validity of a given state enclosure {circumflex over (Z)}.sub.x(t.sub.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.sub.k falls within the observation time interval. This is simply due to the fact that {circumflex over (Z)}.sub.x(t.sub.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.

[0085] The observations are assumed to be governed by the nonlinear map .sup.n×.sup.p×.fwdarw..sup.m(t)+1

[00001]$\begin{array}{cc}\left(\begin{array}{c}y\left(t\right)\\ {\hat{t}}_y\left(t\right)\end{array}\right)=\left(\begin{array}{c}h\left(x\left(t\right),u\left(t\right),t\right)+w\left(t\right)\\ t+w_t\left(t\right)\end{array}\right)& \left(6\right)\end{array}$$\begin{array}{cc}\left(\begin{array}{c}{y}_1\left(t\right)\\ .Math.\\ y_{m\left(t\right)}\left(t\right)\\ {\hat{t}}_y\left(t\right)\end{array}\right)=\left(\begin{array}{c}{h}_1\left(x\left(t\right),u\left(t\right),t\right)+w\left(t\right)\\ .Math.\\ h_{m\left(t\right)}\left(x\left(t\right),u\left(t\right),t\right)+w\left(t\right)\\ t+w_t\left(t\right)\end{array}\right)& \left(7\right)\end{array}$

[0086] Where y(t)∈.sup.m(t) is a vector of sensor observations, w(t)∈W is the corresponding measurement error, {circumflex over (t)}.sub.y(t) is the observed observation time stamp, w.sub.t(t)∈[W.sub.t] is the observation time error and t is the true observation time. The observation time stamp error w.sub.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.

[0087] 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.

[0088] 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

[0089] 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.

[0090] A C-zonotope Z={G, c, A, b}⊂.sup.n 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={Gξ+c|∥ξξ.sub.∞≤1,Aξ=b}  (8)

[0091] 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.

[0092] 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.

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

[0094] Referring to FIG. 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.sub.x(t.sub.k−1) enclosing the state at time t.sub.k−1: [0095] a propagation step 102 wherein the predetermined C-zonotope is processed so as to obtain an a priori C-zonotope {circumflex over (Z)}x(t.sub.k) enclosing the state at time t.sub.k; [0096] an update step 104 wherein the a priori constrained zonotope {circumflex over (Z)}x(t.sub.k) is combined with observation data acquired by the observation unit 4 so as to obtain an a posteriori C-zonotope Z′.sub.x(t.sub.k) enclosing the state at time t.sub.k.

[0097] The a posteriori C-zonotope enclosing the state x at the time t.sub.k and taking into account all observations up to and including t.sub.k is denoted as

Z′.sub.x(t.sub.k)={G′.sub.x(t.sub.k),c′,.sub.x(t.sub.k),A′,.sub.x(t.sub.k),b′,.sub.x(t.sub.k)}  (9)

[0098] While the a priori C-zonotope enclosing the state x at the time t.sub.k, before performing the update, is denoted by

{circumflex over (Z)}.sub.x(t.sub.k)={Ĝ.sub.x(t.sub.k),ĉ.sub.x(t.sub.k),Â.sub.x(t.sub.k),{circumflex over (b)}.sub.x(t.sub.k)}  (10)

[0099] 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.

[0100] Moreover, the method may further comprise a reduction step 106 wherein the filter converts the a posteriori C-zonotope Z′.sub.x(t.sub.k) into another C-zonotope Z.sub.x(t.sub.k) of lower complexity but containing the C-zonotope Z′.sub.x(t.sub.k).

[0101] 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.

[0102] The very first recursion of the method is preceded by an initialization step 100 wherein the initial C-zonotope {circumflex over (Z)}.sub.x(t.sub.k) is determined.

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

3.1. Initialization

[0104] The initial state is known to be contained in some C-zonotope {circumflex over (Z)}.sub.x(t.sub.k), the first a priori state enclosure.

3.2. Propagation

[0105] At the propagation step 102, executed for k>1, the filter computes the a priori state set {circumflex over (Z)}.sub.x(t.sub.k). It is guaranteed to contain the state at time t.sub.k>t.sub.k−1, given the preceding a posteriori state set Z.sub.x(t.sub.k−1).

[0106] 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.sub.k close to the observation time and perform a final propagation to compensate for execution time.

[0107] 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.

[0108] 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 {circumflex over (x)}(t.sub.k−), û(t.sub.k−1). The centers of the interval enclosure of the preceding a posteriori state and input enclosures are chosen as operating point:

{circumflex over (x)}(t.sub.k−1)=mid([Z.sub.x(t.sub.k−1)])  (11)

û(t.sub.k−1)=mid([Z.sub.u(t.sub.k−1)]) where Z.sub.u={G.sub.u, c.sub.u, A.sub.u, b.sub.u}  (12)

[0109] 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.

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

[00002]$\begin{array}{cc}x\left(t_k\right)=f\left(\hat{x}\left(t_{k-1}\right),\hat{u}\left(t_{k-1}\right),t_k,t_{k+1}\right)+q\left(t_{k-1}\right)+F\left(t_{k-1}\right)\left(x\left(t_{k-1}\right)-\hat{x}\left(t_{k-1}\right)\right)+B\left(t_{k-1}\right)\left(u\left(t_{k-1}\right)-\hat{u}\left(t_{k-1}\right)\right)+q\left(t_{k-1}\right)+{\epsilon }_{f}f\left(t_{k-1}\right)& \left(13\right)\end{array}$$\begin{array}{cc}=F\left(t_{k-1}\right)x\left(t_{k-1}\right)+B\left(t_{k-1}\right)u\left(t_{k-1}\right)+q\left(t_{k-1}\right)+{\epsilon }_{f}f\left(t_{k-1}\right)+c_f\left(t_{k-1}\right)& \left(14\right)\end{array}$

[0111] Where the linearization error is ϵ.sub.f(t.sub.k)∈[ϵ.sub.f(t.sub.k)] and with

[00003]$\begin{array}{cc}{F\left(t_k\right)=\frac{\partial f}{\partial x}{.Math.}_{.Math.x=\hat{x}\left(t_{k-1}\right)},B\left(t_k\right)=\frac{\partial f}{\partial u}.Math.}_{.Math.u=\hat{u}\left(t_{k-1}\right)}& \left(15\right)\end{array}$$\begin{array}{cc}{c}_f\left(t_{k-1}\right)=f\left(\hat{x}\left(t_{k-1}\right),\hat{u}\left(t_{k-1}\right),t_k,t_{k+1}\right)-F\left(t_{k-1}\right)\hat{x}\left(t_{k-1}\right)-B\left(t_{k-1}\right)\hat{u}\left(t_{k-1}\right)& \left(16\right)\end{array}$

[0112] The corresponding linear inclusion being given by

x(t.sub.k)∈F(t.sub.k−1)Z.sub.x(t.sub.k−1)+B(t.sub.k−1)+Z.sub.q(t.sub.k−1)+Z.sub.q(t.sub.k−1)+Z([ϵ.sub.f(t.sub.k−1)])+Z([c.sub.f(t.sub.k−1)]  (18)

[0113] a propagation from time t.sub.k−1 to time t.sub.k is then performed as

{circumflex over (Z)}.sub.x(t.sub.k)=F(t.sub.k−1)Z.sub.x(t.sub.k−1)+B(t.sub.k−1)Z.sub.u(t.sub.k−1)+Z.sub.q(t.sub.k−1)+Z([ϵ.sub.f(t.sub.k−1)])+Z([c.sub.f(t.sub.k−1)]  (18)

[0114] 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 Z.sub.q(t.sub.k−1) and Z.sub.u(t.sub.k−1) is a C-zonotope enclosure of any uncertain input. The propagation uncertainty term q(t.sub.k) actually corresponds to the process noise of an EKF, and the C-zonotope Z.sub.q(t.sub.k) corresponds to the process noise covariance matrix Q of an EKF.

[0115] [ϵ.sub.f(t.sub.k)] can be regarded as a set of transition linearization errors at different elements of C-zonotope Z.sub.x(t.sub.k−1). A transition linearization error at a given element of Z.sub.x(t.sub.k−1) 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.

[0116] Besides, the expression Z.sub.q(t.sub.k−1)+Z([ϵ.sub.f(t.sub.k−1)])+Z([c.sub.f(t.sub.k−1)] above mentioned may be regarded as a C-zonotope that contains all the linearization errors. Moreover, the expression F(t.sub.k−1)Z.sub.x(t.sub.k−1)+B(t.sub.k−1)Z.sub.u(t.sub.k−1) can be regarded as a C-zonotope resulting from applying the linear transition model to Z.sub.x(t.sub.k−1). The a posteriori zonotope is the sum of both expressions.

3.2.1. Conditional Propagation

[0117] 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.

[0118] In the present method, there are two situations where a propagation does not need to be performed, reducing filter run time.

[0119] 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.

[0120] 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 Δt≥0 is used by the filter. Before each propagation, the filter determines whether a propagation needs to be performed.

[0121] Algorithm 1 below illustrates how a conditional propagation can be implemented in practice: (see also FIG. 3):

TABLE-US-00001 Algorithm 1: conditionalPropagation  Input  : Δt, [t.sub.y,k], t.sub.k−l  Output: performPropagation 1   Test for closeness 2 if sup(t.sub.k−1 − [t.sub.y,k]) ≤ Δt) then 3 | performPropagation = false 4 else 5 | performPropagation = true 6   Test for observation synchronicity 7 if [t.sub.y,k] = = [t.sub.y,k−1]) then 8 | performPropagation = false

3.3. Update

[0122] 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.

[0123] 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.

[0124] 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:

[00004]$\begin{array}{cc}\left(\begin{array}{c}y\left(t_{y,k}\right)\\ {\hat{t}}_y\left(t_{y,k}\right)\end{array}\right)=\left(\begin{array}{c}h\left(x\left(t_{y,k}\right),u\left(t_{y,k}\right),t\right)+w\left(t_{y,k}\right)\\ t_{y,k}+w_t\left(t_{y,k}\right)\end{array}\right)& \left(19\right)\end{array}$

[0125] Where t.sub.y,k is the true observation time of observation k and {circumflex over (t)}.sub.t,k is the apparent observation time subject to timing error.

[0126] Assuming interval-valued errors, a first-order approximation of the nonlinear observation map valid at time t.sub.y,k has the form

[00005]$\begin{array}{cc}y\left(t_k\right)=h\left(\hat{x}\left(t_{x,k}\right),\hat{u}\left(t_{x,k}\right),t_{x,k}\right)+H\left(t_k\right)\left(x\left(t_k\right)-\hat{x}\left(t_{k-1}\right)\right)+D\left(t_k\right)\left(u\left(t_k\right)-\hat{u}\left(t_k\right)\right)+w\left(t_k\right)+{\epsilon }_{h}f\left(t_k\right)& \left(20\right)\end{array}$$\begin{array}{cc}=H\left(t_k\right)x\left(t_k\right)+D\left(t_k\right)u\left(t_k\right)+{\epsilon }_{h}f\left(t_k\right)+c_h\left(t_k\right)& \left(21\right)\end{array}$$\mathrm{With}$$\begin{array}{cc}{H\left(t_k\right)=\frac{\partial h}{\partial x}{.Math.}_{.Math.x=\hat{x}\left(t_k\right)},D\left(t_k\right)=\frac{\partial h}{\partial u}.Math.}_{.Math.u=\hat{u}\left(t_k\right)}& \left(22\right)\end{array}$$\begin{array}{cc}\hat{x}\left(t_k\right)=\mathrm{mid}\left(\left[{\hat{Z}}_x\left(t_k\right)\right]\right)& \left(23\right)\end{array}$$\begin{array}{cc}\hat{u}\left(t_k\right)=\mathrm{mid}\left(\left[{\hat{Z}}_u\left(t_k\right)\right]\right)& \left(24\right)\end{array}$$\begin{array}{cc}{c}_h\left(t_k\right)=h\left(\hat{x}\left(t_{x,k}\right),\hat{u}\left(t_{x,k}\right),t_{x,k}\right)-H\left(t_k\right)\hat{x}\left(t_{k-1}\right)-D\left(t_k\right)\hat{u}\left(t_k\right)+w\left(t_k\right)& \left(25\right)\end{array}$

[0127] where ϵ.sub.h(t.sub.k)=ϵ.sub.h,x(t.sub.k)+ϵ[ϵ.sub.h(t.sub.k)] is the linearization error over the a priori states {circumflex over (Z)}.sub.x and inputs Z.sub.u. Note that t.sub.k is the time of validity of the a priori C-zonotope. The corresponding linear inclusion is then given by

y(t.sub.k)∈H(t.sub.k)Z.sub.x(t.sub.k)+B(t.sub.k−1)Z.sub.u(t.sub.k)+Z.sub.q(t.sub.k)+Z([ϵ.sub.h(t.sub.k)])+Z([c.sub.h(t.sub.k)]  (26)

[0128] 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: [0129] D(t.sub.k)Z.sub.u(t.sub.k): The impact of system inputs, e.g. control inputs generated by an autopilot or external perturbations. The C-zonotope Z.sub.u contains all possible inputs or perturbations. [0130] Z[ϵ.sub.h(t.sub.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. [0131] Z([c.sub.h(t.sub.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.

[0132] 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.sub.y,k) is contained in the right side of equation 26, we can replace y(t.sub.y,k) by z(t.sub.y,k) in equation 26 to form equation 35. For a scalar observation, equation 35 then represents a strip in state space, see FIG. 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).

[0133] 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

[0134] Recall that the true observation time is known with bounded uncertainty, i.e. is known to fall within an interval [t.sub.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.

[0135] As a simple approach to remedy this situation, the observation error interval [ϵ.sub.t.sub.k] 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

[Δt.sub.k]=t.sub.k−[t.sub.y,k]  (27)

[0136] With a state increment interval

[Δx(t.sub.k)]  (28)

[0137] that satisfies

x(t)∈{circumflex over (Z)}.sub.x(t.sub.k)+Z[Δx(t.sub.k)]  (29)

if

t∈t.sub.k+[t.sub.y,k]  (30)

[0138] we can compute an extended linear inclusion

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

[0139] 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 {circumflex over (Z)}.sub.x(t.sub.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).

[0140] All known terms are lumped together into one inflated observation error interval, simplifying to

y(t.sub.y,k)∈H(t.sub.k)Z.sub.x(t.sub.k)+Z([ϵ′])  (32)

where ([ϵ′])=D(t.sub.k)[Z.sub.u(t.sub.k)]+[ϵ.sub.h(t.sub.k)]+[c.sub.h(t.sub.k)]+H(t.sub.k)[Δx])

[0141] Note that the linearization error ∈.sub.h(t.sub.k) now needs to be computed over the inflated a priori state set {circumflex over (Z)}.sub.x(t.sub.k)+Z[Δx(t.sub.k)]. For implementation, ∈.sub.h(t.sub.k) can be evaluated over the interval enclosure [{circumflex over (Z)}.sub.x]+[Δx] to be able to use interval arithmetic. This is generally faster than choosing the alternative interval {circumflex over (Z)}.sub.x+Z[Δx] due to the increase in C-zonotope complexity involved in the C-zonotope sum operation.

[0142] With the scalar observation z(t.sub.y,k) we then have

z(t.sub.y,k)∈H(t.sub.k)Z.sub.x(t.sub.k)+Z([ϵ′])  (33)

and

H(t.sub.k)iZ.sub.x(t.sub.k)∈z(t.sub.y,k)−Z([ϵ′])  (34)

∈Z([ϵ])  (35)

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

Z′.sub.x(t.sub.k)={circumflex over (Z)}.sub.x(t.sub.k)∩.sub.HZ([ϵ])  (36)

={G′.sub.x(t.sub.k),c′.sub.x(t.sub.k),A′hd x(t.sub.k),b′.sub.x(t.sub.k)}  (37)

[0144] With

[00006]$\begin{array}{cc}{G}_x^{\prime }\left(t_k\right)=\left[\begin{array}{cc}{\hat{G}}_x\left(t_k\right)& 0\end{array}\right]& \left(38\right)\end{array}$$\begin{array}{cc}{c}_x^{\prime }\left(t_k\right)={\hat{c}}_x\left(t_k\right)& \left(39\right)\end{array}$$\begin{array}{cc}{A}_x^{\prime }\left(t_k\right)=\left[\begin{array}{cc}{\hat{A}}_x\left(t_k\right)& 0\\ H\left(\mathrm{tk}\right){\hat{G}}_x& -\mathrm{rad}\left(\left[\epsilon \right]\right)\end{array}\right]& \left(40\right)\end{array}$$\begin{array}{cc}{b}_x^{\prime }\left(t_k\right)=\left[\begin{array}{c}\hat{b}\left(t_k\right)\\ \mathrm{id}\left(\left[\epsilon \right]\right)-H\left(t_k\right)\hat{c}\left(t_k\right)\end{array}\right]& \left(41\right)\end{array}$

[0145] See FIG. 3 for an overview of the propagation and update timing.

3.4. Reduction

[0146] As can be seen from equations 36-41, each update increases the complexity—i.e. the number of generators n.sub.g and the number of constraints n.sub.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.sub.x(t.sub.k) exceeds a certain chosen complexity threshold defined by n.sub.g, n.sub.c.

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

TABLE-US-00002 Algorithm 2: reduce  Input  : Z′.sub.x,(t.sub.k), n.sub.g, n.sub.c,  Output: Z.sub.x(t.sub.k) 1 if n.sub.g(Z′.sub.x(t.sub.k)) > n.sub.g or n.sub.c(Z′.sub.x(t.sub.k)) > n.sub.c, then 2 | Reduce Z′.sub.x(t.sub.k) to Z.sub.x(t.sub.k) by reduction algorithms   |  given it [19, 25] 3 else 4 | Z.sub.x(t.sub.k) = Z′.sub.x(t.sub.k)

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

TABLE-US-00003 Algorithm 3: This loop runs until the filter stops executing  1 Compute {circumflex over (Z)}.sub.x(t.sub.1) with VSIVIA or from a priori   knowledge  2 k = 1  3 Loop  4 | Wait for incoming observation z ϵ .sup.m  5 | Select propagation time t.sub.k  6 | for i ← 1 to m do  7 |  | if k = = 1 then  8 |  |  | Z = {circumflex over (Z)}.sub.x(t.sub.1)  9 |  | else 10 |  |  | Z = Z.sub.x(t.sub.k−1) 11 |  | Z.sub.x(t.sub.k) = czesmf_recursion(Z, z.sub.i) 12 └  └ k += 1

TABLE-US-00004 Algorithm 4: czesmf_recursion  Input  : Z.sub.x(t.sub.k−1), z(t.sub.y,.sub.k)  Output: Z.sub.x(t.sub.k) 1 if conditionaPropagation(Δt, [t.sub.y,k], t.sub.k−1) then 2 | Compute linear inclusion eq. 26 3 | Compute a priori {circumflex over (Z)}.sub.x(t.sub.k) with eq. 18 4 else 5 | {circumflex over (Z)}.sub.x(t.sub.k) − Z.sub.x(t.sub.k−1) 6 Compute linear inclusion eq. 26 7 Compute inflated observation error interval with eq. 31 8 Compute a posteriori Z′.sub.x(t.sub.k) with eq. 36 9 Reduce: Z.sub.x(t.sub.k) = reduce(Z′.sub.x(t.sub.k), n.sub.g, n.sub.c)
4. 1D application

[0148] In this first example, we consider a target device (or agent) whose motion is confined to the x axis of a Cartesian frame, see FIG. 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.

[0149] The dynamics of the state x=(p ν).sup.T are governed by

[00007]$\begin{array}{cc}x\left(t_k\right)=\left[\begin{array}{cc}1& T_s\\ 0& 1\end{array}\right]x\left(t_{k-1}\right)+\left(\begin{array}{c}{\int }_{t_{k-1}}^{t_k}{\int }_{t_{k-1}}^{t_k}a\left(t\right)\mathrm{dt}\mathrm{dt}\\ {\int }_{t_{k-1}}^{t_k}a\left(t\right)\mathrm{dt}\end{array}\right)& \left(42\right)\end{array}$

[0150] wherein T.sub.S=t.sub.k−t.sub.k−1 and α(t) is the acceleration.

[0151] Range observations are governed by

r(t.sub.y,k)=√{square root over (d.sup.2+p.sup.2(t.sub.y,k))}+w.sub.k  (43)

[0152] with w.sub.k∈[w]=[−0.224, 0.168] m.

4.1. Initialization

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

[00008]$\begin{array}{cc}{\hat{Z}}_x\left(t_1\right)=Z\left(\begin{array}{c}\left[0,1000\right]m\\ \left[-0.7,0.7\right]\frac{m}{s}\end{array}\right)& \left(44\right)\end{array}$

4.2. Propagation

[0154] With target accelerations being bounded by

[00009]$a\left(t\right)\in \left[a\right]=\left[-0.4,0.4\right]\frac{m}{s^2}\mathrm{and}{T}_s=t_k-t_{k-1}$

an inclusion for x(t.sub.k) is given by

[00010]$\begin{array}{cc}x\left(t_k\right)\in \left[\begin{array}{cc}1& T_s\\ 0& 1\end{array}\right]Z_x\left(t_{k-1}\right)+\left[\begin{array}{c}\frac{1}{2}{T}_s^2\\ T_s\end{array}\right]Z\left(\left[a\right]\right)& \left(45\right)\end{array}$$\begin{array}{cc}\in F\left(t_{k-1}\right)Z_x\left(t_{k-1}\right)+Z_q\left(t_{k-1}\right)& \left(46\right)\end{array}$

[0155] with Z.sub.q(t.sub.k−1)=QZ([a]).

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

{circumflex over (Z)}.sub.x(t.sub.k)=FZ.sub.x(t.sub.k−1)+Z.sub.q(t.sub.k−1)  (47)

[0157] 4.3. Update

[0158] Since observations times are without uncertainty in this example, t.sub.k=t.sub.y,k, i.e. we propagate the state enclosure to the observations' time of validity. From equation 43 is formed the linear inclusion

r(t.sub.k)∈H(t.sub.k)Z.sub.x(t.sub.k)+Z([w])+Z([∈.sub.h])+c.sub.h(t.sub.k)  (48)

[0159] with

[00011]$\begin{array}{cc}H\left(t_k\right)=\frac{\partial r}{\partial x}& \left(49\right)\end{array}$$\begin{array}{cc}=\left[\begin{array}{cc}\frac{{\hat{p}}_k}{\sqrt{d^2+{\hat{p}}_k^2}}& 0\end{array}\right]& \left(50\right)\end{array}$$\begin{array}{cc}=\left[\begin{array}{cc}{H}_p& H_v\end{array}\right]& \left(51\right)\end{array}$$\mathrm{and}$$\begin{array}{cc}{c}_h\left(t_k\right)=\sqrt{d^2+{\hat{p}}_k^2}-H\left(t_k\right)\hat{x}\left(t_k\right)& \left(52\right)\end{array}$

[0160] 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 ∈.sub.h explicitly can provide tighter enclosures, even more so when exploiting monotonicity. An explicit expression of the linearization error is

∈.sub.h=√{square root over (d.sup.2+p.sup.2)}−H.sub.pp−c.sub.h(t.sub.k)  (53)

[0161] To check for monotonicity, consider the derivative

[00012]$\begin{array}{cc}\frac{\partial {ϵ}_h\left(p\right)}{\partial p}=\frac{p}{\sqrt{d^2+p^2}}-H_p& \left(54\right)\end{array}$

[0162] To compute a tight inclusion of ∈.sub.h over the interval enclosure of the a priori C-zonotope {circumflex over (Z)}.sub.x(t.sub.k), we then only need to evaluate its vertices as well as the possible inflection point given by

[00013]$\begin{array}{cc}\frac{\partial {ϵ}_h\left(\stackrel{\_}{p}\right)}{\partial p}=0& \left(55\right)\end{array}$$\begin{array}{cc}\stackrel{\_}{p}=\sqrt{\frac{H_p{d}^2}{1-H_p^2}}& \left(56\right)\end{array}$

[0163] The update is then performed according to equation 36.

4.4. Simulations

[0164] Snapshots of the propagation step 102, update step 104 and reduction step 106 are displayed in FIG. 5. FIGS. 6 and 7 show the true state trajectory and the CZESMF enclosures, respectively. FIGS. 8 and 9 show the corresponding position and velocity interval enclosures, respectively. After each update, state C-zonotopes have been reduced to n.sub.g=6, n.sub.g=2.

5. 2D Localization Application

[0165] In this example we consider a target device moving on a field with two static ranging beacons (see FIG. 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.

[0166] The target's position x=(x.sub.1 x.sub.2).sup.T is governed by

x(t.sub.k)=x(t.sub.k−1)+q(t.sub.k−1)  (57)

where

|q(t.sub.k)|≤ν(t.sub.k+1−t.sub.k)  (58)

[0167] with the target's maximum velocity ν. 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.

[0168] The target device regularly receives ranging observations to both beacons situated at the uncertain positions

[00014]$\begin{array}{cc}{p}_{b1}\in \left(\begin{array}{c}-10.m\\ -10.m\end{array}\right)+{ϵ}_b& \left(59\right)\end{array}$$\begin{array}{cc}{p}_{b2}\in \left(\begin{array}{c}10.m\\ -10.m\end{array}\right)+{ϵ}_b\mathrm{with}& \left(60\right)\end{array}$$\begin{array}{cc}{ϵ}_b\in \left(\begin{array}{c}\left[-0.1,0.1\right]m\\ \left[-0.1,0.1\right]m\end{array}\right)& \left(61\right)\end{array}$

[0169] The ranging observation to each beacon i.sub.k is governed by

[00015]$\begin{array}{cc}\left(\begin{array}{c}{r}_k\\ t_{y,k}\end{array}\right)=\left(\begin{array}{c}.Math.x\left(t\right)-p_{{\mathrm{bi}}_k}.Math.\\ t+w_t\left(t\right)\end{array}\right)& \left(62\right)\end{array}$

[0170] 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.sub.t(t)∈[−10.sup.−2 10.sup.−2]s. That is to say, a pair of ranging observations arrives approximately every second, but not exactly.

[0171] To obtain periodic state enclosures, the filter recursion is triggered at the upper bound of the observation time intervals, i.e. (t.sub.1, t.sub.2, t.sub.3, t.sub.4, . . . )=(0.01 s, 0.01 s, 1.01 s, 1.01 s,. . . ).

5.1. Initialization

[0172] 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 ϵ=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.

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

5.2. Propagation

[0174] The state dynamics are already linear in the state. The propagation can thus be directly performed by

{tilde over (Z)}.sub.x(t.sub.k)=Z.sub.x(t.sub.k−1)+Z.sub.s(t.sub.k−1)  (63)

[0175] The state dynamics uncertainty ball defined by equation 58 is outer approximated by a C-zonotope (see Appendix A below) to obtain Z.sub.s(tk−1). An example of the resulting a priori C-zonotope is displayed in FIG. 12. Note how {circumflex over (Z)}.sub.x(t.sub.9) contains the true position at t.sub.9. Note also that t.sub.9 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

[0176] It simplifies things to perform each update in the nominal frame of the corresponding ranging beacon i.sub.k. The a priori C-zonotope in the beacon frame being given by

{circumflex over (Z)}.sub.x.sup.b(t.sub.k)={circumflex over (Z)}.sub.x(t.sub.k)−p.sub.big  (64)

[0177] each ranging observation is then governed by

r(t.sub.k)=∥x(t.sub.k)−∈.sub.b∥+w.sub.r,k  (65)

[0178] To simplify further, we absorb the small beacon uncertainty into the observation error w.sub.r,k by virtue of

∥x∥−∥ϵ.sub.b∥≥∥x−ϵ.sub.b∥≤∥x∥+∥ϵ.sub.b∥  (66)

[0179] leading to

r(t.sub.k)=∥x(t.sub.k)∥+w′.sub.t,k  (67)

[0180] with w′.sub.r,k∈[w′.sub.r,n]=[w.sub.r]+[−rad([ϵ.sub.b]),rad([ϵ.sub.b])].

[0181] From equation 67 we form the linear inclusion

[00016]$\begin{array}{cc}r\left(t_k\right)\in H\left(t_k\right)Z_x\left(t_k\right)+Z\left(\left[w_r^{\prime }\right]\right)+Z\left(\left[{\in }_h\right]\right)+c_h\left(t_k\right)& \left(68\right)\end{array}$$\mathrm{with}$$\begin{array}{cc}H\left(t_k\right)=\frac{{\hat{x}}^T\left(t_k\right)}{.Math.{\hat{x}}^T\left(t_k\right).Math.}& \left(69\right)\end{array}$$\mathrm{and}$$\begin{array}{cc}{c}_h\left(t_k\right)=.Math.x_k.Math.-H\left(t_k\right)\hat{x}\left(t_k\right)& \left(70\right)\end{array}$

[0182] 5.3.1. Observation Interval Inflation

[0183] Before compensating for timing jitter, the state increment interval [Δx(t.sub.k)] is obtained from the velocity norm bound ν

[00017]$\begin{array}{cc}\left[\Delta x\right]=2\stackrel{}{\stackrel{\_}{v}}\mathrm{rad}\left(\left[w_t\right]\right)\left[\begin{array}{c}\left[-1,1\right]\\ \left[-1,1\right]\end{array}\right]& \left(71\right)\end{array}$

[0184] and the inflated observation inclusion is computed as follows

r(t.sub.y,k)∈H(t.sub.k)Z.sub.x.sup.b(t.sub.k)+Z([w′.sub.r])+Z([ϵ.sub.h])+c.sub.h(t.sub.k)+H(t.sub.k)Z([Δx])  (72)

[0185] After computing the a posteriori C-zonotope Z.sub.x.sup.b(t.sub.k) in the beacon frame with eqs. 36-41, the a posteriori C-zonotope is translated back to the navigation frame

Z.sub.x(t.sub.k)=Z.sub.x.sup.b(t.sub.k)+p.sub.big  (73)

[0186] FIG. 12 shows the obtained a posteriori C-zonotope at t.sub.9, as well as the reduced a posteriori C-zonotope. FIG. 13 shows the obtained results with increased timing noise value. Note the larger a posteriori C-zonotope due to increased observation interval inflation.

[0187] The full state enclosure trajectory is displayed in FIG. 14, and in FIG. 15 for increased observation timing noise. After each update, state C-zonotopes have been reduced to n.sub.g=15, n.sub.c=1. By setting Δt 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

[0188] 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.

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

6.1. Initialization

[0190] 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

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

x=p.sub.B−p.sub.A

[0192] 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 millimeter-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

[0193] 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

ΔΦ(p.sub.A,p.sub.B,p.sub.S)=∥p.sub.A−p.sub.S∥−∥p.sub.B−p.sub.S∥+ΔNλ+ϵ.sub.ΔΦ+ΔTc  (75)

[0194] where ϵ.sub.ΔΦ captures residual errors due to hardware noise and multipath, ΔT is the differential receiver clock error, c the speed of light, λ is the carrier wavelength and ΔN the differential carrier cycle ambiguity.

[0195] Using d.sub.S=p.sub.A−p.sub.S and p.sub.B=p.sub.A+x we write more compactly

ΔΦ(x,d.sub.S)+∥d.sub.S∥−∥d.sub.S+x∥+ΔNλ+ϵ.sub.ΔΦ+ΔTc  (76)

[0196] The differential receiver clock error is removed by forming double differences with respect to a reference satellite R

∇Φ(x,d.sub.S,d.sub.R)=∥d.sub.S∥−∥d.sub.S+x∥−∥d.sub.R∥+∥d.sub.R+x∥+ϵ.sub.∇Φ+∇Nλ  (77)

[0197] 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.

[0198] 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

Δ∇Φ(t.sub.k)=∇Φ(t.sub.k)−∇Φ(t.sub.k−1)  (78)

[0199] In the following we show how we can compute a C-zonotope that encloses the position increment Δx=x(t.sub.k)−x(t.sub.k−1).

[0200] 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.

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

[0202] The Lagrange remainder is then evaluated using IA and the resulting interval is added to the hardware error interval ϵ.sub.mΔΦ.

[0203] To simplify the computation of the necessary Jacobian and Hessian it is convenient to write equation 76 as

[00018]$\begin{array}{cc}\mathrm{\Delta \Phi }=\underset{i=1}{\overset{2}{.Math.}}{s}_i.Math.E_i\tilde{z}.Math.+\Delta N\lambda +{ϵ}_{\mathrm{\Delta \Phi }}+\Delta \mathrm{Tc}& \left(79\right)\\ =\stackrel{2}{\underset{i=1}{.Math.}}{s_i\left({\tilde{z}}^T{E}_i^T{E}_i\tilde{z}\right)}^{1\text{/2}}+\Delta N\lambda +{ϵ}_{\mathrm{\Delta \Phi }}+\Delta \mathrm{Tc}& \left(80\right)\\ =\stackrel{2}{\underset{i=1}{.Math.}}{s_i\left({\tilde{z}}^T{M}_i\tilde{z}\right)}^{1\text{/2}}+\Delta N\lambda +{ϵ}_{\mathrm{\Delta \Phi }}+\Delta \mathrm{Tc}& \left(81\right)\end{array}$

[0204] with the augmented state vector

z=(d.sub.S.sup.Tx.sup.T).sup.T  (82)

and

E.sub.1=[I.sub.30.sub.3],E.sub.2=[I.sub.3I.sub.3], s=[1 −1]  (83)

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

[00019]$\begin{array}{cc}\frac{\partial \mathrm{\Delta \Phi }}{\partial z}=\stackrel{2}{\underset{i=1}{.Math.}}{\left(z^T{M}_{i}z\right)}^{-1/2}{z}^T{M}_i& \left(84\right)\end{array}$$\begin{array}{cc}=[\begin{array}{cc}\frac{\partial \mathrm{\Delta \Phi }}{\partial d_s}& \frac{\partial \mathrm{\Delta \Phi }}{\partial x}]\end{array}& \left(85\right)\end{array}$$\begin{array}{cc}\frac{{\partial }^2\mathrm{\Delta \Phi }}{\partial z^2}=\stackrel{2}{\underset{i=1}{.Math.}}{\left(z^T{M}_{i}z\right)}^{-1/2}{M}_i^T-M_i^T{z\left(z^T{M}_{i}z\right)}^{-3/2}{z}^T{M}_i& \left(86\right)\end{array}$

[0206] 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

[00020]$\begin{array}{cc}\left[R_{\Delta \Phi }\right]=\frac{\partial \mathrm{\Delta \Phi }}{\partial d_s}(\left[d_s\right]\left(-\mathrm{mid}\left(\left[d_s\right]\right)\right)+{\left(\left[z\right]-\mathrm{mid}\left(\left[z\right]\right)\right)}^T\frac{{\partial }^2\mathrm{\Delta \Phi }\left(\left[z\right]\right)}{\partial z^2}& \left(87\right)\end{array}$

[0207] We proceed to forming the linear single difference observation

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

+pdvΔΦx(x−mid([x]))+R.sub.ΔΦ+ΔNλ+ΔTc  (89)

[0208] As [R.sub.ΔΦ] is generally very narrow, of the order of millimeters, for small separations we can linearize about x=0 and have the more compact form

ΔΦ=H.sub.Sx+∈′.sub.ΔΦ+ΔNλ+ΔTc  (90)

[0209] familiar from the RTK literature, with ϵ′.sub.ΔΦ=ϵ.sub.ΔΦ+R.sub.ΔΦ+pdνΔΦx(x−mid([x])). We can then form a linear double difference observation model that is free of dependencies

[00021]$\begin{array}{cc}\nabla \Phi =\left(H_R-H_S\right)x+{ϵ}_{\nabla \Phi }^{\prime }& \left(91\right)\end{array}$$\begin{array}{cc}=H_{\nabla }x+{ϵ}_{\nabla \Phi }^{\prime }& \left(92\right)\end{array}$

[0210] where ϵ′.sub.VΦ now lumps together observation and linearization errors.

[0211] Taking the difference of two consecutive carrier phase observations with observation times t.sub.k−1 and t.sub.k respectively, the constant ambiguity terms are eliminated. Introducing Δx(t.sub.k)=x(t.sub.k)−x(t.sub.k−1), the T3D observation is given by

[00022]$\begin{array}{cc}\Delta \nabla \Phi =H_{\nabla }\left(t_k\right)x\left(t_k\right)-H_{\nabla }\left(t_{k-1}\right)x\left(t_{k-1}\right)+{ϵ}_{\Delta \nabla \Phi }\left(t_k\right)& \left(93\right)\end{array}$$\begin{array}{cc}=H_{\nabla }\left(t_k\right)x\left(t_k\right)-\left(H_{\nabla }\left(t_k\right)+\Delta H_{\nabla }\left(t_k\right)\right)x\left(t_{k-1}\right)+{ϵ}_{\Delta \nabla \Phi }\left(t_k\right)& \left(94\right)\end{array}$$\begin{array}{cc}=H_{\nabla }\left(t_k\right)\Delta x\left(t_k\right)-\Delta H_{\nabla }\left(t_k\right)x\left(t_{k-1}\right)+{ϵ}_{\Delta \nabla \Phi }\left(t_{k-1}\right)& \left(95\right)\end{array}$$\begin{array}{cc}=H_{\nabla }\left(t_k\right)\Delta x\left(t_k\right)+{ϵ}_{\Delta \nabla \Phi }^{\prime }\left(t_k\right)& \left(96\right)\end{array}$
with

ϵ′.sub.Δ∇Φ(t.sub.k)∈[ϵ′.sub.Δ∇Φ(t.sub.k)]  (97)

[ϵ′.sub.Δ∇Φ(t.sub.k)]=[ϵ.sub.∇Φ(t.sub.k)]−[ϵ.sub.∇Φ(t.sub.k−1)]−[ΔH.sub.∇(t.sub.k)][x(t.sub.k−1)]  (98)

[0212] We exploit here the fact that the geometry matrix H.sub.∇ changes very little between two consecutive time steps due to the large distance between receiver and satellites, leading to a very narrow incremental term ΔH.sub.∇(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

Δ∇Φ∈H.sub.∇Δx+[ϵ]  (99)

[0213] The T3D observation Δ∇Φ constrains Δx to a polytope P whose half-space representation is given by

[00023]$\begin{array}{cc}\left(\begin{array}{c}-H_{\nabla }\Delta x\\ H_{\nabla }\Delta x\end{array}\right)\le \left(\begin{array}{c}\overline{ϵ}\\ \underset{¯}{ϵ}\end{array}\right)-\left(\begin{array}{c}\Delta \nabla \Phi \\ -\Delta \nabla \Phi \end{array}\right)& \left(100\right)\end{array}$$\begin{array}{cc}P\Delta x\le k& \left(101\right)\end{array}$

[0214] The corresponding C-zonotope Z.sub.ΔX(t.sub.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 F(t.sub.k−1)=I.sub.3, B(t.sub.k−1)=I.sub.3, Z.sub.Δx(t.sub.k), Z.sub.q(t.sub.k−1)=0.

6.2.2. Velocity-Norm Propagation

[0215] 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.sub.k−t.sub.k−1 the norm-bound ν.sub.max defines a sphere with radius r=ν.sub.max(t.sub.k−t.sub.k−1). We outer-approximate this sphere by a symmetric C-zonotope, Z.sup.S of user-defined complexity. The propagation is then performed by setting

F(t.sub.k−1)=I.sub.3, B(t.sub.k−1)=I.sub.3, Z.sub.u(t.sub.k−1)=0, Z.sub.q(t.sub.k−1)=Z.sup.S.

6.3. Update

6.3.3. GNSS Update

[0216] GNSS code phase single difference observations are governed by

Δρ(p.sub.A,p.sub.J,p.sub.S)−οp.sub.A−p.sub.S∥−∥p.sub.B−p.sub.S∥+ϵ.sub.Δρ+Tc  (102)

[0217] where ϵ.sub.Δρ captures residual errors due to hardware noise and residual atmospheric errors, ΔT is the differential receiver clock error and c is the speed of light.

[0218] Using d.sub.S=p.sub.A−p.sub.S and p.sub.B=p.sub.A+x we write more compactly

Δρ(x,d.sub.S)=μd.sub.S∥−∥d.sub.S∥+x∥+ϵ.sub.Δρ+ΔTc  (103)

[0219] The differential receiver clock error is removed by forming double differences with respect to a reference satellite R

∇ρ(x,d.sub.S,d.sub.R)=∥d.sub.S∥−∥d.sub.S+x∥−∥d.sub.R∥+∥d.sub.R+x∥+ϵ.sub.∇ρ  (104)

[0220] 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

∇ρ∈H.sub.∇Z.sub.x+Z([ϵ′.sub.∇ρ])  (105)

[0221] allowing for an update of the state enclosure.

[0222] 6.3.2. Ranging Update

[0223] The inter-vehicle ranging observations are given by

r=∥x+R.sup.Ar.sub.A−R.sup.Br.sub.B∥+ŵ.sub.r,k  (106)

[0224] with the positions τ.sub.A, τ.sub.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 ŵ.sub.τ∈[ŵ.sub.τ]. Equation 106 is subject to parametric uncertainties due to uncertainty in the attitudes R.sup.A,R.sup.B of both UAS as well as uncertain antenna positions τ.sub.A, τ.sub.B.

[0225] In the following we assume that τ.sub.A, τ.sub.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.sub.r:

t=∥x∥+w.sub.r  (107)

[0226] To linearize equation 107, we first write range observations as

r=(x.sup.Tx).sup.1/2+w.sub.r  (108)

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

[00024]$r=.Math.\hat{x}.Math.+\frac{\partial r}{\partial x}\left(x-\hat{x}\right)+{ϵ}_r+w_r$

[0228] with the Jacobian and Hessian

[00025]$\begin{array}{cc}\frac{\partial r}{\partial x}={\left(x^{T}x\right)}^{-1/2}{x}^T& \left(109\right)\end{array}$$\begin{array}{cc}\frac{{\partial }^{2}r}{\partial x^2}={\left(x^{T}x\right)}^{-1/2}{I}_3-{x\left(x^{T}x\right)}^{-3/2}{x}^T& \left(110\right)\end{array}$

[0229] Geometrically, this linearized range observation corresponds to the relative position vector projected on the gradient vector

[00026]$\frac{\partial r}{\partial x},$

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.

[0230] 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.

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

[0232] 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.

[0233] The explicit linearization error interval is given by

[00027]$\left[{ϵ}_r\right]=.Math.\left[x\right].Math.-||\mathrm{mid}\left(\left[x\right]\right).Math.-\frac{\partial r}{\partial x}\left(\left[x\right]-\mathrm{mid}\left(\left[x\right]\right)\right)$

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

[00028]$\begin{array}{cc}\left[\frac{\partial {ϵ}_r}{\partial x}\right]={{\left({\left[x\right]}^T\left[x\right]\right)}^{-1/2}\left[x\right]}^T-\frac{\partial r}{\partial x}& \left(111\right)\end{array}$$\begin{array}{cc}=\frac{\left[x^T\right]}{.Math.\left[x\right].Math.}-\frac{{\mathrm{mid}\left(\left[x\right]\right)}^T}{.Math.\mathrm{mid}\left(\left[x\right]\right).Math.}& \left(112\right)\end{array}$

[0235] 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∈.sup.3, the number of these subintervals is at most 2.sup.3, corresponding to at most 27 unique vertices. By evaluating equation (111) for each of these vertices, an exact enclosure of [ϵ.sub.τ] can be computed. To quantify the benefit of exploiting piecewise monotonicity, we compute the interval with both approaches and compare their respective radii. FIG. 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.

[0236] 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

[0237] 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 FIG. 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

[0238] 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 FIG. 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

[0239] 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. FIG. 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

[0240] 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.

[0241] 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

[0242] To approximate a ball of radius r centered at c, we compute an approximation in H-rep and convert it to a C-zonotope.

[0243] Given a vector of angles θ=(θ.sub.1, θ.sub.2, . . . , θ.sub.l) evenly spaced in the interval

[00029]$\left[-\frac{\pi }{2},\frac{\pi }{2}\right]$

and ϕ=(ϕ.sub.1, ϕ.sub.2, . . . , ϕ.sub.l) evenly spaced on [0, 2π] wherein θ is the zenith angle and ϕ is the azimuth angle and l is a complexity parameter, we compute for each combination of angles (θ.sub.i, ϕ.sub.j) a corresponding normal vector

[00030]$\begin{array}{cc}{n}_{i,j}=\left(\begin{array}{c}\sin {\theta }_i\cos {\varphi }_j\\ \sin {\theta }_i\sin {\varphi }_j\\ \cos {\theta }_i\end{array}\right)& \left(A\text{.1}\right)\end{array}$

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

[00031]$\begin{array}{cc}\left[\begin{array}{c}{n}_{1,1}^T\\ n_{1,2}^T\\ n_{2,1}^T\\ .Math.\\ n_{l,l}^T\end{array}\right]x\le r\left(\begin{array}{c}1\\ 1\\ 1\\ \mathrm{.Math.1}\end{array}\right)& \left(A\text{.2}\right)\end{array}$

[0245] 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.

[0246] In two dimensions, the same approach is performed over a single set of angles θ=(θ.sub.1, θ.sub.2, . . . , θ.sub.l) evenly spaced on [0, 2π].