Method of representing a location and pose of an object for tracking and predicting a trajectory thereof, and apparatus implementing the method

Abstract

A method of representing a location and pose of a remote object relative to an ego-object, independent of the shapes and / or sizes of the remote object and the ego-object, comprises: - representing the ego-object as a unit vector (v p ) pointing in the direction of the x-axis of the ego-object's conformation matrix, - receiving a translation vector estimate (t̂) and a rotation matrix estimate (Q̂) representing a distance and orientation, respectively, of the remote object relative to the ego-object, - modelling the remote object as unit vector (v τ ) pointing in the direction of the x-axis of the corresponding conformation space, wherein an estimated target unit vector (v̂ τ ) is determined as the sum of the translation vector estimate (t̂) and the product of the ego-object's unit vector (v p ) and the rotation matrix estimate (Q̂), and - outputting the remote object's unit vector (v τ ).

Description

[0001] 202500276

[0002] -1- METHOD OF REPRESENTING A LOCATION AND POSE OF AN OBJECT FOR TRACKING AND PREDICTING A TRAJECTORY THEREOF, AND APPARATUS IMPLEMENTING THE METHOD

[0003] FIELD OF THE INVENTION

[0004] The present invention relates to object localisation, in particular to localisation relative to an observer that does not require information about a shape and / or size of the object. The present invention further relates to tracking and predicting a trajectory of an object.

[0005] NOTATIONS

[0006] Vectors and matrices are represented herein in bold small letters and bold capital letters, respectively, and the symbol ⊙ indicates an element-wise matrix operation, e.g., multiplication or exponentiation. ⊗ is the Kronecker product.

[0007] BACKGROUND

[0008] Global positioning system (GPS), along with the European counterpart Galileo, the Russian counterpart Glonass, and the Chinese counterpart Beidou, to name the most popular ones, is one of the most popular earth-referenced, satellite-based positioning systems used for localization and navigation. However, in many cases of interest, e.g., in underwater applications or indoor environments, the satellite signals are either unavailable or seriously impaired. In such environments, sensor networks provide effective localization solutions.

[0009] Localization can be either absolute, e.g., using spatial reference points that are also referred to as anchors, or relative, i.e., without any reference or “anchorless”. Often, sensors with known absolute positions, i.e., anchors, are deployed in an area nearby the trajectory of an object to be located for absolute localization.

[0010] When a sensor infrastructure with known absolute positions is not available, relative localisation in such anchorless scenario is solved using multi-dimensional scaling, e.g., as presented by W. S. Torgerson in “Multidimensional scaling: I. Theory and method," Psychometrika, vol. 17, no. 4, pp. 401–419, Dec. 1952 or by J. A. Costa, N. Patwari, and A. O. Hero in “Distributed weighted multidimensional scaling for node

[0011] Internal202500276

[0012] -2-localization in sensor networks,” ACM Trans. Sens. Networks, vol. 2, no. 1, pp. 39-64, Feb. 2006.

[0013] Wireless localization can be seen as a precursor of joint communication and sensing (JCAS), in so far as it demonstrates that communication signals can also be used for sensing an environment and acquiring situation awareness, including localization of users - functionalities that have been identified as key drivers for beyond fifth-generation (B5G) and sixth-generation (6G) wireless communication systems, as well as for new applications such as the internet of vehicles (IoV) and digital twins. There are many types of information that can be extracted from radio signals for the purpose of localization, including finger-prints, received signal strength indicator (RSSI), angle of arrival (AoA), or delay-based estimates of radio range, for the purpose of localisation. Conventionally, such information needed for localization was generally assumed to be obtained by specialized equipment and dedicated protocols, requiring the transmission of purpose-designed signals, implicating in cost- and other constraints which in turn explains the predominance in related literature of methods to find the position of individual points.

[0014] Recently, however, advances in JCAS technology has demonstrated that radar parameters, i.e., range, bearing and velocity, can be acquired by conventional communications signals not only actively, i.e., using signals transmitted by the target to the sensors, but also passively, i.e., using round-trip reflections of signals transmitted by the sensors themselves, which in turn implies a more abundant and richer availability of positioning information. A consequence of this development is an increasing interest in the rigid body localisation (RBL) problem, described for example by Y. Wang, G. Wang, S. Chen, K. C. Ho, and L. Huang, in " An investigation and solution of angle based rigid body localization," IEEE Transactions on Signal Processing, vol. 68, pp. 5457-5472, 2020, or N. Fuhrling, H. S. Rou, G. T. F. de Abreu, D. Gonzalez Gonzalez, and O. Gonsa, in " Soft-connected rigid body localization: State-of-the-art and research directions for 6G, "2023, and, by the same authors, in “Enabling Next-Generation V2X Perception: Wireless Rigid Body Localization and Tracking," arXiv preprint arXiv:2408.00349, 2024, whose objective is to determine not only the location of point targets, or their average, but the shape and orientation of objects, based on a collection of points sufficient to define the latter.

[0015] Internal202500276

[0016] -3- This feature of RBL is particularly attractive to vehicle-to-anything (V2X) networks, where - unlike earlier applications of positioning technology such as asset management in industrial settings and people tracking in indoor settings - information on the size, shape, and orientation of vehicles are crucial to ensure the efficacy and safety of autonomous driving (AD) applications such as collision detection, navigation, and vehicle path prediction, to name only a few examples.

[0017] Focusing on the V2X and loV paradigm in particular, a scenario commonly encountered is that a vehicle is able to obtain relative information between itself and surrounding vehicles, which, if processed adequately, can be utilized to enable RBL as a means to enrich applications such as advanced AD (aAD), platooning and more.

[0018] It is important also to distinguish between the type of RBL system addressed here, which is based on radio signals, preferably under a JCAS paradigm, and conventional simultaneous localization and mapping (SLAM) technologies. The latter relies on rather dedicated equipment, including laser scanners, and require massive amounts of data to function, which is costly and exhibits significant latency, which makes the latter less likely to be useful in AD applications envisioned for a future where autonomous vehicles (AVs) are widely deployed.

[0019] An example of the radio-based RBL approach is the method discussed by S. Bras, M. Izadi, C. Silvestre, A. Sanyal, and P. Oliveira, in " Nonlinear observer for 3D rigid body motion estimation using doppler measurements," IEEE Transactions on Automatic Control, vol. 61, no. 11, pp. 3580-3585, 2016, where the pose, angular velocity and trajectory of a rigid body is estimated using Lyapunov functions of Doppler measurements, obtained by a nonlinear observer. Another example is discussed by S. Chen and K. C. Ho, in " Accurate localization of a rigid body using multiple sensors and landmarks," IEEE Transactions on Signal Processing, vol. 63, no. 24, pp. 6459-6472, 2015, in which a two-stage approach is used to estimate rotation, translation, angular velocity and translational velocity by range and Doppler measurements, making use of various weighted least square (WLS) minimization methods. And going beyond the problem of RBL involving a single object, the scheme discussed by A. Pizzo, S. P. Chepuri, and G. Leus, in " Towards multi-rigid body localization," 2016 IEEE International Conference on Acoustics, Speech and

[0020] Internal202500276

[0021] -4- Signal Processing (ICASSP), 2016, pp. 3166-3170, which, after an earlier presentation of an anchor-based scheme, propose a new relative multi-object RBL method in an anchorless scenario, where the relative translation and rotation between two rigid bodies is estimated by measuring the cross-body line-of-sight (LOS) distances between the points defining the two bodies. The latter case relates to a common scenario in AD where a vehicle is able to measure the distance between itself and vehicles in its surroundings, such that the corresponding RBL solution would find a large, direct and crucial application.

[0022] Unfortunately, however, the aforementioned, as well as most other, state-of-the-art (SotA) RBL methods assume that the shape of the target rigid body is known, which is unrealistic in real life applications since vehicles vary greatly in shape and size. Further, most conventional RBL methods are located in a framework with one rigid body to localize and a few anchors for reference. While S. P. Chepuri, G. Leus and A. -J. van der Veen, in " Rigid Body Localization Using Sensor Networks," IEEE Transactions on Signal Processing, vol. 62, no. 18, pp. 4911-4924, Sept.15, 2014, propose an anchorless multi-dimensional scaling (MDS)-based solution for relative localisation, this conventional method is further limited to estimating translation and rotation of a single object relative to the ego-object.

[0023] Yet further, the data sets determined for RBs during the localisation are large and cumbersome to operate with when it comes to representing their positions and pose in an environment relative to an ego-object, in particular for tracking and predicting their respective trajectories, which is a core requirement for AD and aAD.

