increase in the total number of queries.

Regarding the effect of the bias on the time required by MQuAcq, in problems
with fewer variables, where the number of constraints in the bias is low even
when the whole language is considered, the increase in the time needed to
converge is very small (around 25s in Exam Timetabling and 20s in Golomb).
However, in Sudoku and Latin, where the larger number of variables means that
the size of the bias increases considerably when taking into account more
relations in the language used, the increase in the time needed is sharper.
But still, this increase is manageable. Overall, the results show that
learning problems with expressive biases scales well, even when using a large
language to construct the bias, especially regarding the number of generated
queries.

###  9.4 Scalability Analysis

Finally, we ran experiments to investigate the scalability of MQuAcq as the
problem size increases. Towards this we used the following benchmarks, with
instances of different sizes:

Latin Square. We used instances with the number of rows/columns $n=6,...,12$.
The language used is the same as above, i.e. $\Gamma=\\{=,\neq,>,<\\}$. Thus,
the number of variables varied from 36 to 144 and the size of the target
network from 180 to 1584 constraints.

Exam Timetabling. We used instances with the number of variables (number of
courses) varying from 24 to 54. The size of the target network varied from 276
to 1431 constraints. The language used is the same as the one described above.

Radio Link Frequency Assignment Problem. We used simplified versions of the
problem, with 40, 45, 50, 55 and 60 variables. The size of the target network
varied from 52 to 170 binary distance constraints. The language used was the
same as above ($\\{|x_{i}-x_{j}|>y,|x_{i}-x_{j}|=y\\}$, with 5 different
possible values for $y$).

We ran MQuAcq with FindScope-2, using maxB as the optimization heuristic. We
used bdeg for variable ordering with random value ordering. We evaluated our
algorithm in terms of the number of queries posted to the user and its time
performance. The results are shown in Figure 13 for Latin, Figure 14 for RLFAP
and Figure 15 for the Exam Timetabling problem.

(a)

(b)

Figure 13: Performance of MQuAcq in Latin instances of different size

(a)

(b)

Figure 14: Performance of MQuAcq in RLFAP instances of different size

(a)

(b)

Figure 15: Performance of MQuAcq in Exam Timetabling instances of different
size

As we can observe, the increase in the number of queries is proportional to
the increase in the number of variables for all benchmarks. This confirms our
theoretical analysis. Focusing on the time performance, we can see that the
time needed for convergence in Latin problems rises sharply as the number of
variables grows beyond 100. This can be explained by the substantial increase
in the number of constraints of the target network in these instances. On the
other hand, in RLFAP, the increase in time is not very significant because the
number of constraints remains relatively small. In the Exam Timetabling
problem we see that the time needed is analogous to the number of queries, and
grows proportionally to the number of variables present in the problem.

Hence, our proposed algorithm scales up quite well in terms of the number of
queries required, while the time performance, being highly dependant on the
size of the target network, can rise sharply, and even become unmanageable,
for target networks with large numbers of constraints. We believe that methods
that try to exploit the structure of the problem being learned may help
alleviate this problem, and we intend to work on this in the immediate future.

##  10 Discussion

We now discuss certain aspects of MQuAcq in relevance to its performance,
theoretical guarantees, and applicability. First, we discuss the importance of
partial queries, which is a strong point of the algorithm. Then we elaborate
on a negative result by proving that MQuAcq cannot learn constraint networks
with an optimal number of queries even for very simple languages. Finally, we
discuss a weakness of all the proposed constraint acquisition algorithms which
paves the way for future work.

###  10.1 On partial queries

Given the importance of partial queries in MQuAcq (and QuAcq), a question that
arises is whether such queries are easier or harder for the user to classify
than complete ones.

First of all, a partial example does not have to be part of a complete
solution to be classified as positive. The user only needs to decide if the
example at hand violates any requirement (constraint). Hence, it can be easier
for the user to classify small examples with only a few variables instead of
full assignments, simply because inspecting if a full assignment satisfies all
the requirements can be very tedious. This is especially true when the problem
is large, consisting of many variables. Hence, partial queries can make the
acquisition process easier for the user. In addition, the smaller negative
examples posted to the user require fewer queries from FindScope-2 to locate
the scope of the violated constraint.

Another important factor that supports the argument that partial queries are
easier to classify is that many partial queries are subsets of the same
complete negative query. Thus, they may be easier for the user to classify
simply because the user has already seen the full query and has determined
that it is not a solution. As a result, generating a new example is not always
the best choice if we have not acquired the desired information from the
previous generated one.

Considering the above, the way MQuAcq operates, posting more partial sub-
queries than generating new ones, is favorable for the user, not only because
of the reduced waiting time but also because it makes it easier to answer the
queries.

A drawback of MQuAcq is that searching for all the violated constraints of
each generated example can lead to posting a lot of relatively small positive
queries that violate only a few constraints from the bias. However, it is
desirable to prune the bias from the constraints that are not included in
$C_{T}$ with as few queries as possible. Thus, ideally, we want each positive
query to violate a maximum number of constraints from $B$ to prune it with
just a few queries. This drawback of MQuAcq is the main reason for the
slightly increased number of queries it posts compared to QuAcq when both of
them learn the complete target network including the redundant constraints
with the maxB heuristic (Table 5).

This problem could be avoided if the algorithm focused on some of the violated
constraints by the generated example, instead of trying to acquire all of
them. Non-random problems usually display some structure/pattern in their
constraint network. However, this is not taken into account by the existing
constraint acquisition algorithms. As future work, as we mentioned above, we
plan to adjust the acquisition process to take into account the structure that
is revealed as constraints are learned and hence target specific constraints.

###  10.2 On the (non-)optimality of MQuAcq

An interesting question about concept learning algorithms is whether they can
learn certain types of concepts with an optimal number of queries. In [12] it
was proved that QuAcq is guaranteed to converge after $O(|X|\cdot log(|X|))$
queries in the languages $\\{=,\neq\\}$ and $\\{>\\}$ on the Boolean domain.
However, this is not the case for MQuAcq. We now show that MQuAcq is not
optimal even for very simple languages like the one that includes a basic
binary relation.

######  Proposition 5.

MQuAcq, using max for the generation of the queries, does not learn Boolean
networks on the language $\\{=\\}$ with an optimal number of queries.

###### Proof.

In [12], it is proved that the minimum number of queries required to learn a
constraint network in this language is in $\Omega(|X|\cdot log|X|)$. In such a
network, the maximum number of constraints is equal to the number of
$2$-combinations of the $|X|$ variables which is $\frac{|X|\cdot(|X|-1)}{2}$.
For each constraint a number of queries up to $2\cdot|S|\cdot log|X|$ may be
needed by FindScope-2 (Proposition 4). Assume that MQuAcq generates an example
that violates all the constraints. FindAllCons will find all the minimal
scopes and learn all the constraints. Thus, in this case it will learn both
the redundant and the non-redundant constraints, i.e. it will learn
$\frac{|X|\cdot(|X|-1)}{2}$ constraints. As a result, in the worst case
MQuAcq’s number of queries to learn the constraint network is in
$\Omega(|X|^{2}\cdot log|X|)$, which is not optimal.

∎

The following example illustrates this behaviour of MQuAcq, contrasting it to
QuAcq.

######  Example 6.

Consider a problem consisting of $4$ variables with domains $\\{1,2\\}$. Also,
assume that the target network consists of a single clique of $=$ constraints,
i.e. $C_{T}=\\{=_{12},=_{13},=_{14},=_{23},=_{24},=_{34}\\}$. Note that there
exist equivalent networks to $C_{T}$ with fewer constraints. For instance, the
first three constraints are enough to form an equivalent network, and in this
case, the other three are implied (i.e. they are redundant).

As MQuAcq will learn the entire target constraint network, including the
redundant constraints, it will need $|C_{T}|*log(|X|)=6*2=12$ queries to find
the scopes of the constraints. No query is needed to be made by FindC, as we
have $|\Gamma|=1$. On the other hand, QuAcq will not learn the redundant
constraints. The number of non-redundant constraints in the above constraint
network is equal to $|X|-1=3$. Thus, QuAcq will need $3*2=6$ queries to learn
the constraint network and converge.

We now prove that if $max_{B}$ is used by either QuAcq or MQuAcq, allowing
them to generate partial queries (e.g. at line 5 of MQuAcq or line 4 of
QuAcq), then these algorithms are not optimal in terms of the number of
queries posted to the user even on the very simple language $\\{=\\}$, on
which QuAcq (with max) is optimal.

######  Proposition 6.

Constraint acquisition algorithms QuAcq and MQuAcq do not learn Boolean
networks on the language $\\{=\\}$ with an optimal number of queries, if
partial queries can be generated.

###### Proof.

The minimum number of queries required to learn a constraint network in this
language is in $\Omega(|X|\cdot log|X|)$ [12]. The maximum number of
constraints is equal to the number of $2$-combinations of the $|X|$ variables
which is $\frac{|X|\cdot(|X|-1)}{2}$. For each constraint a number of queries
up to $2\cdot|S|\cdot log|X|$ is needed by FindScope-2 (Proposition 4). In
case that partial queries can be generated, redundant constraints can be
learned too, so in the worst case the number of queries to learn the
constraint network is in $\Omega(|X|^{2}\cdot log|X|)$, which is not optimal.

∎

###  10.3 On errors and omissions

One significant issue that has not been addressed in the context of constraint
acquisition is the possibility of omissions and/or errors in the answers of
the user to the posted queries. All the constraint acquisition algorithms that
have been proposed are guaranteed to operate only under the assumption that
the queries are answered correctly.

