# 3D Extended Object Tracking by Fusing Roadside Sparse Radar Point Clouds and
Pixel Keypoints

Jiayin Deng, Zhiqun Hu, Yuxuan Xia, Zhaoming Lu, and Xiangming Wen This work
was supported in part by the Key Research and Development Program of China
under Grants SQ2022YFB2600041. (corresponding author: Zhiqun Hu)Jiayin Deng,
Zhiqun Hu, Zhaoming Lu, and Xiangming Wen are with the Beijing Laboratory of
Advanced Information Networks, Beijing University of Posts and
Telecommunications, Beijing 100876, China (e-mail<EMAIL_ADDRESS><EMAIL_ADDRESS><EMAIL_ADDRESS>xiangmw@bupt.edu.cn).Yuxuan Xia is
with the Department of Electrical Engineering, Linköping University, Linköping
58183, Sweden (e-mail: yuxuan.xia@liu.se).

###### Abstract

Roadside perception is a key component in intelligent transportation systems.
In this paper, we present a novel three-dimensional (3D) extended object
tracking (EOT) method, which simultaneously estimates the object kinematics
and extent state, in roadside perception using both the radar and camera data.
Because of the influence of sensor viewing angle and limited angle resolution,
radar measurements from objects are sparse and non-uniformly distributed,
leading to inaccuracies in object extent and position estimation. To address
this problem, we present a novel spherical Gaussian function weighted Gaussian
mixture model. This model assumes that radar measurements originate from a
series of probabilistic weighted radar reflectors on the vehicle’s extent.
Additionally, we utilize visual detection of vehicle keypoints to provide
additional information on the positions of radar reflectors. Since keypoints
may not always correspond to radar reflectors, we propose an elastic skeleton
fusion mechanism, which constructs a virtual force to establish the
relationship between the radar reflectors on the vehicle and its extent.
Furthermore, to better describe the kinematic state of the vehicle and
constrain its extent state, we develop a new 3D constant turn rate and
velocity motion model, considering the complex 3D motion of the vehicle
relative to the roadside sensor. Finally, we apply variational Bayesian
approximation to the intractable measurement update step to enable recursive
Bayesian estimation of the object’s state. Simulation results using the Carla
simulator and experimental results on the nuScenes dataset demonstrate the
effectiveness and superiority of the proposed method in comparison to several
state-of-the-art 3D EOT methods.

###### Index Terms:

Extended object tracking, radar point clouds, pixel keypoints, roadside
perception, camera and radar fusion, variational approximation

##  I Introduction

With the rapid development of connected vehicles and vehicle-to-everything
communications technologies, an increasing number of researches have been
conducted to improve sensing range and accuracy by leveraging roadside
perception [1, 2]. As a critical component in this area, object tracking
improves object positioning accuracy by leveraging multi-frame data [3, 4].
Moreover, with the enhancement of sensor resolution, modern sensors can
capture multiple measurements from a single object, enabling inference of both
its extent (_e.g._ , shape and size) and kinematic (_e.g._ , position and
velocity) state.

The mainstream 3D object tracking methods are based on deep learning
frameworks. These methods typically involve initial detection of objects’ 3D
sizes and positions, followed by association with existing objects through a
trajectory management module [5]. Leveraging high-resolution LiDAR and camera
sensors, recent fusion-based approaches have demonstrated promising
performance in vehicle-mounted perception scenarios [6, 7, 8]. However, LiDAR
and camera sensors have limited robustness and efficacy in long-distance
detection. To address these challenges, there has been growing interest in
fusing high-resolution 4D radar with cameras [9, 10, 11, 12]. Nonetheless,
deep learning approaches often require extensive and high-quality annotations,
resulting in higher labor costs. Moreover, these methods exhibit poor
transferability and interpretability, which can potentially lead to security
concerns arising from anomalous outputs.

To construct a model with robust interpretability and meet the demands for
high-quality perception using modern high-resolution radar sensors, extended
object tracking (EOT) is emerging as a promising technology, which constructs
probability models between radar measurements and the object’s extent,
enabling simultaneous estimation of the kinematic and the extent state of the
object [13, 14]. However, due to the object’s self-occlusion, the measurements
could hardly be considered as Gaussian or uniformly distributed, posing a
great challenge to EOT [15].

To address this challenge, recent studies manually divide the vehicle shape
into multiple regions with various reflection centers and noise variances. Cao
_et al._ [16] proposed an EOT method employing a radar measurement model
comprising five parts with fixed relative positions to characterize the point
cloud distribution surrounding a rectangular-shaped vehicle. Zhang _et al._
[17] designed a conditional Gaussian mixture radar measurement model
consisting of four components with dynamically updating positions and
variances based on the point cloud distribution during tracking. Tuncer _et
al._ [18] proposed a multi-ellipsoid-based EOT model capable of dynamically
adjusting the number and position of the ellipsoids. The above methods using
manually designed measurement models rely on exploiting the object extent
information captured in the point clouds, and thus their performance may drop
sharply in the case of sparse radar measurements.

To solve the EOT problem with sparse radar measurements, several studies have
focused on pre-learning radar measurement models from extensive data to better
depict the real-world characteristic of radar point clouds. Scheel _et al._
[19] presented a multi-vehicle tracking method based on a variational radar
model that is learned from actual data using variational Gaussian mixtures.
Xia _et al._ [20] employed a learned-based hierarchical truncated Gaussian
radar measurement model with structural geometry parameters to track vehicles.
However, methods based on learned radar models require an abundance of
annotated data, which increases labor costs. Furthermore, the trained radar
model is limited to the radar used in the training set and may exhibit poor
generalization capabilities.

Considering the inherent problems caused by point cloud data, we propose
integrating a probability model of visual measurements into the EOT framework.
This integration leverages information from both sensors to enhance tracking
results for both the extent and kinematic state of the extended object. The
contributions of this work are summarized as follows:

  * •

We propose a novel radar measurement model called spherical Gaussian function
weighted Gaussian mixture model (SGW-GMM). This model effectively captures the
non-uniform distribution of radar measurements and allows for real-time
parameter updates based on actual radar measurements.

  * •

We propose an elastic skeleton (ES) fusion mechanism, which is integrated into
the Bayesian filtering framework. This mechanism creates a virtual spring-
damping system that facilitates interaction between radar reflectors and the
vehicle’s extent. The radar reflectors contribute to refining the vehicle’s
position, thereby enhancing the accuracy of its kinematic state. Additionally,
the vehicle’s extent provides prior positions of radar reflectors.

  * •

A simple yet effective 3D constant turn rate and velocity (CTRV) motion model
based on quaternion algebra is proposed to describe the vehicle’s maneuvering
characteristics. In addition, due to the more realistic continuous motion
pattern, the proposed model imposes constraints on the vehicle’s extent.

  * •

Variational approximation is adopted to enable a tractable measurement update
step in recursive Bayesian estimation, allowing for accurate extent and
kinematic state tracking of the vehicle. Simulation results using the Carla
simulator demonstrate the significant performance improvement of our proposed
method compared to existing state-of-the-art 3D EOT algorithms.

This paper follows specific notational conventions: scales are represented
with lowercase italic letters, _e.g._ , $x$, vectors with bold lowercase
letters, _e.g._ , $\mathbf{x}$, and matrices with bold uppercase letters,
_e.g._ , $\mathbf{X}$. Additionally, $\mathbf{x}^{\text{T}}$ and
$\mathbf{X}^{\text{T}}$ denote the transpose of vector $\mathbf{x}$ and matrix
$\mathbf{X}$, respectively; $\propto$ indicates equality up to a normalization
factor; $\mathcal{N}\left(\mathbf{x};\bm{\mu},\bm{\Sigma}\right)$ denotes the
Gaussian probability density function PDF (of random vector x) with mean
$\bm{\mu}$ and covariance matrix $\bm{\Sigma}$. The norm of vector
$\mathbf{v}$ is denoted $\|\mathbf{v}\|$. The tilde $(\tilde{\cdot})$ above a
variable indicates that it is a quaternion or the quaternion form of the
vector.

The paper is organized as follows. Section II provides preliminary information
about quaternions and coordinate systems. In section III, we introduce the
system models for our proposed 3D EOT method. Section IV presents the
implementation of inference using the variational approximation. The
simulation and experimental results are outlined in section V. Finally, we
conclude and present future works in section VI.

##  II Quaternions and Coordinate Systems

In this section, we first introduce quaternion and its relationships with
corresponding rotation vector and matrix. Then, we delve into the coordinate
systems employed in this paper and explain the transformations between them.

###  II-A Quaternion

Quaternion is a widely used mathematical tool for compactly representing non-
singular 3D rotation transformation. A quaternion is commonly expressed as a
four-dimensional vector [21, Eq. 7]:

$\tilde{\mathbf{q}}\triangleq\begin{bmatrix}q_{w}&\mathbf{q}_{v}^{\text{T}}\end{bmatrix}^{\text{T}}=\begin{bmatrix}q_{w}&q_{x}&q_{y}&q_{z}\end{bmatrix}^{\text{T}}\text{
, }$ (1)

where $q_{w}$ refers to the real part and $\mathbf{q}_{v}$ is the imaginary
part.

Rotation using quaternions is equivalent to multiplying the vector by a
rotation matrix. For instance, the rotation from a 3D vector $\mathbf{v}$ to
$\mathbf{v}^{\prime}$, can be expressed using either a quaternion or a
rotation matrix as:

$\tilde{\mathbf{v}}^{\prime}=\tilde{\mathbf{q}}\odot\tilde{\mathbf{v}}\odot\tilde{\mathbf{q}}^{*}\text{
}\Leftrightarrow\text{
}\mathbf{v}^{\prime}=\mathbf{R}\\{\tilde{\mathbf{q}}\\}\cdot\mathbf{v}\text{,
}$ (2)

where
$\tilde{\mathbf{q}}^{*}=\begin{bmatrix}q_{w}&-\mathbf{q}_{v}^{\text{T}}\end{bmatrix}^{\text{T}}$
represents the conjugate of the quaternion. The operators $\odot$ and
$\mathbf{R}\\{\cdot\\}$, respectively, denote the product of two quaternions
and the $3\times 3$ rotation matrix form corresponding to the quaternion,
defined as:

$\displaystyle\tilde{\mathbf{p}}\odot\tilde{\mathbf{q}}=$
$\displaystyle\begin{bmatrix}p_{w}q_{w}-\mathbf{p}_{v}^{\text{T}}\mathbf{q}_{v}\\\
p_{w}\mathbf{q}_{v}+q_{w}\mathbf{p}_{v}+\mathbf{p}_{v}\times\mathbf{q}_{v}\end{bmatrix}\text{,
}$ (3) $\displaystyle\mathbf{R}\\{\tilde{\mathbf{q}}\\}=$
$\displaystyle\left(q_{w}^{2}-\mathbf{q}_{v}^{\text{T}}\mathbf{q}_{v}\right)\mathbf{I}+2\mathbf{q}_{v}\mathbf{q}_{v}^{\text{T}}+2q_{w}\left[\mathbf{q}_{v}\right]_{\times}\text{,
}$ (4)

where $\mathbf{I}$ is a $3\times 3$ identical matrix, and $[\cdot]_{\times}$
represents the cross-product matrix of the vector:

$\left[\mathbf{q}_{v}\right]_{\times}=\begin{bmatrix}0&-q_{z}&q_{y}\\\
q_{z}&0&-q_{x}\\\ -q_{y}&q_{x}&0\end{bmatrix}\text{. }$ (5)

Noted that each vector has a corresponding quaternion form, _e.g._ the
quaternion form of $\mathbf{v}$ is
$\tilde{\mathbf{v}}=\begin{bmatrix}0&\mathbf{v}^{\text{T}}\end{bmatrix}^{\text{T}}$,
which is also called pure quaternion.

To simplify the derivation in the following sections, we introduce an
alternative representation of rotation, namely, the rotation vector
$\bm{\theta}$ $(\text{where }\|\bm{\theta}\|\in[0,2\pi))$. The corresponding
rotation matrix can be expressed as follows:

$\displaystyle\mathbf{R}\\{\bm{\theta}\\}=$
$\displaystyle\exp\left(\left[\bm{\theta}\right]_{\times}\right)=\mathbf{I}+\sum_{i=1}^{\infty}\frac{1}{i!}[\bm{\theta}]^{i}_{\times}$
$\displaystyle=$ $\displaystyle\text{
}\mathbf{I}+\sin\|\bm{\theta}\|\left[\frac{\bm{\theta}}{\|\bm{\theta}\|}\right]_{\times}+\left(1-\cos\|\bm{\theta}\|\right)\left[\frac{\bm{\theta}}{\|\bm{\theta}\|}\right]^{2}_{\times}\text{.
}$ (6)

Notably, if the quaternion $\tilde{\mathbf{q}}$ and the rotation vector
$\bm{\theta}$ represent the same rotation transformation, their corresponding
rotation matrices are identical, i.e.,
$\mathbf{R}\\{\tilde{\mathbf{q}}\\}=\mathbf{R}\\{\bm{\theta}\\}$. In this
case, there is a one-to-one correspondence between $\tilde{\mathbf{q}}$ and
$\bm{\theta}$, and to specifically denote these correspondences, we introduce
the following symbols as in [21, Eq. 101, 103]

$\tilde{\mathbf{q}}=\mathrm{Exp}\left(\bm{\theta}\right)\text{ and
}\bm{\theta}=\mathrm{Log}\left(\tilde{\mathbf{q}}\right)\text{. }$ (7)

###  II-B Coordinate Systems

Our research scenario focuses on the roadside integrated radar and camera
sensor, with the camera coordinate system (CCS) axis aligned with the radar
coordinate system (RCS) axis, and their origins are almost coincident.
Therefore, the CCS and RCS can be considered the same, see also [22], and we
refer to this unified system as the sensor coordinate system (SCS). As for the
vehicle coordinate system (VCS), the x-axis, y-axis, and z-axis point forward,
leftward, and upward of the vehicle, respectively. The origin of the VCS is
defined as the midpoint at the bottom of the vehicle.

According to the definition of the coordinate systems and quaternion, a 3D
point in the VCS can be transformed to the image pixel coordinate system (PCS)
through two steps. Specifically, to transform the position of the $t$-th radar
reflector $(\mathbf{u}_{t})$ in VCS:

We first transform $\mathbf{u}_{t}$ from VCS to SCS through rotation and
translation, to obtain the coordinate $\mathbf{u}_{t}^{\mathcal{S}}$:

$\mathbf{u}_{t}^{\mathcal{S}}=\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{u}_{t}+\mathbf{p}\text{,
}$ (8)

where $\bm{\theta}$ and $\mathbf{p}$ are the vehicle’s pose and position,
respectively. Then, we project the point from SCS to the PCS, to obtain the
pixel coordinates $\mathbf{u}_{t}^{\mathcal{I}}$ using the distortion-free
pinhole camera model [23]:

$\displaystyle\kappa\cdot\begin{bmatrix}\left(\mathbf{u}_{t}^{\mathcal{I}}\right)^{\text{T}}&1\end{bmatrix}^{\text{T}}=\mathbf{K}\cdot\mathbf{u}_{t}^{\mathcal{S}}\text{,
}$ (9a) $\displaystyle\mathbf{K}=\begin{bmatrix}\frac{f}{d_{x}}&0&u_{0}\\\
0&\frac{f}{d_{y}}&v_{0}\\\ 0&0&1\end{bmatrix}\text{, }$ (9b)

where $\kappa$ is a scale factor, and the matrix $\mathbf{K}$ is the camera’s
intrinsic parameters matrix, comprising camera focus $(f)$, parameters of the
photosensitive chip $(d_{x},d_{y})$, and the center of the image
$(u_{0},v_{0})$.

##  III System Model

Figure 1: Illustration of 13 of the vehicle’s skeleton knots and their
corresponding radar reflectors, along with 3 of the corner points. The yellow
dashed lines represent predefined associations between skeleton knots and
reflectors. Visual measurements are named keypoints which consist of pixel
coordinates of skeleton knots and corner points, while radar measurements are
3D point clouds generated by radar reflectors based on the probabilities.

We assume the vehicle moves in a 3D space, and the vehicle’s extent is
represented by its length, width, and skeleton. As shown in Fig. 1, the
skeleton consists of $T$ components, each of which contains a skeleton knot
and a radar reflector. The state of the $t$-th component is denoted as
$\bm{\vartheta}_{t}$ and includes the position of the reflector
$(\mathbf{u}_{t})$, the position of the knot $(\bm{\varpi}_{t})$, and the
velocity of the knot $(\mathbf{v}_{t})$:

$\bm{\vartheta}_{t}=\begin{bmatrix}\mathbf{u}_{t}^{\text{T}}&\bm{\varpi}_{t}^{\text{T}}&\mathbf{v}_{t}^{\text{T}}\end{bmatrix}^{\text{T}}\text{.
}$ (10)

The vehicle’s kinematic state is defined as:

$\mathbf{x}=\begin{bmatrix}\mathbf{p}^{\text{T}}&v&\bm{\theta}^{\text{T}}&\bm{\omega}^{\text{T}}&\bm{\xi}^{\text{T}}\end{bmatrix}^{\text{T}}\text{,
}$ (11)

where $\mathbf{p}$ represents the 3D position of the vehicle, $v$ is its
speed, $\bm{\theta}$ denotes the pose in terms of a rotation vector,
$\bm{\omega}$ signifies the angular velocity,
$\bm{\xi}=\begin{bmatrix}l&w\end{bmatrix}^{\text{T}}$ represents the length
and width of the vehicle’s bottom. Here, the quaternion form of $\bm{\theta}$
is denoted as $\tilde{\mathbf{q}}=\mathrm{Exp}\left(\bm{\theta}\right)$.

###  III-A The 3D CTRV Motion Model

The 3D CTRV motion model in vehicle-mounted perception [8] assumes that the
vehicle primarily changes in azimuth relative to the sensor, while the pitch
and roll angles are always 0, whereas our research focuses on roadside
perception, where the vehicle’s orientation relative to the sensor includes
unknown azimuth, pitch, and roll angles.

In these contexts, we extend the 2D CTRV model [24] into 3D space by using
quaternion-based descriptions of 3D rotation [21, Eq. 201] to model all three
angles of the vehicle. The time derivative of the 3D CTRV kinematic state is
formulated as:

$\displaystyle\dot{\mathbf{p}}=v\mathbf{d}\text{, }$ (12a)
$\displaystyle\frac{\mathrm{d}\left(v\mathbf{d}\right)}{\mathrm{d}t}=\dot{v}\mathbf{d}+v\dot{\mathbf{d}}\text{,
}$ (12b) $\displaystyle\dot{v}=n_{v}\text{, }$ (12c)
$\displaystyle\tilde{\mathbf{d}}=\tilde{\mathbf{q}}\odot\tilde{\mathbf{u}}^{d}\odot\tilde{\mathbf{q}}^{*}\text{,
}$ (12d)
$\displaystyle\dot{\tilde{\mathbf{q}}}=\frac{1}{2}\tilde{\bm{\omega}}\odot\tilde{\mathbf{q}}\text{,
}$ (12e) $\displaystyle\dot{\bm{\omega}}=\mathbf{n}_{\omega}\text{, }$ (12f)
$\displaystyle\dot{\bm{\xi}}=\mathbf{n}_{\xi}\text{, }$ (12g)

