# Breaking the Gaussian Barrier: Leveraging ReGVAMP to Extend EKF and IEKF

Fangqing Xiao, Dirk Slock

► To cite this version:

Fangqing Xiao, Dirk Slock. Breaking the Gaussian Barrier: Leveraging ReGVAMP to Extend EKF and IEKF. ASILOMAR 2024, 58th Asilomar Conference on Signals, Systems, and Computers, Oct 2024, Pacific Grove, United States. pp.1774-1778, 10.1109/IEEECONF60004.2024.10942737. hal-05034730

HAL Id: hal-05034730

https://hal.science/hal-05034730v1

Submitted on 15 Apr 2025

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_1.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
# Breaking the Gaussian Barrier: Leveraging ReGVAMP to Extend EKF and IEKF

Fangqing Xiao, Dirk Slock

Communication Systems Department, EURECOM, France
fangqing.xiao@eurecom.fr, dirk.slock@eurecom.fr

***Abstract***—In the non-linear hidden Markov chain (HMC) model, commonly employed in robotics, navigation, signal processing, and control systems, various variants of Kalman filter (KF) like the Extended Kalman Filter (EKF) and Iterated Extended Kalman Filter (IEKF) have been developed for different scenarios, with their limitations and advantages. However, their effectiveness can diminish in the presence of non-Gaussian noise, which is prevalent in many real-world situations. This paper presents a novel approach to tackle non-Gaussian noise leveraging the revisited Generalized Vector Approximate Message Passing (ReGVAMP) within the context of EKF and IEKF. ReGVAMP extends these KF variants to break the Gaussian barrier, thereby enhancing the accuracy of state estimation. A tracking simulation validated the feasibility of our proposed algorithms.

## I. INTRODUCTION

The Kalman Filter (KF) [1] is a foundational tool in state estimation, particularly effective for linear systems with Gaussian noise. However, its utility is restricted by the assumption of linearity and Gaussianity, leading to the development of extensions to handle nonlinearities and non-Gaussian noise sources [2]. Key among these extensions are the Extended Kalman Filter (EKF) [3] and Iterated Extended Kalman Filter (IEKF) [4], which play crucial roles in various fields such as robotics, navigation, signal processing, and control systems [5], [6]. These algorithms expand upon the capabilities of the conventional Kalman Filter, enabling it to effectively deal with nonlinear systems. The EKF, an extension of the KF, addresses nonlinear systems by linearizing system dynamics and measurement functions around the current state estimate using first-order Taylor expansion. Despite its wide application, the EKF has limitations. Its linearization step introduces errors, particularly evident in highly nonlinear systems.

Further enhancing filtering accuracy, the IEKF iteratively refines the state estimate. Unlike the EKF, which relies on a single linearization, the IEKF iterates between prediction and update steps, linearizing around the current best estimate at each iteration. This iterative approach reduces errors introduced by linearization, thereby improving filtering performance, especially for highly nonlinear systems. However, achieving IEKF convergence may depend on initial conditions and could require parameter tuning for optimal performance.

Despite their effectiveness, the assumption of Gaussian noise distributions in these filters restricts their applicability in scenarios with non-Gaussian noise processes. While they can be adapted for non-Gaussian noises, achieving Linear

Minimum Mean Square Error (LMMSE) performance when precise linearization achieve.

A straightforward approach might involve calculating the true posterior at each time index then approximate it as Gaussian prior for next time index. Setting aside considerations of their linearization accuracy and stability, they consistently yield superior performance compared to LMMSE. However, the solvability of MMSE integration is often challenging due to the high-dimensional integration involved, necessitating the use of approximate approaches for Bayesian inference. Previous research has introduced several numerical methods based on message passing to obtain an analytic solution to the integration in the time update step, including generalized approximate message passing (GAMP) [7], vector approximate message passing (VAMP) [8], variational inference (VI) [9], etc.. Nevertheless, these methods typically assume a high system dimension, aiming to avoid complex matrix inversions, and are sensitive to the measurement matrix, which must be right rotationally invariant for VAMP and consist of i.i.d. sub-Gaussian elements for GAMP. Additionally, these algorithms only provide averaged variances. These limitations motivated the introduction of the revisited generalized vector approximate message passing (ReGVAMP) algorithm [10]. Leveraging the ReGVAMP algorithm [10], we approximate the posterior $p(x_k|y_{1:k})$ as Gaussian $q(x_k)$ prior for next time index. The key idea behind ReGVAMP is to approximate the extrinsic information into Gaussian form due to central limit theory. Consequently, the minimization of the Kullback-Leibler divergence (KLD) between $p(x_k|y_{1:k})$ and $q(x_k)$ is separated into several moment matching problems. Optimizing the approximated posterior $q(x_k)$ via KLD($p||q$) may yield better performance than $\text{KLD}(q||p)$ as VI does, as the former provides the exact posterior covariance matrix while the latter only offers correct diagonal precisions even when $p$ is Gaussian. Moreover, while this approximated multivariate Gaussian posterior may not be tight, it captures full second-order moments. Hence, from one perspective, ReGVAMP algorithm minimizes the desirable $\text{KLD}(p||q)$ approximately, made feasible by the asymptotic Gaussianity of the extrinsic. In this paper, we propose combining ReGVAMP with EKF and IEKF to handle different nonlinear and non-Gaussian models, coining them as ReGVAMP-EKF and ReGVAMP-EIEKF. Additionally, comparison experiments is designed to compare them to the unscented Kalman filter (UKF) [11]. Through our simulation, it is evident that when faced with
non-Gaussian noise, introducing the ReGVAMP algorithm to
handle such noise significantly enhances the performance of
both EKF and IEKF.

A. Notations