In the context of concept learning the existence of omissions or errors has
been studied for some classes of concepts. Angluin et. al presented an
algorithm that can learn the target concept function by using equivalence and
incomplete membership queries [4] . In this model, the answers to some of the
learner’s membership queries may be unavailable. Extending this, in the exact
learning model defined in [6] the learning system can learn exactly a target
concept using equivalence and membership queries with at most some number $l$
of errors or omissions in the answers of the user to the membership queries
posted.

In this model the limited membership queries and the malicious membership
queries are introduced. A limited membership query may be answered either by
classifying the example (correctly), or with a special answer i.e. “I don’t
know”, while in a malicious membership query the classification by the user
may be wrong. Although equivalence queries are more difficult to be answered
than membership queries and more expensive [21], in the above model the
assumption is that the answers to equivalence queries remain correct, meaning
that any counterexample returned is indeed a counterexample to the hypothesis
of the learning algorithm.

Extending the above models to more classes, [16] showed that for concepts that
are closed under projection both models are equivalent to the exact learning
model without omission and errors. In addition, the presented system can also
handle errors in the equivalence queries, i.e. the malicious equivalence query
(MEQ) is introduced, in which the user can return a wrong counterexample (an
assignment that is not a counterexample in the hypothesis) for at most $l$
different assignments.

One relevant question regarding the answers to the queries is whether the
omissions or errors are persistent or not. They are persistent if the same
query to the same examples always returns the same answer (even if the answer
is an omission or if it is wrong). In the above models the assumption is that
the answers are persistent. A model of non-persistent errors is defined by
Sakakibara [36], in which the answer to each query may be wrong with some
given probability. In this model, repeated membership queries for the same
example are considered as independent events, so the answer may be different.
In this model, a general technique of repeating each query sufficiently often
to establish the correct answer with high probability is introduced.

Although in the constraint acquisition context we only have membership and
partial queries (as explained, equivalence queries are considered too hard to
be answered by the user), the presence of omissions or errors has not been
studied yet. We plan to deal with this significant issue in the future. Of
course in this context limited and malicious partial queries have to be
examined too.

##  11 Conclusion

Constraint acquisition has started to receive increasing attention as a useful
tool for automated problem modeling in CP. As a result, a number of both
passive and active acquisition algorithms have been proposed, with QuAcq and
MultiAcq being prime examples of active algorithms. However, two bottlenecks
of such algorithms are the large number of queries required to converge to the
target network, and the high cpu times needed to generate queries, especially
near convergence. An additional side effect of the latter is the often
occurrence of premature convergence in constraint acquisition systems.

We have presented new methods that can boost the performance of active
constraint acquisition systems. We proposed the MQuAcq algorithm which extends
QuAcq to discover all the violated constraints from a negative example, just
like MultiAcq does, but with a better complexity bound in terms of the number
of queries. We also proposed an optimization on the process of locating scopes
that, as experiments demonstrate, helps reduce the number of queries by up to
$85\%$ in some cases.

Another contribution of our work is that we focus on query generation which is
a very important but rather overlooked part of the acquisition process. We
described the algorithmic query generation process of standard interactive
acquisition systems in detail, and we proposed several heuristics that can be
applied during query generation to boost the performance of constraint
acquisition algorithms.

Experimental results demonstrate that an algorithm which integrates all our
methods significantly outperforms the state-of-the-art active constraint
acquisition algorithms on all the important metrics. It does not only generate
considerably fewer queries than QuAcq and MultiAcq, but it is also by far
faster than both of them, both in average query generation time and in total
run time. Last but not least, our proposed heuristics for the query generation
process support the generation of more “informative” queries and also largely
alleviate the premature convergence problem.

As future work, it would be very interesting if a hybrid system that
integrades a passive learning method, specifically ModelSeeker, and an active
one, such as MQuAcq, was designed and built. We believe that the two
approaches are orthogonal, and combining their strengths may prove very
beneficial in practice. ModelSeeker can learn constraints in highly structured
problems using only very few examples, but this is not the case in problems
with irregular structure. On the other hand, active methods, such as our own,
require to generate a much larger number of examples to learn problems like
Sudoku, but can handle irregularly structured problems. So ideally, in the
future we would like to have a hybrid system that takes as input a (small) set
of examples, runs ModelSeeker to learn the basic constraints, and then
completes the model using an active technique.

## References

  * Addi and Ezzahir [2019] Addi HA, Ezzahir R (2019) $$p_a$$-quacq: Algorithm for constraint acquisition system. In: Smart Data and Computational Intelligence, Springer International Publishing, pp 249–256
  * Addi et al. [2018] Addi HA, Bessiere C, Ezzahir R, Lazaar N (2018) Time-bounded query generator for constraint acquisition. In: International Conference on the Integration of Constraint Programming, Artificial Intelligence, and Operations Research, Springer, pp 1–17
  * Angluin [1988] Angluin D (1988) Queries and concept learning. Machine learning 2(4):319–342
  * Angluin and Slonim [1994] Angluin D, Slonim DK (1994) Randomly fallible teachers: Learning monotone dnf with an incomplete membership oracle. Machine Learning 14(1):7–26
  * Angluin et al. [1992] Angluin D, Frazier M, Pitt L (1992) Learning conjunctions of horn clauses. Machine Learning 9(2-3):147–164
  * Angluin et al. [1997] Angluin D, Kriķis M, Sloan RH, Turán G (1997) Malicious omissions and errors in answers to membership queries. Machine Learning 28(2-3):211–255
  * Arcangioli et al. [2016] Arcangioli R, Bessiere C, Lazaar N (2016) Multiple constraint aquisition. In: IJCAI: International Joint Conference on Artificial Intelligence, pp 698–704
  * Beldiceanu and Simonis [2012] Beldiceanu N, Simonis H (2012) A model seeker: Extracting global constraint models from positive examples. In: Principles and practice of constraint programming, Springer, pp 141–157
  * Bessiere et al. [2004] Bessiere C, Coletta R, Freuder EC, O’Sullivan B (2004) Leveraging the learning power of examples in automated constraint acquisition. In: International Conference on Principles and Practice of Constraint Programming, Springer, pp 123–137
  * Bessiere et al. [2005] Bessiere C, Coletta R, Koriche F, O’Sullivan B (2005) A sat-based version space algorithm for acquiring constraint satisfaction problems. In: European Conference on Machine Learning, Springer, pp 23–34
  * Bessiere et al. [2007] Bessiere C, Coletta R, O’Sullivan B, Paulin M, et al. (2007) Query-driven constraint acquisition. In: IJCAI, vol 7, pp 50–55
  * Bessiere et al. [2013] Bessiere C, Coletta R, Hebrard E, Katsirelos G, Lazaar N, Narodytska N, Quimper CG, Walsh T, et al. (2013) Constraint acquisition via partial queries. In: IJCAI, vol 13, pp 475–481
  * Bessiere et al. [2014] Bessiere C, Coletta R, Daoudi A, Lazaar N, Mechqrane Y, Bouyakhf EH (2014) Boosting constraint acquisition via generalization queries. In: ECAI, pp 99–104
  * Bessiere et al. [2016] Bessiere C, Daoudi A, Hebrard E, Katsirelos G, Lazaar N, Mechqrane Y, Narodytska N, Quimper CG, Walsh T (2016) New approaches to constraint acquisition. In: Data mining and constraint programming, Springer, pp 51–76
  * Bessiere et al. [2017] Bessiere C, Koriche F, Lazaar N, O’Sullivan B (2017) Constraint acquisition. Artificial Intelligence 244:315–342
  * Bisht et al. [2008] Bisht L, Bshouty NH, Khoury L (2008) Learning with errors in answers to membership queries. Journal of Computer and System Sciences 74(1):2–15
  * Blum and Rudich [1995] Blum A, Rudich S (1995) Fast learning of k-term dnf formulas with queries. Journal of Computer and System Sciences 51(3):367–373
  * Boussemart et al. [2004] Boussemart F, Hemery F, Lecoutre C, Sais L (2004) Boosting systematic search by weighting constraints. In: Proceedings of the 16th European Conference on Artificial Intelligence, IOS Press, pp 146–150
  * Bshouty [1995] Bshouty N (1995) Exact learning boolean functions via the monotone theory. Information and Computation 123(1):146 – 153
  * Bshouty [2018] Bshouty NH (2018) Exact learning from an honest teacher that answers membership queries. Theoretical Computer Science 733:4–43
  * Bshouty et al. [1996] Bshouty NH, Goldman SA, Hancock TR, Matar S (1996) Asking questions to minimize errors. Journal of computer and system sciences 52(2):268–286
  * Cabon et al. [1999] Cabon B, De Givry S, Lobjois L, Schiex T, Warners JP (1999) Radio link frequency assignment. Constraints 4(1):79–89
  * Daoudi et al. [2016] Daoudi A, Mechqrane Y, Bessiere C, Lazaar N, Bouyakhf EH (2016) Constraint acquisition using recommendation queries. In: IJCAI: International Joint Conference on Artificial Intelligence, pp 720–726
  * De Raedt et al. [2018] De Raedt L, Passerini A, Teso S (2018) Learning constraints from examples. In: Proceedings in Thirty-Second AAAI Conference on Artificial Intelligence
  * Freuder [1999] Freuder EC (1999) Modeling: the final frontier. In: The First International Conference on The Practical Application of Constraint Technologies and Logic Programming (PACLP), London, pp 15–21
  * Freuder [2018] Freuder EC (2018) Progress towards the holy grail. Constraints 23(2):158–171
  * Freuder and O’Sullivan [2014] Freuder EC, O’Sullivan B (2014) Grand challenges for constraint programming. Constraints 19(2):150–162
  * Freuder and Wallace [1998] Freuder EC, Wallace RJ (1998) Suggestion strategies for constraint-based matchmaker agents. In: International Conference on Principles and Practice of Constraint Programming, Springer, pp 192–204
  * Khanna et al. [2001] Khanna S, Sudan M, Trevisan L, Williamson DP (2001) The approximability of constraint satisfaction problems. SIAM Journal on Computing 30(6):1863–1920
  * Lallouet et al. [2010] Lallouet A, Lopez M, Martin L, Vrain C (2010) On learning constraint problems. In: Tools with Artificial Intelligence (ICTAI), 2010 22nd IEEE International Conference on, IEEE, vol 1, pp 45–52
  * Lombardi and Milano [2018] Lombardi M, Milano M (2018) Boosting combinatorial problem modeling with machine learning. arXiv preprint arXiv:180705517
  * Mason [1997] Mason J (1997) Purdey’s general store. Dell Magazine April
  * O’Sullivan [2010] O’Sullivan B (2010) Automated modelling and solving in constraint programming. In: AAAI, pp 1493–1497
  * O’Connell et al. [2004] O’Connell S, O’Sullivan B, Freuder EC (2004) A study of query generation strategies for interactive constraint acquisition. In: Applications and Science in Soft Computing, Springer, pp 225–232
  * Paulin et al. [2008] Paulin M, Bessiere C, Sallantin J (2008) Automatic design of robot behaviors through constraint network acquisition. In: Tools with Artificial Intelligence, 2008. ICTAI’08. 20th IEEE International Conference on, IEEE, vol 1, pp 275–282
  * Sakakibara [1991] Sakakibara Y (1991) On learning from queries and counterexamples in the presence of noise. Information Processing Letters 37(5):279–284
  * Shchekotykhin and Friedrich [2009] Shchekotykhin K, Friedrich G (2009) Argumentation based constraint acquisition. In: Data Mining, 2009. ICDM’09. Ninth IEEE International Conference on, IEEE, pp 476–482
  * Tsouros et al. [2018] Tsouros DC, Stergiou K, Sarigiannidis PG (2018) Efficient methods for constraint acquisition. In: 24th International Conference on Principles and Practice of Constraint Programming