where $\mathbf{d}$ is a unit vector represents the direction of velocity, and
the quaternion form of vectors $\mathbf{u}^{d}$, $\mathbf{d}$ and
$\bm{\omega}$ are represented as $\tilde{\mathbf{u}}^{d}$,
$\tilde{\mathbf{d}}$ and $\tilde{\bm{\omega}}$, respectively. The vector
$\mathbf{u}^{d}=\begin{bmatrix}1&0&0\end{bmatrix}^{\text{T}}$ represents the
unit direction vector of the forward direction of the vehicle in the VCS. The
$n_{v}$, $\mathbf{n}_{\xi}$ and $\mathbf{n}_{\omega}$ are zero-mean Gaussian
noises.

To deal with the nonlinearity introduced by quaternion operations, we employ
the error state filtering [25]. Specifically,
$\mathbf{x}=\mathbf{x}_{ref}\oplus\delta\mathbf{x}$, where $\mathbf{x}_{ref}$
represents a time-varying reference-state without randomness, while
$\delta\mathbf{x}$ is a zero-mean random variable, representing the error-
state. As a result, the vehicle’s motion model described in (12) can be
divided into reference-state kinematics and error-state kinematics.

####  III-A1 Reference-state Kinematics

The motion model of the reference-state can be obtained by replacing the
variables in (12) with reference-states:

$\displaystyle\dot{\mathbf{p}}_{ref}$
$\displaystyle=v_{ref}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}\text{, }$ (13a)
$\displaystyle\dot{v}_{ref}$ $\displaystyle=0\text{, }$ (13b)
$\displaystyle\dot{\tilde{\mathbf{q}}}_{ref}$
$\displaystyle=\frac{1}{2}\tilde{\bm{\omega}}_{ref}\odot\tilde{\mathbf{q}}_{ref}\text{,
}$ (13c) $\displaystyle\dot{\bm{\omega}}_{ref}$ $\displaystyle=0\text{, }$
(13d) $\displaystyle\dot{\bm{\xi}}_{ref}$ $\displaystyle=0\text{. }$ (13e)

The time-variant variable $\mathbf{R}_{ref}$ represents the rotation matrix
form of the vehicle’s pose, where
$\mathbf{R}_{ref}=\mathbf{R}\\{\tilde{\mathbf{q}}_{ref}\\}$. Therefore, an
equivalent form of (13c) can be rewritten in rotation matrix form, as stated
in [21, Eq. 67]

$\dot{\mathbf{R}}_{ref}=\left[\bm{\omega}_{ref}\right]_{\times}\mathbf{R}_{ref}\text{,
}$ (14)

Let us denote the current time step as $k$ and the time interval between two
consecutive steps as $\Delta t$. By integrating (13), we obtain the time
update of the reference-state kinematics as follows:

$\mathbf{x}_{ref,k+1}=f\left(\mathbf{x}_{ref,k}\right)\text{, }$ (15)

with

$\displaystyle\mathbf{p}_{ref,k+1}=$ $\displaystyle\text{
}\mathbf{p}_{ref,k}+v_{ref,k}\Bigg{\\{}\mathbf{I}\Delta
t+\frac{1}{\left\|\bm{\omega}_{ref,k}\right\|}\left[\frac{\bm{\omega}_{ref,k}}{\left\|\bm{\omega}_{ref,k}\right\|}\right]_{\times}$
$\displaystyle\times\left(1-\cos\left(\left\|\bm{\omega}_{ref,k}\right\|\Delta
t\right)\right)+\left[\frac{\bm{\omega}_{ref,k}}{\left\|\bm{\omega}_{ref,k}\right\|}\right]_{\times}^{2}$
$\displaystyle\times\left(\Delta
t-\frac{\sin\left(\left\|\bm{\omega}_{ref,k}\right\|\Delta
t\right)}{\left\|\bm{\omega}_{ref,k}\right\|}\right)\Bigg{\\}}\cdot\mathbf{R}\\{\tilde{\mathbf{q}}_{ref,k}\\}\mathbf{u}^{d}\text{,
}$ (16a) $\displaystyle v_{ref,k+1}$ $\displaystyle=v_{ref,k}\text{, }$ (16b)
$\displaystyle\tilde{\mathbf{q}}_{ref,k+1}$
$\displaystyle=\mathrm{Exp}\left(\bm{\omega}_{ref,k}\Delta
t\right)\odot\tilde{\mathbf{q}}_{ref,k}\text{, }$ (16c)
$\displaystyle\bm{\theta}_{ref,k+1}$
$\displaystyle=\mathrm{Log}\left(\tilde{\mathbf{q}}_{ref,k+1}\right)\text{, }$
(16d) $\displaystyle\bm{\omega}_{ref,k+1}$
$\displaystyle=\bm{\omega}_{ref,k}\text{, }$ (16e)
$\displaystyle\bm{\xi}_{ref,k+1}$ $\displaystyle=\bm{\xi}_{ref,k}\text{, }$
(16f)

The derivation for obtaining (16) is in Appendix A-A.

####  III-A2 Error-state Kinematics

The error-state motion model is derived by subtracting (13) from (12). To
obtain a closed form expression for the predicted object state, we linearize
the error-state kinematics using Taylor series and omitting second-order small
quantity. This gives:

$\dot{\delta\mathbf{x}}\approx\mathbf{F}\delta\mathbf{x}+\mathbf{w}\text{, }$
(17)

where

$\displaystyle\delta\mathbf{x}=\begin{bmatrix}\delta\mathbf{p}\\\ \delta v\\\
\delta\bm{\theta}\\\ \delta\bm{\omega}\\\ \delta\bm{\xi}\end{bmatrix}\text{ ,
}\mathbf{F}=\begin{bmatrix}0&\mathbf{M}_{0}&\mathbf{M}_{1}&0&0\\\ 0&0&0&0&0\\\
0&0&\mathbf{M}_{2}&\mathbf{I}&0\\\ 0&0&0&0&0\\\ 0&0&0&0&0\end{bmatrix}\text{ ,
}\mathbf{w}=\begin{bmatrix}0\\\ n_{v}\\\ 0\\\ \mathbf{n}_{\omega}\\\
\mathbf{n}_{\xi}\end{bmatrix}\text{, }$
$\displaystyle\mathbf{M}_{0}=\mathbf{R}_{ref}\mathbf{u}^{d}\text{ ,
}\mathbf{M}_{1}=-v_{ref}[\mathbf{R}_{ref}\mathbf{u}^{d}]_{\times}\text{ ,
}\mathbf{M}_{2}=[\bm{\omega}_{ref}]_{\times}\text{, }$ (18)

where $\mathbf{w}\sim\mathcal{N}\left(0,\mathbf{W}\right)$ represents a zero-
mean Gaussian noise with covariance $\mathbf{W}$. The derivation for obtaining
(17) is in Appendix A-B1.

By integrating (17), we obtain the time update of the error-state kinematics:

$\displaystyle\delta\mathbf{x}_{k+1}$
$\displaystyle=\Phi_{k}\delta\mathbf{x}_{k}+\mathbf{w}_{k}\text{, }$ (19a)
$\displaystyle\delta\mathbf{x}_{0}$
$\displaystyle\sim\mathcal{N}\left(0,\hat{\mathbf{P}}_{0|0}\right)\text{, }$
(19b)

where $\mathbf{w}_{k}\sim\mathcal{N}\left(0,\mathbf{W}\Delta t\right)$,
$\hat{\mathbf{P}}_{0|0}$ is the initialized error-state covariance matrix and
$\Phi_{k}$ is the transition matrix, with the closed form expression:

$\Phi=\begin{bmatrix}\mathbf{I}&\mathbf{M}_{0}\Delta
t&\mathbf{M}_{1}\Sigma_{1}&\mathbf{M}_{1}\Sigma_{2}&0\\\ 0&\mathbf{I}&0&0&0\\\
0&0&\Sigma_{0}&\Sigma_{1}&0\\\ 0&0&0&\mathbf{I}&0\\\
0&0&0&0&\mathbf{I}\end{bmatrix}\text{, }$ (20)

where

$\displaystyle\Sigma_{0}=$
$\displaystyle\exp\left(\left[\bm{\omega}_{ref}\Delta
t\right]_{\times}\right)\text{, }$ (21a) $\displaystyle\Sigma_{1}=$
$\displaystyle\text{ }\mathbf{I}\Delta
t-\frac{\mathbf{M}_{2}}{\|\bm{\omega}_{ref}\|^{2}}\left(\Sigma_{0}-\mathbf{I}-\mathbf{M}_{2}\Delta
t\right)\text{, }$ (21b) $\displaystyle\Sigma_{2}=$ $\displaystyle\text{
}\frac{1}{2}\mathbf{I}\Delta
t^{2}-\frac{1}{\|\bm{\omega}_{ref}\|^{2}}\left(\Sigma_{0}-\mathbf{I}-\mathbf{M}_{2}\Delta
t-\frac{1}{2}\mathbf{M}_{2}^{2}\Delta t^{2}\right)\text{. }$ (21c)

The details of deriving (19) to (21) are given in Appendix A-B2.

###  III-B The SGW-GMM Radar Measurement Model

Let $\mathbf{Z}^{r}=\\{\mathbf{z}_{i}^{r}\\}_{i=1}^{n^{r}}$ be the set of 3D
position measurements from radar. Each measurement is generated from one of
the $T$ reflectors based on probabilities $\bm{\pi}=\\{\pi_{t}\\}_{t=1}^{T}$,
with $\pi_{t}$ representing the probability associated with the $t$-th
reflector. We only consider the case where there is a single object without
generating any clutter measurements, and the radar measurement model can be
expressed as:

$\mathbf{z}^{r}_{i}=\mathbf{p}+\mathbf{w}^{r}_{i}\text{, }$ (22)

where $\mathbf{w}^{r}_{i}$ is the measurement noise, and its condition
probability density can be expressed as Gaussian mixture form:

$p\left(\mathbf{w}^{r}_{i}\mid\bm{\pi},\mathbf{u},\mathbf{x}\right)=\sum_{t=1}^{T}\pi_{t}\mathcal{N}\left(\mathbf{w}^{r}_{i};\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{u}_{t},\mathbf{Q}\right)\text{,
}$ (23)

where $\mathbf{Q}$ denotes the radar measurement noise covariance.

(a) Histograms of radar measurements (b) SGW-GMM model

Figure 2: Histograms of radar measurements and probability density functions
of the SGW-GMM radar measurement model $(\lambda=1)$. For demonstration, the
positions of skeleton knots and radar reflectors are assumed to be identical,
denoted by knots connected with lines, while the observation directions are
indicated by red arrows. It is important to note that our model does not
constrain the positions of skeleton nodes and radar reflectors to be the same.

Different from how the weights in (23) are obtained in existing methods [26,
18, 19, 20], in this paper we propose a SGW-GMM measurement model that
utilizes a spherical Gaussian function [27] to weight the components,
assigning higher weights to the components near the sensor direction and lower
weights to those further away from the sensor direction:

$\pi_{t}\propto\exp\left(\lambda\times\left(-\frac{\mathbf{p}^{\text{T}}\cdot\left(\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{u}_{t}\right)}{\|\mathbf{p}\|\|\mathbf{u}_{t}\|}-1\right)\right)\text{,
}$ (24)

where the parameter $\lambda>0$ represents the lobe sharpness, with a larger
value of $\lambda$ indicating a more concentrated distribution of the
described point cloud. For ease of calculation, we assume that $\pi_{t}$ is
noiseless, and its value is calculated according to the vehicle’s reference-
state.

An illustration of the SGW-GMM measurement model for a car from rear-upper and
front-upper observation directions is presented in Fig. 2. It is evident that
the SGW-GMM model accurately captures the actual point cloud
distribution.111Note that due to multipath effects in millimeter wave
propagation, radar measurements may appear in positions where radar reflectors
do not physically exist. Therefore, as shown in Fig. 2b, the weights of
negative side are non-zero, which means that occluded radar reflectors have
relatively low probabilities of generating measurements, which helps describe
radar measurements attributed to the multipath effect. Furthermore, this model
is also suitable for 3D EOT with sparse point clouds, because the vehicle’s
shape, represented by the positions of its skeleton knots, is partly
supervised by the pixel keypoints detected through the vision algorithm, as
demonstrated in Section III-C.

###  III-C The Pixel Keypoints Measurement Model

Let $\mathbf{Z}^{c}=\\{\mathbf{z}_{i}^{c}\\}_{i=1}^{n^{c}}$ be the set of 2D
visual measurements obtained by the YOLOv8 pixel keypoints detection algorithm
[28]. The measurements are categorized into two parts: measurements of the
vehicle’s skeleton knots $(\mathbf{Z}^{c,b})$ and measurements of the corner
points of the vehicle’s bottom
$(\mathbf{Z}^{c,g}=\mathbf{Z}^{c}\setminus\mathbf{Z}^{c,b})$, as shown in Fig.
3. These two parts are modeled separately:

####  III-C1 Modeling Skeleton Knot Measurements

We denote the $i$-th pixel measurement of the vehicle’s skeleton knots from
the $s_{i}^{b}$-th component of the skeleton as
$\mathbf{z}^{c,b}_{i}\in\mathbf{Z}^{c,b}$, which is modeled based on the
vehicle’s kinematic state and the position of the knot as follows:

$\mathbf{z}^{c,b}_{i}=h^{c,b}\left(\mathbf{x},\bm{\varpi}_{s_{i}^{b}}\right)+\mathbf{w}^{c,b}_{s_{i}^{b}}\text{,
}$ (25)

where
$\mathbf{w}^{c,b}_{s_{i}^{b}}\sim\mathcal{N}\left(0,\mathbf{Q}_{s_{i}^{b}}^{c,b}\right)$
is the measurement noise. The nonlinear function $h^{c,b}()$ maps the
coordinates of the knot $\bm{\varpi}_{s_{i}^{b}}$ from the VCS to the PCS
using (8) and (9), and the corresponding coordinates in the SCS and PCS are
$\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}$ and
$\bm{\varpi}_{s_{i}^{b}}^{\mathcal{I}}$, respectively.

To linearize the model of skeleton knot measurements, we expand the function
$h^{c,b}()$ around
$\bm{\varphi}_{s_{i}^{b}}=\mathbb{E}\left[\bm{\varpi}_{s_{i}^{b}}\right]$ and
$\mathbf{x}=\mathbf{x}_{ref}$ using the Taylor series and neglect the
quadratic or higher-order terms:

$\displaystyle\mathbf{z}^{c,b}_{i}\approx$ $\displaystyle\text{
}h^{c,b}\left(\mathbf{x}_{ref},\bm{\varphi}_{s_{i}^{b}}\right)+\frac{\partial
h^{c,b}}{\partial\delta\mathbf{x}}\bigg{|}_{\mathbf{x}=\mathbf{x}_{ref},\bm{\varpi}_{s_{i}^{b}}=\bm{\varphi}_{s_{i}^{b}}}\delta\mathbf{x}$
$\displaystyle+\frac{\partial
h^{c,b}}{\partial\bm{\varpi}_{s_{i}^{b}}}\bigg{|}_{\mathbf{x}=\mathbf{x}_{ref},\bm{\varpi}_{s_{i}^{b}}=\bm{\varphi}_{s_{i}^{b}}}\left(\bm{\varpi}_{s_{i}^{b}}-\bm{\varphi}_{s_{i}^{b}}\right)+\mathbf{w}^{c,b}_{s_{i}^{b}}\text{,
}$ $\displaystyle=$ $\displaystyle\text{
}h^{c,b}\left(\mathbf{x}_{ref},\bm{\varphi}_{s_{i}^{b}}\right)+\mathbf{H}^{c,b}_{\mathbf{x}}\delta\mathbf{x}$
$\displaystyle+\mathbf{H}^{c,b}_{\bm{\varpi}_{s_{i}^{b}}}\left(\bm{\varpi}_{s_{i}^{b}}-\bm{\varphi}_{s_{i}^{b}}\right)+\mathbf{w}^{c,b}_{s_{i}^{b}}\text{,
}$ (26)

where

$\mathbf{H}^{c,b}_{\mathbf{x}}=\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{I}}}{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}{\partial\delta\mathbf{x}}\text{
,
}\mathbf{H}^{c,b}_{\bm{\varpi}_{s_{i}^{b}}}=\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{I}}}{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}{\partial\bm{\varpi}_{s_{i}^{b}}}\text{.
}$ (27)

The common part of the two equations in (27) is acquired by taking the
derivative of (9):

$\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{I}}}{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}=\begin{bmatrix}\frac{f/d_{x}}{\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[3]}&0&-\frac{\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[1]\cdot
f/d_{x}}{\left(\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[3]\right)^{2}}\\\
0&\frac{f/d_{y}}{\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[3]}&-\frac{\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[2]\cdot
f/d_{y}}{\left(\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[3]\right)^{2}}\end{bmatrix}\text{,
}$ (28)

where $\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}[i]$ represents the $i$-th element
of the vector $\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}$. According to the
derivation of (8), we have that

$\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}{\partial\delta\mathbf{x}}=\left[\mathbf{I}_{3\times
3},\mathbf{0},\frac{\partial\left(\mathbf{R}\\{\bm{\theta}\\}\cdot\bm{\varpi}_{t}\right)}{\partial\delta\bm{\theta}},\mathbf{0},\mathbf{0}\right]\text{
,
}\frac{\partial\bm{\varpi}_{s_{i}^{b}}^{\mathcal{S}}}{\partial\bm{\varpi}_{s_{i}^{b}}}=\mathbf{R}\\{\bm{\theta}\\}\text{,
}$ (29)

where, according to [21, Eq. 188], it holds that:

$\frac{\partial(\mathbf{R}\\{\bm{\theta}\\}\cdot\bm{\varpi}_{s_{i}^{b}})}{\partial\delta\bm{\theta}}\bigg{|}_{\bm{\theta}=\bm{\theta}_{ref}}=-[\mathbf{R}\\{\bm{\theta}_{ref}\\}\bm{\varpi}_{s_{i}^{b}}]_{\times}\mathbf{J}_{l}(\bm{\theta}_{ref})\text{,
}$ (30) $\displaystyle\mathbf{J}_{l}(\bm{\theta}_{ref})=$ $\displaystyle\text{
}\mathbf{I}+\frac{1-\cos\|\bm{\theta}_{ref}\|}{\|\bm{\theta}_{ref}\|^{2}}[\bm{\theta}_{ref}]_{\times}$
$\displaystyle+\frac{\|\bm{\theta}_{ref}\|-\sin\|\bm{\theta}_{ref}\|}{\|\bm{\theta}_{ref}\|^{3}}[\bm{\theta}_{ref}]_{\times}^{2}\text{.
}$ (31)