The operator (·)T denotes the matrix transpose. We denote
the Kullback-Leibler divergence between distributions p and
q as D<sub>KL</sub>(p||q). The notation N(x; μ, Σ) represents the
Gaussian distribution function evaluated at x with mean μ
and covariance matrix Σ. e<sub>i</sub> indicates a unit vector with only
the i th entry set to 1 and all others set to 0. δ(x) denotes the
Dirac delta distribution. I<sub>n</sub> denotes the n × n identity matrix.
The notation |x| is used to denote the absolute value of x.

II. GENERALITIES ON LINEAR STATE SPACE MODEL

Consider a non-linear state space model at each time step
k:

$$
x_k = h(x_{k-1}, \mathbf{u}_k, \mathbf{w}_k) \quad (1a)
$$

$$
y_k = g(x_k, v_k), \qquad (1b)
$$

where $x_k \in \mathbb{R}^N$, $\mathbf{u}_k \in \mathbb{R}^O$, and $\mathbf{y}_k \in \mathbb{R}^M$ represent the true unobserved state vector of the system, control vector, and only observation (measurement) respectively. The process noise $w_k \in \mathbb{R}^L$ drives the dynamic system and the observation noise is denoted as $v_k \in \mathbb{R}^C$. This entire process can be interpreted as a hidden Markov chain (HMC) [12]. While the standard Kalman filter is a powerful estimation tool, its algorithm begin to break down when the system being estimated is nonlinear. Fortunately, a version of the standard Kalman filter, known as the extended Kalman filter (EKF), has been extended to nonlinear systems and relies on linearization in estimating these nonlinear systems. Linearization operates on the principle that at a small section around a selected operating point a nonlinear function can be approximated as a linear function. This linearized function can be derived from the nonlinear function using the first-order terms in a Taylor series expansion shown in below:

$$
\begin{align}
h(\mathbf{x}_{k-1}, \mathbf{u}_k, \mathbf{w}_k) &\approx h(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k) + h_{\mathbf{x}_{k-1}}(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k) \nonumber \\
&\quad (\mathbf{x}_{k-1} - \hat{\mathbf{x}}_{k-1}) + h_{\mathbf{w}_k}(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k)(\mathbf{w}_k - \hat{\mathbf{w}}_k), \tag{2a}
\end{align}
$$

$$
\begin{equation}
\begin{split}
g(\mathbf{x}_k, \mathbf{v}_k) &\approx g(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k) + g_{\mathbf{x}_k}(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k)(\mathbf{x}_k - \hat{\mathbf{x}}_k) \\
&\quad + g_{\mathbf{v}_k}(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k)(\mathbf{v}_k - \hat{\mathbf{v}}_k),
\end{split}
\tag{2b}
\end{equation}
$$

where $h_{\mathbf{x}_{k-1}}(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k)$ and $h_{\mathbf{w}_k}(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k)$ denote the partial derivation with respect to (w.r.t.) $\mathbf{x}_{k-1}$ and $\mathbf{w}_k$ respectively at point $(\hat{\mathbf{x}}_{k-1}, \hat{\mathbf{w}}_k, \mathbf{u}_k)$; $g_{\mathbf{x}_k}(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k)$ and $g_{\mathbf{v}_k}(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k)$ denote the partial derivation w.r.t. $\mathbf{x}_k$ and $\mathbf{v}_k$ respectively at point $(\hat{\mathbf{x}}_k, \hat{\mathbf{v}}_k)$. Neglecting Taylor expansion terms of second and higher orders in (2), we can havesimplifies to:

$$
x_k = H_k x_{k-1} + F_k w_k + h_k, \quad (3a)
$$

$$
y_k = G_k x_k + B_k v_k + g_k, \quad (3b)
$$

where the matrices $H_k \in \mathbb{R}^{N \times N}$, $F_k \in \mathbb{R}^{N \times O}$, $G_k \in \mathbb{R}^{M \times N}$ and $B_k \in \mathbb{R}^{M \times C}$, and vectors $h_k \in \mathbb{R}^{N \times 1}$ and $g_k \in \mathbb{R}^{M \times 1}$ can be expressed as

$$
H_k = h_{x_{k-1}}(\hat{x}_{k-1}, \hat{w}_k, \mathbf{u}_k), \quad F_k = h_{w_k}(\hat{x}_{k-1}, \hat{w}_k, \mathbf{u}_k), \quad (4a)
$$

$$
G_k = g_{x_k}(\hat{x}_k, \hat{v}_k), \quad B_k = g_{v_k}(\hat{x}_k, \hat{v}_k), \quad (4b)
$$

$$
h_k = h(\hat{x}_{k-1}, \hat{w}_k, \mathbf{u}_k) - H_k \hat{x}_{k-1} - F_k \hat{w}_k, \quad (4c)
$$

$$
g_k = g(\hat{x}_k, \hat{v}_k) - G_k \hat{x}_k - B_k \hat{v}_k. \quad (4d)
$$

In accordance with the HMC, at each time step *k*, for brevity,
we omit writing *y*<sub>1:k−1</sub> in the probability density function
(pdf) in the rest of paper, such as *p*(*x*<sub>*k*−1</sub>|*y*<sub>1:k−1</sub>) denoted as

$p(x_{k-1})$. Here, the estimated pdf of the previous state $x_{k-1}$ is represented as $p(x_{k-1}) = \mathcal{N}(x_{k-1} | \hat{x}_{k-1}, P_{k-1})$. For the prediction phase, leveraging Bayes's rule, the predictive pdf of $x_k$ given $w_k$ can be deduced as follows:

$$
\begin{align}
p(x_k|w_k) &= \int p(x_k|x_{k-1}, w_k)p(x_{k-1})dx_{k-1} \tag{5} \\
&= \int \delta(H_k x_{k-1} + F_k w_k + h_k - x_k)\mathcal{N}(x_{k-1}; \hat{x}_{k-1}, P_{k-1})dx_{k-1} \nonumber \\
&= \mathcal{N}(x_k; H_k \hat{x}_{k-1} + F_k w_k + h_k, H_k P_{k-1} H_k^T). \nonumber
\end{align}
$$

Combining this with the prior of $w_k$, the predictive prior of $x_k$ can be articulated as:

$$
p(x_k) = \int p(x_k|w_k) \prod_{i=1}^{L} p(w_{ki}) dw_k = \int p(x_k, w_k) dw_k. \quad (6)
$$

In the measurement phase, the likelihood pdf $p(y_k|x_k)$ can be
computed as:

$$
\begin{align}
p(y_k | x_k) &= \int p(y_k | x_k, v_k) p(v_k) dv_k \nonumber \\
&= \int \delta(y_k - G_k x_k - B_k v_k - g_k) \prod_{j=1}^{C} p(v_{kj}) dv_k. \tag{7}
\end{align}
$$

With (6) and (7), in the update step, the posterior of $x_k$ given $y_k$ can be calculated as:

$$
p(x_k|y_k) = \frac{p(y_k|x_k)p(x_k)}{\int p(y_k|x_k)p(x_k)dx_k} \quad (8a)
$$

$$
= \frac{\iint p(y_k | v_k, x_k) p(x_k | w_k) p(w_k) p(v_k) dv_k dw_k}{\iint p(y_k | v_k, x_k) p(x_k | w_k) p(w_k) p(v_k) dv_k dw_k dv_k dw_k} \quad (8b)
$$

$$
= \frac{\iint p(y_k, x_k, w_k, v_k) dw_k dv_k}{\iiint p(y_k, x_k, w_k, v_k) dw_k dv_k dw_k dv_k}. \quad (8c)
$$

When both measurement and process noises follow Gaussian
distributions, (8a) remains Gaussian, facilitating straightfor-
ward calculation and leading to the Kalman filter. However,
if either the measurement noise $v_k$ or process noise $w_k$
deviates from Gaussianity, exhibiting non-Gaussian distribu-
tions like Gaussian mixture model (GMM) distributions, the
integration in (6) and (7) might become intractable or involve
costly computational complexity. Therefore, we introduce the
ReGVAMP-EKF algorithm in the next section.

III. DERIVATION OF THE REGVAMP-EKF