# A Credible and Robust approach to Ego-Motion Estimation using an Automotive
Radar

Karim Haggag, Sven Lange, Tim Pfeifer and Peter Protzel Manuscript received:
November 26, 2021; Revised February 11, 2022; Accepted March 14, 2022.This
paper was recommended for publication by Editor Sven Behnke upon evaluation of
the Associate Editor and Reviewers’ comments.The authors are with the Faculty
of Electrical Engineering and Information Technology, Technische Universität
Chemnitz, Germany,
(e-mail: {firstname.lastname}@etit.tu-chemnitz.de)Digital Object Identifier
(DOI): 10.1109/LRA.2022.3162644

###### Abstract

Consistent motion estimation is fundamental for all mobile autonomous systems.
While this sounds like an easy task, often, it is not the case because of
changing environmental conditions affecting odometry obtained from vision,
Lidar, or the wheels themselves. Unsusceptible to challenging lighting and
weather conditions, radar sensors are an obvious alternative.

Usually, automotive radars return a sparse point cloud, representing the
surroundings. Utilizing this information to motion estimation is challenging
due to unstable and phantom measurements, which result in a high rate of
outliers.

We introduce a credible and robust probabilistic approach to estimate the ego-
motion based on these challenging radar measurements; intended to be used
within a loosely-coupled sensor fusion framework. Compared to existing
solutions, evaluated on the popular nuScenes dataset and others, we show that
our proposed algorithm is more credible while not depending on explicit
correspondence calculation.

###### Index Terms:

Range Sensing, Probabilistic Inference, Radar Odometry, Gaussian Mixtures

##  I Introduction

In order to develop autonomous vehicles, a robust and fault-aware localization
or SLAM system is essential. Consequently, the combination of various sensors
with their specific strengths is mandatory. Consider the following: a vehicle
drives in an urban environment between tall buildings or inside tunnels on a
rainy, foggy, or snowy day at night. A global navigation satellite system
(GNSS) most likely will have non-line-of-sight and multipath errors. Lidar
suffers from rain and fog, and the vision sensors have poor accuracy because
of the lighting conditions and the presence of snow. However, a radar sensor
would show a robust behavior within these challenging environmental
conditions, and by using a Doppler radar, its detections are augmented by
velocity information based on Doppler shift.

previous point set
(modeled as GMM) current point set
(2 targets) current point set
(3 targets) an outlierlikelihood failsground truth with outlier no outlier
SummingLikelihoodOur approach Figure 1: A simple 1D cost scenario shows the
impact of an outlier on the likelihood and summing approaches compared to our
method. It reveals a robust behavior for the summing approach against outliers
and shows the appearance of multiple local maxima. In contrast, our approach
shows a robust behavior against outliers while remaining almost identical to
the likelihood solution in the non-outlier case.

Recently, radar has been used in different automotive applications, such as
object tracking or ego-motion estimation. While the latter is the focus of our
work, we aim for a solution to be independently used as part of a sensor
fusion or SLAM framework – like a virtual odometry sensor. Specifically, we
use the term ego-motion to describe the relative transformation of the ego-
vehicle between two consecutive measurements or scans. The used pipeline for
estimating the ego-motion typically varies based on the measurement properties
of the sensor – e.g., is the environment sparsely or densely represented. For
sensors with a sparse measurement, point set registration (PSR) is part of the
motion-estimation problem. PSR aims to find the best alignment between two
point sets. Various algorithms emerged over time, but we still see a gap when
it comes to the robustness and credibility of these algorithms. Particularly,
if we think of integrating motion-estimation results into a loosely-coupled
sensor fusion framework, we need a fully probabilistic approach to be
consistent in the end.

We tackle the PSR problem as part of the motion estimation probabilistically
to cope with the challenges regarding outliers in the automotive radar
measurements. By modeling one of the two consecutive point sets as a Gaussian
Mixture Model (GMM), we can omit the often needed preprocessing step for
finding corresponding points. Fig. 1 shows a simple 1D PSR problem, with the
reference point set modeled as GMM and two scenarios for a consecutive
(current) point set, with and without an outlier. While using GMMs is not new
in this context, we optimized the problem in a feasible time without violating
the probability’s assumptions in the objective function – this utilizes our
earlier work [20] and will be explained further in Section III. In
consequence, we get a consistent estimation result, meaning that the
calculated covariance represents the estimation’s true uncertainty – as can be
seen in Section IV. The following Section reviews the existing ego-motion
estimation approaches.

In summary, our main contributions are as follows:  1. we model and solve the
objective function for an automotive ego-motion radar estimation in a thorough
probabilistic way without explicit point-to-point correspondences and  2. we
investigate and compare the consistency of our approach to the often used
summing approximation of this problem.  Furthermore, we provide an exhaustive
credibility evaluation on different scenarios and release our implementation
in the spirit of transparency and benchmarking.

##  II Related Work

Radar sensors got miniaturized over the last years and became a standard
sensor in the automotive domain. These sensors typically provide a limited
number of targets in each measurement, like the ARS 408-21 from Continental
used in the nuScenes datasets [1] or the General Purpose Radar from Bosch used
in our experiments. Instead of these commonly used sensors, the authors in [2]
or [3] used the Navtech CTS350-X, which returns 1D power readings while
rotating $360\text{\,}\mathrm{\SIUnitSymbolDegree}$ resulting in a dense 2D
intensity image. In consequence, they need an additional keypoint extraction
step. Recently, low-cost solutions like the AWR1843 from Texas Instruments
prove to be applicable even to indoor ego-motion estimation [4]. They provide
3D target measurements through a special transmitter antennas setup to
estimate the detection’s elevation. Within this paper, we use the sparse
automotive type and concentrate on its specific properties. However, most of
the addressed challenges like the high outlier rate also apply to the other
sensor variants.

Estimating ego-motion is usually a pipeline starting with raw sensor
information. It aims to compute a robust and credible motion estimation, while
the intermediate steps define the estimation algorithm itself. Depending on
the sensor’s measurement information, the estimation pipeline needs to be
adapted. E.g., in [5], the authors needed a feature detection and feature
matching step for the Navtech radar sensor, as the sensor represents its
surroundings in a dense 2D intensity image. However, as common automotive
radars include such a step already, available approaches usually aim to find
the best alignment between two sparse point sets, without the feature
detection step, like in [6] or [7].

In the following, we try to give a brief overview of used algorithms for the
PSR problem in general and in relation to automotive Doppler radar sensors.
Specifically, we focus on pure motion estimation algorithms feasible to
integrate into a larger sensor fusion framework, which is credible in the end
– often, this is referred to as _loosely-coupled_ integration. Consequently,
we need to stress the algorithms’ covariance estimation capabilities.

_Iterative Closest Point_ (ICP) is a well-known approach to address the PSR
problem. The authors of [8] introduced the standard ICP approach using an
explicit correspondence technique to build the error metric but ignoring
possible uncertainty of target measurements within the metric. Likewise, they
did not discuss how to calculate the estimation’s uncertainty. This is done
later by [9], where the author follows the error propagation model to
additionally estimate the ICP’s uncertainty. Using the error propagation model
is motivated by the missing target noise model within the ICP’s error metric.
In addition to that, the authors of [10] provided an approach to also
integrate the targets’ standard deviation into the error metric. This is done
in two steps: constructing correspondence pairs using the Euclidean distance
metric and following a probabilistic framework to build the cost function for
the point pairs.