####  III-C2 Modeling Bottom Corner Point Measurements

We denote the $i$-th pixel measurement of the vehicle’s bottom corner points
from the $s_{i}^{g}$-th corner point of the vehicle’s bottom as
$\mathbf{z}^{c,g}_{i}\in\mathbf{Z}^{c,g}$, which is modeled based on the
vehicle’s kinematic state as follows:

$\mathbf{z}^{c,g}_{i}=h_{s_{i}^{g}}^{c,g}\left(\mathbf{x}\right)+\mathbf{w}^{c,g}_{s_{i}^{g}}\text{,
}$ (32)

where
$\mathbf{w}^{c,g}_{s_{i}^{g}}\sim\mathcal{N}\left(0,\mathbf{Q}_{s_{i}^{g}}^{c,g}\right)$
is the corresponding noise, and $h_{s_{i}^{g}}^{c,g}()$ is a nonlinear
function that maps the coordinates of vehicle’s bottom from the VCS to the
PCS.

Let the coordinates of the $s_{i}^{g}$-th corner points in the VCS be
$\bm{\xi}_{s_{i}^{g}}^{\mathcal{V}}=\mathbf{G}_{s_{i}^{g}}\cdot\bm{\xi}$,
where $\mathbf{G}_{s_{i}^{g}}$ represents the relative position of the corner
point, given by:

$\mathbf{G}_{s_{i}^{g}}=\begin{bmatrix}\pm\frac{1}{2}&0\\\ 0&\pm\frac{1}{2}\\\
0&0\end{bmatrix}\text{ , }s_{i}^{g}\in\\{1,2,3,4\\}\text{. }$ (33)

As a result, the output of function $h_{s_{i}^{g}}^{c,g}\left(\right)$ is
$\bm{\xi}_{s_{i}^{g}}^{\mathcal{I}}$ by using (8) and (9), and the coordinate
in SCS is expressed as $\bm{\xi}_{s_{i}^{g}}^{\mathcal{S}}$. Similar to
(III-C1), for convenient calculation, we linearize (32) using the first-order
Taylor series approximation:

$\displaystyle\mathbf{z}^{c,g}_{i}$ $\displaystyle\approx
h_{s_{i}^{g}}^{c,g}\left(\mathbf{x}_{ref}\right)+\frac{\partial
h_{s_{i}^{g}}^{c,g}}{\partial\delta\mathbf{x}}\bigg{|}_{\mathbf{x}=\mathbf{x}_{ref}}\delta\mathbf{x}+\mathbf{w}^{c,g}_{s_{i}^{g}}$
$\displaystyle=h_{s_{i}^{g}}^{c,g}\left(\mathbf{x}_{ref}\right)+\mathbf{H}^{c,g}_{\mathbf{x},s_{i}^{g}}\delta\mathbf{x}+\mathbf{w}^{c,g}_{s_{i}^{g}}\text{,
}$ (34)

where

$\mathbf{H}^{c,g}_{\mathbf{x},s_{i}^{g}}=\frac{\partial\bm{\xi}_{s_{i}^{g}}^{\mathcal{I}}}{\partial\bm{\xi}_{s_{i}^{g}}^{\mathcal{S}}}\frac{\partial\bm{\xi}_{s_{i}^{g}}^{\mathcal{S}}}{\partial\delta\mathbf{x}}\text{.
}$ (35)

The first quotient of the above equation has the same form as (28), while the
second quotient is:

$\frac{\partial\bm{\xi}_{s_{i}^{g}}^{\mathcal{S}}}{\partial\delta\mathbf{x}}=\begin{bmatrix}\mathbf{I}_{3\times
3}&\mathbf{0}&\frac{\partial(\mathbf{R}\\{\bm{\theta}\\}\cdot\bm{\xi}_{s_{i}^{g}}^{\mathcal{V}})}{\partial\delta\bm{\theta}}&\mathbf{0}&\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{G}_{s_{i}^{g}}\end{bmatrix}\text{.
}$ (36)

Figure 3: Illustration of car’s 24 skeleton knots and 4 corner points.

###  III-D The Pseudo-measurement Models

The pseudo-measurement models are introduced to process equality constraints
between certain state variables [29]. In this paper, we establish three
constraints: the angular velocity constraint, the ground motion constraint,
and the knot symmetry constraint.

####  III-D1 Angular Velocity Constraint

Considering that the vehicle seldom involves roll rotation, which means that
$\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{u}^{d}$ and $\bm{\omega}$ are
approximately perpendicular. Therefore, the pseudo-measurement model can be
expressed as:

$0=\left(\mathbf{R}\\{\bm{\theta}\\}\mathbf{u}^{d}\right)^{\text{T}}\bm{\omega}+w^{rot}\text{,
}$ (37)

where $w^{rot}\sim\mathcal{N}\left(0,\mathbf{Q}^{rot}\right)$. We linearize
the above model according to (30), which yields

$\displaystyle 0=$
$\displaystyle\left(\mathbf{R}\\{\bm{\theta}_{ref}\\}\mathbf{u}^{d}\right)^{\text{T}}\bm{\omega}_{ref}+\left(\mathbf{R}\\{\bm{\theta}_{ref}\\}\mathbf{u}^{d}\right)^{\text{T}}\delta\bm{\omega}$
$\displaystyle-\omega_{ref}^{\text{T}}\left[\mathbf{R}\\{\bm{\theta}_{ref}\\}\mathbf{u}^{d}\right]_{\times}\mathbf{J}_{l}\left(\bm{\theta}_{ref}\right)\delta\bm{\theta}+w^{rot}\text{.
}$ (38)

####  III-D2 Ground Motion Constraint

Without loss of generality, we assume that the corner points of the vehicle’s
bottom are on the ground, and the ground equation is represented as:

$\mathbf{n}^{\text{T}}\bm{\beta}+d=0\text{, }$ (39)

where $\mathbf{n}$ represents the unit normal vector of the ground plane, and
$\bm{\beta}$ represents any point on the ground. Then the pseudo-measurement
model of the $i$-th ($i\in\left[1,4\right]$) corner is:

$0=\mathbf{n}^{\text{T}}\left(\mathbf{p}+\mathbf{R}\\{\bm{\theta}\\}\cdot\mathbf{G}_{i}\cdot\bm{\xi}\right)+d+w^{grnd}\text{,
}$ (40)

where $w^{grnd}\sim\mathcal{N}\left(0,\mathbf{Q}^{grnd}\right)$. Linearizing
according to the (30) gives:

$\displaystyle 0=$ $\displaystyle\text{
}\mathbf{n}^{\text{T}}\left(\mathbf{p}_{ref}+\mathbf{R}\\{\bm{\theta}_{ref}\\}\cdot\mathbf{G}_{i}\cdot\bm{\xi}_{ref}\right)$
$\displaystyle+\mathbf{n}^{\text{T}}\Big{\\{}\delta\mathbf{p}+[\mathbf{J}_{l}(\bm{\theta}_{ref})\delta\bm{\theta}]_{\times}\mathbf{R}\\{\bm{\theta}_{ref}\\}\mathbf{G}_{i}\cdot\bm{\xi}_{ref}$
$\displaystyle+\mathbf{R}\\{\bm{\theta}_{ref}\\}\cdot\mathbf{G}_{i}\cdot\delta\bm{\xi}\Big{\\}}+d+w^{grnd}\text{.
}$ (41)

####  III-D3 Knot Symmetry Constraint

To infer the positions of the invisible knots from the visible knots, we
leverage the left-right symmetric characteristic of the knots. Assuming the
predefined symmetric knot for $\bm{\varpi}_{t}$ is $\bm{\varpi}^{sym}_{t}$,
which holds that $\left(\bm{\varpi}^{sym}_{t}\right)^{sym}=\bm{\varpi}_{t}$,
the pseudo-measurement model is:

$\bm{\varpi}^{sym}_{t}=\mathbf{D}\cdot\bm{\varpi}_{t}+\mathbf{w}^{sym}_{t}\text{,
}$ (42)

where $\mathbf{D}=diag\left(1,-1,1\right)$ is a diagonal matrix representing
symmetric transformation, and
$\mathbf{w}^{sym}_{t}\sim\mathcal{N}\left(0,\mathbf{Q}^{sym}\right)$.

###  III-E The ES Fusion Mechanism

Figure 4: Demonstration of the ES fusion mechanism. It is worth noting that
while this mechanism enhances the accuracy of the vehicle’s central position,
it may also distort the shape of the vehicle’s extent. However, during the
updating stage, the extent is rectified by visual measurements.

Based on the Bayesian framework, the measurement models update specific
components of the object’s state. For instance, the radar measurement model
updates the coordinates of radar reflectors, while the pixel keypoints
measurement model and the pseudo-measurement model of the ground plane update
the coordinates of skeleton knots, along with the vehicle’s length and width.

However, since radar and visual measurements are independent, the updated
positions of radar reflectors lack correlation with the skeleton knots. While
the positions of radar reflectors can effectively describe the object’s
position, they may not accurately represent its extent. Conversely, the
vehicle skeleton accurately describes the object’s extent but suffers from
reduced position accuracy, particularly at longer distances.

To utilize the position information of radar reflectors, we introduce an ES
fusion mechanism. This mechanism, as shown in Fig. 4, leverages positions of
radar reflectors and accurate extent information from pixel keypoints by
constructing a spring-damping system that allows radar reflectors to pull the
vehicle’s extent to the correct position.

Assuming that there is a virtual spring connection with a natural length of 0
and an elastic coefficient of $\epsilon$ between each pair of reflectors and
knots, the virtual mass of the knot is considered to be 1. To prevent system
oscillation, damping with coefficient $\rho$ is introduced [30]. Based on
these assumptions, the skeleton knot is pulled by spring force from the
reflector, and the dynamic model of the $t$-th component of the skeleton is
described as follows:

$\dot{\bm{\vartheta}}_{t}=\mathbf{F}_{\vartheta}\bm{\vartheta}_{t}+\mathbf{w}^{\vartheta}\text{,
}$ (43)

where

$\mathbf{F}_{\vartheta}=\begin{bmatrix}0&0&0\\\ 0&0&\mathbf{I}\\\
\epsilon\mathbf{I}&-\epsilon\mathbf{I}&-\rho\mathbf{I}\end{bmatrix}\text{ ,
}\mathbf{w}_{\vartheta}=\begin{bmatrix}\mathbf{n}_{u}\\\ \mathbf{0}\\\
\mathbf{n}_{a}\end{bmatrix}\text{, }$ (44)

with
$\mathbf{w}_{\vartheta}\sim\mathcal{N}\left(0,\mathbf{W}_{\vartheta}\right)$,
and $\mathbf{n}_{u}$, $\mathbf{n}_{a}$ denoting the position error of the
reflector and the acceleration error of the knot, respectively.

We can observe that the form of (43) is similar to the transition model,
described in (12). Hence, the influence of the ES fusion mechanism is
reflected in the system’s time update stage, and subsequently the time update
for the vehicle’s shape is obtained by integrating (43):

$\displaystyle\bm{\vartheta}_{k+1,t}$
$\displaystyle=\Phi_{\vartheta}\bm{\vartheta}_{k,t}+\mathbf{w}_{k}^{\vartheta}\text{,
}$ (45a) $\displaystyle\bm{\vartheta}_{0,t}$
$\displaystyle\sim\mathcal{N}\left(\hat{\bm{\mu}}_{0|0,t},\hat{\bm{\Sigma}}_{0|0,t}\right)\text{,
}$ (45b)

where
$\mathbf{w}_{k}^{\vartheta}\sim\mathcal{N}\left(0,\mathbf{W}_{\vartheta}\Delta
t\right)$, the $\hat{\bm{\mu}}_{0|0,t}$ and $\hat{\bm{\Sigma}}_{0|0,t}$ are
the initialized mean and covariance matrix of the $t$-th knot, respectively.
The $\Phi_{\vartheta}$ is the transition matrix, with the closed form
expression:

$\Phi_{\vartheta}=\begin{bmatrix}\mathbf{I}&\mathbf{0}&\mathbf{0}\\\
-\mathbf{M}_{3}&\mathbf{I}+\mathbf{M}_{3}&\mathbf{M}_{4}\\\
\epsilon\mathbf{M}_{4}&-\epsilon\mathbf{M}_{4}&\mathbf{I}+\mathbf{M}_{3}-\rho\mathbf{M}_{4}\end{bmatrix}\text{,
}$ (46)

where

$\displaystyle\mathbf{M}_{3}=$
$\displaystyle\left\\{-1+\exp(-\frac{\rho}{2\Delta
t})\cosh\left(\frac{1}{2}\sqrt{-4\epsilon+\rho^{2}}\Delta t\right)\right.$
$\displaystyle+\left.\frac{\exp(-\frac{\rho}{2\Delta
t})\rho}{\sqrt{-4\epsilon+\rho^{2}}}\sinh\left(\frac{1}{2}\sqrt{-4\epsilon+\rho^{2}}\Delta
t\right)\right\\}\mathbf{I}\text{, }$ (47a) $\displaystyle\mathbf{M}_{4}=$
$\displaystyle\frac{2\exp(-\frac{\rho}{2\Delta
t})\sinh\left(\frac{1}{2}\sqrt{-4\epsilon+\rho^{2}}\Delta
t\right)}{\sqrt{-4\epsilon+\rho^{2}}}\mathbf{I}\text{. }$ (47b)

The details of deriving (45) can be found in Appendix B.

##  IV Inference via Variational Bayesian Approach

We provide a conceptual overview before delving into the technical details.
Specifically, the recursive Bayesian object state estimation relies on assumed
density filtering, where in this work we assume that both the predicted and
updated object’s states follow Gaussian distributions. Here, the object’s
state at time $k-1$ is denoted as
$\Theta_{k-1}=\\{\mathbf{x}_{k-1},\bm{\vartheta}_{k-1}\\}$, where
$\bm{\vartheta}_{k-1}=\\{\bm{\vartheta}_{k-1,t}\\}_{t=1}^{T}$.

We also let
$\mathbf{Z}^{k-1}=\\{\mathbf{Z}^{r}_{1},\mathbf{Z}^{c}_{1},\dots,\mathbf{Z}^{r}_{k-1},\mathbf{Z}^{c}_{k-1}\\}$
represent the radar and visual measurements up to and including time step
$k-1$. We assume that the posterior density at time $k-1$ is of the form:

$\displaystyle p$ $\displaystyle\left(\Theta_{k-1}|\mathbf{Z}^{k-1}\right)$
$\displaystyle=$ $\displaystyle\text{
}\mathcal{N}\left(\mathbf{x}_{k-1};\hat{\mathbf{x}}_{k-1|k-1},\hat{\mathbf{P}}_{k-1|k-1}\right)$
$\displaystyle\times\prod_{t=1}^{T}\mathcal{N}\left(\bm{\vartheta}_{k-1,t};\hat{\bm{\mu}}_{k-1|k-1,t},\hat{\bm{\Sigma}}_{k-1|k-1,t}\right)\text{.
}$ (48)

In this section, we show that the predicted density for the motion model
introduced in Section III-A is also of the form (IV) without approximation.
However, given the measurement model introduced in Section III-B to III-D, the
posterior density at time step $k$ has no closed-form solution. To enable
recursive Bayesian filtering, we use variational inference to approximate the
posterior such that the approximate density is also of the form (IV). To
simplify the derivation and present concise expressions, we introduce the
following lemmas and symbols. For detailed proofs of these lemmas, please
refer to Appendix C.

######  Lemma 1.

For a 3D vector $\mathbf{v}$ and a $3\times n$ matrix $\mathbf{M}$, the
following equation exists:

$\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}=-\left[\mathbf{M}\right]_{*}diag_{n}\left(\mathbf{v}\right)\text{,
}$ (49)

where the symbol $\left[\cdot\right]_{\ast}$ represents the splicing for
cross-product matrices for every column $(\mathbf{M}_{(i)},i=1,\dots,n)$ of
the matrix:

$\left[\mathbf{M}\right]_{\ast}=\left[\begin{matrix}\left[\mathbf{M}_{(1)}\right]_{\times}&,\dots,&\left[\mathbf{M}_{(n)}\right]_{\times}\end{matrix}\right]\text{,
}$ (50)

and the notation $diag_{n}(\cdot)$ represents the expansion of the vector $n$
times in a diagonal manner.

######  Lemma 2.

For two 3D vectors $\mathbf{v}$ and $\mathbf{t}$, and a $3\times 3$ matrix
$\mathbf{M}$, the following equation exists:

$\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}\cdot\mathbf{t}=-\left[\mathbf{M}\right]_{*}\left[\mathbf{v}\cdot\mathbf{t}^{\text{T}}\right]_{\star}\text{,
}$ (51)

where the symbol $\left[\cdot\right]_{\star}$ represents the recombination of
the matrix into a column vector using row-major order.

######  Lemma 3.

For two 3D vectors $\mathbf{v}$ and $\mathbf{t}$, and a $3\times 3$ symmetric
positive definite (SPD) matrix $\mathbf{M}$, the following equation exists:

$\displaystyle\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}\cdot\left[\mathbf{t}\right]_{\times}^{\text{T}}=$
$\displaystyle\left[\mathbf{v}\right]_{\times}^{\text{T}}\cdot\mathbf{M}\cdot\left[\mathbf{t}\right]_{\times}$
$\displaystyle=$
$\displaystyle\left[\mathbf{L}\right]_{\ast}diag_{n}\left(\mathbf{v}\cdot\mathbf{t}^{\text{T}}\right)\left[\mathbf{L}\right]_{\ast}^{\text{T}}\text{,
}$ (52)

where the cholesky decomposition is
$\mathbf{M}=\mathbf{L}\mathbf{L}^{\text{T}}$, and the $\mathbf{L}$ is also
denoted as $\mathbf{L}=\sqrt{\mathbf{M}}$.

###  IV-A Time Update

The time update for the kinematic state is divided into the reference-state
time update and the error-state time update, as shown in (15) and (19):

$\displaystyle\mathbf{x}_{k}$
$\displaystyle=\mathbf{x}_{ref,k}+\delta\mathbf{x}_{k}$
$\displaystyle=f\left(\mathbf{x}_{ref,k-1}\right)+\Phi_{k-1}\delta\mathbf{x}_{k-1}+\mathbf{w}_{k-1}\text{.
}$ (53)

The resulting predicted density is:

$p\left(\mathbf{x}_{k}|\mathbf{Z}^{k-1}\right)=\mathcal{N}\left(\mathbf{x}_{k};\hat{\mathbf{x}}_{k|k-1},\hat{\mathbf{P}}_{k|k-1}\right)\text{,
}$ (54)

where

$\displaystyle\hat{\mathbf{x}}_{k|k-1}=$ $\displaystyle\text{
}f\left(\hat{\mathbf{x}}_{k-1|k-1}\right)\text{, }$ (55a)
$\displaystyle\hat{\mathbf{P}}_{k|k-1}=$ $\displaystyle\text{
}\Phi_{k}\hat{\mathbf{P}}_{k-1|k-1}\Phi_{k}^{\text{T}}+\mathbf{W}\Delta
t\text{. }$ (55b)