[0024] It is, therefore, desirable to obtain a method yielding an improved and simpler representation of locations and poses of remote objects suitable for egoistic RBL methods that does not require information on the size and / or shape of the remote object, e.g., another vehicle, and which facilitates tracking and predicting the remote objects’ trajectories.

[0025] SUMMARY OF THE INVENTION

[0026] This object is attained by the methods presented in the claims and the apparatus implementing the methods. Advantageous embodiments and developments are

[0027] Internal202500276

[0028] -5-provided in the respective dependent claims. A computer program product and a corresponding computer-readable medium, respectively, are likewise provided.

[0029] Prior to discussing the method in accordance with the invention, an underlying system model will be discussed, referring to a system as exemplarily shown in figure 1. Figure 1 shows an exemplary rigid body to be localised and tracked, here a vehicle, at two distinct locations

[0030]

[0031] and

[0032] In the system model the rigid body is represented by a collection of N landmark points cn∈ ℝ3×1in the three-dimensional (3D) space, with n = {1, —, N}, such that the shape of said body is well described by the corresponding conformation matrix C = [c1, •••, cN] ∈ ℝ3×Nconstructed by the column-wise collection of the vectors cnwith reference to the 3D space defined by the spatial dimensions ql, q2, and q3. In other words, the conformation matrix captures the locations of all 3d landmark points of a rigid body in a reference frame - typically centred at the origin, which is defined by the user itself, i.e., in terms of its initial orientation. Note that the landmark points are represented by the small antenna symbols located the vehicles in the figure. Landmark points are preferably located at extreme positions of the rigid body, e.g., comers and edges. Then, consider the representation of the location

[0033]

[0034] of said rigid body relative to another location S(o), e.g., a temporally earlier location in case the body is in motion, which without loss of generality can be set to be a "canonical reference, i.e., centred at the absolute origin, such that the initial position

[0035]

[0036] = C, which defines the shape and orientation of the rigid body, and thus one can write

[0037] S = Q C + t 1NT= [Q | t]

[0038]

[0039] N.

[0040] where t e IR3X1is a translation vector given by the difference of the geometric centres of the body at the two locations, lwis a column vector with N entries all equal to 1, and Q e IR3X3is a rotation matrix determined by corresponding yaw, pitch and roll angles a, / 3 and y, respectively, namely

[0041] Internal202500276

[0042] Q = QzWQyWQxW

[0043] cosy — siny 0 ' cos (3 0 sin / ? 1 0 0

[0044] =siny cosy 0 0 1 0 0 cos a — sin a

[0045]