_Normal Distribution Transformation_ (NDT) is a probabilistic, cell based,
condensed representation of measurements, introduced in [11] for a 2D laser
range finder. Additionally, the authors proposed a scan matching algorithm,
applying a point-to-distribution error metric. In [12] the authors extended
this approach for three dimensions and achieved a 3D-NDT registration while
implementing a distribution-to-distribution error metric. Both works miss a
thorough discussion about the estimation’s uncertainty, though. Later in [7],
an uncertainty estimation was added by applying the error propagation
described in [9]. In addition to that, [6] introduced a method to weight the
NDT cells based on the sensor’s noise.

A full _probabilistic approach_ to the PSR problem was introduced in [6]. The
authors used a Gaussian Mixture Model (GMM) to represent not only the model
point set but also the current point set. In consequence, the registration
problem lies in aligning two GMMs. To solve this, the authors introduced the
$\mathbf{L}_{2}$ distance as a statistical metric for minimizing the GMMs’
alignment error. Later, the authors in [13] applied this approach for an ego-
motion estimation based on an automotive Doppler radar. Additionally, they
joint the spatial and Doppler information to suppress the impact of moving
targets. In [14], the former approach is combined with the NDT representation,
and a method for calculating the estimator’s uncertainty is integrated. For
this, they follow the error propagation model of [9].

For completeness, we also refer to the idea of registration-free approaches
where the vehicle’s motion is estimated based only on the current radar scan.
In [15], the authors used the Doppler information and the targets’ azimuth
angle to estimate the motion vector (in 2DoF), using a single-track Ackermann
constraint. Then, the authors proposed a second work [16], which optimizes a
full 2D vehicle motion state (3DoF) using at least two Doppler radar sensors.
In [17], the authors extend [16] to achieve a full 3D ego-motion, using two
mmWave radars in an orthogonal configuration. In the same vein, [18] proposed
the RADARODO approach, where it follows the idea of [15] to estimate the
translational motion and the NDT approach for the yaw rate estimation.
Recently, [19] introduced a radar-inertial sensor platform to estimate a 3D
velocity through jointing the linear velocity with yaw rate from an IMU.

#### Discussion

We are focusing on an uncertainty-aware ego-motion estimation algorithm for
sensors returning a very sparse environment representation with high outlier
probability, e.g., an automotive Doppler radar. Having this in mind, ICP may
come up as a possible way of solving at least the PSR problem in this task,
but mainly because of the high amount of outliers, it is not feasible. With
the work using the NDT representation, we mentioned another possible way of
solving the PSR problem. Even though it is robust against outliers, it is more
suitable for sensors returning a dense environment representation. Building
the NDT with a very limited number of points will be difficult and
insufficient. Furthermore, most of the approaches use a summing-based
likelihood approximation to be robust against outliers. This may not be
obvious at first glance, but even if the correct measurement noise is used,
the likelihood approximation corrupts the estimator’s internal uncertainty
representation. Thus, most of these approaches need to utilize the error
propagation model to return an uncertainty estimation. In contrast, we propose
a fully probabilistic framework based on correct likelihood joints with
feasible computation effort by leveraging our earlier work [20]. Moreover, we
provide a way to make the likelihood robust against the impact of outliers, as
well as get credible results of the estimator’s uncertainty.

##  III Probabilistic Motion Estimation

In the following, we derive our full likelihood implementation and clarify the
differences to the sum approximation method [7]. The estimation process is
additionally visualized in Fig. 1 as a simple 1D scenario to give an intuitive
summary of the algorithms. As part of the motion estimation, PSR aims to find
the best transformation to align two point sets. In the automotive domain, PSR
aligns the current or moving point set $\mathcal{M}$ with the previous or
model point set $\mathcal{F}$, to find the best estimate for the state
$\mathbf{x}=\left[\mathbf{x}^{\text{tran}},\mathbf{x}^{\text{rot}}\right]^{\mathsf{T}}$.
We assume, that each point set consists of $K$ targets, given in polar space
$\check{\mathbf{m}}_{k}=\left[r_{k},\theta_{k}\right]^{\mathsf{T}}$, where $r$
is the radius and $\theta$ the azimuth angle. Further, we assume information
about the targets’ standard deviation with $\sigma_{r_{k}}$ and
$\sigma_{\theta_{k}}$ for both dimensions. In Cartesian coordinates, we speak
of $\mathbf{m}_{k}$ for the target’s position within the sensor frame and
${\boldsymbol{\Sigma}}_{k}$ for its propagated covariance. For including the
velocity measurements into the process, we additionally need the targets’
Doppler information with $v_{k}$ and $\sigma_{v_{k}}$.

Following [6] or [7], the measurement’s point set could also be represented as
multivariate GMM with $K$ components. If we do this for the model point set
with $K=|\mathcal{F}|$ targets, we get

$P(\mathbf{m}\mid\mathcal{F})=\sum_{j=1}^{|\mathcal{F}|}w\cdot\mathcal{N}\left(\mathbf{m}\,|\,\mathbf{m}_{j},{\boldsymbol{\Sigma}}_{j}\right)\qquad\text{with
}w=\frac{1}{\left|\mathcal{F}\right|}.$ (1)

This represents a measurement state probability of an arbitrary point
$\mathbf{m}$ within the model coordinate frame. The GMM components
$\mathcal{N}\left(\mathbf{m}\,|\,\mathbf{m}_{j},{\boldsymbol{\Sigma}}_{j}\right)$
represent the target measurements. A simple example of two 1D targets,
combined into a GMM, is shown in Fig. 1 titled _previous point set_.

Thinking of the PSR problem, we need an objective function to be optimized. As
the objective consists of aligning the two scans, we need a distance metric,
which accounts for measurement uncertainties in both consecutive point sets.
Meaning a distribution-to-distribution error metric – often only point-to-
distribution is used. A possible distance metric, which has been used and
derived in [6] or [7], is the $\mathbf{L}_{2}$ metric between two
distributions
$\mathcal{N}\left(\mathbf{x}\,|\,{\boldsymbol{\mu}}_{1},{\boldsymbol{\Sigma}}_{1}\right)$
and
$\mathcal{N}\left(\mathbf{x}\,|\,{\boldsymbol{\mu}}_{2},{\boldsymbol{\Sigma}}_{2}\right)$
with

$D_{L_{2}}=\mathcal{N}\left(0\,|\,{\boldsymbol{\mu}}_{1}-{\boldsymbol{\mu}}_{2},{\boldsymbol{\Sigma}}_{1}+{\boldsymbol{\Sigma}}_{2}\right).$
(2)

By using the GMM of the previous scan (missing) 1 and the distance metric
(missing) 2, we can formulate the likelihood for a target $\mathbf{m}_{i}$ out
of $\mathcal{M}$ to be present in the previous measurement $\mathcal{F}$ as

$P\left(\mathbf{m}_{i}\mid\mathcal{F},\mathbf{x}\right)\propto\sum_{j=1}^{|\mathcal{F}|}s_{j}\exp\left(-\frac{1}{2}\left\|\mathbf{r}_{i}-{\boldsymbol{\mu}}_{j}\right\|_{{\boldsymbol{\Sigma}}_{j}+\tilde{{\boldsymbol{\Sigma}}}_{i}}^{2}\right).$
(3)

Now, we have an additional condition in the likelihood, which is the state
$\mathbf{x}$. It is used for the residual function $\mathbf{r}_{i}$ (please
note the difference to the scalar $r$ – the radius in polar space of a target)
representing the transformation of point $\mathbf{m}_{i}$ into the model
frame:

$\displaystyle\mathbf{r}_{i}(\mathbf{x})=\mathbf{R}(\mathbf{x}^{\text{rot}})\mathbf{m}_{i}+\mathbf{x}^{\text{tran}}.$
(4)

Please also note that we use ${\boldsymbol{\mu}}_{j}$ now for a GMM
component’s mean instead of $\mathbf{m}_{j}$ to stress that it is not part of
a residual function but the noise model. Since we transform the measurement of
target $\mathbf{m}_{i}$, we also have to propagate its covariance matrix by
$\tilde{\boldsymbol{\Sigma}}_{i}=\mathbf{R}{\boldsymbol{\Sigma}}_{i}\mathbf{R}^{\mathsf{T}}$
using the current estimate’s rotation matrix
$\mathbf{R}(\mathbf{x}^{\text{rot}})$. Combined with
${\boldsymbol{\Sigma}}_{j}$, the covariance matrix of GMM component $j$,
represents the component’s noise as of (missing) 2. The weight and a
normalizing component is represented by

$s_{j}=w\det\left({\boldsymbol{\Sigma}}_{j}+\tilde{{\boldsymbol{\Sigma}}}_{i}\right)^{-\frac{1}{2}}\qquad\text{with
}w=\frac{1}{\left|\mathcal{F}\right|}.$ (5)

#### Objective Function

Optimizing the vehicle’s motion based on the stated PSR problem, is the
process of maximizing

$P\left(\mathbf{x}\,|\,\mathcal{M},\mathcal{F}\right)\propto
P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right),$ (6)

where $P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right)$ is the joint
model of all targets’ measurement probabilities
$P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)$ – or expressed in
words: how likely is it, that the set $\mathcal{M}$ is been generated out of
$\mathcal{F}$ with transformation $\mathbf{x}$. At this point, we have to
differ between the approximation via summing (missing) 7 e.g. proposed in [7]
and the natural way of using the full likelihood (missing) 8.

$\displaystyle P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right)$
$\displaystyle\sim\sum_{i=1}^{|\mathcal{M}|}P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)$
(7) $\displaystyle P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right)$
$\displaystyle=\prod_{i=1}^{|\mathcal{M}|}P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)$
(8)