According to (45), the time update for the vehicle’s shape is

$\bm{\vartheta}_{k,t}=\Phi_{\vartheta}\bm{\vartheta}_{k-1,t}+\mathbf{w}_{k-1}^{\vartheta}\text{.
}$ (56)

The corresponding predicted density is then

$p\left(\bm{\vartheta}_{k,t}|\mathbf{Z}^{k-1}\right)=\mathcal{N}\left(\bm{\vartheta}_{k,t};\hat{\bm{\mu}}_{k|k-1,t},\hat{\bm{\Sigma}}_{k|k-1,t}\right)\text{.
}$ (57)

where

$\displaystyle\hat{\bm{\mu}}_{k|k-1,t}=$ $\displaystyle\text{
}\Phi_{\vartheta}\hat{\bm{\mu}}_{k-1|k-1,t}\text{, }$ (58a)
$\displaystyle\hat{\bm{\Sigma}}_{k|k-1,t}=$ $\displaystyle\text{
}\Phi_{\vartheta}\hat{\bm{\Sigma}}_{k-1|k-1,t}\Phi_{\vartheta}^{\text{T}}+\mathbf{W}_{\vartheta}\Delta
t\text{. }$ (58b)

Hence, the predicted joint probability density is:

$\displaystyle p$ $\displaystyle\left(\Theta_{k}|\mathbf{Z}^{k-1}\right)$
$\displaystyle=$ $\displaystyle\text{
}\mathcal{N}\left(\mathbf{x}_{k};\hat{\mathbf{x}}_{k|k-1},\hat{\mathbf{P}}_{k|k-1}\right)$
$\displaystyle\times\prod_{t=1}^{T}\mathcal{N}\left(\bm{\vartheta}_{k,t};\hat{\bm{\mu}}_{k|k-1,t},\hat{\bm{\Sigma}}_{k|k-1,t}\right)\text{.
}$ (59)

###  IV-B Likelihood Function

The likelihood function consists of three parts corresponding to the three
measurement models: the SGW-GMM radar measurement model, the keypoints
measurement model, and the pseudo-measurement models.

####  IV-B1 The Radar Likelihood Function

The likelihood function of the radar measurements is obtained by multiplying
the likelihood (23) of individual radar measurements:

$\displaystyle p(\mathbf{Z}_{k}^{r}|\Theta_{k})$
$\displaystyle=\prod_{i=1}^{n^{r}_{k}}\sum_{t=1}^{T}\pi_{k,t}\mathcal{N}\left(\mathbf{z}_{k,i}^{r};\bm{\zeta}_{k,t},\mathbf{Q}\right)\text{,
}$ (60)

where

$\displaystyle\bm{\zeta}_{k,t}$
$\displaystyle=\mathbf{p}_{k}+\mathbf{R}_{k}\mathbf{u}_{k,t}+[\mathbf{J}_{k}\delta\bm{\theta}_{k}]_{\times}\mathbf{R}_{k}\mathbf{u}_{k,t}\text{,
}$ (61a) $\displaystyle\mathbf{R}_{k}$
$\displaystyle=\mathbf{R}\\{\bm{\theta}_{ref,k}\\}\text{, }$ (61b)
$\displaystyle\mathbf{J}_{k}$
$\displaystyle=\mathbf{J}_{l}(\bm{\theta}_{ref,k})\text{. }$ (61c)

Then we expand (60), and introduce a latent variable
$A_{k}=\\{\\{a_{it}\\}_{i=1}^{n^{r}_{k}}\\}_{t=1}^{T}$, which represent the
data associations between measurements and radar reflectors, expressed as:

$a_{it}=\left\\{\begin{array}[]{l
l}{{1,}}&{{\mathrm{if~{}the~{}}t^{\text{th}}\text{ reflector generates
}\mathbf{z}_{k,i}^{r}}}\\\ {{0,}}&{{\mathrm{otherwise.}}}\end{array}\right.$
(62)

According to [31], the equivalent expression for (60) is:

$p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})=\prod_{t=1}^{T}\prod_{i=1}^{n^{r}_{k}}\left[\mathcal{N}\left(\mathbf{z}_{k,i}^{r};\bm{\zeta}_{k,t},\mathbf{Q}\right)\right]^{a_{it}}\text{.
}$ (63)

The conditional probability density of $A_{k}$ is:

$p(A_{k}|\Theta_{k})=\prod_{t=1}^{T}\prod_{i=1}^{n^{r}_{k}}(\pi_{k,t})^{a_{it}}\text{,
}$ (64)

where $\pi_{k,t}$ can be calculated according to (24), and normalization is
required to ensure that $\sum_{t=1}^{T}\pi_{k,t}=1$.

####  IV-B2 The Keypoints Likelihood Function

$\displaystyle p(\mathbf{Z}_{k}^{c}|\Theta_{k})=$ $\displaystyle\text{
}p(\mathbf{Z}_{k}^{c,b}|\Theta_{k})\cdot p(\mathbf{Z}_{k}^{c,g}|\Theta_{k})$
$\displaystyle=$
$\displaystyle\prod_{\mathbf{z}^{c,b}_{k,i}\in\mathbf{Z}_{k}^{c,b}}\mathcal{N}\left(\mathbf{z}_{k,i}^{c,b};\mathbf{h}_{k,s_{i}^{b}}^{c,b},\mathbf{Q}_{s_{i}^{b}}^{c,b}\right)$
$\displaystyle\times\prod_{\mathbf{z}^{c,g}_{k,i}\in\mathbf{Z}_{k}^{c,g}}\mathcal{N}\left(\mathbf{z}_{k,i}^{c,g};\mathbf{h}_{k,s_{i}^{g}}^{c,g},\mathbf{Q}_{s_{i}^{g}}^{c,g}\right)\text{,
}$ (65)

where $\mathbf{h}_{k,s_{i}^{b}}^{c,b}$ and $\mathbf{h}_{k,s_{i}^{g}}^{c,g}$
are the noiseless parts of the right-hand sides of equations (III-C1) and
(III-C2), respectively.

####  IV-B3 The Pseudo-measurement Likelihood Function

$\displaystyle
p\left(z_{k}^{rot}|\Theta_{k}\right)p\left(\mathbf{Z}_{k}^{grnd}|\Theta_{k}\right)p\left(\mathbf{Z}_{k}^{sym}|\Theta_{k}\right)$
$\displaystyle=$ $\displaystyle\text{
}\mathcal{N}\left(0;h_{k}^{rot},\mathbf{Q}^{rot}\right)\times\prod_{i=1}^{4}\mathcal{N}\left(0;h_{k,i}^{grnd,g},\mathbf{Q}^{grnd}\right)$
$\displaystyle\times\prod_{t=1}^{T}\mathcal{N}\left(\bm{\varpi}^{sym}_{k,t};\mathbf{D}\cdot\bm{\varpi}_{k,t},\mathbf{Q}^{sym}\right)\text{,
}$ (66)

where $h_{k}^{rot}$ and $h_{k,i}^{grnd,g}$ are the noiseless parts of the
right-hand sides of equations (III-D1) and (III-D2), respectively.

According to the above three likelihood functions, the joint likelihood
function for all the measurements is:

$\displaystyle p(\mathbf{Z}_{k}|\Theta_{k},A_{k})=$ $\displaystyle\text{
}p(\mathbf{Z}_{k}^{r},\mathbf{Z}_{k}^{c},z_{k}^{rot}|\Theta_{k},A_{k})$
$\displaystyle=$ $\displaystyle\text{
}p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})p(\mathbf{Z}_{k}^{c}|\Theta_{k})p\left(z_{k}^{rot}|\Theta_{k}\right)$
$\displaystyle\times
p\left(\mathbf{Z}_{k}^{grnd}|\Theta_{k}\right)p\left(\mathbf{Z}_{k}^{sym}|\Theta_{k}\right)\text{.
}$ (67)

###  IV-C Measurement Update

Given the predicted density (IV-A) and likelihood functions (IV-B3), the joint
posterior probability density of the object’s state can be computed using the
Bayes rule

$\displaystyle p(\Theta_{k},A_{k}|\mathbf{Z}^{k})$
$\displaystyle=\frac{p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})}{p(\mathbf{Z}_{k}|\mathbf{Z}^{k-1})}$
$\displaystyle=\frac{p(\Theta_{k}|\mathbf{Z}^{k-1})p(A_{k}|\Theta_{k})p(\mathbf{Z}_{k}|\Theta_{k},A_{k})}{p(\mathbf{Z}_{k}|\mathbf{Z}^{k-1})}\text{.
}$ (68)

The above posterior probability density has no analytical expression, so we
adopt a variational Bayesian approach [32] to find a tractable approximation
such that the approximated density $p\left(\Theta_{k}|\mathbf{Z}^{k}\right)$
is still of the form (IV), which is required for recursive Bayesian filtering.
The posterior probability density can be approximated as the product of three
marginal densities according to mean-field theory:

$p(\Theta_{k},A_{k}|\mathbf{Z}^{k})\approx
q_{A}(A_{k})q_{\mathbf{x}}(\mathbf{x}_{k})q_{\bm{\vartheta}}(\bm{\vartheta}_{k})\text{.
}$ (69)

A nice property of the factorized form (69) is that the posterior density
$p(\Theta_{k}|\mathbf{Z}^{k})$ can be easily obtained from (69) by
marginalizing out the data association variables $A_{k}$. In addition, the
approximate distribution can be derived by minimizing the KL divergence [33]
between the approximate distribution and the true posterior distribution

$\operatorname*{arg\,min}_{q_{A},q_{\mathbf{x}},q_{\bm{\vartheta}}}\mathrm{KL}\left(q_{A}\left(A_{k}\right)q_{\mathbf{x}}\left(\mathbf{x}_{k}\right)q_{\bm{\vartheta}}\left(\bm{\vartheta}_{k}\right)\|p\left(\Theta_{k},A_{k}|\mathbf{Z}^{k}\right)\right)\text{.
}$ (70)

The optimal solution of the above problem satisfies:

$\log q_{\varkappa}(\kappa_{k})=\mathbb{E}_{\\\ \varkappa^{*}}\\!\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]+c_{\varkappa^{*}}\text{,
}$ (71)

where $\varkappa\in\\{A,\mathbf{x},\bm{\vartheta}\\}$, $\varkappa^{*}$
represents a set of all elements excluding $\varkappa$, and
$c_{\varkappa^{*}}$ represents a constant unrelated to $\varkappa$. For
example, $\mathbb{E}_{A^{*}}[\cdot]$ represents the expectation with respect
to $\mathbf{x}$ and $\bm{\vartheta}$.

We use iterative coordinate ascent to solve the optimization problem (71) for
each probability distribution $q_{A}$, $q_{\mathbf{x}}$, $q_{\bm{\vartheta}}$
in turn. We assume that the probability density function for the variables
$\mathbf{x}$ and $\bm{\vartheta}$ in the $\iota$-th iteration are

$\displaystyle q^{(\iota)}_{\mathbf{x}}(\mathbf{x}_{k})$
$\displaystyle=\mathcal{N}\left(\mathbf{x}_{k};\hat{\mathbf{x}}_{k|k}^{(\iota)},\hat{\mathbf{P}}_{k|k}^{(\iota)}\right)\text{,
}$ (72a) $\displaystyle q^{(\iota)}_{\bm{\vartheta}}(\bm{\vartheta}_{k})$
$\displaystyle=\prod_{t=1}^{T}\mathcal{N}\left(\bm{\vartheta}_{k,t};\hat{\bm{\mu}}_{k|k,t}^{(\iota)},\hat{\bm{\Sigma}}_{k|k,t}^{(\iota)}\right)\text{.
}$ (72b)

Notably, we initialize (72) using predicted densities, where
$q^{(0)}_{\mathbf{x}}(\mathbf{x}_{k})=p\left(\mathbf{x}_{k}|\mathbf{Z}^{k-1}\right)$
and
$q^{(0)}_{\bm{\vartheta}}(\bm{\vartheta}_{k})=p\left(\bm{\vartheta}_{k,t}|\mathbf{Z}^{k-1}\right)$.
Subsequently, we proceed to describe how to calculate the $(\iota+1)$-th
iteration. For notational brevity, we use the shorthand notation
$\left(\mathbf{v}\right)^{\text{T}}\mathbf{Q}^{-1}(\cdot)$ notation for
$\left(\mathbf{v}\right)^{\text{T}}\mathbf{Q}^{-1}\left(\mathbf{v}\right)$.

####  IV-C1 Computing $q^{(\iota+1)}_{A}(A_{k})$

By substituting $A$ for $\varkappa$ in (71), we obtain

$\displaystyle\text{ }\log
q^{(\iota+1)}_{A}(A_{k})=\mathbb{E}_{A^{*}}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]+c_{A^{*}}$
$\displaystyle=$
$\displaystyle\sum_{t=1}^{T}\sum_{i=1}^{n^{r}_{k}}a_{it}\left(\log\pi_{k,t}^{(\iota)}-\frac{1}{2}\mathbb{E}\left[\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}^{(\iota)}\right)^{\text{T}}\mathbf{Q}^{-1}(\cdot)\right]\right)+c_{A^{*}}\text{.
}$ (73)

Then, applying the exponential to both sides, we can obtain the $(\iota+1)$-th
iteration of $q^{(\iota+1)}_{A}(A_{k})$:

$q^{(\iota+1)}_{A}(A_{k})=\prod_{t=1}^{T}\prod_{i=1}^{n^{r}_{k}}\left(\upsilon_{it}^{(\iota)}\right)^{a_{it}}\text{,
}$ (74)

with

$\displaystyle\upsilon_{it}^{(\iota)}$
$\displaystyle=\frac{\rho_{it}^{(\iota)}}{\sum_{t=1}^{\text{T}}\rho_{it}^{(\iota)}}\text{,
}$ (75) $\displaystyle\rho_{it}^{(\iota)}$
$\displaystyle=\exp\left\\{\log\pi_{k,t}^{(\iota)}-\frac{1}{2}\mathbb{E}\left[\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}^{(\iota)}\right)^{\text{T}}\mathbf{Q}^{-1}(\cdot)\right]\right\\}\text{.
}$ (76)

The expression of the expectation in (76) can be simplified using Lemmas 1 to
3, yielding:

$\displaystyle\mathbb{E}\left[\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}^{(\iota)}\right)^{\text{T}}\mathbf{Q}^{-1}(\cdot)\right]$
$\displaystyle=$
$\displaystyle\left(\mathbf{z}_{k,i}^{r}-\mathbb{E}\left[\bm{\zeta}_{k,t}^{(\iota)}\right]\right)^{\text{T}}\mathbf{Q}^{-1}(\cdot)+\mathrm{tr}\left\\{\mathbf{Q}^{-1}\mathrm{Cov}\left[\mathbf{p}_{k}^{(\iota)}\mathbf{p}_{k}^{(\iota),\text{T}}\right]\right\\}$
$\displaystyle+\mathrm{tr}\left\\{\mathbf{R}^{(\iota),\text{T}}_{k}\mathbf{Q}^{-1}\mathbf{R}^{(\iota)}_{k}\mathrm{Cov}\left[\mathbf{u}^{(\iota)}_{k,t}\mathbf{u}^{(\iota),\text{T}}_{k,t}\right]\right.$
$\displaystyle+\overline{\mathbf{R}}^{(\iota),\text{T}}_{k}\mathrm{diag}_{3}\left(\mathbf{J}_{k}^{(\iota)}\mathbb{E}\left[\delta\bm{\theta}_{k}^{(\iota)}\delta\bm{\theta}_{k}^{(\iota),\text{T}}\right]\mathbf{J}_{k}^{(\iota),\text{T}}\right)(\cdot)\mathbb{E}\left[\mathbf{u}^{(\iota)}_{k,t}\mathbf{u}^{(\iota),\text{T}}_{k,t}\right]$
$\displaystyle-\left.2\mathbf{Q}^{-1}\left[\mathbf{R}^{(\iota)}_{k}\mathbb{E}\left[\mathbf{u}^{(\iota)}_{k,t}\right]\right]_{\times}\mathbf{J}_{k}^{(\iota)}\mathbb{E}\left[\delta\bm{\theta}_{k}^{(\iota)}\delta\mathbf{p}_{k}^{(\iota),\text{T}}\right]\right\\}\text{,
}$ (77)

where we denote

$\displaystyle\mathbf{R}^{(\iota)}_{k}$
$\displaystyle=\mathbf{R}\\{\bm{\theta}_{ref,k}^{(\iota)}\\}\text{, }$ (78a)
$\displaystyle\overline{\mathbf{R}}^{(\iota)}_{k}$
$\displaystyle=\left[\sqrt{\mathbf{Q}^{-1}}\right]_{\ast}^{\text{T}}\mathbf{R}^{(\iota)}_{k}\text{,
}$ (78b) $\displaystyle\mathbf{J}_{k}^{(\iota)}$
$\displaystyle=\mathbf{J}_{l}(\bm{\theta}_{ref,k}^{(\iota)})\text{. }$ (78c)

####  IV-C2 Computing $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$

The expectation of the log joint probability density with respect to variable
$A$ for the $(\iota+1)$-th iteration is

$\displaystyle\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$ $\displaystyle=$
$\displaystyle-\frac{1}{2}\Bigg{\\{}\sum_{t=1}^{T}\bigg{\\{}\mathrm{tr}\left(\overline{\mathbf{Z}}^{(\iota+1)}_{k,t}\mathbf{Q}^{-1}\right)$
$\displaystyle+\left(\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)^{\text{T}}\left(\frac{\mathbf{Q}}{n^{(\iota+1)}_{k,t}}\right)^{-1}(\cdot)$
$\displaystyle+\left(\bm{\vartheta}_{k,t}-\hat{\bm{\mu}}^{(\iota)}_{k|k,t}\right)^{\text{T}}\left(\hat{\bm{\Sigma}}^{(\iota)}_{k|k,t}\right)^{-1}(\cdot)$
$\displaystyle+\left(\bm{\varpi}_{k,t}^{sym}-\mathbf{D}\cdot\bm{\varpi}_{k,t}\right)^{\text{T}}\left(\mathbf{Q}^{sym}\right)^{-1}(\cdot)\bigg{\\}}$
$\displaystyle+\left(\mathbf{x}_{k}-\hat{\mathbf{x}}^{(\iota)}_{k|k}\right)^{\text{T}}\left(\hat{\mathbf{P}}^{(\iota)}_{k|k}\right)^{-1}(\cdot)$
$\displaystyle+\sum_{\mathbf{z}^{c,b}_{k,i}\in\mathbf{Z}_{k}^{c,b}}\left(\mathbf{z}^{c,b}_{k,i}-\mathbf{h}_{k,s_{i}^{b}}^{c,b,(\iota)}\right)^{\text{T}}\left(\mathbf{Q}_{s_{i}^{b}}^{c,b,(\iota)}\right)^{-1}(\cdot)$
$\displaystyle+\sum_{\mathbf{z}^{c,g}_{k,i}\in\mathbf{Z}_{k}^{c,g}}\left(\mathbf{z}_{k,i}^{c,g}-\mathbf{h}_{k,s_{i}^{g}}^{c,g,(\iota)}\right)^{\text{T}}\left(\mathbf{Q}_{s_{i}^{g}}^{c,g,(\iota)}\right)^{-1}(\cdot)$
$\displaystyle+\left(h_{k}^{rot,(\iota)}\right)^{2}\left(\mathbf{Q}^{rot}\right)^{-1}+\sum_{i=1}^{4}\left(h_{k,i}^{grnd,g,(\iota)}\right)^{2}\left(\mathbf{Q}^{grnd}\right)^{-1}\Bigg{\\}}$
$\displaystyle+c_{\mathbf{x}^{*}}+c_{\bm{\vartheta}^{*}}\text{, }$ (79)