This iterative approach reduces errors introduced by lin-
earization, thereby improving filtering performance, espe-
cially for highly nonlinear systems. In the ReGVAMP-
EKF algorithm framework, the primary objective is to ap-
proximate $p(\mathbf{x}_k, \mathbf{y}_k, \mathbf{w}_k, \mathbf{v}_k)$ in (8c) using a Gaussian pdf
$q(\mathbf{x}_k, \mathbf{y}'_k, \mathbf{w}_k, \mathbf{v}_k)$ with $\mathbf{y}'_k = \mathbf{y}_k - \mathbf{g}_k$ such that:

$$
\begin{align}
p(\mathbf{x}_k, \mathbf{y}_k, \mathbf{w}_k, \mathbf{v}_k) &\approx q(\mathbf{x}_k, \mathbf{y}'_k, \mathbf{w}_k, \mathbf{v}_k) \nonumber \\
&= p(\mathbf{x}_k | \mathbf{w}_k) \delta(\mathbf{y}'_k - \mathbf{G}_k \mathbf{x}_k - \mathbf{B}_k \mathbf{v}_k) \prod_{i=1}^{L} f(w_{ki}) \prod_{j=1}^{C} f(v_{kj}). \tag{9}
\end{align}
$$

All $f(w_{ki})$ and $f(v_{kj})$ in (9) are assumed to be Gaussian
factors, expressed as:

$$
f(w_{ki}) = \mathcal{N}(w_{ki}; \hat{w}_{ki}, \tau w_{ki}), f(w_k) = \mathcal{N}(w_k; \hat{w}_k, T_w k); \quad (10a)
$$

$$
f(v_{kj}) = \mathcal{N}(v_{kj}; \hat{v}_{kj}, \tau v_{kj}), f(v_k) = \mathcal{N}(v_k; \hat{v}_k, T_v k), \quad (10b)
$$

where the *i*th entry of $\hat{w}_k \in \mathbb{R}^L$ and the *j*th entry of $\hat{v}_k \in \mathbb{R}^C$
are denoted as $\hat{w}_{ki}$ and $\hat{v}_{kj}$ respectively. Moreover, $T_{w_k} \in$
$\mathbb{R}^{L \times L}$ and $T_{v_k} \in \mathbb{R}^{C \times C}$ are diagonal matrices with the *i*th and
*j*th diagonal entries represented as $\tau_{w_{ki}}$ and $\tau_{v_{kj}}$ respectively.

The ReGVAMP-EKF algorithm for each time index, detailed in Algorithm 1, will be explained as below.
Algorithm 1 ReGVAMP-EKF Algorithm at time index k

Input: $p(\mathbf{x}_{k-1})$ and $\mathbf{y}_k$

1: Initialization: All $f(w_{ki})$, $f(v_{kj})$, $\mathbf{H}_k$, $\mathbf{F}_k$, $\mathbf{G}_k$, $\mathbf{B}_k$, $\mathbf{h}_k$, $\mathbf{g}_k$ and $\mathbf{y}'_k$

2: repeat

3: Initialization of index $i \leftarrow 0$ and $j \leftarrow 0$

4: Update $q(\mathbf{y}'_k|\mathbf{w}_k)$ in (11)

5: repeat

6: $i \leftarrow i + 1$

7: Update extrinsic $m(w_{ki})$ of $w_{ki}$ in (15)

8: Update approximated posterior $\hat{b}_i(w_{ki})$ of $w_{ki}$ via KL divergence in (18)

9: Update $f(w_{ki})$ in (20)

10: until $i = L$

11: Update $q(\mathbf{y}'_k|\mathbf{v}_k)$ in (22)

12: repeat

13: $j \leftarrow j + 1$

14: Update extrinsic $m(v_{kj})$ of $v_{kj}$ in (26)

15: Update approximated posterior $\hat{b}_j(v_{kj})$ of $v_{kj}$ via KL divergence in (29)

16: Update $f(v_{kj})$ in (31)

17: until $j = C$

18: until All $f(w_{ki})$ and $f(z_{kj})$ converge or maximum iteration times

19: Calculate approximated posterior $q(\mathbf{x}_k|\mathbf{y}'_k)$ in (34)

**Output:** Treat $q(\mathbf{x}_k|\mathbf{y}'_k)$ as a prior $p(\mathbf{x}_k)$ of $\mathbf{x}_k$ for $k+1$ index

A. Initialization

If we know the prior distributions of the noises, we can initialize the parameters in (10a) and (10b) with the actual mean and variance of each process noise $p(w_{ki})$ and measurement noise $p(v_{kj})$ respectively. However, when all parameters are unknown, combining ReGVAMP-EKF with Expectation Maximization (EM) [13] allows for a feasible solution. In such cases, initializing all means to 0 and all variances to 1 serves as a pragmatic approach. Nonetheless, addressing this scenario is considered beyond the current scope of our contribution. For the Taylor expansion, we choose the expanding point around the posterior mean of the process noise, measurement noise, and previous state mean. For initialization, we use the prior means.

B. Update $q(\mathbf{y}'_k|\mathbf{w}_k)$

With initialized $f(z_{kj})$, according to Bayes' rule, the approximated likelihood $q(\mathbf{y}'_k|\mathbf{w}_k)$ can be expressed as:

$$ 
\begin{aligned}
q(\mathbf{y}'_k|\mathbf{w}_k) &= \iint p(\mathbf{x}_k|\mathbf{w}_k) \delta(\mathbf{y}'_k - \mathbf{G}_k \mathbf{x}_k - \mathbf{B}_k \mathbf{v}_k) f(\mathbf{v}_k) \mathrm{d}\mathbf{x}_k \\
&= \mathcal{N}(\mathbf{y}'_k; \mathbf{G}_k \mathbf{F}_k \mathbf{w}_k + \mathbf{G}_k \mathbf{H}_k \hat{\mathbf{x}}_{k-1} + \mathbf{G}_k \mathbf{h}_k + \mathbf{B}_k \hat{\mathbf{v}}_k, \\
& \qquad \mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T + \mathbf{B}_k \mathbf{T}_{v_k} \mathbf{B}_k^T).
\end{aligned} 
\quad (11) $$

C. Update all $f(w_{ki})$

1) Update extrinsic $m(w_{ki})$ of $w_{ki}$: Since both $q(\mathbf{w}_k)$ and $f(w_{kn})$ are Gaussian pdfs, we can define an approximate posterior $\tilde{q}(\mathbf{w}_k)$ of $\mathbf{w}_k$ as follows:

$$ \tilde{q}(\mathbf{w}_k) = \mathcal{N}(\mathbf{w}_k; \mathbf{m}_{\mathbf{w}_k}, \mathbf{C}_{\mathbf{w}_k}) = \frac{q(\mathbf{y}'_k|\mathbf{w}_k) \prod_{i=1}^{L} f(w_{ki})}{\int q(\mathbf{y}'_k|\mathbf{w}_k) \prod_{i=1}^{L} f(w_{ki}) \mathrm{d}\mathbf{w}_k}, \quad (12) $$

where

$$ C_{w_k} = (T_{w_k}^{-1} + \mathbf{F}_k^T \mathbf{G}_k^T (\mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T + \mathbf{B}_k \mathbf{T}_{v_k} \mathbf{B}_k^T)^{-1} \mathbf{G}_k \mathbf{F}_k)^{-1}; \quad (13) $$

$$ \begin{aligned} \mathbf{m}_{w_k} = & \mathbf{C}_{w_k} [\mathbf{G}_k^T \mathbf{H}_k^T (\mathbf{T}_{z_k} + \mathbf{F}_k^T \mathbf{G}_k^T (\mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T + \mathbf{B}_k \mathbf{T}_{v_k} \mathbf{B}_k^T)^{-1} (\mathbf{y}'_k - (\mathbf{G}_k \mathbf{H}_k \hat{\mathbf{x}}_{k-1} + \mathbf{G}_k \mathbf{h}_k + \mathbf{B}_k \hat{\mathbf{v}}_k)) + \mathbf{T}_{w_k}^{-1} \hat{\mathbf{w}}_k]^T \\ & + \mathbf{T}_{w_k}^{-1} \hat{\mathbf{w}}_k]. \quad (14) \end{aligned} $$

Removing $f(w_{ki})$, the extrinsic $m(w_{ki})$ can be calculated as:

$$ m(w_{ki}) = \frac{\int \tilde{q}(\mathbf{w}_k) \mathrm{d}\mathbf{w}_{k \setminus i} / f(w_{ki})}{\iint \tilde{q}(\mathbf{w}_k) \mathrm{d}\mathbf{w}_{k \setminus i} / f(w_{ki}) \mathrm{d}\mathbf{w}_{ki}} = \mathcal{N}(w_{ki}; \mu_{w_{ki}}, \xi_{w_{ki}}), \quad (15) $$

where

$$ \xi_{w_{ki}} = \frac{\tau_{w_{ki}} e_i^T \mathbf{C}_{w_k} e_i}{\tau_{w_{ki}} - e_i^T \mathbf{C}_{w_k} e_i}; \quad (16) $$

$$ \mu_{w_{ki}} = \xi_{w_{ki}} \left( \frac{e_i^T \mathbf{m}_{w_k}}{e_i^T \mathbf{C}_{w_k} e_i} - \frac{\hat{\mathbf{w}}_{ki}}{\tau_{w_{ki}}} \right) $$

2) Update approximated posterior (belief) $\hat{b}_i(w_{ki})$ of $w_{ki}$ via KL divergence: Firstly, defining $\hat{b}_i(w_{ki})$ as a Gaussian pdf:

$$ \hat{b}_i(w_{ki}) = \mathcal{N}(w_{ki}; \alpha_{w_{ki}}, \beta_{w_{ki}}), \quad (17) $$

then combining the extrinsic $m(w_{ki})$ from (15) with the actual prior $p(w_{ki})$ and minimizing the Kullback-Leibler (KL) divergence to obtain an approximate marginal posterior (belief) $\hat{b}_i(w_{ki})$:

$$ \hat{b}_i(w_{ki}) = \arg \min_{\hat{b}_i(w_{ki})} D_{KL} \left[ \frac{m(w_{ki}) p(w_{ki})}{\int m(w_{ki}) p(w_{ki}) \mathrm{d}\mathbf{w}_{ki}} \middle\| \hat{b}_i(w_{ki}) \right]. \quad (18) $$

With some mathematical algebra, the solution to (18) can be provided as:

$$ \alpha_{w_{ki}} = \frac{\int w_{ki} m(w_{ki}) p(w_{ki}) \mathrm{d}\mathbf{w}_{ki}}{\int m(w_{ki}) p(w_{ki}) \mathrm{d}\mathbf{w}_{ki}} = g_{w_{ki}}(\mu_{w_{ki}}, \xi_{w_{ki}}); \quad (19a) $$

$$ \beta_{w_{ki}} = \frac{\int (\alpha_{w_{ki}} - w_{ki})^2 m(w_{ki}) p(w_{ki}) \mathrm{d}\mathbf{w}_{ki}}{\int m(w_{ki}) p(w_{ki}) \mathrm{d}\mathbf{w}_{ki}} = \xi_{w_{ki}} \frac{\partial g_{w_{ki}}(\mu_{w_{ki}}, \xi_{w_{ki}})}{\partial \mu_{w_{ki}}}. \quad (19b) $$

3) Update $f(w_{ki})$: Once we obtain $\hat{b}_i(w_{ki})$, removing the extrinsic $m(w_{ki})$, we can update $f(w_{ki})$ as:

$$ f(w_{ki}) = \frac{\hat{b}_i(w_{ki}) / m(w_{ki})}{\int \hat{b}_i(w_{ki}) / m(w_{ki}) \mathrm{d}\mathbf{w}_{ki}} = \mathcal{N}(w_{ki}; \hat{w}_{ki}, \tau_{w_{ki}}), \quad (20) $$

where the updated variance $\tau_{w_{ki}}$ and updated mean $\hat{w}_{ki}$ can be expressed as:

$$ \tau_{w_{ki}} = \frac{\xi_{w_{ki}} \beta_{w_{ki}}}{\xi_{w_{ki}} - \beta_{w_{ki}}}, \quad (21) $$

$$ \hat{w}_{ki} = \tau_{w_{ki}} \left( \frac{\beta_{w_{ki}}}{\alpha_{w_{ki}}} - \frac{\mu_{w_{ki}}}{\xi_{w_{ki}}} \right). $$

D. Update $q(\mathbf{y}'_k|\mathbf{v}_k)$

Similar to III-B, with $f(\mathbf{w}_k)$, the approximated likelihood $q(\mathbf{y}'_k|\mathbf{v}_k)$ can be expressed as:

$$ 
\begin{aligned}
q(\mathbf{y}'_k|\mathbf{v}_k) &= \iint p(\mathbf{x}_k|\mathbf{w}_k) \delta(\mathbf{y}'_k - \mathbf{G}_k \mathbf{x}_k - \mathbf{B}_k \mathbf{v}_k) f(\mathbf{w}_k) \mathrm{d}\mathbf{x}_k \mathrm{d}\mathbf{v}_k \\
&= \mathcal{N}(\mathbf{y}'_k; \mathbf{B}_k \mathbf{v}_k + \mathbf{G}_k \mathbf{H}_k \hat{\mathbf{x}}_{k-1} + \mathbf{G}_k \mathbf{F}_k \mathbf{w}_k + \mathbf{G}_k \mathbf{h}_k, \\
& \qquad \mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T + \mathbf{G}_k \mathbf{F}_k \mathbf{T}_{w_k} \mathbf{F}_k^T \mathbf{G}_k^T).
\end{aligned} 
\quad (22) $$

E. Update all $f(v_{kj})$

1) Update extrinsic $m(v_{kj})$ of $v_{kj}$: Since both $q(\mathbf{y}'_k|\mathbf{v}_k)$ and $f(v_{kj})$ are Gaussian pdfs, we can define an approximated posterior $\tilde{q}(v_k)$ of $v_k$ as follows:

$$ \tilde{q}(v_k) = \mathcal{N}(v_k; \mathbf{m}_{v_k}, \mathbf{C}_{v_k}) = \frac{q(\mathbf{y}'_k|v_k) \prod_{j=1}^{C} f(v_{kj})}{\int q(\mathbf{y}'_k|v_k) \prod_{j=1}^{C} f(v_{kj}) \mathrm{d}v_k}, \quad (23) $$

where

$$ \mathbf{C}_{v_k} = (T_{v_k}^{-1} + \mathbf{B}_k^T (\mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T + \mathbf{G}_k \mathbf{F}_k \mathbf{T}_{w_k} \mathbf{F}_k^T \mathbf{G}_k^T)^{-1} \mathbf{B}_k)^{-1}; \quad (24) $$
$$
\mathbf{m}_{v_k} = \mathbf{C}_{v_k} [B_k^T (\mathbf{G}_k \mathbf{H}_k \mathbf{P}_{k-1} \mathbf{H}_k^T \mathbf{G}_k^T \mathbf{G}_k \mathbf{F}_k \mathbf{T}_{w_k} \mathbf{F}_k^T \mathbf{G}_k^T)^{-1} \\
(y_k' - (\mathbf{G}_k \mathbf{H}_k \hat{\mathbf{x}}_{k-1} + \mathbf{G}_k \mathbf{F}_k \mathbf{w}_k + \mathbf{G}_k \mathbf{h}_k)) + \mathbf{T}_{v_k}^{-1} \hat{\mathbf{v}}_k]. \quad (25)
$$

Removing $f(v_{kj})$, the extrinsic $m(v_{kj})$ can be calculated as
follows:

$$
m(v_{kj}) = \frac{\int \bar{q}(v_k) dv_{k \setminus j} / f(v_{kj})}{\iint \bar{q}(v_k) dv_{k \setminus j} / f(v_{kj}) dv_{kj}} = \mathcal{N}(v_{kj}; \mu_{v_{kj}}, \xi_{v_{kj}}), \quad (26)
$$

where

$$
\xi_{v_{kj}} = \frac{\tau_{v_{kj}} e_j^T \mathbf{C}_{v_k} e_j}{\tau_{v_{kj}} e_j^T \mathbf{C}_{v_k} e_j}; \quad (27a)
$$

$$
\mu_{v_{kj}} = \xi_{v_{kj}} \left( \frac{e_j^T \mathbf{m}_{v_k}}{e_j^T \mathbf{C}_{v_k} e_j} - \frac{\hat{v}_{kj}}{\tau_{v_{kj}}} \right). \quad (27b)
$$

2) Update approximated posterior (belief) $\hat{b}_j(v_{kj})$ of $v_{kj}$ via KL divergence: Defining $\hat{b}_j(v_{kj})$ as a Gaussian pdf:

$$
\hat{b}_j(v_{kj}) = \mathcal{N}(v_{kj}; \alpha_{v_{kj}}, \beta_{v_{kj}}). \quad (28)
$$

We combine the extrinsic $m(v_{kj})$ with the real prior $p(v_{kj})$ and minimize the KL divergence to obtain an approximate marginal posterior (belief) $\hat{b}_j(v_{kj})$.

$$
\hat{b}_j(v_{kj}) = \arg \min_{b_j(v_{kj})} D_{KL} \left[ \frac{m(v_{kj})p(v_{kj})}{\int m(v_{kj})p(v_{kj})dv_{kj}} \middle\| b_j(v_{kj}) \right]. \quad (29)
$$

Following the approach used for (18), we can derive the
solution for (29) as:

$$
\alpha_{v_{kj}} = \frac{\int v_{kj} m(v_{kj}) p(v_{kj}) dv_{kj}}{\int m(v_{kj}) p(v_{kj}) dv_{kj}} = g_{v_{kj}} (\mu_{v_{kj}}, \xi_{v_{kj}}); \quad (30a)
$$

$$
\beta_{v_{kj}} = \frac{\int (\alpha_{v_{kj}} - v_{kj})^2 m(v_{kj}) p(v_{kj}) dz_{kj}}{\int m(v_{kj}) p(v_{kj}) dv_{kj}} = \xi_{v_{kj}} \frac{\partial g_{v_{kj}}(\mu_{v_{kj}}, \xi_{v_{kj}})}{\partial \mu_{v_{kj}}}. \quad (30b)
$$

3) Update $f(v_{kj})$: Removing the extrinsic $m(v_{kj})$ from belief $\hat{b}_j(v_{kj})$, the approximated factor $f(v_{kj})$ can be updated as

$$
f(v_{kj}) = \frac{\hat{b}_j(v_{kj}) / m(v_{kj})}{\int \hat{b}_j(v_{kj}) / m(v_{kj}) dv_{kj}} = \mathcal{N}(v_{kj}; \hat{v}_{kj}, \tau_{v_{kj}}), \quad (31)
$$

where the updated variance $\tau_{v_{kj}}$ and mean $\hat{v}_{kj}$ can be expressed as

$$
\tau_{v_{kj}} = \frac{\xi_{v_{kj}} \beta_{v_{kj}}}{\xi_{v_{kj}} - \beta_{v_{kj}}}, \quad (32a)
$$

$$
\hat{v}_{kj} = \tau_{v_{kj}} \left( \frac{\beta_{v_{kj}}}{\alpha_{v_{kj}}} - \frac{\mu_{v_{kj}}}{\xi_{v_{kj}}} \right). \quad (32b)
$$

F. Convergence judgment criteria

Convergence is not assured with the ReGVAMP. In the
majority of cases, however, convergence is assured with the
ReGVAMP algorithm within simulations. As for the conver-
gence judgment criteria, we choose:

$$
\sum_{i=1}^{L} \| \hat{w}_{ki}^{new} - \hat{w}_{ki} \|_{2}^{2} + \sum_{j=1}^{C} \| \hat{v}_{kj}^{new} - \hat{v}_{kj} \|_{2}^{2} < \epsilon, \quad (33)
$$

where $\hat{w}_{ki}^{new}$ and $\hat{v}_{kj}^{new}$ indicate the updated mean of $w_{ki}$ and $v_{kj}$, respectively. $\epsilon$ is set to be $10^{-3}$ in our numerical simulation, and the maximum iteration time is set to be 10.

G. Calculate approximated posterior $q(\mathbf{x}_k|\mathbf{y}'_k)$

With the approximated $f(\mathbf{w}_k)$ and $f(\mathbf{z}_k)$, $q(\mathbf{x}_k|\mathbf{y}'_k)$ can be calculated as follows:

$$
\begin{align}
q(\mathbf{x}_k | \mathbf{y}'_k) &= \frac{\iint p(\mathbf{x}_k | \mathbf{w}_k) \delta(\mathbf{y}'_k | \mathbf{G}_k \mathbf{x}_k - \mathbf{B}_k \mathbf{v}_k) f(\mathbf{w}_k) f(\mathbf{v}_k) dv_k dw_k}{\iiint p(\mathbf{x}_k | \mathbf{w}_k) \delta(\mathbf{y}'_k | \mathbf{G}_k \mathbf{x}_k - \mathbf{B}_k \mathbf{v}_k) f(\mathbf{w}_k) f(\mathbf{v}_k) dv_k dw_k d\mathbf{x}_k} \nonumber \\
&= \mathcal{N}(\mathbf{x}_k | \hat{\mathbf{x}}_k, \mathbf{P}_k), \tag{34}
\end{align}
$$

where

$$
P_k = [G_k^T (B_k T_{v_k} B_k^T)^{-1} G_k + (H_k P_{k-1} H_k^T + F_k T_{w_k} F_k^T)^{-1}]^{-1}. \quad (35a)
$$

$$
\hat{x}_k = P_k [G_k^T (B_k T_{v_k} B_k^T)^{-1} (y_k' - B_k \hat{v}_k) + (H_k P_{k-1} H_k^T + F_k T_{w_k} F_k^T)^{-1} (H_k \hat{x}_{k-1} + F_k \dot{w}_k + h_k)]. \quad (35b)
$$

For the next time step $k+1$, $q(\mathbf{x}_k|\mathbf{y}'_k)$ is set as the approximated pdf of time step $\mathbf{x}_k$.

IV. DERIVATION OF THE REGVAMP-IEKF

Further enhancing filtering accuracy, the IEKF iteratively
refines the state estimate. Unlike the EKF, which relies on
a single linearization, the IEKF iterates between prediction
and update steps, linearizing around the current best estimate
at each iteration. The ReGVAMP-EKF is extended to the
ReGVAMP-IEKF by incorporating iterative updates at each
time step, where the state estimate is refined through repeated
linearization around the updated estimate until convergence.
This approach enhances the accuracy of state estimation in
highly nonlinear systems. The ReGVAMP-IEKF can be found
in Algorithm 2.

**Algorithm 2** ReGVAMP-IEKF Algorithm at time index *k*

Input: $p(\mathbf{x}_{k-1})$ and $\mathbf{y}_k$

1: Initialization: All $f(w_{ki})$, $f(v_{kj})$, $\mathbf{H}_k$, $\mathbf{F}_k$, $\mathbf{G}_k$, $\mathbf{B}_k$, $\mathbf{h}_k$, $\mathbf{g}_k$ and $\mathbf{y}'_k$

2: repeat

3: Update $f(w_{ki})$, $f(v_{kj})$ and $q(\mathbf{x}_k|\mathbf{y}'_k)$ via ReGVAMP-EKF algorithm until converge.

4: Update $\mathbf{H}_k$, $\mathbf{F}_k$, $\mathbf{G}_k$, $\mathbf{B}_k$, $\mathbf{h}_k$, $\mathbf{g}_k$ and $\mathbf{y}'_k$

5: **until** Until $f(w_{ki})$, $f(v_{kj})$, $q(\mathbf{x}_k|\mathbf{y}'_k)$ converged

6: Output the approximate posterior $q(\mathbf{x}_k|\mathbf{y}'_k)$

**Output:** Treat $q(\mathbf{x}_k|\mathbf{y}'_k)$ as a prior $p(\mathbf{x}_k)$ of $\mathbf{x}_k$ for $k+1$ index

V. NUMERICAL SIMULATION

We have designed a fully nonlinear state-space model to
describe the motion trajectory of an object. The state vari-
ables include position $p_x, p_y$, and velocity $v_x, v_y$. Both the
state transition and observation equations are nonlinear. The
specific equations are as follows: The state vector $\mathbf{x}_k =$
$[p_{x,k}, p_{y,k}, v_{x,k}, v_{y,k}]^T$ represents the combination of position
and velocity. The nonlinear state transition equation is:

$$
\mathbf{x}_k = \begin{bmatrix} p_{x,k-1} + T \cdot \sin(v_{x,k-1}) \\ p_{y,k-1} + T \cdot \cos(v_{y,k-1}) \\ v_{x,k-1} + T \cdot \sin(p_{x,k-1}) \\ v_{y,k-1} + T \cdot \cos(p_{y,k-1}) \end{bmatrix} + \mathbf{w}_k \quad (36)
$$

where $T$ is the sampling time which is set to be 0.05 in
our simulation and $\mathbf{w}_k$ represents process noise. The initial-
ized state $\mathbf{x}_0$ is set to in Gaussian distribution with means
$[1 \ 1 \ 0.1 \ 0.1]^T$ and covariance matrix $\mathbf{I}_4$. The observation
vector consists of the measured distance and angle, modeled
by the following nonlinear observation equation:

$$
\mathbf{y}_k = \begin{bmatrix} \sqrt{p_{x,k}^2 + p_{y,k}^2} \\ \arctan\left(\frac{p_{y,k}}{p_{x,k}}\right) \end{bmatrix} + \mathbf{v}_k \quad (37)
$$
where $v_k$ represents observation noise. To simulate a complex noise environment, both process noise $w_k$ and observation noise $v_k$ are modeled as the GMM noise. The GMM is represented as a weighted sum of multiple Gaussian distributions, described as: Process Noise $w_k$:

$$w_k \sim 0.3\mathcal{N}(0, \mathbf{I}_4) + 0.7\mathcal{N}(0, 0.09\mathbf{I}_4); \quad (38a)$$

$$v_k \sim 0.3\mathcal{N}(0, \mathbf{I}_2) + 0.7\mathcal{N}(0, 0.09\mathbf{I}_2). \quad (38b)$$

The Averaged Cumulative Mean Error (ACME) is a metric used to evaluate the performance of an estimated vector $\hat{x}$ compared to its ground truth counterpart $x$ over multiple time steps. The ACME of $\hat{x}_T(i)$ is defined as:

$$\text{ACME}_{x_T(i)} = \frac{1}{T} \sum_{t=1}^{T} |x_t(i) - \hat{x}_t(i)|, \quad (39)$$

where $T$ is the total number of time steps. In our simulation, we assume both of them are known during simulations. Fig. 1 illustrates the true positions, velocities, and their estimations and Fig 2 shows the ACME results. It can be seen that the performance of EKF and IEKF is improved by combining the ReGVAMP algorithm. Although the UKF demonstrates similar performance, it frequently crashes in our simulations due to arithmetic precision issues. The simulation codes can be found in https://github.com/FqXIAO/ReGVAMP-EKF-IEKF.git.

<figure><img src="image_2.png" /><figcaption>Fig. 1. Estimation Results of Each State</figcaption></figure>

## VI. CONCLUSION

This paper introduces the ReGVAMP-EKF filter to address the filtering problem in stochastic systems characterized by nonlinear system models and non-Gaussian noise. Unlike traditional methods that rely on MMSE, the ReGVAMP algorithm is utilized in this filter to approximate the posterior of each state vector $x_k$ as Gaussian. This capability allows the filter to effectively handle non-Gaussian noise while maintaining computational complexity similar to that of the Extended Kalman Filter (EKF). Numerical examples demonstrate that the ReGVAMP-EKF and ReGVAMP-IEKF achieve high performance, especially compared to the EKF and IEKF.

<figure><img src="image_3.png" /><figcaption>Fig. 2. Averaged Cumulative Mean Error Comparison</figcaption></figure>

However, future research should include more comprehensive comparisons with other state-of-the-art methods. Additionally, further investigation is needed to analyze the performance and convergence of the ReGVAMP-EKF and ReGVAMP-IEKF, particularly in larger systems and with unknown hyperparameters, through extensive simulations.

**Acknowledgements** EURECOM's research is partially supported by its industrial members: ORANGE, BMW, SAP, iABG, Norton LifeLock, Principauté de Monaco, and by the Franco-German projects 5G-OPERA and CellFree6G.

## REFERENCES

[1] R. Kalman, "A New Approach to Linear Filtering and Prediction Problems," *J. Basic. Eng.*, vol. 82, no. 1, pp. 35-45, 1960.

[2] K. Jahan and S. K. Rao, "Implementation of Underwater Target Tracking Techniques for Gaussian and non-Gaussian Environments," *Comput. Electr. Eng.*, vol. 87, p. 106783, 2020.

[3] R. Bass, V. Norum, and L. Schwartz, "Optimal Multichannel Nonlinear Filtering," *J. Math. Anal. Appl.*, vol. 16, no. 1, pp. 152-164, 1966.

[4] J. Havlík and O. Straka, "Performance evaluation of iterated extended Kalman filter with variable step-length," *J. Phys. Conf.*, vol. 659, no. 1, p. 012022, nov 2015.

[5] P. Rohal and J. Ochodnicky, "Radar Target Tracking by Kalman and Particle Filter," in *IEEE KIT*, 2017, pp. 1-4.

[6] M. Jiang, S. Guo, H. Luo, Y. Yao, and G. Cui, "A Robust Target Tracking Method for Crowded Indoor Environments Using mmWave Radar," *Remote Sens.*, vol. 15, no. 9, p. 2425, 2023.

[7] S. Rangan, "Generalized Approximate Message Passing for Estimation with Random Linear Mixing," in *IEEE ISIT*, 2011, pp. 2168-2172.

[8] S. Rangan, P. Schniter, and A. K. Fletcher, "Vector approximate message passing," *IEEE Trans. Inf. Theory*, vol. 65, no. 10, pp. 6664-6684, 2019.

[9] D. M. Blei, A. Kucukelbir, and J. D. McAuliffe, "Variational inference: A review for statisticians," *J. Am. Stat. Assoc.*, vol. 112, no. 518, pp. 859-877, 2017.

[10] Z. Zhao, F. Xiao, and D. Slock, "Vector Approximate Message Passing for Not So Large N.I.I.D. Generalized I/O Linear Models," in *IEEE ICASSP*, 2024, pp. 13 281-13 285.

[11] E. Wan and R. Van Der Merwe, "The Unscented Kalman Filter for Nonlinear Estimation," in *IEEE AS-SPCC*, 2000, pp. 153-158.

[12] S. R. Eddy, "Hidden Markov models," *Curr. Opin. Struct. Biol.*, vol. 6, no. 3, pp. 361-365, 1996.

[13] T. K. Moon, "The expectation-maximization algorithm," *IEEE Signal Process. Mag.*, vol. 13, no. 6, pp. 47-60, 1996.