Again, via the above measurement likelihood, both approaches aim to find the
best transformation ($\mathbf{x}$), maximizing the likelihood for all current
targets to have the best correspondence with the model GMM distribution.

The summing approach (SA) is commonly used in the literature, as it shows a
robust behavior against the impact of outliers, whereas the _pure_ likelihood
approach most likely will fail because of them, due to the mathematical
multiplication, see Fig. 1. However, because of the approximation via summing,
the SA induces multiple local maxima, which makes it sensitive to a good
initial guess in the optimization step. Moreover, the covariance estimation is
problematic, which will be explained later on. On the other hand, we have the
drawbacks of outlier integration in the likelihood approach, which will be
addressed now.

#### Cost Function

For efficient optimization in the context of Gaussian distributed
measurements, least-squares optimization is the obvious choice. Therefore, the
searched maximum likelihood solution

$\displaystyle\hat{\mathbf{x}}$
$\displaystyle=\operatornamewithlimits{argmax}_{\mathbf{x}}P\left(\mathbf{x}\,|\,\mathcal{M},\mathcal{F}\right),$
(9)

of the objective function is transformed into a minimization problem

$\displaystyle\hat{\mathbf{x}}$
$\displaystyle=\operatornamewithlimits{argmin}_{\mathbf{x}}-\log
P\left(\mathbf{x}\,|\,\mathcal{M},\mathcal{F}\right),$ (10)

using the negative log-likelihood – now we speak of the cost function. In
combination with only normal distributed constraints, the cost function
simplifies into the common least squares formulation. However, we have a GMM
as the noise model for the model point set. Therefore, we follow the robust
least squares approach in [20], converting the GMM (missing) 3 into a loss
function ${\boldsymbol{\rho}}$ resulting in the following least squares
problem:

$\hat{\mathbf{x}}=\operatornamewithlimits{argmin}_{\mathbf{x}}\sum^{\left|\mathcal{M}\right|}_{i=1}\frac{1}{2}\left\|{\boldsymbol{\rho}}\left(\mathbf{r}_{i}\left(\mathbf{x}\right)\right)\right\|^{2}.$
(11)

For the GMM’s loss function implementation, we use the proposed Max-Sum-
Mixture (MSM). This way, we get an optimization problem to be solved in
feasible processing time – please see [20] for a more complete elaboration.

#### State definition

Till now, we did not restrict the estimated transformation $\mathbf{x}$ in its
DoF. While in general, the presented PSR methods would be applicable to full
3D motion estimation, we can restrict it in our automotive examples to 2D.
Further, we assume a nonholonomic car-like vehicle, reducing our estimated
state to 2DoF with $\mathbf{x}$ having only a longitudinal translation and an
angle of rotation. If a non-negligible sideslip is expected, the method can
easily be extended to 3DoF.

#### Outlier Model

In contrast to the SA, which has a natural robustness against outliers, the
likelihood approach needs to model them explicitly – as can be seen in our
simple example in Fig. 1. Robust cost functions or M-estimators like presented
in [21, Appendix. 6] are one possibility. As we already use a loss function
implementing a GMM, the natural way is to model the outlier component directly
into it. Therefore, we can approximate the uniform distribution over the
measurement space of the radar sensor with additional GMM components and merge
the inlier and outlier distribution for the previous scan $\mathcal{F}$.

For the outlier distribution, we approximate a model based on the radar’s
measurement space as Gaussians in the form of a conic ray, like it is
described in [22, Ch. 6.4.1]. There, two parameters $\alpha$ and $\beta$
regulate the number and width of the components generated and two additional
parameters $s_{min}$ and $s_{max}$ to limit the range accordingly. Following
the equations, we get range parameters for a number of $N_{g}$ components.
Transforming it into polar space, we need the additional parameter
$\sigma_{\theta}$ for the angle uncertainty, which is specified according to
the sensor’s FoV. Then, we can transform the components into Cartesian space
getting $x_{k}$ and the two by two covariance ${\boldsymbol{\Sigma}}_{k}$ to
get the GMM

$P\left(\mathbf{m}_{\text{o}}\right)=\frac{1}{\sum
w_{k}}\cdot\sum_{k=1}^{N_{g}}w_{k}\cdot\mathcal{N}\left(\mathbf{m}\,|\,\begin{pmatrix}x_{k}\\\
0\end{pmatrix},{\boldsymbol{\Sigma}}_{k}\right)$ (12)

with its components weighted by $w_{k}=\sqrt{\det{\boldsymbol{\Sigma}}_{k}}$.

Finally, this new GMM (missing) 12 has to be combined with the model GMM
(missing) 3 for each current measurement:

$P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x},\mathbf{m}_{\text{o}}\right)=w_{\text{o}}P\left(\mathbf{m}_{\text{o}}\right)+\left(1-w_{\text{o}}\right)P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)$

where it introduces a tuning parameter $w_{\text{o}}$, which represents the
expected amount of outliers – it weights both sets of GMM components
accordingly.

#### Doppler Integration

Till now, we addressed ego-motion estimation as an application for PSR
utilizing range sensors like lidar and radar. However, apart from spatial
information, the automotive radar provides Doppler measurements. In [13, equ.
14], these are jointly used with the correspondence likelihood to suppress
non-stationary objects. We followed this approach in our SA implementation, as
can be seen in (missing) 13. However, as the Doppler information is a nearly
independent measurement, we could also model it like an extra sensor within
the objective function, which we did in our likelihood approach, see (missing)
14.

$\displaystyle P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right)$
$\displaystyle\sim\sum_{i=1}^{|\mathcal{M}|}P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)P\left(v_{i}\,|\,\mathbf{x},\theta_{i},\Delta
t\right)$ (13) $\displaystyle
P\left(\mathcal{M}\,|\,\mathcal{F},\mathbf{x}\right)$
$\displaystyle=\prod_{i=1}^{|\mathcal{M}|}P\left(\mathbf{m}_{i}\,|\,\mathcal{F},\mathbf{x}\right)P\left(v_{i}\,|\,\mathbf{x},\theta_{i},\Delta
t\right)$ (14)

With the Doppler information, we get a good measurement of the vehicle’s
longitudinal translation over time. Even the information about the relative
rotation may be improved if the sensor is not located in the vehicle’s center
of rotation. A measurement model for the expected Doppler velocity of each
target can be found in [13]. As we work with relative transformation in our
state definition $\mathbf{x}$ instead of velocities, we use the proposed model
with small modifications, as stated in (missing) 15.

$P\left(v_{i}\,|\,\mathbf{x},\theta_{i},\Delta
t\right)\propto\exp\left(-\frac{1}{2}\left\|\hat{u}_{i}-v_{i}\cdot\Delta
t\right\|^{2}_{\hat{\sigma}_{\theta_{i}}^{2}+\sigma_{u_{i}}^{2}}\right)$ (15)

Here, $\hat{u}_{i}$ is the measurement model (missing) 16 for the expected
Doppler velocity, expressed as relative translation. In comparison to [13], we
omit the lateral translation, as we use only 2DoF in combination with Doppler
information in our implementation. Further, we use the same notation for the
sensor offset relative to the car’s center of rotation with $x_{S}$, $y_{S}$,
and $\alpha_{S}$. With $\hat{\sigma}_{\theta_{i}}$, we account for the
measured and uncertain azimuth angle of target $i$ – see (missing) 17.

$\displaystyle\hat{u}_{i}$
$\displaystyle=\begin{bmatrix}\mathbf{x}^{\text{rot}}\cdot
y_{S}-\mathbf{x}^{\text{tran}}\\\ -\mathbf{x}^{\text{rot}}\cdot
x_{S}\end{bmatrix}^{\mathsf{T}}\cdot\begin{bmatrix}\cos(\theta_{i}+\alpha_{S})\\\
\sin(\theta_{i}+\alpha_{S})\end{bmatrix}$ (16)
$\displaystyle\hat{\sigma}_{\theta_{i}}^{2}$
$\displaystyle=\left(\tfrac{\partial\hat{u}_{i}}{\partial\theta_{i}}\sigma_{\theta_{i}}\right)^{2}$
(17)

In consequence of our state definition, we do not use the pure Doppler
measurement in (missing) 15, but transform it by using the time difference
between the measurements with $\Delta t$. This measurement may also be
uncertain, why we need to consider it in our measurement variance
$\sigma_{u_{i}}^{2}$, see (missing) 19.

$\displaystyle u_{i}$ $\displaystyle=v_{i}\cdot\Delta t$ (18)
$\displaystyle\sigma_{u_{i}}^{2}$ $\displaystyle=\begin{bmatrix}\frac{\partial
u_{i}}{\partial v_{i}}&\frac{\partial u_{i}}{\partial\Delta
t}\end{bmatrix}\cdot\operatorname{diag}(\sigma_{v_{i}}^{2},\sigma_{\Delta
t}^{2})\cdot\begin{bmatrix}\frac{\partial u_{i}}{\partial
v_{i}}&\frac{\partial u_{i}}{\partial\Delta t}\end{bmatrix}^{\mathsf{T}}$ (19)

#### Covariance Estimation

Unfortunately, the importance of a good covariance estimate is often
neglected, and the evaluation focus of new registration approaches lies mostly
on the mean error. Aiming at more complex sensor fusion with several sensors
and possible pre-processing steps to reduce the problem’s size, we need to
stress the consistency of a pre-processing step like the present ego-motion
estimation. An algorithm with a good mean error but a bad covariance estimate
will probably lead to a bad overall performance compared to the one with less
quality in the mean error but consistent covariances. In work like [9] or [7],
the covariance is calculated in a post-registration step, as the measurements’
noise models are not taken into account within the registration step. In
contrast to this, we have all necessary noise models already included within
the optimization problem. Accordingly, we can get the estimate’s covariance by
inverting the Hessian, which is a byproduct of the optimization111To be exact:
typically, this is an approximated Hessian based on the squared Jacobian. and
represents the cost function’s shape around the optimized value. Further, as
we are minimizing a negative log-likelihood, we can also refer to the Hessian
as Fisher Information Matrix (FIM), indicating how much information about the
estimated state is given by the observations.