where

$\displaystyle n^{(\iota+1)}_{k,t}$
$\displaystyle=\sum_{i=1}^{n^{r}_{k}}\upsilon^{(\iota)}_{it}\text{, }$ (80a)
$\displaystyle\overline{\mathbf{z}}^{(\iota+1)}_{k,t}$
$\displaystyle=\frac{1}{n^{(\iota+1)}_{k,t}}\sum_{i=1}^{n^{r}_{k}}\upsilon^{(\iota)}_{it}\mathbf{z}_{k,i}^{r}\text{,
}$ (80b) $\displaystyle\overline{\mathbf{Z}}^{(\iota+1)}_{k,t}$
$\displaystyle=\sum_{i=1}^{n^{r}_{k}}\upsilon^{(\iota)}_{it}\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}\right)\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}\right)^{\text{T}}\text{.
}$ (80c)

For the detailed simplification of (IV-C2)-(80), please refer to Appendix D.

####  IV-C3 Computing $q^{(\iota+1)}_{\mathbf{x}}(\mathbf{x}_{k})$ and
$q^{(\iota+1)}_{\bm{\vartheta}}(\bm{\vartheta}_{k})$

By replacing $\varkappa$ in (71) with $\mathbf{x}$ and $\bm{\vartheta}$
respectively, we obtain

$\displaystyle\log q^{(\iota+1)}_{\mathbf{x}}(\mathbf{x}_{k})=$
$\displaystyle\text{
}\mathbb{E}_{\bm{\vartheta}}\left[\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]\right]+c_{\mathbf{x}^{*}}\text{,
}$ (81a) $\displaystyle\log
q^{(\iota+1)}_{\bm{\vartheta}}(\bm{\vartheta}_{k})=$ $\displaystyle\text{
}\mathbb{E}_{\mathbf{x}}\left[\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]\right]+c_{\bm{\vartheta}^{*}}\text{.
}$ (81b)

After applying exponential to both sides, we represent the closed-form
expressions of the probability densities as weighted combinations of prior
information and measurements. The detailed expressions can be found Appendix
E. Here, we offer simplified versions

$\displaystyle q^{(\iota+1)}_{\mathbf{x}}(\mathbf{x}_{k})=$
$\displaystyle\text{
}\mathcal{N}\left(\mathbf{x}_{k};\hat{\mathbf{x}}_{k|k}^{(\iota+1)},\hat{\mathbf{P}}_{k|k}^{(\iota+1)}\right)\text{,
}$ (82a) $\displaystyle q^{(\iota+1)}_{\bm{\vartheta}}(\bm{\vartheta}_{k})=$
$\displaystyle\prod_{t=1}^{T}\mathcal{N}\left(\bm{\vartheta}_{k,t};\hat{\bm{\mu}}_{k|k,t}^{(\iota+1)},\hat{\bm{\Sigma}}_{k|k,t}^{(\iota+1)}\right)\text{.
}$ (82b)

Finally, after $N_{vb}$ rounds of iterations, the posterior probability
density of the object’s state can be written as:

$p(\Theta_{k}|\mathbf{Z}^{k})\approx
q^{(N_{vb})}_{\mathbf{x}}(\mathbf{x}_{k})\cdot
q^{(N_{vb})}_{\bm{\vartheta}}(\bm{\vartheta}_{k})\text{. }$ (83)

The parameters of the probability density function represent the inferred mean
and covariance of the object’s state.

##  V Simulation and Experimental Results

###  V-A Simulation Results

The performance of the proposed algorithm is evaluated in several simulation
scenarios using the CARLA simulator [34], against three other 3D EOT methods
using only radar measurements222Python implementation of all the simulation
methods and data are available at https://github.com/RadarCameraFusionTeam-
BUPT/ES-EOT: GPEOT and GPEOT-P as described in [35], and the ellipsoid-based
method MEM-EKF* [36]. In this section, we refer to our proposed method as “ES-
EOT” (short for elastic skeleton extended object tracker). Additionally, to
highlight the effectiveness of our proposed 3D CTRV motion model and the ES
fusion mechanism, we conducted ablation studies in the same test scenarios.
Specifically, by setting $\bm{\omega}_{ref,k}$ in (16) and (21) to 0 while
maintaining consistency in other aspects, a constant velocity (CV) motion
model is constructed as a comparison method. In addition, we also compared the
situation without using the ES fusion mechanism.

We conducted roadside experiments using radar and camera sensors, analyzing
two types of trajectories involving two different vehicle types: a bus
changing lanes and a sedan car turning around. Trajectories are automatically
generated by the simulator using specified start and end points, as well as
intermediate points along the path. For vision keypoints detections, we first
annotate the true 3D keypoints (24 knots and 4 bottom corner points) on
vehicle models used within the simulator, and then trained the YOLOv8
network333The architecture of the network used in this paper is the same as
[28], which has image input and keypoints outputs. However, the key
distinction lies in training the network on our dedicated Carla dataset. to
predict the pixel coordinates of the visible keypoints. For radar
measurements, we synthesize radar points with 3D positions derived from
visible skeleton knots, and introduce a measurement noise with covariance
matrix of $0.5\mathbf{I}_{3\times 3}$. In order to evaluate the algorithm’s
performance across varying radar point densities, we introduce different
densities by assuming a Poisson distribution for the number of radar points in
each frame. The Poisson rates $(\alpha)$ are set at 1, 5, and 10,
respectively, while ensuring a minimum of one radar point. These experiments
are repeated 50 times for each scenario and Poisson rates setting.

We evaluated the algorithm’s performance by employing the Intersection-Over-
Union (IOU) metric [37] of the vehicle’s shape projections onto the VCS’s “xy-
plane”, “xz-plane”, and “yz-plane”:

$\text{IOU}_{\text{*-plane}}\left(V,\hat{V}\right)=\frac{\text{area}\left(\text{proj}_{\text{*}}\left(V\right)\cap\text{proj}_{\text{*}}\left(\hat{V}\right)\right)}{\text{area}\left(\text{proj}_{\text{*}}\left(V\right)\cup\text{proj}_{\text{*}}\left(\hat{V}\right)\right)}\text{,
}$ (84)

where $V$ represents the true 3D shape of the vehicle, and $\hat{V}$ is the
estimated shape. The function $\text{proj}_{\text{*}}(\cdot)$ projects the 3D
shape onto the “*-plane”, and the function $\text{area}(\cdot)$ calculates the
area of the resulting 2D shape. The IOU metric reflects the accuracy of both
the vehicle’s shape and kinematic state estimation, with a higher IOU
indicating better estimates.

We calculate the average performance in each tracking experiment, which
includes the mean value of the IOU and the root mean squared error (RMSE) of
the velocity:

$\displaystyle\overline{\text{IOU}}_{\text{*-plane}}=$ $\displaystyle\text{
}\frac{1}{N}\sum_{i=1}^{N}\text{IOU}_{\text{*-plane}}\left(V_{i},\hat{V}_{i}\right)\text{,
}$ (85) $\displaystyle\text{RMSE}_{v}=$
$\displaystyle\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left(v_{i}-\hat{v}_{i}\right)^{2}}\text{,
}$ (86)

where $v_{i}$ and $V_{i}$ represent the true velocity and shape of the vehicle
in the $i$-th frame, while $\hat{v}_{i}$ and $\hat{V}_{i}$ correspond to the
estimated velocity and shape, respectively. The $N$ is the total number of
frames.

We set the parameters of process noise described in (17) and (43) as
$\mathbf{W}=\mathrm{diag}\left(\mathbf{0}_{3\times 3},0.01,\mathbf{0}_{3\times
3},\mathbf{I}_{3\times 3},0.1\mathbf{I}_{2\times 2}\right)$ and
$\mathbf{W}_{\vartheta}=\mathbf{I}_{9\times 9}$, respectively. The parameters
of ES fusion mechanism described in Section III-E are $\epsilon=100$ and
$\rho=20$, ensuring critical damping behavior. The hyper-parameters of the
algorithm are set to $\mathbf{Q}=0.5\mathbf{I}_{3\times 3}$,
$\mathbf{Q}_{s_{i}^{b}}^{c,b}=\mathbf{Q}_{s_{i}^{g}}^{c,g}=5\mathbf{I}_{2\times
2}$, $\mathbf{Q}^{rot}=0.1$, $\mathbf{Q}^{grnd}=10^{-4}$,
$\mathbf{Q}^{sym}=0.05\mathbf{I}_{3\times 3}$,
$\mathbf{D}=\mathrm{diag}\left(1,-1,1\right)$,
$\mathbf{u}_{d}=\left[1,0,0\right]$. The ground equation described in (39) is
obtained using our previous FusionCalib method [38]. The vehicle’s shape has
$T=24$ components, and we perform $N_{vb}=3$ iterations for variational
Bayesian inference.

####  V-A1 Scenario of Changing Lane

(a)

(b)

Figure 5: The detections and results from the changing lane scenario. In Fig.
(a), the blue line is the actual vehicle trajectory, green points indicate
detected vehicle skeleton knots, and magenta points represent detected bottom
corner points. Fig. (b) shows radar detections $(\alpha=10)$ as red points,
the vehicle shape detected using ES-EOT (CTRV+ES) as blue surfaces, and the
true vehicle skeletons outlined in black (frame 30, 150 and 240,
respectively).

Figure 6: The IOU of the vehicle’s shape projections onto the VCS’s “xy-
plane”, “xz-plane”, and “yz-plane” in the change lane scenario (averaged over
50 runs with $\alpha=10$)

TABLE I: Mean IOU and Velocity RMSE for the Change Lane Scenario Methods | Models | $\overline{\text{IOU}}_{\text{xy-plane}}\uparrow$ | $\overline{\text{IOU}}_{\text{yz-plane}}\uparrow$ | $\overline{\text{IOU}}_{\text{zx-plane}}\uparrow$ | $\text{RMSE}_{v}\downarrow$
---|---|---|---|---|---
$\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$
ES-EOT | CTRV+ES | 0.624 | 0.680 | 0.721 | 0.790 | 0.812 | 0.827 | 0.625 | 0.727 | 0.776 | 0.809 | 0.811 | 0.813
CTRV | 0.252 | 0.307 | 0.364 | 0.731 | 0.736 | 0.743 | 0.248 | 0.301 | 0.354 | 0.809 | 0.809 | 0.811
CV+ES | 0.685 | 0.736 | 0.741 | 0.808 | 0.819 | 0.810 | 0.708 | 0.779 | 0.795 | 0.851 | 0.856 | 0.861
CV | 0.464 | 0.482 | 0.493 | 0.783 | 0.784 | 0.784 | 0.463 | 0.481 | 0.496 | 0.849 | 0.854 | 0.860
GPEOT | - | 0.380 | 0.501 | 0.528 | 0.245 | 0.303 | 0.323 | 0.392 | 0.516 | 0.539 | 1.952 | 1.149 | 0.991
GPEOT-P | - | 0.479 | 0.611 | 0.639 | 0.512 | 0.617 | 0.633 | 0.504 | 0.632 | 0.648 | 1.801 | 1.356 | 1.139
MEM-EKF* | - | 0.412 | 0.437 | 0.437 | 0.498 | 0.525 | 0.520 | 0.432 | 0.457 | 0.455 | 1.781 | 1.460 | 1.337

In this scenario, the bus moves away from the sensor, enters the interaction,
and then changes lanes, as depicted in Fig. 6.

Fig. 6 shows the corresponding IOU values for the vehicle’s shape projects
over frame sequence. At around frame 180, during a lane change by the vehicle,
the IOU values of our proposed method and MEM-EKF* method decrease, while the
GPEOT and GPEOT-P methods remain relatively unaffected. Despite the decrease,
our proposed method still outperforms all the other methods in this scenario.
Furthermore, the IOU values of ES-EOT (CV+ES) are slightly better than those
of ES-EOT (CTRV+ES), which means that in the case of weak maneuvering, the CV
motion model can better represent the object’s motion characteristics compared
to the CTRV motion model. The performance of ES-EOT (CTRV) and ES-EOT (CV) is
worse than that of ES-EOT (CTRV+ES) and ES-EOT (CV+ES), respectively,
indicating that the ES fusion mechanism has excellent performance in improving
object position accuracy.

Table I displays the mean IOU errors and velocity RMSE for the lane change
scenario. Our proposed method exhibits significantly higher IOU values and
lower velocity RMSE compared to the other methods.

####  V-A2 Scenario of Turning around

In this scenario, the vehicle’s motion includes a lane change, a turnaround,
and culminating with a precise adjustment to its heading. This intricate
sequence of maneuvers is visually depicted in Fig. 8.

(a)

(b)

Figure 7: The detections and results from the turning around scenario. The
lines and points have the same meaning as depicted in Fig. 6. The Fig. (b)
shows frame 30, 90, 150, 180 and 240, respectively.

Figure 8: The IOU of the vehicle’s shape projections onto the VCS’s “xy-
plane”, “xz-plane”, and “yz-plane” in the turn around scenario (averaged over
50 runs with $\alpha=10$)

TABLE II: Mean IOU and Velocity RMSE for the Turn Around Scenario Methods | Models | $\overline{\text{IOU}}_{\text{xy-plane}}\uparrow$ | $\overline{\text{IOU}}_{\text{yz-plane}}\uparrow$ | $\overline{\text{IOU}}_{\text{zx-plane}}\uparrow$ | $\text{RMSE}_{v}\downarrow$
---|---|---|---|---|---
$\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$ | $\alpha=1$ | $\alpha=5$ | $\alpha=10$
ES-EOT | CTRV+ES | 0.490 | 0.570 | 0.602 | 0.703 | 0.725 | 0.716 | 0.451 | 0.580 | 0.622 | 0.997 | 0.976 | 0.966
CTRV | 0.246 | 0.256 | 0.264 | 0.641 | 0.656 | 0.666 | 0.238 | 0.247 | 0.256 | 1.003 | 0.976 | 0.954
CV+ES | 0.342 | 0.400 | 0.411 | 0.504 | 0.493 | 0.470 | 0.317 | 0.388 | 0.399 | 4.292 | 4.247 | 4.193
CV | 0.087 | 0.096 | 0.101 | 0.427 | 0.418 | 0.410 | 0.100 | 0.100 | 0.100 | 4.290 | 4.246 | 4.194
GPEOT | - | 0.188 | 0.285 | 0.340 | 0.117 | 0.143 | 0.165 | 0.115 | 0.173 | 0.205 | 2.276 | 1.667 | 1.480
GPEOT-P | - | 0.282 | 0.350 | 0.386 | 0.230 | 0.238 | 0.255 | 0.234 | 0.260 | 0.280 | 1.560 | 1.410 | 1.344
MEM-EKF* | - | 0.250 | 0.475 | 0.501 | 0.157 | 0.331 | 0.369 | 0.150 | 0.308 | 0.342 | 1.349 | 1.178 | 1.107

Fig. 8 illustrates the IOU values for the vehicle’s shape projections over
frame sequence. The IOU values decrease sharply in three places, each
corresponding to a vehicle turn. At around frame 50, our proposed method and
the MEM-EKF* method are almost unaffected by the lane change, whereas the
GPEOT and GPEOT-P methods exhibit significant performance decline. At around
frame 150, though all methods’ performance degrades significantly during a
vehicle turnaround, our method quickly recovers accurate tracking. However,
ES-EOT (CV+ES) and ES-EOT (CV) fail after the vehicle turns around, indicating
it is unsuitable for complex maneuvering situations. Near frame 200, as the
vehicle adjusts its heading direction, the performance of all methods
decreases slightly.

In summary, our proposed method shows strong adaptability to complex movements
during the entire tracking process. Furthermore, Table II presents the mean
IOU errors and velocity RMSE for the turn around scenario, consistently
illustrating our proposed method’s superior performance compared to existing
methods within complex maneuvering scenarios.

###  V-B Experimental Results

We also evaluated our proposed method in a challenging scenario using the
nuScenes dataset [39], and compared with the state-of-the-art deep learning
based radar camera fusion methods named CRN [12] and HVDetFusion [9]. In this
scenario, the ego vehicle remained stationary while the SUV we tracked turned
left from a position approximately 15 meters away from the ego vehicle to a
position approximately 45 meters away444Python implementation and data of the
methods on nuScenes test scenario are available at
https://github.com/RadarCameraFusionTeam-BUPT/ES-EOT-real-nuscenes.

The sampling rate in the nuScenes dataset is 0.5Hz, which is relatively low.
To adapt to this low sampling rate, we adjusted the prediction uncertainty
$\mathbf{W}$ to $\mathrm{diag}\left(\mathbf{0}_{3\times
3},0.1,\mathbf{0}_{3\times 3},\mathbf{I}_{3\times 3},\mathbf{I}_{2\times
2}\right)$. Additionally, we adjusted the hyper-parameters of the ground
motion constraint $\mathbf{Q}^{grnd}$ to $10^{-3}$ to account for the
estimation error of the road plane equation, while keeping the other hyper-
parameters unchanged from the simulation scenarios.

Given that the nuScenes dataset provides object labels in the form of 3D
bounding boxes rather than skeletons, we utilize the evaluation metrics
Average Translation Error (ATE) and Average Scale Error (ASE) provided by the
dataset to assess the methods. As shown in Fig. 9, our proposed method
effectively captures the sparse and non-uniform distribution characteristics
of radar point clouds, seamlessly integrating with visual measurements to
provide accurate estimations of position and size. Notably, our method
necessitates no further training on the new dataset. The only adjustments
needed are the road plane equation and some parameters described in the above
paragraph.

Table III shows the ATE and ASE values of the methods. Although our proposed
method may not surpass the performance of deep learning based methods in
vehicle-mounted scenarios, it still achieves satisfactory accuracy.
Importantly, our proposed method is better suited for applications in roadside
scenes, especially in situations with limited pixel annotations or even no
annotations.

TABLE III: ATE and ASE For the Experimental Scene | Trained | $\text{ATE}\downarrow$ | $\text{ASE}\downarrow$
---|---|---|---
ES-EOT (CTRV+ES) | no | 0.760 | 0.175
CRN | yes | 0.228 | 0.060
HVDetFusion | yes | 0.337 | 0.063
Figure 9: Bird-eye-view of the tracking results with nuScenes dataset, and the
keypoints detections.