[0046] . 0 0 1..— sin / ? 0 cos (3,.0 sin a cos a.

[0047] cos / ? cosy sin a sin (3 cosy — cos a sin y cos a sin (3 cosy + sin a sin y cos (3 sin y sin a sin (3 sin y + cos a cosy cos a sin (3 sin y — sin a cosy

[0048]

[0049] . — sin (3 sin a cos (3 cos a cos (3

[0050]

[0051] Ql,l Ql,2 Ql,3'

[0052] Q2,l Q2,2 ^2,3

[0053] /

[0054]

[0055] ?3,1 Q3,2 Q3,3.

[0056] The second location

[0057]

[0058] of the rigid body is then determined according to equation (1), and is obtained by the transformation

[0059]

[0060] of via a rotation matrix Q and a translation vector t.

[0061] Note that, for the sake of simplicity, superscripts may be omitted herein whenever clarity is not compromised.

[0062] Note further that, again for the sake of simplicity, detecting the orientation of a rigid body will be interpreted as estimating of the 9 elements of the corresponding rotation matrix Q as a whole. However, as shown by. Vizitiv, H. S. Rou, N. Fuhrling, and G. T. F. de Abreu, in " Belief Propagation-based Rotation and Translation Estimation for Rigid Body Localization," arXiv preprint arXiv:2407.09232, 2024, this representation can be extended by replacing the estimation of Q with the estimation of the associated and fundamental yaw, pitch and roll angles (a,(3,y).

[0063] Next, consider a scenario as illustrated in figure 2, in which two distinct rigid bodies, hereafter referred to by their indices i = {1,2}, have generally different shapes and / or are characterized by generally distinct numbers N and N2of landmark points, respectively, such that under a common absolute reference, the bodies are represented by the corresponding distinct conformation matrices C e IR3xW1and C2e IR3XW2. The translation vector t between the bodies, depicted in a dash-dotted line, is defined by the difference between the geometric centres of the two bodies.

[0064] Since C2, it is obvious that in such a scenario the location of one body relative to the other cannot be described in terms of equation (1 ). A common problem in V2X

[0065] Internal202500276

[0066] -7-systems with relevance to AD applications is, however, that one rigid body - say, the truck in figure 2- is able to estimate not only its distance to the other - in this case, the car in figure 2 - but also its shape and orientation, based on a set of measurements of the distances between their corresponding landmark points.

[0067] It will be considered, in what follows, that the distance measurements among the landmark points, hereinafter referred to as “sensors”, on both rigid bodies can be obtained either via point-to-point communications or other technologies such as radar, video or JCAS. It will further be assumed that each body is only aware of its own shape, described by the corresponding conformation matrices

[0068] Ct=

[0069]

[0070] eD3xWi, where cin, is the location of the n-th point of the i-th body, with respect to its geometric centre.

[0071] When subject to unbiased estimation errors, the estimates of the distance between a pair of sensors slnon the first body and s2 mon the second can be described by

[0072] ^

[0073]

[0074] •n,m dn m"f" ^n,m> (3)

[0075] where dnm||sl n- s2 in|| is the true pairwise distance between the sensors, while vnmdenotes measurement noise modelled as i.i.d. zero mean Gaussian random variables with variance < J2. Note that the notation si nis used herein as a reference to both, the sensor and its location.

[0076] In order to avoid negative numbers and to linearize the relationship between the acquired squared distances and corresponding measurement errors, the equivalent model shall also be considered, given by

[0077] d

[0078]

[0079] n,m d-n,m "f" ^n,m> (4)

[0080] where the mean and variance of the squared-distance estimation error )nmare respectively given b

[0081]

[0082] y IE[a>nm] = < J2and IE - E[a>nm])2] = 4d2ma2+ 2< J4, as described by S. P. Chepuri, A. Simonetto, G. Leus, and A. -J. van der Veen, in " Tracking position and orientation of a mobile rigid body," 20135th IEEE International

[0083] Internal202500276

[0084] -8- Workshop on Computational Advances in Multi-Sensor Adaptive Processing (CAMSAP), 2013, pp. 37-40.

[0085] It proves convenient, to collect the true distances dnmfrom above into the euclidean distance matrix (EDM)

[0086] £>i D12

[0087] G ]]j(wi+W2)x(wi+W2) (5)

[0088]

[0089] ^12 ^2 -

[0090] which includes both the pairwise distances between the rigid bodies D12, as well as the intra-distances of the two individual rigid bodies D and D2.

[0091] With the system and measurement model introduced above, the problem to be solved can be clearly defined. To that end, first observe that assuming, without loss of generality, that the rigid body 1, i.e., the truck, attempts to egoistically locate body 2, i.e., the car, the system model conditions described earlier translate to the assumption that the ego intra-distance matrix D is known exactly, the target intradistance matrix D2is unknown, and the squared cross-distance matrix D12can be written as

[0092] D

[0093]

[0094] ?22= D12o D12= 1^2+ - 2SlS2, (6)

[0095] where S and S2are matrices containing the locations of the sensors in bodies 1 and 2, respectively, the auxiliary vectors

[0096]

[0097] S^St carry the the squared norms of the corresponding individual sensor locations.

[0098] Since the cross-body measurements typically can only be assured to be available under LOS conditions, it is possible that some distances between sensors on both bodies cannot be measured. To incorporate this into the model developed above the modified notation of the squared measurements is given by

[0099] D2Q W = diagC ^) + diag( >2) - 2(S]S2) Q w, (7)

[0100] where the so-called connectivity matrix W, which captures the M available measurements, is defined as

[0101] Internal202500276

[0102] 1 1T1 1TW = (8)

[0103]

[0104] 0WI-M0^2-M

[0105] Next, consider an augmented sensor location matrix carrying the positions of all landmark points in both bodies, such that we may write, in similarity to equation (1 )

[0106] C 03xWfll Ol

[0107] s = [S1 I S2] = [Qi I Q2] - + [ti I t2], (9)

[0108]

[0109] HhxNiC2 |0Wi1W2]

[0110] where Qtand ttrespectively denote the rotation matrix and translation vector of the i-th body, while 03xw,0wand lwdenote an all-zero matrix and an all-zero / all-one column vector, respectively.

[0111] Under the egoistic assumption, i.e., S =

[0112]

[0113] and t = 03, however, equation (9) reduces to

[0114] - <• Q jT QT S = [S1| S2] = [ / | Q] -X— + [0 | t], (10)

[0115]

[0116] LU C2J [0W1! N2J

[0117] where the notation is simplified by omitting subscripts that can be inferred from context, which includes relabelling Q = Q2and t = t2.

[0118] In view of the foregoing discussion the problem, i.e., the attempt by rigid body 1, e.g., the truck in figure 2, to locate rigid body 2, e.g., the car in figure 2, without support from infrastructure, translates to estimating, based on equations (6) and (10), the rotation matrix Q and translation vector t, given perfect knowledge of the conformation matrix - which implies exact knowledge of D - and possession of an estimate of the matrix D12subject to noise, under the egoistic condition that C2is unknown and for a general case where N N2.

[0119] A known approach for estimating the rotation matrix Q is discussed in " Towards multi-rigid body localization" (full citation further above), however limited to the special case where N = N2and with the idealistic assumption that C2is perfectly and fully known. In addition to this significant difference, the known approach is limited to estimating a translation distance, which is not useful in the scenario considered

[0120] Internal202500276

[0121] -10-herein, as a full translation vector is required. Upon closer inspection, the known approach is ineffective for the estimation of the translation vector t. Nevertheless, the known approach is useful for understanding the estimation of the rotation matrix Q.

[0122] First, consider the N x N classic Schoenberg double-centring matrix (DCM), defined by W. S. Torgerson in " Multidimensional scaling: I. theory and method," (full citation further above),

[0123] 7 = / -^llT. (11)

[0124]

[0125] Taking into account incomplete observation and making use of the Schonberg DCM, a projection matrix P can be formulated, written as

[0126] P = JM 0M0N-M

[0127] (12)

[0128]

[0129] JN-M -

[0130] Left- and right-multiplying a measured distance matrix by the projection matrix P, and scaling the result by

[0131]

[0132]

[0133] as well as applying the connectivity matrix W, yields

[0134] o?22= - 1 O W)P = (PSlS2P) O W

[0135]

[0136] = (ClQC2') Q W, (13)

[0137] Where, due to the missing measurements, only the visible measurements Civ, i.e., the first M columns of the conformation matrix Ctare required.

[0138] In order to facilitate the formulation of a problem to estimate Q, it proves convenient to apply an orthogonal Procrustes problem (OPP) onto equation (13), which under the assumption of perfect knowledge of C2can be achieved by defining

[0139] ftO2A. nO2 t _ / -T zj

[0140] 1712 —2712G2, V— Gl,vV’ (14a) where

[0141]

[0142] (14b)

[0143] Internal202500276

[0144] -11- Then, the relative rotation Q of body 2 with respect to the orientation of body 1 can be estimated by solving the problem

[0145] QOPP = arg min||D®2-

[0146] QeK

[0147]

[0148] 3x3

[0149] which can be obtained in closed form via singular value decomposition (SVD) of the matrix D®.

[0150] In particular, the solution of the problem represented by equation (12) is given by

[0151] QOPP = UVT, (16a) with U and V such that

[0152] D®2= UΣVT. (16b)

[0153] While the solution to this problem is well known, it is noted that in order to use the orthogonal projection specific rank restrictions have to be fulfilled. Specifically, in the scenario considered herein, it is necessary to achieve rank(C̄2,v) = 3, where the rank can be generalized by the projection in equation (13), leading to rank(C̄2,v) = M - 1. This means that at least 4 links are needed in order to perform the estimation and avoid singularities.

[0154] It is emphasized that although it was assumed in " Towards multi-rigid body localization" (full citation further above) that both rigid bodies have the same number of landmark points, i.e., N = N2, the notion of a relative rotation between two bodies of different shapes and number of landmark points is geometrically well defined, as can be inferred from equation (10). In particular, by aligning the rotation matrix of the first rigid body with the cartesian coordinates, such that Q = I, the orientation Q2of the second body with respect to the first, becomes simply the relative rotation itself. In other words, Q1= I ⟹ Q2= Q, or more generally, Q = Q1T· Q2

[0155] The assumption of pre-existing knowledge of the conformation matrix C2, which is typical for the conventional RBL methods, is hard to meet in practical conditions. In AD-related V2X applications, for instance, satisfying such assumption would require

[0156] Internal202500276

[0157] -12-that a vehicle attempting to locate other vehicles in its vicinity is aware of their shapes, an obviously impractical requirement given the enormous diversity in vehicle models, which are also are constantly updated. In order to mitigate this problem, the present invention proposes methods to estimate t and Q, respectively, without the requirement that C2is known.

[0158] For the estimation of the translation vector t it is crucial to acknowledge that not knowing C2implicates not knowing the intra-distances matrix D2. And while the reverse implication is not logically true - i.e., in principle one could have knowledge of D2but not C2- the assumption that D2is also not available to the rigid body 1 is consistent with the egoistic principle assumed herein, as indeed, an assumption of knowledge of D2would require that the target vehicle broadcasts such information. Notice that an / V-point 3D conformation matrix contains 3N entries, while the corresponding intra-distance matrix contains N(N - l) / 2 distinct entires, such that the intra-distances data is larger than the conformation data for N > 7, which is a small number of points to define a rigid body in 3D. In what follows, therefore, it is assumed that D2is not known.

[0159] Under such conditions, the first problem at hand is one of matrix completion, and although several methods to solve such a problem exist, a number of which could be used in the present context, here the simple and well-known Nyström approximation method as presented by C. K. I. Williams and M. Seeger, in "Using the Nyström method to speed up kernel machines," Proceedings of the 13th International Conference on Neural Information Processing Systems, ser. NIPS'00. Cambridge MA, USA: MIT Press, 2000, p. 661-667, is exemplarily considered, which, applied to the EDM D from equation (5), yields the following estimate of D2

[0160] b

[0161]

[0162] 2« (17)

[0163] where IHI[-] denotes a hollowing operator that enforces all elements of the diagonal matrix to be zero. Note that the Nyström approximation in general only works if rank > rank (D2), which means that the first body must have at least the same amount of sensors as the second body. If that condition is not satisfied, alternative matrix completion methods may yield better results. Note further that, although more

[0164] Internal202500276

[0165] -13-sophisticated matrix completion (MC) methods exist, which could possibly yield better performance, the RBL method discussed herein offers additional robustness to incompletion.

[0166] With the knowledge of the intra-distances matrix of the first body D, calculated from the conformation matrix Critself, the measurements b12corresponding to the distances between the two bodies, and the latter estimate b2of the intra-distances matrix corresponding to the second rigid body, the full sample EDM corresponding to all distances within and between the two rigid bodies can be constructed as

[0167] (18)

[0168]

[0169] such than a multi-dimensional scaling (MDS)-based first estimate of the position of all sensors from both rigid bodies can be obtained as described by W. S. Torgerson, in " Multidimensional scaling: I. theory and method" (full citation further above), i.e., as

[0170] [

[0171]

[0172] [Ŝ1, Ŝ2] = V̂Â1 / 2, (19a)

[0173] where V and A are the eigenvectors and eigenvalues of the corresponding doublecentred EDM, that is

[0174] D = VAVT, (19b) with

[0175] H

[0176]

[0177] =~ 2^ (19c)

[0178] where /

[0179]

[0180] W1+N2isan(Ni + N2) -point Schonberg DCM built as described in

[0181] equation (11).

[0182] The initial MDS solution given by equation (19a) can then be brought to the reference frame of the first rigid body via a Procrustes transformation

[0183] s

[0184]

[0185] 2= R S2+ t* ® (20a) where

[0186] Internal202500276

[0187] (R*,t*) = arg min ||Ŝ1- RŜ2+ t ⊗ 1T||F, (20b)

[0188] R ∈ ℝ3×3, t ∈ ℝ3×1

[0189] from which the estimate

[0190] S

[0191]

[0192] = [C1, S2] (21a)

[0193] can be obtained, which, substituted into equation (10) and using the relation Q2C2= S2JN2, yields

[0194] [C103×N₂] [1TN₁0TN₂]

[0195] s = + [0 I t] (21b)

[0196]

[0197] . OjxNi (R*S*2+ t* ® lNTJjN2\ 0 IwV 1N2.

[0198] Utilizing the latter expression, finally a quadratic program can be formulated to find the translation vector t, namely

[0199] t̂ = arg min ||JN₁+N₂(Ŝ + [0 | t] ⊗ 1TN₁+N₂)||2F(22a)

[0200]

[0201] t F

[0202] which can easily be solved by common optimization tools, such as gradient descent or interior point methods.

[0203] The notion of incomplete observations has already been integrated further above, by capturing incomplete observations via the erasure matrix

[0204] W = (22b)

[0205]

[0206] N2

[0207] In view of the foregoing discussion a method 100 for the MDS-based egoistic translation estimation of a remote rigid body without knowledge of the conformation matrix of the remote rigid body, i.e., exclusively based on the range information between the two rigid bodies and the conformation matrix of the ego-rigid body, i.e., the rigid body from whose perspective the translation is estimated, can be summarized as follows, with reference numerals referring to the flow diagram shown in figure 3.

[0208] Internal202500276

[0209] -15-

[0210] Inputs to the method 100 are the conformation matrix C associated with the ego-rigid body and the distance measurements represented by the inter-distance matrix b12that contains the pair-wise distances between sensors of the ego-rigid body and of the remote rigid body. The output of the method 100 is the translation vector estimate t. In step 110 the inter-distance matrix b12containing the respective pairwise inter-object distance measurements is determined using wireless signals or is received from an apparatus configured to perform such measurements, and further the conformation matrix C associated with the ego-object carrying out the method 100 is received. In step 120 an intra-distances matrix D of the first body is constructed based on the knowledge of the conformation matrix C and the measurements comprised in the inter-distance matrix b12, and subsequently, in step 140, a full EDM is calculated via equation (18). Calculating the full EDM may comprise determining the unknown intra-distances matrix D2of the remote body, e.g., through a Nyström approximation, in an optional step 130. In step 150, an initial estimate S2of the positions of the sensors of the second body is obtained via MDS as per equation (19), which estimate is refined, in step 160, into S2via equation (20). In step 170 a stacked RBL estimate S is constructed via equation (21). Finally, in step 180, the quadratic program represented in equation (22) is solved, for obtaining the translation vector estimate t, which is output in step 190.

[0211] In the following section the performance of the method 100 for the estimation of the translation vector will be evaluated through simulations. As no equivalent conventional method exists for the egoistic set-up considered herein, figure 4 only shows a comparison of the results of the estimation of the translation vector t obtained via the non-egoistic method discussed by S. Chen and K. C. Ho, in " Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks," IEEE Transactions on Signal Processing, vol. 63, no. 24, pp. 6459-6472, 2015, labelled SotA-RBL in the figures, and a variation of the proposed technique captured in method 100, in which an estimate of the rotation matrix Q obtained by the conventional method is used, such that equation (21b) is effectively replaced by equation (10). For the sake of disambiguation, the method will herein be dubbed Ego RBL, while the method modified by an externally fed conformation matrix is referred to as the " Genie-Aided" scheme.

[0212] Internal202500276

[0213] -16-

[0214] To analyse the behaviour and performance of the two methods the root mean squared error, RMSE, is chosen as a metric for comparison. The RMSE for the translation vector is defined as

[0215] (23)

[0216]

[0217] where t is the estimated translation vector and K = 103is the number of Monte-Carlo simulations. For the simulations, the methods were implemented in MATLAB, with the minimization problems solved using the CVX optimization package.

[0218] The considered scenario is represented by two rigid bodies as shown in figure 2, where the parameters, including translation, rotation, as well as the original reference frames of the rigid bodies, are summarized in table I and will be consistently utilized hereafter, unless when otherwise stated.

[0219] TABLE I SIMULATION PARAMETERS

[0220] —1.25 1.25 -1.25 1.25 -1.25 1.25 -1.25 1.25 -1.25 1.25 -1.25 1.25 Ci - -4 -4 - -4 —4 0 0 0 0 4 4 4 4 0.5 0.5 1 1 1 1 4 4 4 4 0.5 0.5 Reference frames

[0221] 1 1 -1 1 -1 1 -1 1 -1 1

[0222] c2= 2 2 1 1 -1 -1 -2 -2 0 0

[0223] 1 1 1.5 1.5 1.5 1.5 1 1 0.5 0.5

[0224] tl = [0,0,0]T

[0225] Translations

[0226] t2= t = [7, 3, 0.5]T

[0227] Rotations [α₁, β₁, γ₁] = [0°, 0°, 0°]

[0228]

[0229] [α₂, β₂, γ₂] = [10°, 20°, 45°]

[0230] The first results, shown in figure 4, provide a comparison of the translation estimates, in a non-egoistic scenario, between the Ego RBL method and the conventional method discussed by S. Chen and K. C. Ho, in " Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks" (full citation further above) respectively in terms of RMSE in meter over the range error a. Note that the range error is not equivalent to the exact error in meters but rather the error used in the noise calculations given in equation (4).

[0231] Internal202500276

[0232] -17- It can be observed in figure 4 that the variation of the Ego RBL method, the Genie-Aided method, outperforms the conventional method for range errors below 20 cm, which is well within the typical accuracy of sensing technology used in the Automotive Industry. In turn, figure 5 demonstrates that the egoistic MDS-based scheme for a fully complete egoistic scenario, i.e., all pair-wise measurements are available, achieves a performance very close to that of the Genie-Aided variation, confirming that the egoistic MDS-based method is capable of handling the practical condition that no prior knowledge on the shape of the target object is available.

[0233] Further simulations are performed to evaluate the performance of method 100 in systems under different levels of completeness of the inter-distances matrix b12. The results, also shown in figure 5, illustrate the impact of incomplete information on the accuracy of the RBL. In particular, it is visible that the performance degrades quickly, leading to rather high error floors, as the available information goes from fully complete to 80%, to 70% complete. Note that the percentage of completion is computed based on the number of zeros and non-zeros in the connectivity matrix W given in equation (8).

[0234] As seen in figure 5, incompleteness in the inter-distances matrix b12significantly harms the estimation performance. To counter this harming effect, matrix completion (MC) methods can be used.

[0235] Well known and high-performing matrix completion algorithms include the simple rank enforcing algorithm, e.g., as discussed by W. Glunt, T. L. Hayden, S. Hong, and J. Wells, in " An Alternating Projection Algorithm for Computing the Nearest Euclidean Distance Matrix," SIAM Journal on Matrix Analysis and Applications, vol. 11, no. 4, pp. 589-600, 1990, the OptSpace algorithm discussed by R. H. Keshavan,

[0236] A. Montanari, and S. Oh, in " Matrix Completion from a Few Entries," IEEE Transactions on Information Theory, vol. 56, no. 6, pp. 2980-2998, 2010, the soft-impute (SI) method discussed by R. Mazumder, T. Hastie, and R. Tibshirani, in " Spectral Regularization Algorithms for Learning Large Incomplete Matrices," Journal of Machine Learning Research, vol. 11, no. 80, pp. 2287-2322, 2010, and the accelerated and inexact soft-impute (AlS-impute) approach discussed by Q. Yao and J. T. Kwok, in " Accelerated and Inexact Soft-impute for Large-Scale Matrix and

[0237] Internal202500276

[0238] -18- Tensor Completion," IEEE Transactions on Knowledge and Data Engineering, vol. 31, no. 9, pp. 1665-1679, 2019. Among these techniques, OptSpace is selected herein due to its trade-off between complexity and performance, but obviously any other suitable method can be used.

[0239] It is noted, however, that performing MC directly over the full EDM matrix D, which is a hollow symmetric matrix, usually yields poor results. Instead, MC can be applied for filling the missing elements of the partially observed inter-distances matrix b12, and then the estimate of b is constructed via Nyström approximation, i.e., via equation (18).

[0240] Figure 6 illustrates the impact of matrix completion onto the egoistic MDS-based translation estimation technique, under the same conditions of figure 5. It can be observed that indeed matrix completion can lower the error floors observed previously, approximately by a factor of 10. Additionally, the performance of the conventional method enhanced via matrix completion is shown for 70% and 80% available information, demonstrating that the conventional approach is not improved by matrix completion.

[0241] As already mentioned above, the completion on the complete distance matrix is the most efficient. However, this does not take into account the egoistic assumptions in the model. Since the conformation matrix C2is unknown, the intra-distances carried in intra-distances matrix D2have to be estimated, e.g., using a Nystrdm approximation, as shown in equation (17). If the number of observable links M is too small, the approximation of the intra-distances matrix D2is poor, which leads to a poor matrix completion result. Alternatively, matrix completion can be performed directly on the inter-distances matrix D12, which can achieve similar results, but requires more iterations.

[0242] To illustrate the performance of the two options, figure 7 shows the normalized mean square error (NMSE) of completing the full EDM D and completing the inter-distances matrix D12, along with the GA variant of the present invention, while figures 8 and 9 show the corresponding convergence behaviour for different numbers of available links M over the number of iterations. Thus, the performances of a GA variation of the

[0243] Internal202500276

[0244] -19-present invention, i.e., knowing D2, an egoistic full EDM b completion, and completing the partial EDM D12are compared. Note that the comparison assumes an error-free scenario.

[0245] It can be observed in figure 7 that for fewer observed links performing the completion on the inter-distances matrix D12, i.e., the cross-body measurements, labelled partial EDM in the figure, performs much better than the completion on the full EDM.

[0246] However, it is noted that the number of iterations for the two alternatives required for reaching convergence differs significantly.

[0247] Figures 8 and 9 show the MC convergence behaviour over the number of iterations for the two alternatives for M = 8 observed links, i.e., 93% completeness, and M = 5 observed links, i.e., 70% completeness, respectively. While the convergence of MC on the full EDM is reached very fast, the completion on the partial EDM requires significantly more iterations. As can be seen in the examples shown in the figures, performing the completion on the full EDM only requires around one hundred iterations to come to a stable result, while the performing completion on the partial EDM needs five thousand iterations.

[0248] As is apparent from the comparison of the simulations shown in figures 8 and 9, with a small number of M = 5 observed links, the MC on the inter-distances matrix D12is needed in order to achieve satisfying results, i.e., low NMSE, since the MC on the full EDM stagnates at rather high NMSE values. On the other hand, with a higher number of M = 8 observed links, even though the MC on the inter-distances matrix D12results in a slightly lower NMSE compared to the MC on the full EDM D, the MC on the inter-distances matrix D12converges very slow and thus, the lower complexity solution will be preferred. Thus, the matrix completion method could be adjusted depending on the number of observed links M.

[0249] In spite of its attractive features, in particular the self-reliant framework in terms of not requiring prior knowledge on the conformation matrix of the remote or target object, and the ability to handle rigid bodies of arbitrary shapes with distinct numbers of landmark points, the technique discussed and evaluated above has one limitation that can be improved upon. Although robustness to incompleteness in the full EDM b

[0250] Internal202500276

[0251] -20-can be partially alleviated by the incorporation of MC, as discussed above, the method itself offers no particular additional means to increase robustness.

[0252] To address this issue a second method for estimating the translation vector t is discussed hereafter, which builds on, and extends the known approach discussed in " Towards multi-rigid body localization" (full citation further above) to, a method that is designed to enable the robust estimation of the translation vector t in an egoistic setup.

[0253] To that end, first consider a translation distance estimator developed based on the conventional method discussed in " Towards multi-rigid body localization" (full citation further above)

[0254] N

[0255] 1 1 X- ’ / 2 2\ f=)y2 11^12 IIF (JIC1.’JI2+llC2,n ||2) n=l 2 + — l\ClQlQ2C2+ Crort2lT+ 14Q2C2)1 / VzN

[0256] + 2^^), (24)

[0257]

[0258] n=l

[0259] where the additive and subtractive terms in lines 2 and 3 of the equation are added in a first step to the known distance estimator for addressing the present egoistic scenario, notably to make the estimator independent of the rigid bodies’ locations S and S2.

[0260] Next, the unknown conformation matrix of the second rigid body in the egoistic scenario has to be considered, using

[0261] N

[0262] — 1 X- ’ / 2 2\ 1 f=(jlS1.’l|l2+ ||S2,n||2J + II fl II 2 + II ^2 II2 + 11^12 IIF n=l 2 + — l\ClQlQ2C2+ Crort2lT+ ltlQ2C2)l. (25)

[0263]

[0264] 7V2

[0265] By applying the same knowledge, relations and assumptions as before, equation (25) can be rewritten to

[0266] Internal202500276

[0267] Ni N21=tTt= (iis«i0+-^Z (ns-i0+n=l n=l 2 + (11^12 II F + INJSI SZJNZ + ^1 ^21jv2)liV2)- (26)

[0268]

[0269] **2

[0270] Since the objective of the method is to estimate the translation vector t instead of the distance only, the problem requires the isolation of the translation vector t in equation (26) to be able to construct a corresponding optimization problem. The translation vector t is contained in the cross terms, which can be isolated by using vectorization, with the corresponding term written as

[0271]

[0272] N1N2 -N1N2^ ^ (27)

[0273] By following the assumptions taken before and the isolation of the translation vector t, the estimator can be reformulated and simplified to

[0274] Ni N2tTt— 1 X- ’ / 2\ 1 X- ’ / 2\ 1 = «; Z n=l - ^ nZ=l (ll^-IL)+ + i|d« 2 2

[0275] (28)

[0276]

[0277] +JvjvZ1"1"2(lw2 0 +N^N2~1"1

[0278] Next, rearranging the equation with its only unknown being the translation vector t, the resulting problem can be written as

[0279] 0 = at + b, (29a) where

[0280] (29b) ~ N±N21wiW2N20 ^r)< N±N2b- (IM© (IMO+N^DI2^F +^1^(C^2)IW2-(29C)

[0281]

[0282] 1n=l2n=l

[0283] Since the unknown variable is not a scalar but a vector, the problem is underdetermined and does not have a closed form solution. Thus, before formulating

[0284] Internal202500276

[0285] -22-an optimization problem, a constraint has to be designed that resolves this issue, additionally adding robustness to the problem. To this end, a constraint is added to the problem that minimizes the distance of an artificial distance matrix found by the optimization itself, compared to the measured one. The artificial distance matrix can be formed via equation (6), written as

[0286] Dh = + Ml - 25^2- (30a)

[0287] r 2 21 T with ipt = ||sl f||, •••, ||sw,i||2, and t contained in

[0288]

[0289] S*2= QC2+ til = S2JN2+ t*ll, (30b)

[0290] which uses the estimated centred S2from the MDS solution and the variable to optimize, i.e., the translation vector t.

[0291] Adding the constraint to the objective, the constraint optimization problem can be written as

[0292] min | at + b |, (31a) s.t.

[0293] |

[0294]

[0295] |(DI2-DI2) O M^ ^ (31*)

[0296] where a and b denote the parts defined in equations (29b) and (29c) respectively, e denotes a term representing noise, and D*12denotes the auxiliary variable representing the reconstructed distance matrix from equation (6), as described in equation (30a).

[0297] As before, the problem can be solved via conventional optimization tools, such as gradient descent or interior point methods, and its robustness compared to the method described further above can be evaluated by comparing the two problems itself.

[0298] In particular, while in the first method discussed herein the impact of missing measurements in equation (22) can only be compensated by the terms depending on

[0299] Internal202500276

[0300] -23-t, in the second problem, zeros in the distance matrix do not have such a big impact on the estimate. This is observable in the constraint shown in equation (31b), where the zeros are common in both required matrices, and in the objective shown in equation (31a) itself, where the Frobenius norm of the distance matrix is scaled, which does not harm the estimate.

[0301] The second method discussed hereinbefore, which for the sake of disambiguation is dubbed herein Robust Ego RBL, is summarized in method 200 described below with reference to figure 10.

[0302] Inputs to the method 200 are the conformation matrix C associated with the ego-rigid body, the distance measurements represented by the inter-distance matrix b12that contains the pair-wise distances between sensors of the ego-rigid body and of the remote rigid body, and the hyper-parameter e. The output of the method 200 is the translation vector estimate t. In step 210 the inter-distance matrix b12containing the respective pairwise inter-object distance measurements is determined using wireless signals or is received from an apparatus configured to perform such measurements, and further the conformation matrix C associated with the egoobject carrying out the method 200 and the hyper-parameter e are received. In step 220 an intra-distances matrix D of the first body is constructed based on the knowledge of the conformation matrix and the measurements comprised in the inter-distance matrix b12, and subsequently, in step 240, a full EDM is calculated via equation (18). Calculating the full EDM may comprise determining the unknown intra-distances matrix D2of the remote body, e.g., through a Nyström approximation, in an optional step 230. In step 250, an initial estimate S2of the positions of the sensors of the second body is obtained via MDS as per equation (19), which estimate is refined, in step 260, into S2via equation (20). In step 270 a stacked RBL estimate S is constructed via equation (21). Finally, in step 280, the constrained optimisation problem represented in equation (31) is solved, for obtaining the translation vector estimate t, which is output in step 290.

[0303] To evaluate the performance of the robust method, the scenario used in the previous performance analysis is revisited, comparing the two methods with a focus on the robustness to incomplete observations. The corresponding results are presented in

[0304] Internal202500276

[0305] -24-figure 11. The first set of results compares the translation estimates between both discussed methods in a non-egoistic genie-aided (GA) scenario, and the egoistic scenario respectively in terms of RMSE. It can be observed that while the robust method performs worse than the MDS-based method presented further above, the performance of the egoistic robust method is nearly identical to the GA variation.

[0306] Further illustrated in figure 11 are simulations performed for different levels of available information, also incorporating the matrix completion aided method 100 presented further above. While for 80% available information the performance of method 200 is close to the fully complete scenario, for 70% available information the method results in an error floor that can be improved by using matrix completion.

[0307] Additionally, as shown by the dashed lines that correspond to the error floors of the MDS-based method 100 at different levels of completeness. For 70% the two methods 100 and 200 perform similar, while for 80% method 200 outperforms the MDS-based method 100 in low range error regimes.

[0308] Finally, an egoistic rotation matrix estimator is discussed that solely relies on distance measurements without requiring knowledge of the conformation matrix C2, and which is entirely complementary to the methods presented hereinbefore in so far as it does not require translation vector estimates.

[0309] In particular, with the initial location estimate matrix S2obtained via equation (20a) in hands, an estimate Q of the rotation matrix Q of the target rigid body can be obtained via a procedure similar to that described further above.

[0310] In principle, Q can be extracted from the relation

[0311] = QAQT= QC2ClQT, (32)

[0312] w

[0313]

[0314] here Q2C2= was used in equation (30b).

[0315] Internal202500276

[0316] -25- Notice, however, that the eigenvalue decomposition in equation (32) is such that the eigenvectors are ordered according to the magnitude of the corresponding eigenvalues, which in turn relate to the largest orthogonal dimensions of the body itself. Consequently, the columns of the estimate obtained from equation (32) may be swapped for rigid bodies without distinctly different length, width and height, which may lead to large estimation errors. In order to circumvent this issue, a different method is used instead, starting from equation (13), but this time accounting for the fact that S and S2can have different numbers N and N2of landmark points, such that

[0317] — 02 1 T

[0318] D12= - JN1D?22JN2= C QC2, (33)

[0319] which, if left-multiplied by the pseudo-inverse of yields

[0320] D

[0321]

[0322] g2± = QC2, (34a) where

[0323] C

[0324]

[0325] \ ± (34b)

[0326] Then, squaring equation (34a) yields

[0327] ^

[0328]

[0329] I°22^I°22T= = QAQT, (35)

[0330] from which the following optimization problem can be constructed

[0331] Q = arg min || / >g2 / >g2T- QAQT||. (36)

[0332]

[0333] It is emphasized that although the solution of the problem represented by equation (36) can be easily obtained via common optimization theory tools, the result can also be severely degraded by the order of the eigenvalues in A. Fortunately, however, in the 3D space there are only 6 distinct permutations of A, such that the solution with the permutation that yields the smallest objective can be estimated as the correct one.

[0334] Internal202500276

[0335] -26- The egoistic rotation matrix estimation method 300 is summarized below with reference to figure 12 for rotation estimation without rigid body conformation knowledge, solely based on the range information between the two rigid bodies, and independent of the translation vector estimate.

[0336] Inputs to the method 300 are the conformation matrix associated with the ego-rigid body and the distance measurements represented by the inter-distance matrix b12that contains the pair-wise distances between sensors of the ego-rigid body and of the remote rigid body. The output of the method 300 is the rotation matrix estimate Q. In step 310 the inter-distance matrix b12containing the respective pairwise interobject distance measurements is determined using wireless signals or is received from an apparatus configured to perform such measurements, and further the conformation matrix associated with the ego-object carrying out the method 300 is received. In step 320 an intra-distances matrix D of the first body is constructed based on the knowledge of the conformation matrix and the measurements comprised in the inter-distance matrix b12, and subsequently, in step 340, a full EDM is calculated via equation (18). Calculating the full EDM may comprise determining the unknown intra-distances matrix D2of the remote body, e.g., through a Nyström approximation, in an optional step 330. In step 350, an initial estimate S2of the positions of the sensors of the second body is obtained via MDS as per equation (19), which estimate is refined, in step 360, into S2via equation (20). In step 370 a —02

[0337] double-centred distance matrix D12is constructed via equation (33), which is refined into D®2in step 380 via equation (34). Finally, in step 390, the optimisation problem represented in equation (36) is solved, for obtaining the rotation matrix Q, which is output in step 395.

[0338] In the following section the performance of the methods discussed herein for determining both the translation vector and rotation matrix will be evaluated in terms of their impact on the overall pose estimation of the target RB. In doing so, it is also ruled out to simply compute the RMSE of all the landmark points of the target RB, so as to keep true to the egoistic characteristic, which is not to rely on knowledge of the shape of the target.

[0339] Internal202500276

[0340] -27- Thus, to capture the errors due to translation estimation and rotation estimation errors jointly, without the explicit knowledge of the conformation matrix of the target object, the whole rigid body is modelled as a unit vector pointing in a “forward” direction of the object, e.g., a vehicle, instead of using the whole conformation matrix, which might be unknown.

[0341] It can be assumed that all conformation matrices that act as reference frames to the global model are modelled identically, such that each object’s conformation matrix is designed that the object faces in the direction of the x-axis.

[0342] As illustrated in figure 13, remodelling what is shown in figure 1, the vehicle is now modelled as a vector vP, pointing in the direction of the x-axis. As before, the vector is transformed by a translation and rotation to its target destination, modelled by the vector vT. Next, when the estimates of the rotation and translation are found, an estimate of the vector can be found, illustrated by the arrow vT. Finally, to model the error of the pose estimation, the distance between the two arrows can be calculated, shown by the arrow connecting the vectors vTand vT. Note that the error calculation could lead to an error of zero, even though there is an error, however, this is an exception and is therefore neglected.

[0343] To clarify, referring to figure 14, vPand vTrepresent the primary and target rigid bodies, respectively. Given the true translation vector t and rotation matrix Q the location and orientation of the target body relative to the primary is expressed as

[0344] vT= Q • vP\t= Q • vP+ t. (37)

[0345] As illustrated in figure 14, the entity described in equation (37) is the image of the target, constructed in terms of a translation and rotation of the primary body to the location and orientation of the target. The equivalence in equation (37) refers to the fact that the end-point of the image Q • vP\tis at the location indicated by the vector Q • vP+ t. The projection is visualised by the red and yellow parallelograms, while all estimated parameters are denoted by the circumflex symbol on top. The error is expressed as the distance between the estimated and true vector, represented by the green arrow.

[0346] Internal202500276

[0347] -28-

[0348] As the true translation vector t and rotation matrix Q are not available, they are replaced by estimated representations thereof, i.e., t and Q. The estimations may be obtained by any suitable method, e.g., using one of the methods described hereinbefore. Thus, equation (37) is formulated as

[0349] v

[0350]

[0351] T= Q • vP\t= Q -vP+ t, (38)

[0352] such that the egoistic pose estimation error for a given fc-th Monte-Carlo realization can be defined as

[0353] (39)

[0354]

[0355] which yields a corresponding RMSE (over multiple realizations) given by

[0356] i / K \ 2

[0357]

[0358] * \ / c=l / )(40)

[0359] Since no alternative conventional method for rigid body orientation estimation exists for the egoist set-up with unknown C2that is considered herein against which to compare the combined presented herein, combinations of GA approaches of the methods presented herein are compared with the SotA, as well as the novel egoistic methods only. To clarify, the conventional technique discussed by S. Chen and K. C. Ho, in " Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks" (full citation further above) enables the joint estimation of location and orientation, but only under the assumption that full conformation matrix information is available, which is unrealistic and out of scope of the present invention.

[0360] The first set of results is shown in figure 15, illustrating the performance of the full orientation estimation in a GA framework including the estimated translation and rotation in terms of the RMSE of the previously described transformed vector. To that extend, the performance of methods 100 and 200 are compared in combination with the rotation estimator described in " Towards multi-rigid body localization" (full citation further above) with the conventional method discussed by S. Chen and K. C. Ho, in

[0361] Internal202500276

[0362] -29- " Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks" (full citation further above), where the translation and rotation are jointly estimated (labelled SotA-RBL in the figure). It can be observed that the results are similar to the pure translation vector estimation results, with method 100 (labelled Alg1 +

[0029] in the figure) performing best for the fully complete scenario and method 200 (labelled Alg2+

[0029] in the figure) achieving the best results for 80% available information.

[0363] Next, figure 16 shows the result for the egoistic framework, where method 100 (labelled Alg1 in the figure) and method 200 (labelled Alg2 in the figure) are combined with the rotation estimation of method 300 (labelled Alg3 in the figure). It is shown that while for the fully complete scenario methods 100 and 200 respectively complemented by method 300 perform well, for the case of 80% available information both combinations result in an error floor, with method 200 in combination with method 300 performing slightly better than method 200 in combination with method 300.

[0364] For the sake of completeness, to further assess the performance of the RBL parameter estimation methods discussed herein, the computational complexity is compared against the conventional method. The computational complexities of the proposed methods have been outlined in Table II in terms of the complexity order on the system size parameters using the well-known Big-0 notation for both the genie-aided (GA), and the egoistic scenario.

[0365] TABLE II COMPUTATIONAL COMPLEXITY

[0366] Method Complexity

[0367] GA Ego

[0368] Prop. MDS-RBL t Ale. 1 ) (. Vi - A> ):1- A'3) + AP A A? ) Prop. CO-RBL (Alg. 2) O((Wi + Aa)3+ K3) O(2(M + AM3+ A3) Prop. Rotation Est. (Alg. 3) (SotA ]2V] 1 O( ( A\ + A2)3+ A3+ 2 / v1) SotA Stationary Parameter Est. 127] O( A'3- Xj. V.j i /

[0369]

[0370] It is important to note that the complexities include all required prior steps of enabling the corresponding scenario, additionally to the solution of the optimization problem. Furthermore, as mentioned above, the RBL SotA discussed by S. Chen and

[0371] K. C. Ho, in " Accurate Localization of a Rigid Body Using Multiple Sensors and

[0372] Internal202500276

[0373] -30- Landmarks" (full citation further above), does not offer an egoistic solution. Finally, if additional matrix completion is applied to the methods, the complexity increases by O((^ +W2)3).

[0374] As is readily apparent, tracking, and predicting, a trajectory of a remote body or object requires capturing multiple prior locations and orientations of the remote body and associated time instants. However, as initially pointed to, simply recording the full EDMs and the rotation matrices requires significant memory resources, and determining a trajectory, and even more so predicting a trajectory, from the EDMs is computationally expensive. There is, thus, a need to provide a simpler representation of the location and pose of the remote body with respect to the ego object, which reduces the memory requirement and which reduces the computational load for tracking and predicting the location and pose of the remote body.

[0375] The present invention builds on the learnings from the performance evaluation discussed in the foregoing section, adopting the idea of modelling each RB as a unit vector with its origin at the location of the origin of the respective conformation matrix, i.e., the centre of the rigid body, and its direction as determined by its rotation matrix. It is reminded that the conformation matrix is defined such that the rigid body’s front is facing in the direction of the x-axis. The unit vector thus indicates a front facing direction of the RB. Modelling the RB as a unit vector avoids having to consider all landmark points and provides a representation of the rigid bodies that is easier to handle, as has is apparent when taking a look at figure 18. While the vehicles on the left side of the figure carry a large number of landmark points, represented by the antenna symbols, the remodelled RBs are represented by the corresponding unit vector. This representation alone already permits evaluating the RBs respective poses and identifying, at least for some of the RBs, at first glance if there might be a trajectory that crosses the future trajectory of the ego RB. In order to capture the lack of information about the RBs’ sizes a perimeter may be defined around the respective unit vectors. The perimeter may be defined as the maximum expected or permitted dimensions, e.g., according to road regulations, and may be complemented by information from other sensors, like cameras and the like, for adjusting.

[0376] Internal202500276

[0377] -31-In light of the foregoing and in accordance with a first aspect of the invention a method of representing a location and pose of a remote object relative to an egoobject, independent of the shapes and / or sizes of the remote object and the egoobject, is presented. The method comprises representing the ego-object as a unit vector vPpointing in the direction of the x-axis of the ego-object’s conformation matrix, and receiving a translation vector estimate t and a rotation matrix estimate Q representing a distance and orientation, respectively, of the remote object relative to the ego-object. The method further comprises modelling the remote object as unit vector vTpointing in the direction of the x-axis of the corresponding conformation space. An estimated target unit vector vTis then determined as the sum of the translation vector estimate t and the product of the ego-object’s unit vector vPand the rotation matrix estimate Q.

[0378] In accordance with a second aspect of the invention a method of tracking a location and pose of a remote object relative to an ego-object, independent of the shapes and / or sizes of the remote object and the ego-object, is presented. The method comprises receiving estimated target unit vectors vTdetermined using the method in accordance with the first aspect of the invention for two or more consecutive time instants preceding a current time, and recording the received estimated target unit vectors vT. Using the received information a tracked trajectory of the remote object is generated by connecting the received estimated target unit vectors vTwith respective temporally immediately subsequent received estimated target unit vectors vT. As the origins of the vectors are at the origins of the respective conformation matrices, which is also the centre of the RB, it may be preferred to connect the origins of the unit vectors for generating a tracked trajectory.

[0379] In accordance with a third aspect of the invention a method of predicting a location and pose of a remote object relative to an ego-object, independent of the shapes and / or sizes of the remote object and the ego-object, is presented. The method comprises receiving estimated target unit vectors vTdetermined using the method in accordance with the first aspect of the invention for two or more consecutive time instants preceding a current time, and recording the received estimated target unit vectors vT. Using the received information a velocity and / or rate of change in the velocity, a corresponding direction, and a relative pose and / or a rate of change in the

[0380] Internal202500276

[0381] -32-relative pose, of the remote object with respect to the ego-object is determined from two or more vectors recorded at consecutive time instants immediately preceding a current time. The previously determined velocity and / or rate of change in the velocity, the corresponding direction, and the relative pose and / or rate of change in the relative pose are then used for extrapolating a target unit vector representing a location and pose relative to the ego-object for a future time instant, starting from the immediate last prior location and pose.

[0382] In accordance with a fourth aspect of the invention a method of estimating a possible collision of a remote object and an ego-object, independent of the shapes and / or sizes of the remote object and the ego-object, is presented. The method comprises receiving at least one estimated target unit vector vTdetermined using the method in accordance with the first aspect of the invention for a current time instant, or receiving a target unit vector extrapolated in accordance with the third aspect of the invention, and recording the received estimated target unit vector vTor the extrapolated target vector. Using the recorded information a future trajectory of the remote object associated with the respective target unit vector vTis projected. A future trajectory of the ego-object is projected based on velocity, location and pose information available at the ego-object. If the projected future trajectories intersect, a signal indicating a possible collision is generated.

[0383] Projecting the future trajectories may comprise forward-extending the respective unit vectors by a predetermined factor. The predetermined factor may be selected in accordance with a velocity of the respective object. If information about a rate of change in the velocity and pose of an object is available, this information may also be considered in the projection, yielding more accurate projections that may also include curved predicted trajectories.

[0384] In embodiments of the invention a perimeter is assumed for the ego-object and the remote object, which is arranged at a predicted location and in accordance with a predicted pose. The perimeter may be a default-sized perimeter of rectangular or ellipsoid shape, or may be determined based on input from other sensors, e.g., cameras or the like, or by information directly received from a remote object. The

[0385] Internal202500276

[0386] -33-perimeter may be used, inter alia, for further refining the collision estimation accuracy.

[0387] In embodiments of aspects of the invention the respective methods further comprise mapping a position and / or pose of the ego-object and the tracked and / or predicted location and / or pose with geo information. If, for example, the remote object is a vehicle, and the geo-information is a street map, the mapped information may be used for detecting or predicting a lane change of the remote object based on a tracked and / or predicted pose. A lane change may be detected or predicted by a typical pose change pattern associated with lane changes. Alternatively, the mapped information may be used for detecting or predicting a turn, i.e., a direction change, of the remote object at an intersection, an off ramp or a property entrance based on a tracked and / or predicted pose and a change in the remote object’s velocity, likewise by comparing the tracked and / or predicted information with corresponding typical pose and velocity change pattern.

[0388] In accordance with a further aspect of the invention an apparatus is presented, which comprises a communication interface configured for receiving translation vector and pose information of an ego-object and / or a remote object. The apparatus further comprises one or more microprocessors, and associated volatile and non-volatile memory. The communication interface and the memory are communicatively coupled to the one or more microprocessors via one or more signal and / or data lines and / or buses. The non-volatile memory stores computer program instructions which, when executed by the one or more microprocessors, configure the apparatus to execute embodiments of the methods of the invention described hereinbefore, and to accordingly control hardware components of the apparatus.

[0389] The methods described hereinbefore may be represented by computer program instructions. Accordingly, a computer program product comprises computer program instructions which, when executed by a microprocessor of a transmitter, cause the microprocessor to execute the methods in accordance with the invention as presented above and to accordingly control hardware components of the apparatus in accordance with the previous aspect of the invention.

[0390] Internal202500276

[0391] -34- The computer program instructions may be retrievably stored or transmitted on a computer-readable medium or data carrier. The medium or the data carrier may by physically embodied, e.g., in the form of a hard disk, solid state disk, flash memory device or the like. However, the medium or the data carrier may also comprise a modulated electro-magnetic, electrical, or optical signal that is received by the computer by means of a corresponding receiver, and that is transferred to and stored in a memory of the computer.

[0392] While in the conventional methods anchors are required for obtaining the full set of location, size, orientation - or rotation -, translation, and tracking, of a rigid body, the method in accordance with the present invention dispenses with the need for such anchors. Further, the method in accordance with the present invention does not mandate identical numbers of sensors for relative localisation, and provides the desired information solely based on cross-body or cross-object sensor-to-sensor range measurements.

[0393] The anchorless RBL methods proposed herein permit egoistically detecting the relative translation, i.e., effective distance, and / or orientation, i.e., relative rotation, of a target object, e.g., a vehicle, based only on a set of measurements of the distances between sensors of an ego-object to the target object and without knowledge of the shape of the latter. A key point of the proposed methods is that the rotation and translation estimation can be performed independent of each other. While the first proposed translation estimation method performs well in a fully connected scenario where all distance measurements are available, the second estimation method offers a solution more robust to incomplete observation up to a certain degree, which can be further improved by performing matrix completion to the available EDMs.

[0394] Simulation results illustrate the good performance of the proposed techniques in terms of RMSE as a function of the ranging error, in the desired, and typical, moderate to low ranging errors regime, for multiple scenarios, while the computational complexities of the proposed methods are also provided.

[0395] BRIEF DESCRIPTION OF THE DRAWING

[0396] Fig. 1 shows an illustration of a rigid body at two distinct locations and S

[0397] Internal202500276

[0398] -35- Fig. 2 exemplarily shows localising two different rigid bodies at two distinct locations S1and S2,

[0399] Fig. 3 shows an exemplary schematic flow diagram of a method of MDS-based egoistic translation estimation,

[0400] Fig. 4 shows a comparison of the RMSE of the translation estimate of the Genie- Aided variant of the method and the SotA, over the range error a,

[0401] Fig. 5 shows a comparison of the RMSE of the translation estimate of the method for different levels of available information compared to the Genie-Aided variant, over the range error a,

[0402] Fig. 6 shows a comparison of the RMSE of the translation estimate of the method aided by matrix completion for different levels of available information, over the range error a,

[0403] Fig. 7 illustrates the performance of alternative MC options over the number of observed links,

[0404] Fig. 8 shows the convergence behaviour of the MC options over the number of iterations for M = 8 observed links,

[0405] Fig. 9 depicts the convergence behaviour of the MC options over the number of iterations for M = 5 observed links,

[0406] Fig. 10 shows an exemplary schematic flow diagram of a further method of MDS- based egoistic translation estimation,

[0407] Fig. 11 shows simulations performed for different levels of available information for the various methods presented herein,

[0408] Fig. 12 shows an exemplary schematic flow diagram of a method of egoistic rotation estimation,

[0409] Fig. 13 shows a remodelling of the scenario shown in figure 1,

[0410] Fig. 14 shows a refined version of the remodelled scenario of figure 13,

[0411] Fig. 15 illustrates the performance of the full orientation estimation of the GA variants of the methods presented herein,

[0412] Fig. 16 illustrates the performance of the full orientation estimation of the egoistic RBL methods presented herein,

[0413] Fig. 17 shows an exemplary block diagram of an apparatus in accordance with embodiments of the second aspect of the present invention, and

[0414] Fig. 18 shows an exemplary illustration highlighting the simplification provided by embodiments of the proposed methods.

[0415] Internal202500276

[0416] -36-

[0417] In the figures identical and similar elements may be referenced using the same reference designators.

[0418] DETAILED DESCRIPTION OF EMBODIMENTS

[0419] Figures 1 through 16 and 18 have been described further above and will not be discussed again.

[0420] Figure 17 shows an exemplary block diagram of an apparatus 500 in accordance with embodiments of the second aspect of the present invention. The apparatus 500 comprises a communication interface configured for receiving translation vector and pose information of an ego-object and / or a remote object. The apparatus 500 further comprises one or more microprocessors 550, volatile memory 552, and non-volatile memory 554. The aforementioned elements are communicatively connected via one or more signal or data connections or buses 558. The non-volatile memory 554 stores computer program instructions which, when executed by the microprocessor 550, cause the apparatus 500 to execute the method according to the aspects of the present invention as presented herein, and to accordingly control hardware components of the apparatus 500.

[0421] Internal

Claims

1. 202500276-37- CLAIMS1. Method of representing a location and pose of a remote object relative to an ego-object, independent of the shapes and / or sizes of the remote object and the ego-object, the method comprising:- representing the ego-object as a unit vector (vP) pointing in the direction of the x-axis of the ego-object’s conformation matrix,- receiving a translation vector estimate (t) and a rotation matrix estimate (Q) representing a distance and orientation, respectively, of the remote object relative to the ego-object,- modelling the remote object as unit vector (vT) pointing in the direction of the x-axis of the corresponding conformation space, wherein an estimated target unit vector (vT) is determined as the sum of the translation vector estimate (t) and the product of the ego-object’s unit vector (vP) and the rotation matrix estimate (Q), and- outputting the remote object’s unit vector (vT).

2. Method of tracking a location and pose of a remote object relative to an egoobject, independent of the shapes and / or sizes of the remote object and the ego-object, the method comprising:- receiving, estimated target unit vectors (vT) determined in accordance with the method of claim 1 for two or more consecutive time instants preceding a current time, and recording the received estimated target unit vectors (vT), - generating a tracked trajectory of the remote object by connecting received estimated target unit vectors (vT) with respective temporally immediately subsequent received estimated target unit vectors (vT), and- outputting the tracked trajectory.

3. Method of predicting a location and pose of a remote object relative to an egoobject, independent of the shapes and / or sizes of the remote object and the ego-object, the method comprising:- receiving, estimated target unit vectors (vT) determined in accordance with the method of claim 1 for two or more consecutive time instants preceding a current time, and recording the received estimated target unit vectors (vT),Internal202500276-38- - determining a velocity and / or rate of change in the velocity, a corresponding direction, and a relative pose and / or a rate of change in the relative pose, of the remote object with respect to the ego-object, from two or more vectors recorded at consecutive time instants immediately preceding a current time, - extrapolating a target unit vector representing a location and pose relative to the ego-object from the immediate last prior location and pose and the determined velocity and / or rate of change in the velocity, the corresponding direction, and the relative pose and / or rate of change in the relative pose, for a future time instant, and- outputting the extrapolated target unit vector.

4. Method of estimating a possible collision of a remote object and an ego-object, independent of the shapes and / or sizes of the remote object and the egoobject, the method comprising:- receiving, at least one estimated target unit vector (vT) determined in accordance with the method of claim 1 for a time instant preceding a current time, or receiving a target unit vector extrapolated in accordance with the method of claim 3, and recording the received estimated target unit vector (vT) or the extrapolated target unit vector, respectively,- projecting a future trajectory of the remote object associated with the respective target unit vector (vT) based on the previously recorded information,- projecting a future trajectory of the ego-object based on velocity, location and pose information available at the ego-object, and- if the projected future trajectories intersect, generating a signal indicating a possible collision.

5. The method of any one of claims 2 to 4, further comprising:- mapping a position and / or pose of the ego-object and the tracked and / or predicted location and / or pose with geo information, and- detecting or predicting a lane change of the remote object based on a tracked and / or predicted pose,or- detecting or predicting a turn of the remote object at an intersection, an offInternal202500276-39- ramp or a property entrance based on a tracked and / or predicted pose and a change in the remote object’s velocity.

6. Apparatus (500) comprising a communication interface (556) configured for receiving translation vector and pose information of an ego-object and / or a remote object, further comprising one or more microprocessors (550), and associated volatile (552) and non-volatile memory (554), the communication interface (556) and the memory (552, 554) being communicatively coupled to the one or more microprocessors (550) via one or more signal and / or data lines and / or buses (558), wherein the non-volatile memory (554) stores computer program instructions which, when executed by the one or more microprocessors (550), configure the apparatus (500) to execute the method of one or more of claims 1 to 5.

7. Computer program product comprising computer program instructions which, when executed by a microprocessor (552) of an apparatus (500) according to claim 6, cause the microprocessor (552) to execute the method and to accordingly control hardware components of the apparatus (500) in accordance with the method of one or more of claims 1 to 5.

8. Computer readable medium or data carrier retrievably transmitting or storing the computer program product of claim 7.

9. A vehicle comprising an apparatus (500) as claimed in claim 6.Internal

Images