#### Implementation

The presented likelihood approach is implemented within GTSAM and used through
the accompanied Matlab wrapper. For implementing the objective (missing) 9
based on (missing) 14, one variable vector representing a translation and
rotation ist used. Each targets’ position within the current measurement is
used in a unary factor, connected to the variable vector. The previous
measurement and outlier distribution is represented within a special noise
model for GMMs and added to each of these factors. Additionally, the same
amount of unary factors is added for the Doppler measurements. For additional
robustness against local minima, we also used the heuristic concept of
covariance scaling (e.g., mentioned by [6]) to get better initialization
points, meaning that we first run a maximum of 5 iteration steps with scaled
covariances (by factor 5), before the correct ones are used.

For the SA, a pure Matlab implementation leveraging the nonlinear programming
solver is used.

##  IV Evaluation

We start our evaluation with a common PSR problem, motivated by the idea to
compare against a standard ICP implementation using error propagation for the
covariance estimate. In the following experiment, we extended this Monte Carlo
simulation with regard to representing automotive radar sensor measurements.
With the perfectly-known measurements’ uncertainty in both simulation
scenarios, a consistency analysis can be done. Finally, real-world data
gathered with a mobile robot and a car is evaluated to show the applicability
to real-world scenarios, whereas the automotive scenario is based on the
publically available nuScenes dataset. According to our main objective of a
loosely-coupled algorithm, all experiments evaluate only the scan-to-scan
performance with no previous knowledge, i.e., zero initialization.

TABLE I: Parameters for the Simulation Experiments

Experiment | PSR | Radar
---|---|---
Num. of Configurations | 100 | 50
lightgrayNum. of Landmarks | 20 (+16) | 20 (+16)
lightgrayLandmark
Generation | $\begin{aligned} &\mathcal{U}\left(r\,|\,$10\pm 5\text{\,}\mathrm{m}$\right)\\\ &\mathcal{U}\left(\theta\,|\,\pm\pi\right)\end{aligned}$ | $\begin{aligned} &\mathcal{U}\left(r\,|\,$20\pm 18\text{\,}\mathrm{m}$\right)\\\ &\mathcal{U}\left(\theta\,|\,$\pm 55\text{\,}\mathrm{\SIUnitSymbolDegree}$\right)\end{aligned}$
Clustering (size) |  $40\text{\,}\mathrm{\char 37\relax}$ ($3$) |  $40\text{\,}\mathrm{\char 37\relax}$ ($3$)
Cluster spread | $\mathcal{N}\left(x,y\,|\,0,0.1^{2}\right)$ | $\mathcal{N}\left(x,y\,|\,0,0.1^{2}\right)$
blackRuns per Configuration | 1000 | 500
lightgrayTransformation
sampling | $\begin{aligned} &\mathcal{U}\left(x,y\,|\,$\pm 0.25\text{\,}\mathrm{m}$\right)\\\ &\mathcal{U}\left(\alpha\,|\,$$\right)\end{aligned}$ | $\begin{aligned} &\mathcal{U}\left(x\,|\,$\pm 0.25\text{\,}\mathrm{m}$\right)\\\ &\mathcal{U}\left(\alpha\,|\,$$\right)\end{aligned}$
lightgrayMeasurement noise | $\begin{aligned} &\mathcal{N}\left(r\,|\,0,\left($0.2\text{\,}\mathrm{m}$\right)^{2}\right)\\\ &\mathcal{N}\left(\theta\,|\,0,\left($$\right)^{2}\right)\end{aligned}$ | $\begin{aligned} &\mathcal{N}\left(r\,|\,0,\left($0.2\text{\,}\mathrm{m}$\right)^{2}\right)\\\ &\mathcal{N}\left(\theta\,|\,0,\left($$\right)^{2}\right)\end{aligned}$
black |  |

###  IV-A Simulated Point Set Registration

In PSR, we try to estimate a transformation aligning two sets of points.
Regarding the radar-based motion estimation at hand, this is referred to as
the spatial registration and can be evaluated separately. Accordingly, we
generate multiple 2D PSR problems with 3DoF222Note: Only for the PSR
experiment, we ignore the nonholonomic constraint and use a full 2D state for
better comparison to PSR problems in general. evaluating the algorithms’
accuracy and uncertainty output (i.e., credibility). As already mentioned, in
this scenario, we also compare against the standard ICP. The implementation is
based on the well-known PCL-Library [23], combined with covariance estimation
based on the accompanied implementation of [24].

We simulated $100$ different landmark configurations and $1000$ different
transformations. All transformations are applied to each landmark
configuration to generate the two point sets, which results in
$100\text{\,}\mathrm{k}$ experiments. Gaussian measurement noise is applied
independently to each of the $20$ landmarks within the
$200\text{\,}\mathrm{k}$ point sets in polar space. The landmark
configurations themselves are sampled from two uniform distributions in polar
space ($[r,\theta]$) as also specified in Section IV.

As part of this synthetic PSR scenario, we run the $100\text{\,}\mathrm{k}$
experiments again with clustered landmarks – clustered landmarks often
appeared in our radar measurements. Specifically, we randomly select
$40\text{\,}\mathrm{\char 37\relax}$ out of the $20$ landmarks to duplicate
them two times using the spread parameters given in Section IV. As a result,
we now have 12 unmodified landmarks and 8 clusters of three landmarks –
indicated in Section IV with $(+16)$.

###  IV-B Simulated Radar

For the synthetic radar dataset, the PSR experiment is repeated but with small
modifications. Considering now a sensor with limited FoV, we generate the
landmarks only in front of the sensor. Naturally, the limited FoV in
combination with the applied transformations for the second point set fosters
an outlier appearance. In consequence, the number of landmarks, shown in
Section IV, is only the number of created landmarks. However, the number of
corresponding landmarks for estimating the motion estimation is less. The
ratio between corresponding and non-corresponding landmarks is shown in Fig.
2. Again, we also simulate a problem with clustered landmarks, and
additionally, a Doppler velocity is simulated for each of the landmarks with a
standard deviation of $0.3\text{\,}\mathrm{m}\text{\,}{\mathrm{s}}^{-1}$.

TABLE II: Summarized Results | Alg. |  | RMSE
---
[$\mathrm{m}$]
| RMSE
---
[$\mathrm{\SIUnitSymbolDegree}$]
ANEES |  | Average
---
Iterations
| Average
---
Runtime [$\mathrm{ms}$]
PSR | ICP | $0.200\,05$ | $1.301\,59$ | $1.305\,88$ | $3.500\,00$ | $0.288\,75$
MSM | $0.120\,94$ | $0.988\,66$ | $1.071\,23$ | $4.200\,00$ | $1.979\,12$
SA | $0.265\,67$ | $10.579\,57$ | $1.565\,53$ | $13.600\,00$ | $26.918\,91$
PSR-C | ICP | $0.156\,49$ | $1.072\,85$ | $1.510\,39$ | $5.900\,00$ | $0.331\,98$
MSM | $0.096\,75$ | $0.765\,75$ | $1.212\,39$ | $5.500\,00$ | $3.758\,11$
SA | $0.206\,39$ | $8.220\,75$ | $0.723\,85$ | $13.600\,00$ | $74.386\,04$
Sim | MSM | $0.051\,99$ | $0.348\,93$ | $0.981\,65$ | $6.400\,00$ | $2.837\,90$
SA | $0.265\,57$ | $8.572\,38$ | $23.643\,83$ | $9.800\,00$ | $10.652\,22$
MSM-D | $0.016\,75$ | $0.346\,50$ | $1.019\,56$ | $6.100\,00$ | $5.685\,17$
SA-D | $0.049\,19$ | $8.542\,90$ | $21.808\,11$ | $8.100\,00$ | $9.622\,64$
Sim-C | MSM | $0.044\,10$ | $0.285\,82$ | $1.256\,61$ | $7.500\,00$ | $4.736\,66$
SA | $0.255\,99$ | $8.329\,64$ | $15.720\,37$ | $10.000\,00$ | $26.981\,51$
MSM-D | $0.012\,26$ | $0.278\,48$ | $1.085\,59$ | $6.900\,00$ | $9.791\,12$
SA-D | $0.040\,07$ | $8.309\,44$ | $16.898\,86$ | $8.000\,00$ | $23.481\,46$
Robo | MSM | $0.106\,57$ | $1.117\,17$ | $2.527\,11$ | $8.200\,00$ | $2.951\,80$
SA | $0.084\,51$ | $1.358\,21$ | $0.160\,09$ | $7.500\,00$ | $11.015\,60$
MSM-D | $0.010\,96$ | $1.113\,21$ | $1.683\,44$ | $5.900\,00$ | $6.043\,14$
SA-D | $0.017\,04$ | $1.365\,38$ | $0.203\,75$ | $6.100\,00$ | $9.363\,64$
NuSc | MSM | $0.163\,06$ | $0.153\,68$ | $1.608\,20$ | $11.100\,00$ | $23.238\,52$
SA | $0.091\,49$ | $0.126\,78$ | $0.004\,63$ | $11.200\,00$ | $242.854\,25$
MSM-D | $0.011\,58$ | $0.104\,96$ | $3.359\,96$ | $6.200\,00$ | $33.968\,18$
SA-D | $0.226\,21$ | $5.305\,89$ | $2.328\,28$ | $7.200\,00$ | $225.043\,92$
NuScT | MSM | $0.167\,64$ | $0.152\,23$ | $1.634\,48$ | $10.900\,00$ | $22.675\,67$
MSM-D | $0.012\,16$ | $0.095\,55$ | $4.492\,42$ | $6.100\,00$ | $34.346\,84$