###  V-C Discussion

The proposed CTRV motion model, SGW-GMM radar measurement model, and ES fusion
mechanism are all manually designed models for recursive Bayesian estimate.
This means that our method can be directly applied to new scenes by changing a
few parameters without requiring additional training or manual annotations.
This is beneficial to large-scale roadside deployment without annotations.
Furthermore, our method has strong interpretability and offers valuable
insights for heterogeneous data fusion.

However, despite these advantages, our method does have certain limitations.
For instance, the measurement models assume Gaussian or Gaussian mixture
distributions. Consequently, if the actual measurements deviate from these
distributions, the performance of our method may suffer a significant decline.
Additionally, during complex maneuvers of the object, there might be a
temporary decline in algorithm performance due to the high degree of freedom
in the extended state. Moreover, although our proposed method can be directly
applied to vehicle perception scenarios, the performance in such scenarios may
be reduced compared to roadside perception scenarios, mainly due to severe
self-occlusion of vehicles.

##  VI Conclusion and Future Work

This paper introduces a novel 3D EOT method tailored for roadside scenarios,
effectively combining radar point clouds and vision pixel keypoints. We
propose a spherical Gaussian weighted radar measurement model to handle non-
uniform radar point clouds. To cope with sparse point clouds, we propose an
elastic skeleton model that fuses vision pixel keypoints with 3D radar point
clouds, using the positions of point clouds as scale factors. This approach
ensures both precise depiction of the vehicle shape and an accurate
determination of the vehicle’s position, even in the presence of sparse point
clouds. Additionally, we extend the existing 2D constant turn rate and
velocity model to a 3D setting to accommodate vehicle motion. Through
simulations and experiments in diverse scenarios, our method demonstrates
effectiveness in handling sparse and non-uniform point clouds, as well as
complex vehicle maneuvers.

For future work, we will consider the effect of sensing range on resolution in
simulation and evaluate our methods on more public datasets, such as VoD
dataset [40].

## References

  * [1] S. Jung, J. Kim, M. Levorato, C. Cordeiro, and J.-H. Kim, “Infrastructure-assisted on-driving experience sharing for millimeter-wave connected vehicles,” _IEEE Transactions on Vehicular Technology_ , vol. 70, no. 8, pp. 7307–7321, 2021.
  * [2] P. Wu, X. Li, H. Zheng, K. Wang, J. Qin, and M. Tang, “3d modeling and analysis of cooperative perception-oriented millimeter-wave V2I networks with information value-based relay,” _IEEE Transactions on Vehicular Technology_ , vol. 72, no. 8, pp. 10 505–10 520, 2023.
  * [3] S. Wang, R. Pi, J. Li, X. Guo, Y. Lu, T. Li, and Y. Tian, “Object tracking based on the fusion of roadside lidar and camera data,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 71, pp. 1–14, 2022.
  * [4] H. Liu, C. Lin, B. Gong, and H. Liu, “Lane-level and full-cycle multivehicle tracking using low-channel roadside lidar,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 72, pp. 1–12, 2023.
  * [5] J. Mao, S. Shi, X. Wang, and H. Li, “3d object detection for autonomous driving: A review and new outlooks,” _arXiv preprint arXiv:2206.09474_ , 2022\.
  * [6] L. Wang, X. Zhang, W. Qin, X. Li, J. Gao, L. Yang, Z. Li, J. Li, L. Zhu, H. Wang _et al._ , “Camo-mot: Combined appearance-motion optimization for 3d multi-object tracking with camera-lidar fusion,” _IEEE Transactions on Intelligent Transportation Systems_ , 2023.
  * [7] X. Li, T. Xie, D. Liu, J. Gao, K. Dai, Z. Jiang, L. Zhao, and K. Wang, “Poly-mot: A polyhedral framework for 3d multi-object tracking,” in _2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2023, pp. 9391–9398.
  * [8] G. Guo and S. Zhao, “3d multi-object tracking with adaptive cubature kalman filter for autonomous driving,” _IEEE Transactions on Intelligent Vehicles_ , vol. 8, no. 1, pp. 512–519, 2022.
  * [9] K. Lei, Z. Chen, S. Jia, and X. Zhang, “Hvdetfusion: A simple and robust camera-radar fusion framework,” _arXiv preprint arXiv:2307.11323_ , 2023\.
  * [10] W. Xiong, J. Liu, T. Huang, Q.-L. Han, Y. Xia, and B. Zhu, “Lxl: Lidar excluded lean 3d object detection with 4d imaging radar and camera fusion,” _IEEE Transactions on Intelligent Vehicles_ , 2023.
  * [11] L. Zheng, S. Li, B. Tan, L. Yang, S. Chen, L. Huang, J. Bai, X. Zhu, and Z. Ma, “Rcfusion: Fusing 4d radar and camera with bird’s-eye view features for 3d object detection,” _IEEE Transactions on Instrumentation and Measurement_ , 2023.
  * [12] Y. Kim, J. Shin, S. Kim, I.-J. Lee, J. W. Choi, and D. Kum, “Crn: Camera radar net for accurate, robust, efficient 3d perception,” in _Proceedings of the IEEE/CVF International Conference on Computer Vision_ , 2023, pp. 17 615–17 626.
  * [13] K. Granström and M. Baum, “A tutorial on multiple extended object tracking,” _TechRxiv_ , 2022.
  * [14] K. Granstrom, M. Baum, and S. Reuter, “Extended object tracking: Introduction, overview and applications,” _Journal of Advances in Information Fusion_ , vol. 12, no. 2, pp. 139–174, 2017.
  * [15] S. Haag, B. Duraisamy, W. Koch, and J. Dickmann, “Radar and lidar target signatures of various object types and evaluation of extended object tracking methods for autonomous driving applications,” in _2018 21st International Conference on Information Fusion (FUSION)_. IEEE, 2018, pp. 1746–1755.
  * [16] X. Cao, J. Lan, X. R. Li, and Y. Liu, “Automotive radar-based vehicle tracking using data-region association,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 23, no. 7, pp. 8997–9010, 2021.
  * [17] L. Zhang and J. Lan, “Tracking of extended object using random matrix with non-uniformly distributed measurements,” _IEEE Transactions on Signal Processing_ , vol. 69, pp. 3812–3825, 2021.
  * [18] B. Tuncer, U. Orguner, and E. Özkan, “Multi-ellipsoidal extended target tracking with variational bayes inference,” _IEEE Transactions on Signal Processing_ , vol. 70, pp. 3921–3934, 2022.
  * [19] A. Scheel and K. Dietmayer, “Tracking multiple vehicles using a variational radar model,” _IEEE Transactions on Intelligent Transportation Systems_ , vol. 20, no. 10, pp. 3721–3736, 2018.
  * [20] Y. Xia, P. Wang, K. Berntorp, L. Svensson, K. Granström, H. Mansour, P. Boufounos, and P. V. Orlik, “Learning-based extended object tracking using hierarchical truncation measurement model with automotive radar,” _IEEE Journal of Selected Topics in Signal Processing_ , vol. 15, no. 4, pp. 1013–1029, 2021.
  * [21] J. Sola, “Quaternion kinematics for the error-state kalman filter,” _arXiv preprint arXiv:1711.02508_ , 2017.
  * [22] C. Schöller, M. Schnettler, A. Krämmer, G. Hinz, M. Bakovic, M. Güzet, and A. Knoll, “Targetless rotational auto-calibration of radar and camera for intelligent transportation systems,” in _2019 IEEE Intelligent Transportation Systems Conference (ITSC)_. IEEE, 2019, pp. 3934–3941.
  * [23] Z. Zhang, “A flexible new technique for camera calibration,” _IEEE Transactions on pattern analysis and machine intelligence_ , vol. 22, no. 11, pp. 1330–1334, 2000.
  * [24] X. R. Li and V. P. Jilkov, “Survey of maneuvering target tracking. part i. dynamic models,” _IEEE Transactions on aerospace and electronic systems_ , vol. 39, no. 4, pp. 1333–1364, 2003.
  * [25] J. He, C. Sun, B. Zhang, and P. Wang, “Adaptive error-state kalman filter for attitude determination on a moving platform,” _IEEE Transactions on Instrumentation and Measurement_ , vol. 70, pp. 1–10, 2021.
  * [26] M. Feldmann, D. Fränken, and W. Koch, “Tracking of extended objects and group targets using random matrices,” _IEEE Transactions on Signal Processing_ , vol. 59, no. 4, pp. 1409–1420, 2010.
  * [27] J. Wang, P. Ren, M. Gong, J. Snyder, and B. Guo, “All-frequency rendering of dynamic, spatially-varying reflectance,” in _ACM SIGGRAPH Asia 2009 papers_ , 2009, pp. 1–10.
  * [28] J. Glenn, “Ultralytics yolov8,” https://github.com/ultralytics/ultralytics, 2023.
  * [29] S. J. Julier and J. J. LaViola, “On kalman filtering with nonlinear equality constraints,” _IEEE Transactions on Signal Processing_ , vol. 55, no. 6, pp. 2774–2784, 2007.
  * [30] W. L. Hallauer Jr, _Introduction to Linear, Time-Invariant, Dynamic Systems for Students of Engineering_. Virginia Tech, 2016.
  * [31] C. M. Bishop and N. M. Nasrabadi, _Pattern recognition and machine learning_. Springer, 2006, vol. 4, no. 4.
  * [32] U. Orguner, “A variational measurement update for extended target tracking with random matrices,” _IEEE Transactions on Signal Processing_ , vol. 60, no. 7, pp. 3827–3834, 2012.
  * [33] I. Csiszár, “I-divergence geometry of probability distributions and minimization problems,” _The annals of probability_ , pp. 146–158, 1975\.
  * [34] A. Dosovitskiy, G. Ros, F. Codevilla, A. Lopez, and V. Koltun, “Carla: An open urban driving simulator,” in _Conference on robot learning_. PMLR, 2017, pp. 1–16.
  * [35] M. Kumru and E. Özkan, “Three-dimensional extended object tracking and shape learning using gaussian processes,” _IEEE Transactions on Aerospace and Electronic Systems_ , vol. 57, no. 5, pp. 2795–2814, 2021.
  * [36] S. Yang and M. Baum, “Tracking the orientation and axes lengths of an elliptical extended object,” _IEEE Transactions on Signal Processing_ , vol. 67, no. 18, pp. 4720–4729, 2019.
  * [37] H. Rezatofighi, N. Tsoi, J. Gwak, A. Sadeghian, I. Reid, and S. Savarese, “Generalized intersection over union: A metric and a loss for bounding box regression,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2019, pp. 658–666.
  * [38] J. Deng, Z. Hu, Z. Lu, and X. Wen, “Fusioncalib: Automatic extrinsic parameters calibration based on road plane reconstruction for roadside integrated radar camera fusion sensors,” _Pattern Recognition Letters_ , 2023\.
  * [39] 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,” in _Proceedings of the IEEE/CVF conference on computer vision and pattern recognition_ , 2020, pp. 11 621–11 631.
  * [40] A. Palffy, E. Pool, S. Baratam, J. F. Kooij, and D. M. Gavrila, “Multi-class road user detection with 3+ 1d radar in the view-of-delft dataset,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 2, pp. 4961–4968, 2022\.

Supplementary Materials

## Appendix A
Derivation For the Time Update

We provide the derivations for the time update of the reference-state and
error-state kinematics that are involved in (16)-(21).

###  VI-A Time Update for Reference-state Kinematics

From (13), it is established that velocity, angular velocity, and the
vehicle’s dimensions remain constant during $\Delta t$. Consequently, (16b),
(16e) and (16f) are self-evident, while derivations are needed for (16a) and
(16c). In this section, we simplify notation by omitting the discrete time
index $k$ and the subscript ‘ref’. Additionally, we introduce a continuous-
time variable $t$ for clearer illustration of states throughout the time
period.

####  VI-A1 Time Update for Position State

We first prove that the rotation matrix format of the vehicle’s pose at time
$t$ is

$\mathbf{R}\left(t\right)=\exp\left([\bm{\omega}t]_{\times}\right)\mathbf{R}\left(0\right)\text{,
}$ (87)

where $\mathbf{R}\left(0\right)=\mathbf{R}\\{\tilde{\mathbf{q}}_{ref,k}\\}$
represents the rotation matrix format of vehicle’s pose at last time step, and
the definition of $\exp\left(\cdot\right)$ is in (II-A). We will continue to
demonstrate the correctness of (87) by proving that its time derivative is
equivalent to (14)

$\displaystyle\frac{\partial\mathbf{R}\left(t\right)}{\partial t}=$
$\displaystyle\text{
}\frac{\partial\exp\left([\bm{\omega}t]_{\times}\right)}{\partial
t}\mathbf{R}\left(0\right)$ $\displaystyle=$
$\displaystyle\left(\cos\left(\|\bm{\omega}\|t\right)\|\bm{\omega}\|\left[\frac{\bm{\omega}}{\|\bm{\omega}\|}\right]_{\times}\right.$
$\displaystyle+\left.\sin\left(\|\bm{\omega}\|t\right)\|\bm{\omega}\|\left[\frac{\bm{\omega}}{\|\bm{\omega}\|}\right]^{2}_{\times}\right)\mathbf{R}\left(0\right)$
$\displaystyle=$
$\displaystyle\left(\cos\left(\|\bm{\omega}\|t\right)\left[\bm{\omega}\right]_{\times}+\frac{\sin\left(\|\bm{\omega}\|t\right)}{\|\bm{\omega}\|}\left[\bm{\omega}\right]^{2}_{\times}\right)\mathbf{R}\left(0\right)\text{,
}$ (88)

where the $\left[\bm{\omega}\right]_{\times}$ is a skew-symmetric matrix that
represents the cross-product of the $\bm{\omega}$, which is defined in (5).
The third power of a skew-symmetric matrix has the following property:

$\left[\bm{\omega}\right]_{\times}^{3}=-\|\bm{\omega}\|^{2}\left[\bm{\omega}\right]_{\times}\text{.
}$ (89)

According to this property, (VI-A1) can be simplified as

$\displaystyle\frac{\partial\mathbf{R}\left(t\right)}{\partial t}=$
$\displaystyle\text{
}\left[\bm{\omega}\right]_{\times}\exp\left([\bm{\omega}t]_{\times}\right)\mathbf{R}\left(0\right)\text{,
}$ (90a) $\displaystyle\dot{\mathbf{R}}(t)=$ $\displaystyle\text{
}\left[\bm{\omega}\right]_{\times}\mathbf{R}\left(t\right)$ (90b)

The expression in (90b) is equivalent to (14). Thus, (87) is correct.
According to this, the time update for position state can be obtained by
integrating (13a)

$\displaystyle\mathbf{p}(t)=$ $\displaystyle\text{
}\mathbf{p}(0)+v\times\int_{0}^{t}\mathbf{R}(t)dt\mathbf{u}^{d}$
$\displaystyle=$ $\displaystyle\text{
}\mathbf{p}(0)+v\Bigg{\\{}\mathbf{I}\times
t+\frac{1}{\left\|\bm{\omega}\right\|}\left[\frac{\bm{\omega}}{\left\|\bm{\omega}\right\|}\right]_{\times}\left(1-\cos\left(\left\|\bm{\omega}\right\|t\right)\right)$
$\displaystyle+\left[\frac{\bm{\omega}}{\left\|\bm{\omega}\right\|}\right]_{\times}^{2}\left(t-\frac{\sin\left(\left\|\bm{\omega}\right\|t\right)}{\left\|\bm{\omega}\right\|}\right)\Bigg{\\}}\cdot\mathbf{R}(0)\mathbf{u}^{d}\text{,
}$ (91)

where $\mathbf{p}(0)$ denotes the vehicle’s position at the previous time
step, implying that $\mathbf{p}(0)=\mathbf{p}_{\text{ref},k}$. Consequently,
(16a) is correct when $t=\Delta t$ in (VI-A1).

####  VI-A2 Time Update for Pose State

We assume that the reference-state of pose at time $t$ is
$\tilde{\mathbf{q}}(t)$, and $\tilde{\mathbf{q}}(0)$ is equal to the state at
the last time step, which means

$\tilde{\mathbf{q}}(0)=\begin{bmatrix}q_{w}(0)&\mathbf{q}_{v}^{\text{T}}(0)\end{bmatrix}^{\text{T}}=\tilde{\mathbf{q}}_{ref,k}\text{.
}$ (92)

Thus, the continuous-time format of (16c) can be written as

$\tilde{\mathbf{q}}(t)=\mathrm{Exp}\left(\bm{\omega}\times
t\right)\odot\tilde{\mathbf{q}}(0)\text{. }$ (93)

Similar to Section VI-A1, we will demonstrate the correctness of (93) by
proving that its time derivative is equivalent to (12e). According to the
definitions of $\mathrm{Exp}(\cdot)$ in (7), the vector form of
$\mathrm{Exp}(\bm{\omega}\times t)$ is

$\mathrm{Exp}(\bm{\omega}\times
t)=\begin{bmatrix}\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)\\\
\frac{\bm{\omega}}{\|\bm{\omega}\|}\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)\end{bmatrix}\text{.
}$ (94)

We then expand the right-hand side of (93)

$\displaystyle\tilde{\mathbf{q}}(t)=$
$\displaystyle\begin{bmatrix}q_{w}(0)\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)-\frac{\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)}{\|\bm{\omega}\|}\bm{\omega}^{\text{T}}\mathbf{q}_{v}(0)\vspace{5pt}\\\
\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)\mathbf{q}_{v}(0)+q_{w}(0)\frac{\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)}{\|\bm{\omega}\|}\bm{\omega}\\\
+\frac{\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)}{\|\bm{\omega}\|}\bm{\omega}\times\mathbf{q}_{v}(0)\end{bmatrix}\text{,
}$ (95)

and its time derivative is

$\displaystyle\frac{\partial\tilde{\mathbf{q}}(t)}{\partial t}=$
$\displaystyle\begin{bmatrix}-q_{w}(0)\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)\frac{\|\bm{\omega}\|}{2}-\frac{\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)}{2}\bm{\omega}^{\text{T}}\mathbf{q}_{v}(0)\\\
\\\
-\frac{\|\bm{\omega}\|\sin\left(\frac{\|\bm{\omega}\|t}{2}\right)}{2}\mathbf{q}_{v}(0)+q_{w}(0)\frac{\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)}{2}\bm{\omega}\\\
+\frac{\cos\left(\frac{\|\bm{\omega}\|t}{2}\right)}{2}\bm{\omega}\times\mathbf{q}_{v}(0)\end{bmatrix}$
$\displaystyle=$ $\displaystyle\frac{1}{2}\begin{bmatrix}0\\\
\bm{\omega}\end{bmatrix}\odot\left(\mathrm{Exp}(\bm{\omega}\times
t)\odot\tilde{\mathbf{q}}(0)\right)$ $\displaystyle=$
$\displaystyle\frac{1}{2}\tilde{\bm{\omega}}\odot\tilde{\mathbf{q}}(t)\text{,
}$ (96)