###  IV-C Mobile Robot Dataset

Our main motivation for using the radar sensor in a loosely-coupled way for
motion estimation emerged from developing a robust sensor fusion algorithm for
two mobile robots, described in [25]. Lately, we extended the robots’ sensor
setup by an automotive radar333The specific type is a General Purpose Radar
V1.0 from Bosch. sensor. The sensor is mounted near to the center of the robot
and detects up to 48 targets with $10\text{\,}\mathrm{Hz}$. It is interfaced
via CAN-bus and measures the horizontal angle, distance, Doppler velocity, and
additional properties for each target. Among the additional properties, a
standard deviation for the measurements is given.

For evaluation of our algorithm, we recorded a dataset at the TU Chemnitz
campus. The area is mostly surrounded by buildings with a small street and
about $60\text{\,}\mathrm{\char 37\relax}$ grass-covered with some trees.
Unfortunately, no exact ground truth is available. So, we used batch
optimization over several sensor information originating from wheel odometry,
IMU, and an external UWB distance-measuring system instead. Based on visual
verification of the calculated trajectory with the satellite images, we claim
to have a remaining error of less than $0.5\text{\,}\mathrm{m}$. We
additionally compared our trajectory to a solution calculated using the
Cartographer framework [26] based on measurements recorded with an onboard
Velodyne VLP-16, resulting in negligible differences.

In contrast to the synthetic scenarios, information about ground truth
correspondences between consecutive scans is not available. However, this is
an important evaluation metric, so we use a simple classification scheme for
correspondence estimation based on the Bhattacharyya distance [27]

$\displaystyle D_{\text{Bhat}}$
$\displaystyle=\frac{1}{8}\left\|{\boldsymbol{\mu}}_{1}-{\boldsymbol{\mu}}_{2}\right\|^{2}_{\bar{\boldsymbol{\Sigma}}}+\frac{1}{2}\ln\left(\left|\bar{\boldsymbol{\Sigma}}\right|\left(\left|{\boldsymbol{\Sigma}}_{1}\right|\left|{\boldsymbol{\Sigma}}_{2}\right|\right)^{-\frac{1}{2}}\right).$
(20)

Basically, this gives us a similarity rating between two distributions
$\mathcal{N}\left(\mathbf{x}\,|\,{\boldsymbol{\mu}}_{1},{\boldsymbol{\Sigma}}_{1}\right)$
and
$\mathcal{N}\left(\mathbf{x}\,|\,{\boldsymbol{\mu}}_{2},{\boldsymbol{\Sigma}}_{2}\right)$,
where smaller values represent higher overlapping. For clarification,
$\bar{\boldsymbol{\Sigma}}$ stands for a mean with
$\bar{\boldsymbol{\Sigma}}=0.5\cdot\left({\boldsymbol{\Sigma}}_{1}+{\boldsymbol{\Sigma}}_{2}\right)$,
$\left\|.\right\|^{2}_{{\boldsymbol{\Sigma}}}$ and $\left|.\right|$ is the
squared Mahalanobis distance respective the determinant. Using the vehicle’s
ground truth, we can transform the current scan and evaluate $D_{\text{Bhat}}$
for all possible landmark combinations. Then, we select for each landmark in
the current scan one landmark with the smallest distance from the previous. A
landmark pair is then classified as correspondence if the distance is smaller
than a threshold. We defined the empirically determined threshold to be $1$.
All landmarks within the current and the previous scan without correspondence
represent the number of outliers or non-corresponding landmarks. In Fig. 2,
the classification result can be seen. Compared to the simulation scenario,
the amount of non-corresponding targets is increased. Further, some motion
estimates will be based on a very small number of correspondences.

###  IV-D nuScenes

With the nuScenes dataset [1], we chose a publicly available dataset for
further evaluation and the possibility of benchmarking against our solution.
The dataset provides ground truth and automotive radar data444The used type is
an ARS 408-21 from Continental. for $850$ driving scenes with changing
complexity each of $20\text{\,}\mathrm{s}$ in length. Out of the five radar
sensors the car is equipped with, we use only the front radar, which is
captured with $13\text{\,}\mathrm{Hz}$ and a maximum of 125 targets. Each
measurement can be read as a point cloud with additional information like
Doppler velocity, standard deviation, or quality classification for each
target – e.g. regarding a validity or ambiguity state. Each scene also
includes the extrinsic calibration describing the sensor’s position and angle
within the car’s body frame. In contrast to our other experiments, the
sensor’s lever arm increases the angular accuracy when using doppler
information.

Our evaluation is done for the _mini_ dataset and the first $500$ of the $700$
_trainval_ training scenes. For each dataset, we consider all scenes together
as a single experiment, resulting in $2600$ respective
$130\text{\,}\mathrm{k}$ estimation problems. As the scenes are very different
in their properties, it is also interesting to look at each scene separately,
which we do for the _trainval_ dataset. For the _mini_ dataset, the outlier
statistics are shown again in Fig. 2, calculated in the same way as for the
mobile robot dataset. Compared to the other datasets, we generally have more
landmarks but a similar outlier to correspondence ratio like in the mobile
robot dataset.

Side notes:  1. To remain consistent with our algorithms, we convert the
dataset’s radar sensor information regarding the given Doppler information
from Cartesian coordinates to an absolute Doppler velocity pointing towards
the sensor, including the given noise.  2. As we convert the Doppler into a
translation by using the time difference between two consecutive measurements,
we need to give it an uncertainty, which is $\sigma=$5\text{\,}\mathrm{ms}$$.
This can be evaluated by the timestamps, which fluctuate around
$13\text{\,}\mathrm{Hz}$.

##  V Results

In the following, we evaluate our results based on two metrics:  1. the
relative angle and position error as RMSE555The
$\text{RMSE}\mathrel{\widehat{=}}(\frac{1}{m}\sum_{k=1}^{m}\left\|\text{err}_{k}\right\|^{2})^{1/2}$
where $\text{err}_{k}$ corresponds to either the translational or rotational
part of the scan-to-scan performance compared to the ground truth. and  2. the
average normalized estimation error squared (ANEES).  For the ANEES, we follow
[28, Chp. 5.4], but normalize with the problem’s dimension as stated in [29].
The later metric gives us insight into the goodness (or credibility) of the
estimators’ covariance calculation. If the estimator is perfectly credible,
the ANEES value equals $1$. However, this holds only under the assumption of a
$\chi^{2}$-distributed NEES – otherwise, the ANEES is not meaningful.
Additionally, we provide the average runtime per registration, evaluated on a
standard AMD Ryzen 7 desktop PC.

#### Simulation

We start by comparing the results of the simulated experiments. Because of a
bigger chance to reach local minima, the sum approximation has a higher RMSE
for both, the translation and rotation, as can be seen in Table II (PSR, Sim,
PSR-C, and Sim-C – C refers to the clustered dataset). Of course, the common
ICP approach with its closed-form covariance estimation is the fastest
solution but does not include the landmarks’ covariances within the
registration process. Accordingly, it performs a bit worse regarding RMSE and
consistency. Generally, we find the ANEES values for all simulation
experiments for our likelihood approach reasonably close to one, which means a
consistent estimation. The estimated covariance based on the SA, on the other
hand, is not useful, as the SA violates the Gaussian assumption by adding the
probabilities instead of jointing them. This results in NEES values not
following a $\chi^{2}$-distribution666A plot is omitted here because of space
constraints., making the ANEES evaluation inconclusive. In the simulation with
clustered landmarks, more information is introduced into the problem,
resulting in a decreased error and increased runtime. We can also see in the
ANEES that the covariance is slightly more overconfident. By adding the
Doppler information in the simulated radar experiment (marked with -D in Table
II), the translational RMSE gets better – as we would expect. Likewise, we see
an increase in runtime for the likelihood approach as additional factors are
introduced into the optimization problem.

#### Real World

As before, the results are summarized in Table II, where _Robo_ corresponds to
the mobile robot dataset. The combined results of the two considered nuScenes
datasets are named _NuSc_ and _NuScT_ respective for the _mini_ and _trainval_
variant. Looking at the mobile robot dataset results, we see basically
identical RMSE values for the approximation and the likelihood approach
considering the assumed accuracy of the ground truth. For the nuScenes _mini_
dataset however, the RMSE values for the pure spatial estimation show a
slightly better performance in advance to the approximation approach, but
worse if we include the Doppler information – again, the approximation
approach is more sensitive to local minima. The _trainval_ dataset is only
evaluated for the likelihood and confirms the evaluation of the _mini_
dataset, as it has nearly identical RMSE errors.

Regarding the ANEES values, we see that all results show more or less a trend
to be overconfident. The worst result is with about $4.5$ – to get an
impression, what this means: if we get an ANEES of about $4$, we need to
multiply the estimated standard deviation by a factor of two to be consistent
again. If we take the uncertainties of the provided data into account, the
deviations to the simulated results are explainable. In real-world scenarios,
there is always the question of how good is the sensor’s information about the
noise. Unfortunately, this is not provided within the used sensors’ datasheets
but states a critical parameter for the overall consistency. Likewise, there
is no information about the algorithm the sensors use for estimating the
targets’ standard deviation for velocity and position. Additionally, further
unknowns like the goodness of ground truth, time stability between
measurements, or the number of outliers play a role within the evaluation and
lead to tunable parameters.