which can be rewritten as

$\dot{\tilde{\mathbf{q}}}=\frac{1}{2}\tilde{\bm{\omega}}\odot\tilde{\mathbf{q}}\text{.
}$ (97)

The expression in (97) is equivalent to (12e). As a result, (93) is verified,
affirming the validity of (16c) when $t=\Delta t$.

###  VI-B Time Update for Error-state Kinematics

####  VI-B1 Proof of (17)

To prove the correctness of (17), we subtract the reference-state from (12)
and discard second-order small quantity. Since the time derivatives of
$v_{ref}$, $\bm{\omega}_{ref}$ and $\bm{\xi}_{ref}$ are zero, the time
derivations of the corresponding error-state kinematics are straightforward to
obtain from (12). Therefore, we focus on deriving the error-state kinematics
for position and pose.

To derive the time derivative of error-state kinematics for position, we first
expand (12a) as reference-state and error-state kinematics

$\displaystyle\dot{\mathbf{p}}=$ $\displaystyle\text{ }v\mathbf{d}$
$\displaystyle\dot{\mathbf{p}}_{ref}+\dot{\delta\mathbf{p}}=$
$\displaystyle\left(v_{ref}+\delta
v\right)\delta\mathbf{R}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}$ $\displaystyle=$
$\displaystyle\text{
}v_{ref}\cdot\delta\mathbf{R}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}+\delta
v\cdot\delta\mathbf{R}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}\text{. }$ (98)

According to the definition (II-A) of the transformation from the rotation
vector $\bm{\theta}$ to the rotation matrix $\mathbf{R}$, a small rotation
follows the subsequent property

$\delta\mathbf{R}=\exp\left([\delta\bm{\theta}]_{\times}\right)\approx\mathbf{I}+[\delta\bm{\theta}]_{\times}\text{.
}$ (99)

Substituting (99) into (VI-B1), the expression can be further simplified by
ignoring second-order small quantity

$\displaystyle\dot{\mathbf{p}}_{ref}+\dot{\delta\mathbf{p}}\approx$
$\displaystyle\text{
}v_{ref}\cdot\delta\mathbf{R}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}+\delta
v\cdot\mathbf{R}_{ref}\mathbf{u}^{d}$ $\displaystyle=$ $\displaystyle\text{
}v_{ref}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}+v_{ref}\cdot[\delta\bm{\theta}]_{\times}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}$
$\displaystyle+\delta v\cdot\mathbf{R}_{ref}\mathbf{u}^{d}\text{, }$ (100)

then we subtract the time derivative of reference-state kinematics (13a), and
the error-state kinematics for position can be obtained

$\displaystyle\dot{\delta\mathbf{p}}=$ $\displaystyle\text{
}v_{ref}\cdot[\delta\bm{\theta}]_{\times}\cdot\mathbf{R}_{ref}\mathbf{u}^{d}+\delta
v\cdot\mathbf{R}_{ref}\mathbf{u}^{d}$ $\displaystyle=$ $\displaystyle\text{
}-v_{ref}\cdot[\mathbf{R}_{ref}\mathbf{u}^{d}]_{\times}\delta\bm{\theta}+\delta
v\cdot\mathbf{R}_{ref}\mathbf{u}^{d}\text{. }$ (101)

Similar to the above derivation, we expand (12e) as reference-state and error-
state kinematics and derive the time derivative of error-state kinematics for
pose. We first expand the left side of (12e)

$\dot{\tilde{\mathbf{q}}}=\dot{\delta\tilde{\mathbf{q}}}\odot\tilde{\mathbf{q}}_{ref}+\delta\tilde{\mathbf{q}}\odot\dot{\tilde{\mathbf{q}}}_{ref}\text{.
}$ (102)

and (102) can be expanded by substituting reference-state kinematics for pose
described in (13c) into it

$\dot{\tilde{\mathbf{q}}}=\dot{\delta\tilde{\mathbf{q}}}\odot\tilde{\mathbf{q}}_{ref}+\frac{1}{2}\delta\tilde{\mathbf{q}}\odot\tilde{\bm{\omega}}_{ref}\odot\tilde{\mathbf{q}}_{ref}\text{,
}$ (103)

Then we expand the right-hand side of (12e)

$\frac{1}{2}\tilde{\bm{\omega}}\odot\tilde{\mathbf{q}}=\frac{1}{2}\tilde{\bm{\omega}}\odot\delta\tilde{\mathbf{q}}\odot\tilde{\mathbf{q}}_{ref}\text{.
}$ (104)

According to the original equation, (103) is equal to (104)

$\dot{\delta\tilde{\mathbf{q}}}\odot\tilde{\mathbf{q}}_{ref}+\frac{1}{2}\delta\tilde{\mathbf{q}}\odot\tilde{\bm{\omega}}_{ref}\odot\tilde{\mathbf{q}}_{ref}=\frac{1}{2}\tilde{\bm{\omega}}\odot\delta\tilde{\mathbf{q}}\odot\tilde{\mathbf{q}}_{ref}\text{,
}$ (105)

and the right product of $\tilde{\mathbf{q}}_{ref}$ in both sides of the
formula can be extracted [21, Eq. 16]

$\left(\dot{\delta\tilde{\mathbf{q}}}+\frac{1}{2}\delta\tilde{\mathbf{q}}\odot\tilde{\bm{\omega}}_{ref}\right)\odot\tilde{\mathbf{q}}_{ref}=\left(\frac{1}{2}\tilde{\bm{\omega}}\odot\delta\tilde{\mathbf{q}}\right)\odot\tilde{\mathbf{q}}_{ref}\text{,
}$ (106)

therefore, the equations within the parentheses on both sides are equivalent

$\dot{\delta\tilde{\mathbf{q}}}+\frac{1}{2}\delta\tilde{\mathbf{q}}\odot\tilde{\bm{\omega}}_{ref}=\frac{1}{2}\tilde{\bm{\omega}}\odot\delta\tilde{\mathbf{q}}\text{,
}$ (107)

which can be further reorganized as

$2\dot{\delta\tilde{\mathbf{q}}}=\tilde{\bm{\omega}}\odot\delta\tilde{\mathbf{q}}-\delta\tilde{\mathbf{q}}\odot\tilde{\bm{\omega}}_{ref}\text{.
}$ (108)

where $\delta\tilde{\mathbf{q}}$ can be approximated by a small rotation
vector $\delta\bm{\theta}$

$\delta\tilde{\mathbf{q}}=\mathrm{Exp}\left(\delta\bm{\theta}\right)\approx\begin{bmatrix}1\\\
\frac{1}{2}\delta\bm{\theta}\end{bmatrix}\text{. }$ (109)

Then, subtracting (109) into (108), we get

$\begin{bmatrix}0\\\
\frac{1}{2}\dot{\delta\bm{\theta}}\end{bmatrix}\approx\begin{bmatrix}0&-\delta\bm{\omega}^{\text{T}}\\\
\delta\bm{\omega}&[2\bm{\omega}_{ref}+\delta\bm{\omega}]_{\times}\end{bmatrix}\begin{bmatrix}1\\\
\delta\bm{\theta}/2\end{bmatrix}\text{. }$ (110)

Omitting the second-order small quantity, we obtain

$\dot{\delta\bm{\theta}}=[\bm{\omega}_{ref}]_{\times}\delta\bm{\theta}+\delta\bm{\omega}\text{.
}$ (111)

In summary, the matrix form of error-state kinematics can be rewritten as
(17).

####  VI-B2 Proof of (19)-(21)

By integrating (17), the time update for error-state kinematics can be
expressed as

$\delta\mathbf{x}_{k+1}=\exp\left(\mathbf{F}\Delta
t\right)\delta\mathbf{x}_{k}+\mathbf{w}_{k}\text{, }$ (112)

where $\mathbf{w}$ is the same as the definition in (19a) and

$\exp\left(\mathbf{F}\Delta
t\right)=\mathbf{I}+\sum_{i=1}^{\infty}\frac{\left(\mathbf{F}\Delta
t\right)^{i}}{i!}\text{, }$ (113)

where $\mathbf{F}$ is defined in (III-A2).

According to mathematical induction, the exponential of $\mathbf{F}\Delta t$
has the following properties, when $i\geq 2$

$\displaystyle\left(\mathbf{F}\Delta t\right)^{i}$
$\displaystyle=\begin{bmatrix}0&0&\mathbf{M}_{1}\left(\mathbf{M}_{2}\right)^{i-1}&\mathbf{M}_{1}\mathbf{M}_{2}^{i-2}&0\\\
0&0&0&0&0\\\
0&0&\left(\mathbf{M}_{2}\right)^{i}&\left(\mathbf{M}_{2}\right)^{i-1}&0\\\
0&0&0&0&0\\\ 0&0&0&0&0\end{bmatrix}\Delta t^{i}\text{, }$ (114)

for ease of reference, we define a symbol $\Sigma_{n}$

$\Sigma_{n}\triangleq\Delta
t^{n}\sum_{k=0}^{\infty}\frac{1}{(k+n)!}\mathbf{M}_{2}^{k}\Delta t^{k}\text{,
}$ (115)

then the expression of (113) can be simplified as

$\exp\left(\mathbf{F}\Delta
t\right)=\begin{bmatrix}\mathbf{I}&\mathbf{M}_{0}\Delta
t&\mathbf{M}_{1}\Sigma_{1}&\mathbf{M}_{1}\Sigma_{2}&0\\\ 0&\mathbf{I}&0&0&0\\\
0&0&\Sigma_{0}&\Sigma_{1}&0\\\ 0&0&0&\mathbf{I}&0\\\
0&0&0&0&\mathbf{I}\end{bmatrix}\text{, }$ (116)

which is the same as (20). We next provide the closed-form of $\Sigma_{0}$,
$\Sigma_{1}$ and $\Sigma_{2}$ in (116). When $n=0$, $\Sigma_{0}$ is

$\Sigma_{0}=\sum_{k=0}^{\infty}\frac{1}{k!}\mathbf{M}_{2}^{k}\Delta
t^{k}=\exp\left(\mathbf{M}_{2}\Delta t\right)\text{, }$ (117)

which is the same as (21a). When $n=1$, $\Sigma_{1}$ is

$\Sigma_{1}=\Delta
t\sum_{k=0}^{\infty}\frac{1}{(k+1)!}\mathbf{M}_{2}^{k}\Delta t^{k}\text{, }$
(118)

due to that $\mathbf{M}_{2}$ is a skew-symmetric matrix, and the third power
property described in (89)

$\mathbf{M}_{2}=-\frac{1}{\|\bm{\omega}_{ref}\|^{2}}\mathbf{M}_{2}^{3}\text{.
}$ (119)

Then substituting (119) into (118)

$\Sigma_{1}=\mathbf{I}\Delta
t-\frac{\mathbf{M}_{2}}{\|\bm{\omega}_{ref}\|^{2}}\left(\Sigma_{0}-\mathbf{I}-\mathbf{M}_{2}\Delta
t\right)\text{, }$ (120)

which is the same as (21b), and the similar derivation can be implemented to
obtain $\Sigma_{2}$.

## Appendix B
Derivation For ES Fusion Mechanism

In this section, we derive the closed-form of transition model of ES fusion
mechanism (45)-(47). By integrating (43), the ES fusing mechanism has the
similar form of (112)

$\bm{\vartheta}_{k+1,t}=\exp\left(\mathbf{F}_{\vartheta}\Delta
t\right)\bm{\vartheta}_{k,t}+\mathbf{w}_{k}^{\vartheta}\text{, }$ (121)

where $\mathbf{F}_{\vartheta}$ and $\mathbf{w}_{k}^{\vartheta}$ is the same as
(44) and (45a), respectively, and

$\exp\left(\mathbf{F}_{\vartheta}\Delta
t\right)=\mathbf{I}+\sum_{i=1}^{\infty}\frac{\left(\mathbf{F}_{\vartheta}\Delta
t\right)^{i}}{i!}\text{, }$ (122)

following the mathematical induction, we find that

$\left(\mathbf{F}_{\vartheta}\right)^{i}=\begin{bmatrix}\mathbf{0}&\mathbf{0}&\mathbf{0}\\\
-\mathbf{M}_{3,i}&\mathbf{M}_{3,i}&\mathbf{M}_{4,i}\\\
\epsilon\mathbf{M}_{4,i}&-\epsilon\mathbf{M}_{4,i}&\mathbf{M}_{3,i}-\rho\mathbf{M}_{4,i}\end{bmatrix}\text{
, }i\geq 1\text{, }$ (123)

with

$\displaystyle\mathbf{M}_{3,i}$
$\displaystyle=-\epsilon\mathbf{M}_{4,i-1}\text{, }$ (124a)
$\displaystyle\mathbf{M}_{4,i}$
$\displaystyle=-\epsilon\mathbf{M}_{4,i-2}-\rho\mathbf{M}_{4,i-1}\text{, }$
(124b)

and

$\displaystyle\mathbf{M}_{4,i}$
$\displaystyle=-\frac{2^{-i}}{\sqrt{-4\epsilon+\rho^{2}}}\left(\left(-\rho-\sqrt{-4\epsilon+\rho^{2}}\right)^{i}\right.$
$\displaystyle-\left.\left(-\rho+\sqrt{-4\epsilon+\rho^{2}}\right)^{i}\right)\mathbf{I}\text{
, }i\geq 1\text{, }$ (125a) $\displaystyle\mathbf{M}_{3,i}$
$\displaystyle=-\epsilon\mathbf{M}_{4,i-1}\text{ , }i\geq 2\text{, }$ (125b)

then the closed-form of transition matrix $\Phi_{\vartheta}$ can be obtained
by simplifying (122), and the results are concluded in (47).

## Appendix C
Proofs of Lemma 1 to 3

###  VI-A Proof of Lemma 1

The product of a skew-matrix with a vector is equivalent to the cross product,
_i.e._ ,

$[\mathbf{a}]_{\times}\mathbf{b}=\mathbf{a}\times\mathbf{b}\text{, }$ (126)

where $\mathbf{a}$ and $\mathbf{b}$ are two arbitury 3D vectors. As a result

$\displaystyle\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}$
$\displaystyle=\begin{bmatrix}\mathbf{v}\times\mathbf{M}_{(1)}&,\dots,&\mathbf{v}\times\mathbf{M}_{(n)}\end{bmatrix}$
$\displaystyle=-\begin{bmatrix}\mathbf{M}_{(1)}\times\mathbf{v}&,\dots,&\mathbf{M}_{(n)}\times\mathbf{v}\end{bmatrix}$
$\displaystyle=-\left[\mathbf{M}\right]_{*}diag_{n}\left(\mathbf{v}\right)\text{,
}$ (127)

where $\mathbf{v}$ and $\mathbf{M}$ are a 3D vector and a $3\times n$ matrix,
respectively. The definition of symbol $\left[\cdot\right]_{\ast}$,
$diag_{n}(\cdot)$ and $\mathbf{M}_{(i)}$ are the same as Lemma 1.

###  VI-B Proof of Lemma 2

By using (VI-A), Lemma 2 can be written as

$\displaystyle\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}\cdot\mathbf{t}$
$\displaystyle=-\left[\mathbf{M}\right]_{*}diag_{3}\left(\mathbf{v}\right)\cdot\mathbf{t}$
$\displaystyle=-\left[\mathbf{M}\right]_{*}\left[\mathbf{v}\cdot\mathbf{t}^{\text{T}}\right]_{\star}\text{,
}$ (128)

where the definition of $\left[\cdot\right]_{\star}$ is the same as Lemma 2.

###  VI-C Proof of Lemma 3

By using (VI-A) and cholesky decomposition, Lemma 3 can be written as

$\displaystyle\left[\mathbf{v}\right]_{\times}\cdot\mathbf{M}\cdot\left[\mathbf{t}\right]_{\times}^{\text{T}}$
$\displaystyle=$
$\displaystyle\left(\left[\mathbf{v}\right]_{\times}\mathbf{L}\right)\left(\left[\mathbf{t}\right]_{\times}\mathbf{L}\right)^{\text{T}}$
$\displaystyle=$
$\displaystyle\left[\mathbf{L}\right]_{\ast}diag_{n}\left(\mathbf{v}\right)diag_{n}\left(\mathbf{t}^{\text{T}}\right)\left[\mathbf{L}\right]_{\ast}^{\text{T}}$
$\displaystyle=$
$\displaystyle\left[\mathbf{L}\right]_{\ast}diag_{n}\left(\mathbf{v}\cdot\mathbf{t}^{\text{T}}\right)\left[\mathbf{L}\right]_{\ast}^{\text{T}}\text{.
}$ (129)

## Appendix D
Derivation For $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$

We first expand the logarithm of the joint probability density according to
(IV-B3) and (IV-C)

$\displaystyle\text{ }\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})$ $\displaystyle=$
$\displaystyle\text{ }\log p(\Theta_{k}|\mathbf{Z}^{k-1})+\log
p(A_{k}|\Theta_{k})+\log p(\mathbf{Z}_{k}|\Theta_{k},A_{k})$ $\displaystyle=$
$\displaystyle\text{ }\log p(\Theta_{k}|\mathbf{Z}^{k-1})+\log
p(A_{k}|\Theta_{k})+\log p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})$
$\displaystyle+\log p(\mathbf{Z}_{k}^{c}|\Theta_{k})+\log
p\left(z_{k}^{rot}|\Theta_{k}\right)+\log
p\left(\mathbf{Z}_{k}^{grnd}|\Theta_{k}\right)$ $\displaystyle+\log
p\left(\mathbf{Z}_{k}^{sym}|\Theta_{k}\right)\text{, }$ (130)

where $\log p(A_{k}|\Theta_{k})$ and $\log
p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})$ are related to $A_{k}$, while other
components are independent of $A_{k}$. As a result, the expectation of
(Appendix D Derivation For $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$) only influences
the aforementioned two components, leaving the other parts unchanged

$\displaystyle\text{ }\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$ $\displaystyle=$
$\displaystyle\text{ }\mathbb{E}_{A}\left[\log p(A_{k}|\Theta_{k})+\log
p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})\right]$ $\displaystyle+\log
p(\Theta_{k}|\mathbf{Z}^{k-1})+\log p(\mathbf{Z}_{k}^{c}|\Theta_{k})+\log
p\left(z_{k}^{rot}|\Theta_{k}\right)$ $\displaystyle+\log
p\left(\mathbf{Z}_{k}^{grnd}|\Theta_{k}\right)+\log
p\left(\mathbf{Z}_{k}^{sym}|\Theta_{k}\right)\text{. }$ (131)

Therefore, we focus on the simplification of the affected components, and
expand it according to (63) and (64).

$\displaystyle\text{ }\mathbb{E}_{A}\left[\log p(A_{k}|\Theta_{k})+\log
p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})\right]$ $\displaystyle=$
$\displaystyle\text{
}\sum_{t=1}^{T}\sum_{u=1}^{n_{k}^{r}}\mathbb{E}_{A}\left[a_{it}\right]\bigg{(}\log\left(\pi_{k,t}^{(\iota)}\right)$
$\displaystyle-\frac{1}{2}\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)\bigg{)}+c_{A^{*}}\text{,
}$ (132)