If we look into the error and consistency statistics over the $500$ scenes of
the _trainval_ dataset in Fig. 3, the impact of scene property variation
becomes evident. We see a big spread for the error as well as for the ANEES.
Further investigation could be done here, in which special situations, the
likelihood approach does not perform well. Especially in scenes where the car
does not move, there is the trend for the ANEES value to become very low.

$0$$10$$20$$30$$40$$50$$60$$70$$80$$90$$100$$110$SimSim-
CRoboNuScCorrespondencesOutliers Figure 2: The diagram shows the number of
corresponding and non-corresponding landmarks for the relevant scenarios. The
ratio between inlier and outlier as well as the overall amount of landmarks
characterizes the difference between the datasets. Further can be noticed,
that especially in the mobile robot dataset, the number of correspondences is
in some experiments nearly zero.
$0$$2$$4$$6$$8$$10$$12$$14$$16$$18$$20$$22$ANEESMSMMSM-D$0$$5\cdot
10^{-2}$$0.1$$0.15$$0.2$$0.25$$0.3$$0.35$$0.4$$0.45$$0.5$$0.55$RMSE
[m]$0$$5\cdot
10^{-2}$$0.1$$0.15$$0.2$$0.25$$0.3$$0.35$$0.4$$0.45$$0.5$$0.55$RMSE [∘] Figure
3: To give a deeper insight regarding possible result variations, the plots
show evaluation results for the nuScenes _trainval_ dataset per scene, whereas
the blue boxes correspond to the results using Doppler velocity and the black
boxes without. Note, that the axis units for the position RMSE are given in
meters and for the angular RMSE in degrees.

##  VI Conclusion

We presented a feasible way to solve the ego-motion estimation based on a
thorough probabilistic strategy. Utilizing the challenging automotive Doppler
radar measurements with sparse environment representation and a high outlier
rate, we were able to achieve robust estimation results, including a
reasonable covariance quality. Whereas the estimator’s covariances in
simulation showed a nearly perfect credibility of our algorithm – released as
open source777Available under www.mytuc.org/creme. Our extensive evaluation of
about $125\text{\,}\mathrm{k}$ simulation experiments and about
$130\text{\,}\mathrm{k}$ experiments based on the nuScenes dataset underlined
the robustness of our method with and without using Doppler information.
Overall, it is perfectly suitable as simplified sensor information to be
included within a multi-sensor fusion algorithm in a loosely-coupled way, as
it is uncertainty aware.

As our method is based on standard least-squares minimization, but with more
complex noise models representing the measurements and outliers, we think it
is universal to be also used in related tasks. These could be pure PSR on
unstable point sets with known measurement noise or feature-based registration
in 3D laser scans. However, a remaining problem of optimization algorithms are
local minima, which also lead to wrong covariance estimates. Even if our
approach seems to be robust against this problem, further investigation can be
done regarding this, e.g., by adding this possibility to the estimated
covariance like done in [30] by using the unscented transform.

## References

  * [1] H. Caesar, V. Bankiti, A. H. Lang, S. Vora, V. E. Liong, Q. Xu, A. Krishnan, Y. Pan, G. Baldan, and O. Beijbom, “nuScenes: A multimodal dataset for autonomous driving,” _arXiv:1903.11027 [cs, stat]_ , 2020.
  * [2] D. Barnes, M. Gadd, P. Murcutt, P. Newman, and I. Posner, “The Oxford Radar RobotCar Dataset: A Radar Extension to the Oxford RobotCar Dataset,” _arXiv:1909.01300 [cs, eess]_ , 2020.
  * [3] G. Kim, Y. S. Park, Y. Cho, J. Jeong, and A. Kim, “MulRan: Multimodal Range Dataset for Urban Place Recognition,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2020, pp. 6246–6253.
  * [4] Y. Almalioglu, M. Turan, C. X. Lu, N. Trigoni, and A. Markham, “Milli-RIO: Ego-Motion Estimation With Low-Cost Millimetre-Wave Radar,” _IEEE Sensors Journal_ , vol. 21, no. 3, pp. 3314–3323, 2021.
  * [5] S. H. Cen and P. Newman, “Precise Ego-Motion Estimation with Millimeter-Wave Radar Under Diverse and Challenging Conditions,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2018, pp. 6045–6052.
  * [6] B. Jian and B. C. Vemuri, “Robust Point Set Registration Using Gaussian Mixture Models,” _IEEE Trans. on Pattern Analysis and Machine Intelligence_ , vol. 33, no. 8, pp. 1633–1645, 2011.
  * [7] T. Stoyanov, M. Magnusson, H. Andreasson, and A. J. Lilienthal, “Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations,” _The Int. Journal of Robotics Research_ , vol. 31, no. 12, pp. 1377–1393, 2012.
  * [8] P. J. Besl and N. D. McKay, “A Method for Registration of 3-D Shapes,” _IEEE Trans. Pattern Anal. Mach. Intell._ , vol. 14, pp. 239–256, 1992.
  * [9] A. Censi, “An accurate closed-form estimate of ICP’s covariance,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2007, pp. 3167–3172.
  * [10] A. V. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” _Robotics: science and systems_ , 2009.
  * [11] P. Biber and W. Straßer, “The Normal Distributions Transform: A New Approach to Laser Scan Matching,” in _Proc. of Intl. Conference on Intelligent Robots and Systems (IROS)_ , vol. 3, 2003, pp. 2743–2748.
  * [12] M. Magnusson, A. Lilienthal, and T. Duckett, “Scan registration for autonomous mining vehicles using 3D-NDT,” _Journal of Field Robotics_ , vol. 24, no. 10, pp. 803–827, 2007.
  * [13] M. Barjenbruch, D. Kellner, J. Klappstein, J. Dickmann, and K. Dietmayer, “Joint spatial- and Doppler-based ego-motion estimation for automotive radars,” in _Proc. of Int. Symposium on Intelligent Vehicles (IV)_ , 2015, pp. 839–844.
  * [14] M. Rapp, M. Barjenbruch, M. Hahn, J. Dickmann, and K. Dietmayer, “Probabilistic ego-motion estimation using multiple automotive radar sensors,” _Robotics and Autonomous Systems_ , vol. 89, pp. 136–146, 2017\.
  * [15] D. Kellner, M. Barjenbruch, J. Klappstein, J. Dickmann, and K. Dietmayer, “Instantaneous ego-motion estimation using Doppler radar,” in _Proc. of Intl. Conf. on Intelligent Transportation Systems (ITSC)_ , 2013, pp. 869–874.
  * [16] ——, “Instantaneous ego-motion estimation using multiple Doppler radars,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2014, pp. 1592–1597.
  * [17] Y. S. Park, Y.-S. Shin, J. Kim, and A. Kim, “3D ego-Motion Estimation Using low-Cost mmWave Radars via Radar Velocity Factor for Pose-Graph SLAM,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 4, pp. 7691–7698, 2021.
  * [18] C. D. Monaco and S. Brennan, “RADARODO: Ego-Motion Estimation from Doppler and Spatial Data in RADAR Images,” _IEEE Transactions on Intelligent Vehicles_ , vol. 5, no. 3, pp. 475–484, 2020.
  * [19] A. Kramer, C. Stahoviak, A. Santamaria-Navarro, A.-a. Agha-mohammadi, and C. Heckman, “Radar-Inertial Ego-Velocity Estimation for Visually Degraded Environments,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2020, pp. 5739–5746.
  * [20] T. Pfeifer, S. Lange, and P. Protzel, “Advancing Mixture Models for Least Squares Optimization,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 3941–3948, 2021.
  * [21] R. Hartley and A. Zisserman, _Multiple View Geometry in Computer Vision_ , 2nd ed. New York, NY, USA: Cambridge University Press, 2003.
  * [22] J. Solà, “Towards Visual Localization, Mapping and Moving Objects Tracking by a Mobile Robot: A Geometric and Probabilistic Approach.” Ph.D. dissertation, LAAS, Toulouse, 2007.
  * [23] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_ , 2011.
  * [24] S. M. Prakhya, L. Bingbing, Y. Rui, and W. Lin, “A closed-form estimate of 3D ICP covariance,” in _Proc. of Intl. Conf. on Machine Vision Applications (MVA)_ , 2015, pp. 526–529.
  * [25] S. Lange, D. Wunschel, S. Schubert, T. Pfeifer, P. Weissig, A. Uhlig, M. Truschzinski, and P. Protzel, “Two Autonomous Robots for the DLR SpaceBot Cup – Lessons Learned from 60 Minutes on the Moon,” in _Proc. of Intl. Symposium on Robotics (ISR)_ , 2016.
  * [26] W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure in 2D LIDAR SLAM,” in _Proc. of Intl. Conf. on Robotics and Automation (ICRA)_. IEEE, 2016, pp. 1271–1278.
  * [27] C. Hennig, “Methods for merging Gaussian mixture components,” _Advances in Data Analysis and Classification_ , vol. 4, no. 1, pp. 3–34, 2010.
  * [28] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, _Estimation with Applications to Tracking and Navigation: Theory Algorithms and Software_ , 1st ed. New York, NY, USA: John Wiley & Sons, Inc., 2001.
  * [29] X. R. Li, Z. Zhao, and V. P. Jilkov, “Estimator’s credibility and its measures,” in _Proc. of Int. Federation of Automatic Control (IFAC)_ , 2002.
  * [30] M. Brossard, S. Bonnabel, and A. Barrau, “A New Approach to 3D ICP Covariance Estimation,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 744–751, 2020.