where $\mathbb{E}_{A}\left[a_{it}\right]=\upsilon_{it}^{(\iota)}$ at the
$\iota$-th iteration, according to (74). For simplicity, we assume that
$\pi_{k,t}^{(\iota)}$ is not a random variable, and its logarithm is
calculated based on the reference-states in every iteration, which means that
it is a constant towards $\mathbf{x}_{k}$ and $\bm{\vartheta}_{k}$.
Consequently, (Appendix D Derivation For $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$) can be
reformulated as

$\displaystyle\text{ }\mathbb{E}_{A}\left[\log p(A_{k}|\Theta_{k})+\log
p(\mathbf{Z}_{k}^{r}|\Theta_{k},A_{k})\right]$ $\displaystyle=$
$\displaystyle\text{
}-\frac{1}{2}\sum_{t=1}^{T}\sum_{u=1}^{n_{k}^{r}}\upsilon_{it}^{(\iota)}\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)+c$
$\displaystyle=$ $\displaystyle\text{
}-\frac{1}{2}\sum_{t=1}^{T}\sum_{u=1}^{n_{k}^{r}}\mathrm{tr}\left\\{\upsilon_{it}^{(\iota)}\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)\left(\mathbf{z}_{k,i}^{r}-\bm{\zeta}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}\right\\}+c$
$\displaystyle=$ $\displaystyle\text{
}-\frac{1}{2}\mathrm{tr}\left\\{\sum_{t=1}^{T}\sum_{u=1}^{n_{k}^{r}}\upsilon_{it}^{(\iota)}\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}+\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)\right.$
$\displaystyle\cdot\left.\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}+\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}\right\\}+c$
$\displaystyle=$ $\displaystyle\text{
}-\frac{1}{2}\mathrm{tr}\Bigg{\\{}\sum_{t=1}^{T}\sum_{u=1}^{n_{k}^{r}}\upsilon_{it}^{(\iota)}\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}\right)\left(\mathbf{z}_{k,i}^{r}-\overline{\mathbf{z}}^{(\iota+1)}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}$
$\displaystyle+\upsilon_{it}^{(\iota)}\left(\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)\left(\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)^{\text{T}}\mathbf{Q}^{-1}\Bigg{\\}}+c$
$\displaystyle=$ $\displaystyle\text{
}-\frac{1}{2}\sum_{t=1}^{T}\Bigg{\\{}\mathrm{tr}\Big{(}\overline{\mathbf{Z}}^{(\iota+1)}_{k,t}\mathbf{Q}^{-1}\Big{)}$
$\displaystyle+\left(\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)^{\text{T}}\left(\frac{\mathbf{Q}}{n^{(\iota+1)}_{k,t}}\right)^{-1}\left(\overline{\mathbf{z}}^{(\iota+1)}_{k,t}-\bm{\zeta}_{k,t}\right)\Bigg{\\}}+c\text{,
}$ (133)

where $c$ represents a constant independent to $A_{k}$, $\mathbf{x}_{k}$ and
$\bm{\vartheta}_{k}$, and the definitions of $n^{(\iota+1)}_{k,t}$,
$\overline{\mathbf{z}}^{(\iota+1)}_{k,t}$ and
$\overline{\mathbf{Z}}^{(\iota+1)}_{k,t}$ are the same as (80). In summary,
$\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$ can be obtained by
substituting (Appendix D Derivation For $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$) into (Appendix D
Derivation For $\mathbb{E}_{A}\left[\log
p(\Theta_{k},A_{k},\mathbf{Z}_{k}|\mathbf{Z}^{k-1})\right]$).

## Appendix E
Closed-Form Expressions for the Posterior Probability Densities of
$\mathbf{x}$ and $\bm{\vartheta}$

The posterior probability densities of $\mathbf{x}$ and $\bm{\vartheta}$ are
in (82), where each of the parameters of the densities function consists of
six parts, with five corresponding to the likelihood functions and one
corresponding to the prior. The parameters in (82a) are as follows

$\displaystyle\hat{\mathbf{P}}_{k|k}^{(\iota+1)}=$
$\displaystyle\Bigg{\\{}\sum_{t=1}^{T}\left(\left(\mathbf{H}_{1,t,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\mathbf{H}_{1,t,\mathbf{x}_{k}}^{(\iota)}\right.$
$\displaystyle+\left.\left(\mathbf{H}^{\theta}\right)^{T}\left(\mathbf{Q}_{t,\mathbf{x}_{k}}^{(\iota)}\right)^{-1}\mathbf{H}^{\theta}\right)+\left(\hat{\mathbf{P}}_{k|k}^{(\iota)}\right)^{-1}$
$\displaystyle+\sum_{i=1}^{|\mathbf{Z}_{k}^{c,b}|}\left(\mathbf{H}_{k,\mathbf{x}}^{c,b,(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{b}}^{c,b}\right)^{-1}\mathbf{H}_{k,\mathbf{x}}^{c,b,(\iota)}$
$\displaystyle+\sum_{i=1}^{|\mathbf{Z}_{k}^{c,g}|}\left(\mathbf{H}_{k,\mathbf{x}}^{c,g,(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{g}}^{c,g}\right)^{-1}\mathbf{H}_{k,\mathbf{x}}^{c,g,(\iota)}$
$\displaystyle+\left(\mathbf{H}_{5,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\mathbf{Q}^{rot}\right)^{-1}\mathbf{H}_{5,\mathbf{x}_{k}}^{(\iota)}$
$\displaystyle+\sum_{i=1}^{4}\left(\mathbf{H}_{6,i,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\mathbf{Q}^{grnd}\right)^{-1}\mathbf{H}_{6,i,\mathbf{x}_{k}}^{(\iota)}\Bigg{\\}}^{-1}\text{,
}$ (134) $\displaystyle\delta\hat{\mathbf{x}}_{k}^{(\iota+1)}=$
$\displaystyle\hat{\mathbf{P}}_{k|k}^{(\iota+1)}\Bigg{\\{}\sum_{t=1}^{T}\left(\left(\mathbf{H}_{1,t,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\mathbf{h}_{1,t,\mathbf{x}_{k}}^{(\iota)}\right.$
$\displaystyle+\left.\left(\mathbf{H}^{\theta}\right)^{T}\left(\mathbf{Q}_{t,\mathbf{x}_{k}}^{(\iota)}\right)^{-1}\mathbf{h}_{2,t,\mathbf{x}_{k}}^{(\iota)}\right)$
$\displaystyle+\sum_{i=1}^{|\mathbf{Z}_{k}^{c,b}|}\left(\mathbf{H}_{k,\mathbf{x}}^{c,b,(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{b}}^{c,b}\right)^{-1}\mathbf{h}_{3,s_{i}^{b},\mathbf{x}_{k}}^{(\iota)}$
$\displaystyle+\sum_{i=1}^{|\mathbf{Z}_{k}^{c,g}|}\left(\mathbf{H}_{k,\mathbf{x}}^{c,g,(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{g}}^{c,g}\right)^{-1}\mathbf{h}_{4,s_{i}^{g},\mathbf{x}_{k}}^{(\iota)}$
$\displaystyle+\left(\mathbf{H}_{5,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\mathbf{Q}^{rot}\right)^{-1}\mathbf{h}_{5,\mathbf{x}_{k}}^{(\iota)}$
$\displaystyle+\sum_{i=1}^{4}\left(\mathbf{H}_{6,i,\mathbf{x}_{k}}^{(\iota)}\right)^{T}\left(\mathbf{Q}^{grnd}\right)^{-1}\mathbf{h}_{6,i,\mathbf{x}_{k}}^{(\iota)}\Bigg{\\}}\text{,
}$ (135)

and

$\hat{\mathbf{x}}_{k|k}^{(\iota+1)}=\mathbf{x}_{ref,k}^{(\iota)}+\delta\hat{\mathbf{x}}_{k}^{(\iota+1)}\text{,
}$ (136)

with

$\displaystyle\widetilde{\mathbf{J}}_{k}^{(\iota)}=$
$\displaystyle\left[\sqrt{\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}}\right]_{*}^{T}\mathbf{J}_{k}^{(\iota)}\text{,
}$ (137a) $\displaystyle\mathbf{H}_{1,t,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\mathbf{H}^{r}-\left[\mathbf{R}^{(\iota)}_{k}\mathbb{E}\left[\mathbf{u}^{(\iota)}_{k,t}\right]\right]_{\times}\mathbf{J}_{k}^{(\iota)}\mathbf{H}^{\theta}\text{,
}$ (137b) $\displaystyle\mathbf{h}_{1,t,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\overline{\mathbf{z}}_{k,t}^{(\iota)}-\mathbf{R}^{(\iota)}_{k}\mathbb{E}\left[\mathbf{u}^{(\iota)}_{k,t}\right]-\mathbf{p}_{ref,k}^{(\iota)}\text{,
}$ (137c)
$\displaystyle\left(\mathbf{Q}_{t,\mathbf{x}_{k}}^{(\iota)}\right)^{-1}=$
$\displaystyle\widetilde{\mathbf{J}}_{k}^{(\iota),T}\mathrm{diag}_{3}\left(\mathbf{R}^{(\iota)}_{k}\mathrm{Cov}\left[\mathbf{u}^{(\iota)}_{k,t}\right]\mathbf{R}^{(\iota),T}_{k}\right)(\cdot)\text{,
}$ (137d) $\displaystyle\mathbf{h}_{2,t,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\mathbf{Q}_{t,\mathbf{x}_{k}}^{(\iota),T}\mathbf{J}_{k}^{(\iota),T}\cdot\left[\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\right]_{*}$
$\displaystyle\cdot\left[\mathbf{R}^{(\iota)}_{k}\mathrm{Cov}\left[\mathbf{u}^{(\iota)}_{k,t}\right]\mathbf{R}^{(\iota),T}_{k}\right]_{\star}\text{,
}$ (137e) $\displaystyle\mathbf{h}_{3,s_{i}^{b},\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\mathbf{z}_{k,i}^{c,b}-h^{c,b}\left(\mathbf{x}_{ref,k}^{(\iota)},\bm{\varphi}_{k,s_{i}^{b}}^{(\iota)}\right)\text{,
}$ (137f) $\displaystyle\mathbf{h}_{4,s_{i}^{g},\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\mathbf{z}_{k,i}^{c,g}-h_{s_{i}^{g}}^{c,g}\left(\mathbf{x}_{ref,k}^{(\iota)}\right)\text{,
}$ (137g) $\displaystyle\mathbf{H}_{5,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\left(\mathbf{R}^{(\iota)}_{k}\mathbf{u}^{d}\right)^{T}\mathbf{H}^{\omega}$
$\displaystyle-\bm{\omega}_{ref,k}^{(\iota),T}\left[\mathbf{R}^{(\iota)}_{k}\mathbf{u}^{d}\right]_{\times}\mathbf{J}_{k}^{(\iota)}\mathbf{H}^{\theta}\text{,
}$ (137h) $\displaystyle\mathbf{h}_{5,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle
z_{k}^{rot}-\left(\mathbf{R}^{(\iota)}_{k}\mathbf{u}^{d}\right)^{T}\bm{\omega}_{ref,k}^{(\iota)}\text{,
}$ (137i) $\displaystyle\mathbf{H}_{6,i,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle\mathbf{n}^{T}\left(\mathbf{H}^{r}-\left[\mathbf{R}_{k}^{(\iota)}\mathbf{G}_{i}\bm{\xi}_{ref}^{(\iota)}\right]_{\times}\mathbf{J}_{k}^{(\iota)}\mathbf{H}^{\theta}\right.$
$\displaystyle+\left.\mathbf{R}_{k}^{(\iota)}\mathbf{G}_{i}\mathbf{H}^{\xi}\right)\text{,
}$ (137j) $\displaystyle\mathbf{h}_{6,i,\mathbf{x}_{k}}^{(\iota)}=$
$\displaystyle-\mathbf{n}^{T}\left(\mathbf{p}_{ref}^{(\iota)}+\mathbf{R}_{k}^{(\iota)}\mathbf{G}_{i}\bm{\xi}_{ref}^{(\iota)}\right)-d\text{,
}$ (137k) $\displaystyle\mathbf{H}^{r}=$
$\displaystyle\begin{bmatrix}\mathbf{I}_{3\times
3}&0&\mathbf{0}&\mathbf{0}&\mathbf{0}\end{bmatrix}\text{, }$ (137l)
$\displaystyle\mathbf{H}^{\theta}=$
$\displaystyle\begin{bmatrix}\mathbf{0}&0&\mathbf{I}_{3\times
3}&\mathbf{0}&\mathbf{0}\end{bmatrix}\text{, }$ (137m)
$\displaystyle\mathbf{H}^{\omega}=$
$\displaystyle\begin{bmatrix}\mathbf{0}&0&\mathbf{0}&\mathbf{I}_{3\times
3}&\mathbf{0}\end{bmatrix}\text{, }$ (137n) $\displaystyle\mathbf{H}^{\xi}=$
$\displaystyle\begin{bmatrix}\mathbf{0}&0&\mathbf{0}&\mathbf{0}&\mathbf{I}_{2\times
2}\end{bmatrix}\text{ }$ (137o)

The parameters in (82b) are as follows

$\displaystyle\hat{\bm{\mu}}_{k|k,t}^{(\iota+1)}=$
$\displaystyle\hat{\bm{\Sigma}}_{k|k,t}^{(\iota+1)}\Bigg{\\{}\left(\mathbf{H}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{T}\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\mathbf{h}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}$
$\displaystyle+$
$\displaystyle\left(\mathbf{H}^{u}\right)^{T}\left(\mathbf{Q}_{\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{-1}\mathbf{h}_{2,\bm{\vartheta}_{k,t}}^{(\iota)}+\left(\hat{\bm{\Sigma}}_{k|k,t}^{(\iota)}\right)^{-1}\hat{\bm{\mu}}_{k|k,t}^{(\iota)}$
$\displaystyle+$
$\displaystyle\left(\mathbf{H}_{\varpi}\right)^{T}\left(\mathbf{Q}^{sym}\right)^{-1}\mathbf{h}_{3,\bm{\vartheta}_{k,t}}^{(\iota)}$
$\displaystyle+$
$\displaystyle\sum_{i=1}^{|\mathbf{Z}_{k}^{c,b}|}\left(\mathbf{H}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{b}}^{(\iota)}\right)^{-1}\mathbf{h}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}\cdot\mathbf{1}_{t}\left(s_{i}^{b}\right)\text{,
}$ (138) $\displaystyle\hat{\bm{\Sigma}}_{k|k,t}^{(\iota+1)}=$
$\displaystyle\Bigg{\\{}\left(\mathbf{H}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{T}\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\mathbf{H}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}+\left(\hat{\bm{\Sigma}}_{k|k,t}^{(\iota)}\right)^{-1}$
$\displaystyle+$
$\displaystyle\left(\mathbf{H}^{u}\right)^{T}\left(\mathbf{Q}_{\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{-1}\mathbf{H}^{u}+\left(\mathbf{H}^{\varpi}\right)^{T}\left(\mathbf{Q}^{sym}\right)^{-1}\mathbf{H}_{\varpi}$
$\displaystyle+$
$\displaystyle\sum_{i=1}^{|\mathbf{Z}_{k}^{c,b}|}\left(\mathbf{H}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{T}\left(\mathbf{Q}_{k,s_{i}^{b}}^{(\iota)}\right)^{-1}\mathbf{H}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}\cdot\mathbf{1}_{t}\left(s_{i}^{b}\right)\text{,
}$ (139)

with

$\displaystyle\widetilde{\mathbf{R}}^{(\iota),T}_{k,t}=$
$\displaystyle\left[\sqrt{\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}}\right]_{*}^{T}\mathbf{R}_{k}^{(\iota)}\text{,
}$ (140a) $\displaystyle\mathbf{H}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle\mathbf{R}_{k}^{(\iota)}\mathbf{H}^{u}\text{, }$ (140b)
$\displaystyle\mathbf{h}_{1,\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle\overline{\mathbf{z}}_{k,t}^{(\iota)}-\mathbb{E}\left[\mathbf{p}_{k}^{(\iota)}\right]\text{,
}$ (140c)
$\displaystyle\left(\mathbf{Q}_{\bm{\vartheta}_{k,t}}^{(\iota)}\right)^{-1}=$
$\displaystyle\widetilde{\mathbf{R}}^{(\iota),T}_{k,t}diag_{3}\left(\mathbf{J}_{k}^{(\iota)}\mathbb{E}\left[\delta\bm{\theta}\delta\bm{\theta}^{T}\right]\mathbf{J}_{k}^{(\iota),T}\right)(\cdot)\text{,
}$ (140d) $\displaystyle\mathbf{h}_{2,\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle-\mathbf{Q}_{\bm{\vartheta}_{k,t}}^{(\iota),T}\mathbf{R}_{k}^{(\iota),T}\cdot\left[\left(\frac{\mathbf{Q}}{n_{k,t}^{(\iota+1)}}\right)^{-1}\right]_{*}$
$\displaystyle\cdot$
$\displaystyle\left[\mathbf{J}_{k}^{(\iota)}\mathbb{E}\left[\delta\bm{\theta}_{k}^{(\iota)}\delta\mathbf{p}_{k}^{(\iota),T}\right]\right]_{\star}\text{,
}$ (140e) $\displaystyle\mathbf{h}_{3,\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle\mathbf{H}^{\varpi}\mathbb{E}\left[\bm{\vartheta}_{k,t}^{sym}\right]\text{,
}$ (140f)
$\displaystyle\mathbf{H}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle\mathbf{H}^{c,b,(\iota)}_{\bm{\varpi}_{s_{i}^{b}}}\mathbf{H}^{\varpi}\text{,
}$ (140g)
$\displaystyle\mathbf{h}_{4,s_{i}^{b},\bm{\vartheta}_{k,t}}^{(\iota)}=$
$\displaystyle\mathbf{z}_{k,i}^{c,b}-h^{c,b}\left(\mathbf{x}_{ref,k}^{(\iota)},\bm{\varphi}_{k,s_{i}^{b}}^{(\iota)}\right)+\mathbf{H}_{k,\bm{\varpi}_{s_{i}^{b}}}^{c,b,(\iota)}\bm{\varphi}_{k,s_{i}^{b}}^{(\iota)}\text{,
}$ (140h) $\displaystyle\mathbf{H}^{u}=$
$\displaystyle\begin{bmatrix}\mathbf{I}_{3\times
3}&\mathbf{0}&\mathbf{0}\end{bmatrix}\text{, }$ (140i)
$\displaystyle\mathbf{H}^{\varpi}=$
$\displaystyle\begin{bmatrix}\mathbf{0}&\mathbf{I}_{3\times
3}&\mathbf{0}\end{bmatrix}\text{, }$ (140j)

and $\mathbf{1}_{x}\left(y\right)$ is an indicator function, which takes the
value 1 when $x=y$, and 0 otherwise.