1 Introduction↩︎

Inferring the hidden state of dynamical systems in real time from the noisy measurements forms the backbone of modern state estimation and control, with many applications including the simultaneous localization and mapping in robotics [1], the camera motion estimation in signal processing [2], the weather prediction in meteorology [3], the target tracking in control [4], to name only a few. The pioneering works of this field trace back to a paper co-authored by Rudolf Kalman and Richard S. Bucy [5], which gives an unbiased minimum estimator under a linear model with Gaussian noise. It is further extended by Schmidt’s group for nonlinear systems with application to solving the Apollo guidance and navigation [6]. After that, many variants of the Kalman filter (KF), e.g., UKF [7], CKF [8], correntropy-based KF [9], consensus-based filter [10], have been developed with special emphasis on the nonlinear systems, non-Gaussian noise, and distributed systems.

Many estimators assume exact knowledge of the noise statistics. However, this assumption is rarely satisfied in practical applications. One important reason is that an exact system model is usually unattainable, and approximations are inevitable. In such cases, unmodeled dynamics can be interpreted as time-varying process or measurement noises. Modeling errors may also arise from the linearization of nonlinear systems. For example, in the extended Kalman filter (EKF), the system is linearized at the a priori estimate of the state using a first-order approximation, implying that the resulting modeling errors depend on the degree of system nonlinearity. If not properly handled, such errors may induce a positive feedback loop and lead to filter divergence [11].

Operational conditions and external environments also contribute to time-varying noise statistics. For instance, in quadruped robot localization, contact-related noise varies as the robot traverses different terrains [12]. In Global Positioning System (GPS) navigation, measurement noise statistics change with satellite visibility and are further affected by multipath and non-line-of-sight (NLOS) propagation [13]. Moreover, false data injection (FDI) and denial-of-service (DoS) attacks can also be modeled as switched, or time-varying noise statistics [14], [15], which has attracted increasing attention in cyber-physical system security.

There exist several lines of research on state estimation with time-varying noise statistics, which can be broadly classified into model-based and learning-based approaches. The former category includes moment matching [16], innovation-based methods [17], robust loss–based estimators [9], [18], interacting multiple-model (IMM) approaches [19], and variational Bayesian (VB) inference [4], [20]. Covariance matching and innovation-based methods typically rely on second-order statistics of the residuals, which require a sufficiently large sliding window to obtain reliable covariance estimates and may suffer from a risk of divergence [21]. Robust loss–induced estimators offer guaranteed convergence; however, by discarding historical noise covariance information, they exhibit limited adaptivity. The IMM-based approach employs a bank of Kalman filters to approximate the unknown noise covariance, but suffers a significant computational burden and becomes impractical for high-dimensional systems, as the number of required filters grows exponentially with the system dimension [19]. The VB-based approach, originally proposed by Särkkä and Nummenmaa [4], jointly estimates the state and noise variance by minimizing the Kullback–Leibler divergence between the true posterior and a designated distribution. It achieves performance comparable to that of IMM-based methods while significantly reducing the computational cost, and is favored by many researches.

Unlike purely model-based methods, KalmanNet [22] integrates the KF framework with Gated Recurrent Units (GRUs) to alleviate model mismatch by learning a specialized Kalman gain. This architecture has been further extended in [23] and [24], where Transformers and deep neural networks (DNNs) replace the GRUs to offer alternative performance trade-offs. More recently, Buchnik et al. [25] introduced latent-KalmanNet, which operates in a latent space to jointly learn feature representations and filtering operations, achieving notable performance gains in visual target tracking. Dahan et al. [26] further augmented it by incorporating uncertainty quantification through a Bayesian Kalman architecture. Another emerging line of research integrates generative modeling with state estimation, leveraging normalizing flows or diffusion models [27], [28] to enable high-dimensional, multimodal estimation and extend standard Gaussian posteriors to much more complex distributions.

It is worth noting that while learning-based methods raise the performance upper bound compared to conventional techniques, they rely heavily on extensive ground-truth state trajectories for training, which are frequently unavailable in practice [22]–[25]. Furthermore, their generalization capabilities are often compromised when they encounter unseen operational conditions. Finally, learning-based approaches, particularly generative models, inherently operate as black boxes, introducing potential safety risks and making deployment on embedded devices difficult. Alternatively, the VB-based method solves a similar problem by explicitly parameterizing the noise statistics and inferring their posterior distributions via variational Bayesian approximation, which is tractable and explainable. However, existing VB-based methods have some limitations: (1) They are sensitive to outliers and the estimator’s performance degenerates significantly with heavy-tailed noises [29]; (2) The coordinate ascent variational inference, as a standard tool for variational optimization, converges in a zig-zag way, which is computationally inefficient. (3) Heuristic noise variance dynamics is widely used but without in-depth analysis on its tracking speed [4], [30]–[35]; (4) The existing solutions cannot well accommodate the time-varying rank-deficient process. (5) The VB-based methods can only track slow-varying noise variance and possess a delay in fast-varying noise statistics [32], [36]. These limitations restrict the application of VB-based estimators.

To address the above-mentioned challenges, in this work, we decouple the conventional VB into a sequential marginalized MAP (MMAP) and hyper-parameter update. Firstly, we reformulate the conventional state-space model as a lifted regression framework, which allows us to separate the process noise and perform inference channel by channel, giving flexibility in handling rank-deficient process covariance. Secondly, we decompose the VB inference as a two-stage problem: MMAP estimation and one single step hyper-parameter update. This operation can not only save the computation resources, but also decouple the state update and variational hyper-parameter update, given a room in handling variational-robust problems [37]. Thirdly, we compensate for the variational delay by exploiting the historical errors by adding regulations or considering higher order filters. In the case of covering all historical data, the covariance tracking effectively functions as a zero-phase filter. The contributions of this work are summarized as follows:

• We formulate the state estimation problem as a lifted whitened regression problem and provide the identifiability condition (Lemma 2), which allows us to treat the process and measurement indiscriminately. In addition, we separate the conventional VB estimation as a two-stage problem: MMAP state estimation and hyper-parameter update, which is computationally efficient and enables robustness and adaptiveness to be achieved sequentially.

• We solve the MMAP using an iterative reweighted least squares (IRLS) method. Meanwhile, we provide the convergence proof of the lifted equation under a class of robust losses in Theorem 1.

• We provide a detailed analysis of covariance-tracking speed in Theorem 2 and compensate for the delay by exploiting historical information in Algorithms 6 and 7. Simulations verify the effectiveness of the proposed method.

The rest of this paper is organized as follows. Section II provides preliminaries and problem formulation. Section III gives the detailed methodologies. Section IV provides some simulations. Section V concludes the paper.

Notations: \(A^T\) and \(A^{-1}\) denote the transpose and inverse of matrix \(A\). \(X \succ 0\) (\(X \succcurlyeq 0\)) denotes a positive (or semi-positive) definite matrix, and \(I_n\) is a \(n \times n\) identity matrix. \(\mathcal{N}(\mu, \Sigma)\), \(\operatorname{St}(0, \tau^2, \nu)\), and \(\operatorname{IG}(\alpha, \beta)\) represent the Gaussian, zero-mean Student’s \(t\), and inverse-gamma distributions, respectively. \(\mathbb{E}[\cdot]\) and \(\mathbb{E}_q[\cdot]\) denote expectations (standard, and with respect to distribution \(q\)). Finally, \([v]_i\) and \([M]_{ii}\) represent the \(i\)-th element of vector \(v\) and \(i\)-th diagonal element of matrix \(M\).

2 Preliminaries and Problem Formulation↩︎

We first provide some preliminaries and then give the problem formulation.

2.1 Hierarchical Formulation of the Student’s \(t\)-Distribution↩︎

A random variable \(x \sim \operatorname{St}(\mu, \tau^2, \nu)\) with degrees of freedom (DOF) \(\nu\) has the probability density function: \[p(x) = \frac{\Gamma((\nu+1)/2)}{\Gamma(\nu/2)\sqrt{\pi\nu\tau^2}} \left( 1 + \frac{(x-\mu)^2}{\nu\tau^2} \right)^{-(\nu+1)/2}. \label{stdis}\tag{1}\] This heavy-tailed distribution can be equivalently formulated as a Gaussian-scale mixture: \[x \sim \mathcal{N}(\mu, \lambda), \quad \lambda \sim \operatorname{IG}\left(\frac{\nu}{2}, \frac{\nu\tau^2}{2}\right) \label{hdis}\tag{2}\] where \[\nonumber \operatorname{IG}\left(\lambda \mid \frac{\nu}{2}, \frac{\nu\tau^2}{2}\right) = \frac{\left(\frac{\nu\tau^2}{2}\right)^{\frac{\nu}{2}}}{\Gamma\left(\frac{\nu}{2}\right)} \lambda^{-\left(\frac{\nu}{2}+1\right)} \exp\left(-\frac{\nu\tau^2}{2\lambda}\right)\] is the inverse-gamma distribution.

By taking the negative logarithm on the right side of 1 and defining \(e=x-\mu\), we obtain (ignoring the constant terms): \[\mathcal{L}_{st}^{*}(e) = \frac{\nu+1}{2}\log\left(1+ \frac{e^2}{\nu\tau^2}\right). \label{stloss}\tag{3}\]

This can be proved by taking limit on 3 .

This can be proved by verifying \(\frac{d^2}{du^2}\mathcal{L}_{st}(u) < 0\).

2.2 Problem Formulation↩︎

The system dynamics is as follows: \[\begin{align} x_{k}&=A x_{k-1} + B w_k \\ y_k &= C x_{k} +D v_k \end{align} \label{sys}\tag{4}\] where \(A \in \mathbb{R}^{n\times n}\) and \(C \in \mathbb{R}^{m \times n}\) are the state-transition and observation matrices, respectively; \(B \in \mathbb{R}^{n \times p}\) and \(D \in \mathbb{R}^{m \times m}\) map the process and measurement noises to the state and observation. The noise vectors \(w_k \in \mathbb{R}^{p}\) and \(v_k \in \mathbb{R}^{m}\) are element-wise uncorrelated zero-mean random variables (i.e., \(E(w_{i,k} w_{j,k}) = 0\) and \(E(v_{i,k} v_{j,k}) = 0\) for \(i \neq j\), where \(w_{i,k}\) and \(v_{i,k}\) denote the \(i\)-th components of \(w_k\) and \(v_k\)). Without loss of generality, we assume the nominal covariances \(E(w_k w_k^{T}) = I_{p}\) and \(E(v_k v_k^{T}) = I_{m}\), and define \(Q_k = B I_{p} B^{T} \succeq 0\) and \(R_k = D I_{m} D^{T} \succ 0\) (implying \(D\) is non-singular). The pair \((A, \sqrt{Q_k})\) is controllable and \((A, C)\) is observable. The matrix \(B\) has full column rank, i.e., \(\text{rank}(B) = p\).

In practical applications, the noise covariance may be time-varying, i.e., \(E(w_k w_k^{T}) = Q_{w,k}\) and \(E(v_k v_k^{T}) = R_{v,k}\) are time-varying. The associated process and measurement covariances become \(Q_k=BQ_{w,k}B^{T}\) and \(R_k=DR_{v,k}D^{T}\). The initial state \(x_0\) is zero-mean Gaussian with known covariance matrix \(P_0\), and is independent of \(w_k\) and \(v_k\) for all \(k>0\).

Our purpose is to jointly estimate \(\{\hat{x}_k, \hat{Q}_k, \hat{R}_k\}\) under the governing equation 4 .

3 Main Results↩︎

In this section, we formulate the joint estimation problem as a two-stage sequential problem: marginalized maximum a posteriori (MMAP) estimation and one step hyper-parameter update.

3.1 The Proposed Method↩︎

The mean-field variational approximation provides a backbone for joint estimate \(\{\hat{x}_k, \hat{Q}_k, \hat{R}_k\}\). However, it should be noted that the state \(x_k\) depends on the process noise covariance \(Q_k\), which complicates the variational inference process, as the joint posterior cannot be directly factorized into independent components. To address this, Huang et al. perform inference on \(P_{k|k-1}\) rather than \(Q_k\), which simplifies the problem. However, it overlooks the intrinsic structure of \(Q_k\) and produces sub-optimal results. Alternatively, we introduce an auxiliary variable \(m_k \sim \mathcal{N} (m_k^{-}, \Sigma_k) = \mathcal{N} (A x_{k-1}, AP_{k-1|k-1}A^{T})\), termed the unperturbed prior state, representing the state excluding process noise. Thus, we have \[\scalebox{0.9}{ \begin{bmatrix} m_{k}^{-}\\ 0\\ y_k \end{bmatrix} = \begin{bmatrix} I & 0\\ I& -I\\ 0 & C \end{bmatrix}\begin{bmatrix} m_k\\ x_k \end{bmatrix}+ \begin{bmatrix} p_{k}\\ Bw_k \\ Dv_k \end{bmatrix}} \label{sys1}\tag{5}\] where \(p_k\) denotes the virtual noise, and \(m_k \sim \mathcal{N}(m_k^{-},\Sigma_{Pk})\) with \(m_k^{-} = A x_{k-1}\) and \(\Sigma_{Pk} = A P_{k-1|k-1} A^T\). Denote \[\scalebox{0.9}{ B_kB_k^{T}=\begin{bmatrix} B_pB_p^{T} & 0 & 0\\ 0& BB^{T} & 0\\ 0& 0 &DD^{T} \end{bmatrix}=\begin{bmatrix} \Sigma_{Pk} & 0 & 0\\ 0& Q_k & 0\\ 0& 0 &R_k \end{bmatrix}} \label{Bk}\tag{6}\] where \(B_p\) is obtained by Cholesky decomposition through \(B_pB_p^{T}=\Sigma_{Pk}\). It follows that \[B_k^{-1}= \begin{bmatrix} B_p^{-1} & 0 & 0\\ 0& B^\dagger & 0\\ 0& 0 &D^{-1} \end{bmatrix}\] where \(B^\dagger \in \mathbb{R}^{p \times n}\) is the Moore-Penrose pseudo-inverse of \(B\). Left multiplying \(B_k^{-1}\) in both side of 5 gives the following whitened regression problem: \[t_k = W_k \chi_k + \xi_k \label{wreg}\tag{7}\] where \[\label{tkwk} \scalebox{0.9}{ \begin{align} t_k &= B_k^{-1}\begin{bmatrix} m_{k}^{-}\\ 0\\ y_k \end{bmatrix}, \quad W_k=B_k^{-1}\begin{bmatrix} I & 0\\ I& -I\\ 0 & C \end{bmatrix},\quad \chi_k=\begin{bmatrix} m_k\\ x_k \end{bmatrix}\\ \xi_k&=\begin{bmatrix} \xi_p\\ \xi_w\\ \xi_v \end{bmatrix}=B_k^{-1} \begin{bmatrix} p_{k}\\ Bw_k \\ Dv_k \end{bmatrix}. \end{align}}\tag{8}\] Note that the prior state estimate \(m_k^{-}\) is treated as a pseudo-measurement in the above equation. In the nominal case, one has \(E(\xi_k\xi_k^{T})={I}_{n+p+m}\).

The proof is provided in Appendix 6.1. Throughout this work, we restrict our attention to systems that satisfy the condition stated in Lemma 2.

We define \(e_k \triangleq\xi_k=t_k- W_k \chi_k\) and denote \[t_k=\begin{bmatrix} t_p\\ t_w\\ t_v \end{bmatrix}, e_k=\begin{bmatrix} e_p\\ e_w\\ e_v \end{bmatrix}, \Sigma_{\chi}= \begin{bmatrix} I_n & 0&0\\ 0& \Sigma_{Qk}&0\\ 0& 0& \Sigma_{Rk} \end{bmatrix}\] where \(\Sigma_{Qk}\) and \(\Sigma_{Rk}\) are two diagonal matrices to be estimated, reflecting the characteristic of time-varying \(w_k\) and \(v_k\). Recall that our purpose is to jointly estimate both the state and associated unknown covariance matrices, we need to assign a prior distribution for \(e_w\) and \(e_v\) to avoid over-fitting [18]. To proceed, we model them as element-wise independent Student’s \(t\)-distributions as follows: \[\begin{align} e_w&= [e_{w,1},\ldots,e_{w,i},\ldots,e_{w,p}]^{T},\\ e_v&= [e_{v,1},\ldots,e_{v,j},\ldots,e_{v,m}]^{T}. \end{align}\] Each element in the above vector follows a zero-mean Student’s \(t\)-distribution, i.e., \[\begin{align} e_{w,i} &\sim \operatorname{St}(e_{w,i} \mid 0, \tau_i^2, \nu_i), \quad i = 1, 2, \ldots, p, \\ e_{v,j} &\sim \operatorname{St}(e_{v,j} \mid 0, \tau_j^2, \nu_j), \quad j = 1, 2, \ldots, m, \end{align}\] where \(\nu_i\) and \(\nu_j\) denote the degrees of freedom (DOF) controlling the tail heaviness of the corresponding distributions, and \(\tau_i^2\) and \(\tau_j^2\) are the associated nominal variances. Note that although \(e_p \sim \mathcal{N}(0,I_n)\) is Gaussian. Based on Property 2, it can be regarded as a Student’s \(t\)-distribution as follows: \[\begin{align} e_{p,i} &\sim \operatorname{St}(e_{p,i} \mid 0, \tau_i^2, \nu_i), \quad i = 1, 2, \ldots, n, \\ \end{align}\] with \(\nu_i \to \infty\). This allows us to treat the different elements of \(e_p\) in a uniform way. Applying Bayes’ rule alongside the Student’s \(t\)-distribution properties from 1 , the joint posterior can be factorized as follows: \[\begin{align} p&(\chi_k, \Sigma_{Qk}, \Sigma_{Rk} \mid t_{1:k}) = \frac{p(\chi_k, \Sigma_{Qk}, \Sigma_{Rk}, t_k \mid t_{1:k-1})}{p(t_k \mid t_{1:k-1})} \\ &\propto p(t_k, \chi_k \mid \Sigma_{Qk}, \Sigma_{Rk}) p(\Sigma_{Qk}) p(\Sigma_{Rk}), \end{align} \label{map}\tag{9}\] where \(p(t_k \mid t_{1:k-1})\) is the marginal likelihood obtained by integrating out the optimization variables, and thus acts merely as a normalizing constant. Observe that the Gaussian prior \(p(\chi_k \mid t_{1:k-1})\) is already equivalently represented as a pseudo-measurement embedded within the first block of the whitened regression model in 7 . Consequently, the prior is inherently absorbed into the augmented likelihood term \(p(t_k \mid \chi_k, \Sigma_{Qk}, \Sigma_{Rk})\), removing the need for an explicit, separate prior term to avoid double-counting. By denoting \(\Theta=\{\chi_k, \Sigma_{Qk},\Sigma_{Rk}\}=\{\chi_k, \lambda^{wv}\}\), we rewrite the joint posterior in a compact way as \[\begin{align} p&(\Theta|t_{1:k})\propto p(\Theta,t_k|t_{1:k-1})\\ =& \mathcal{N}(t_k;W_k\chi_k, \Sigma_{\chi})\prod_{i=1}^{n+p+m}\operatorname{IG}(\lambda_i| \frac{\nu_i}{2},\frac{\nu_i \tau_i^2}{2}). \end{align} \label{map1}\tag{10}\] The exact solution for 10 is intractable due to the coupling between \(\chi_k\) and \(\lambda^{wv}\). Thus, we resort to variational Bayesian inference to construct a tractable approximation. Specifically, by the mean-field assumption, we assume the target distribution follows \[p(\Theta|t_{1:k}) \approx q(\chi_k,\lambda^{wv})=q(\chi_k)\prod_{1}^{p+m}q(\lambda_i^{wv}),\] where \(q(\chi_k)\) is a Gaussian distribution and \(\lambda_i^{wv}\) is diagonal inverse-Gamma variational factors. Then, we minimize the Kullback-Leibler (KL) divergence between the separable approximation and the true posterior: \[\begin{align} \mathrm{KL}&(q(\chi_k,\lambda^{wv})||p(\Theta|t_{1:k}))\\ &=\iint q(\chi_k,\lambda^{wv}) \ln \left( \frac{q(\chi_k,\lambda^{wv})}{p(\Theta|t_{1:k})} \right) d\chi_k d\lambda^{wv}, \end{align} \label{kldiv}\tag{11}\] which is equivalent to maximizing the following evidence lower bound (ELBO) [38]: \[\begin{align} \mathcal{L}(q) &= \mathbb{E}_q\left[\log p(t_k \mid \chi_k, \lambda^{wv})\right]+ \mathbb{E}_q\left[\log p(\lambda^{wv})\right]\\ &- \mathbb{E}_q\left[\log q(\chi_k)\right] - \mathbb{E}_q\left[\log q(\lambda^{wv})\right]. \end{align} \label{elbo0}\tag{12}\] Maximizing the ELBO with respect to \(\chi_k\) and \(\lambda^{wv}\) gives: \[\begin{align} \log q(\chi_k) &\propto \left( \mathbb{E}_{q(\lambda^{wv})} \left[ \log p(t_k \mid \chi_k, \lambda^{wv}) \right] \right), \tag{13}\\ \log q(\lambda^{wv}) &\propto \left( \mathbb{E}_{q(\chi_k)} \left[ \log p(t_k \mid \chi_k, \lambda^{wv}) + \log p(\lambda^{wv}) \right] \right) \tag{14}. \end{align} \tag{15}\] The coupled equations in 15 can be solved iteratively via coordinate-ascent variational inference (CAVI), see details in Appendix 6.2. The full iterative procedure is computationally expensive and tightly couples the state estimation with covariance tracking at every iteration. To decouple this and alleviate the computational burden, we resort to a computationally efficient approximation. Initially, we marginalize \(\lambda^{wv}\) out from the joint distribution 10 , which gives \[\begin{align} &p(\chi_k, t_k | t_{1:k-1}) = \int p(\Theta, t_k | t_{1:k-1}) d\lambda_1 \dots d\lambda_{n+p+m} \\ &\propto\prod_{i=1}^{n+p+m} \left( (t_{k,i} - (W_k\chi_k)_i)^2 + \nu_i\tau_i^2 \right)^{-\frac{\nu_i + 1}{2}}, \end{align} \label{mposteriori}\tag{16}\] which is a product of Gaussian distributions and heavy-tailed Student’s \(t\)-distributions. Then, the KL divergence is redefined to evaluate the difference exclusively between the approximate posterior \(q(\chi_k)\) and the true marginalized posterior \(p(\chi_k|t_{1:k})\): \[\begin{align}\mathrm{KL}&(q(\chi_k)||p(\chi_k|t_{1:k}))=\int q(\chi_k) \ln \left( \frac{q(\chi_k)}{p(\chi_k|t_{1:k})} \right) d\chi_k. \end{align}\] The associated ELBO is: \[\begin{align} \mathcal{L}(q(\chi_k)) &= \mathbb{E}_{q(\chi_k)}\left[\log p(\chi_k, t_k \mid t_{1:k-1})\right] \\ &- \mathbb{E}_{q(\chi_k)}\left[\log q(\chi_k)\right]. \end{align} \label{elbo95marginalized}\tag{17}\]

It is worth noting that the marginalized posterior in 16 is generally asymmetric. Specifically, by evaluating the third-order derivative tensor of the log-posterior at the MAP estimate \(\chi^*\), the local skewness along any direction \(\Delta\) is driven by the coefficient \(k_i \propto e_i (e_i^2 - 3\nu_i\tau_i^2) / (\nu_i\tau_i^2 + e_i^2)^3\), where \(e_i = t_{k, i} - (W_k\chi^*)_i\) is the steady-state residual. This reveals that the skewness vanishes as either \(\nu_i\tau_i^2 \gg e_i^2\) or \(\nu_i\tau_i^2 \ll e_i^2\) (Practical applications are dominated by these two regimes). Under the symmetric assumption, by applying Lemma 1, we convert 17 to the following marginalized MAP (MMAP) optimization problem: \[\begin{align} \hat{\chi}_k &= \arg\max_{\chi_k} \log p(\chi_k, t_k \mid t_{1:k-1})\\ &= \arg\min_{\chi_k} \sum_{i=1}^{n+p+m} \underbrace{\frac{\nu_i + 1}{2} \log \left( 1 + \frac{e_i^2}{\nu_i\tau_i^2} \right)}_{J_i}, \end{align} \label{mapmargalized}\tag{18}\] where \(e_i= t_{k,i} -(W_k\chi_k)_i\). Note that the posterior covariance for 17 does not have a closed-form solution. In addition, the exact solution for 17 is slightly over-confident due to the zero-force effect caused by \(\mathrm{KL}(q||p)\), extensive discussion on \(\mathrm{KL}(q||p)\) versus \(\mathrm{KL}(p||q)\) is available on [39]. To simplify the solution, we apply Laplace approximation as \(q(\chi_k) \approx \mathcal{N}(\hat{\chi}_k, \hat{P}_{\chi_k})\) around the MAP point, where \(\hat{P}_{\chi_k})= \left( W_k^T \Lambda_{\text{Lap}} W_k \right)^{-1}\) with \(\Lambda_{\text{Lap}, ii} = \left. \frac{\partial^2}{\partial e_i^2} \left[ \frac{\nu_i + 1}{2} \log \left(J_i \right) \right] \right|_{e_i = \hat{e}_i}\), proving a sufficiently good starting point. Subsequently, we advocate for updating \(q(\lambda^{wv})\) via 14 in a single step. The entire algorithm, called the hierarchical variational estimator (HVE), is summarized in Algorithm 1. A graphical comparison between standard CAVI and the proposed HVE is shown in Fig. 2. As demonstrated by the 1D example, the proposed method achieves similar performance with just a single \(q(\lambda)\) update compared to multiple iterations of conventional CAVI.

3.2 Algorithm Derivation↩︎

This subsection provides details for Algorithm 1 under the whitened regression problem 7 . Based on 18 , we define the following objective function: \[\mathcal{J}(\chi_k) = \sum_{i=1}^{n+p+m} \frac{\nu_i + 1}{2} \log \left( 1 + \frac{e_i^2}{\nu_i\tau_i^2} \right). \label{rbobj}\tag{19}\] Recalling Proposition [prop95hq], the robust objective 19 can be transformed into a sequence of tractable surrogate problems by introducing an auxiliary optimization variable \(\lambda_i > 0\) for each residual term \(e_i\). Thus, we have: \[\mathcal{J}(\chi_k) = \inf_{\lambda_i > 0} \sum_{i=1}^{n+p+m}\left[ \frac{e_i^2}{2\lambda_i} + \psi(\lambda_i) \right],\] where the optimum of the augmented dual function is achieved at: \[\lambda_i = \frac{e_i}{\rho'(e_i)} = \frac{\nu_i\tau_i^2 + e_i^2}{\nu_i + 1}.\] Consequently, minimizing the original objective function \(\mathcal{J}(\chi_k)\) is mathematically equivalent to solving an iteratively reweighted least squares (IRLS) problem. Let \(j\) denote the iteration index. At each iteration, we define the equivalent scalar weight as the inverse of the auxiliary variable, \(w_i^{(j)} \triangleq (\lambda_i^{(j)})^{-1}\), which is computed based on the current state estimate \(\chi_k^{(j)}\): \[w_i^{(j)} = \frac{\nu_i + 1}{\nu_i\tau_i^2 + \left(e_i^{(j)}\right)^2}. \label{weight}\tag{20}\] By assembling these scalar weights into a diagonal weighting matrix \(\Lambda_k(e_k^{(j)}) \triangleq \operatorname{diag}\left[w_1^{(j)}, \ldots, w_{n+p+m}^{(j)}\right]\), the non-linear optimization problem is solved by the following normal equations: \[\chi_k^{(j+1)} = \left(W_k^{T}\Lambda_k(e_k^{(j)})W_k\right)^{-1} W_k^{T}\Lambda_k(e_k^{(j)})\,t_k, \label{solu1}\tag{21}\] until the sequence \(\{\chi_k^{(j)}\}\) converges to the optimal estimate.

The proof is available in Appendix 6.3.

By applying matrix inversion lemma [1], 21 can be identically expressed in a Kalman update format. To proceed, we denote \(\bar{C}=[0, C]\) and \({\chi}_{k,0}^{+}={\chi}_{k}^{-}\) as the initial state estimation at \(t=0\) iteration. Subsequently, for \(t \ge 1\), we have (see details in [9], [42]) \[\begin{align} \chi_{k,t}^{+}&=\chi_k^{-}+\tilde{K}_{k,t}({y}_k-\bar{C}\chi_k^{-})\\ \tilde{K}_{k,t}&=\tilde{P}_{\chi k}^{-}\bar{C}^{T}(\bar{C}\tilde{P}_{\chi k}^{-}\bar{C}^{T}+\tilde{R}_{k})^{-1}\\ \tilde{P}_{\chi k}^{-} &= \begin{bmatrix} \Sigma_{Pk}&\Sigma_{Pk}\\ \Sigma_{Pk}&\Sigma_{Pk}+\tilde{Q}_{ k} \end{bmatrix}\\ \tilde{Q}_{ k}& = B \Lambda_w^{-1} B^T\\ \Lambda_{w}&=\operatorname{diag}[d_{\nu_{n+1}}(e_{n+1}),\ldots,d_{\nu_{n+p}}(e_{n+p})]\\ \tilde{R}_{k}&=D{\Lambda}_{v}^{-1}D^{T}\\ \Lambda_{v}&=\operatorname{diag}[d_{\nu_{n+p+1}}(e_{n+p+1}),\ldots,d_{\nu_{n+p+m}}(e_{n+p+m})]\\ e_k &= t_k - W_k \chi_k^{t-1} \end{align} \label{update}\tag{22}\] where the cross term of \(\tilde{P}_{\chi k}^{-}\) comes from the fact that \(E[(m_k-m_k^{-})(x_k-x_k^{-})^{T}]=E[(-p_k)(-p_k+w_k)^{T}]= \Sigma_{P_k}\). This process is iterated until convergence, subject to a predefined termination condition or a maximum number of iterations. At the final iteration, one can update the posteriori error covariance as: \[P_{\chi k}^{+} = (I - \tilde{K}_{k,t} \bar{C}) {P}_{\chi k}^{-} (I - \tilde{K}_{k,t} \bar{C})^T + \tilde{K}_{k,t} {R}_{k} \tilde{K}_{k,t}^{T}. \label{errcov}\tag{23}\] The complete algorithm, named as MMAP estimator, is summarized in Algorithm 3. An alternative expression of 22 and 23 , which update \(m_k\) and \(x_k\) explicitly, is also provided in Appendix 6.4.

Let \(\mathcal{I} = \{n+1, \dots, n+p+m\}\) denote the set of inference channels selected for covariance inference, and \(\mathcal{F} = \{1, \dots, n\}\) denotes the frozen set. We then focus on the update of \(\{\lambda_i\}_{i \in \mathcal{I}}\). By applying mean-field approximation, we have \[q(\chi_k, \{\lambda_i\}_{i \in \mathcal{I}}) = q(\chi_k) \prod_{i \in \mathcal{I}} q(\lambda_i). \label{mean95field95assumption}\tag{24}\] According to Appendix 6.2, the one-step \(q(\lambda_i)\) update follows \[q(\lambda_i) = \operatorname{IG}\left(\lambda_i \middle| \frac{\nu_i^*}{2}, \frac{\nu_i^*(\tau_i^{2})^{*}}{2}\right), \label{posterior95lambda}\tag{25}\] where \[\begin{align} \nu_i^* &= \nu_i + 1, \\ (\tau_i^{2})^{*} &= \frac{\mathbb{E}_{q(\chi_k)}[([t_k - W_k \chi_k]_i)^2]}{\nu_i+1} + \frac{\nu_i \tau_i^2}{\nu_i+1}, \end{align} \label{posterior95params}\tag{26}\] and \[\notag \mathbb{E}_{q(\chi_k)}[([t_k - W_k \chi_k]_i)^2] = \left[t_k - W_k {\chi}_k^{+}\right]_i^2 + \left[W_k P_{\chi k}^{+} W_k^T\right]_{ii}.\]

The proof is available in Appendix 6.5. Proposition [scaleinv] implies that simultaneously estimating all channels of \(Q_k\) and \(R_k\) (equivalently, \(\{\lambda_i\}_{i \in \mathcal{I}}\)) is ill-posed. Furthermore, in practical applications, It is common that only a subset of the process and measurement noise statistic characteristics change over time. To reflect this fact, we split 4 as: \[\begin{align} x_{k}&=A x_{k-1} + B_f w_{fk} + B_v w_{vk} \\ y_k &= C x_{k} +D_f v_{fk} + D_v v_{vk} \label{syssplit} \end{align}\tag{27}\] where \(B_f w_{fk}\) and \(D_f v_{fk}\) denote the fixed components, while \(B_v w_{vk}\) and \(D_v v_{vk}\) represent the time-varying components to be inferred. We define an active set \(\mathcal{A} \in \mathcal{I}\) which is a subset of \(\mathcal{I}\) and contains the non-zero channels of \(B_v w_{vk}\) and \(D_v v_{vk}\). Consequently, one can perform inference on \(\mathcal{A}\) to avoid ill-posedness.

The update rule \(\nu_i^* = \nu_i + 1\) in 26 implies that the degrees of freedom increase monotonically over time. As \(\nu_i \to \infty\), the innovation weight \(\frac{1}{\nu_i+1}\) vanishes, causing the covariance inference to saturate and lose sensitivity to new data. Meanwhile, as shown by Property 2, the increment of \(\nu_i\) suppresses the robust mechanism by forcing the weight function \(d_{\nu_i}(e_i)\) to converge to the constant \(1/\tau_i^2\), degenerating the robust estimator into a standard Kalman filter. To maintain persistent adaptivity, we introduce a forgetting factor \(0 < \rho_i < 1\) to bound the growth of the degrees of freedom. The sequential update for \(\nu_i\) is modified as: \[\begin{align} \nu_{i,k|k-1} &= \rho_i \nu_{i,k-1|k-1} \\ \nu_{i,k|k} &= \nu_{i,k|k-1} + 1. \end{align}\] The whole hierarchical variational Kalman filtering (HVKF) is summarized in Algorithm 4.

3.3 Covariance Tracking Analysis and Delay Compensation↩︎

The whitened and lifted modeling in 8 allows us to analyze the covariance tracking speed channel by channel. At the steady state, one has \(\rho_i = \frac{\nu_{i,k|k-1}}{\nu_{i,k|k-1}+1}\). Defining the realized variational error as \(\mathcal{E}_{i,k} \triangleq [t_k - W_k {\chi}_k^{+}]_i^2 + [W_k P_{\chi k}^{+} W_k^T]_{ii}\), we assume it is a weakly stationary sequence with mean \(\sigma_i^2\) and variance \(\eta_i^4\). Subsequently, we rewrite the scale parameter update function as shown in Line 14 of Algorithm 4 (at the steady state) as \[(\tau_{i,k|k})^2 = \rho_i (\tau_{i,k-1|k-1})^{2} + (1-\rho_i) \mathcal{E}_{i,k}. \label{scale95update}\tag{28}\]

The proof is available in Appendix 6.6. It is obvious that there is a trade-off between the covariance tracking speed and tracking smoothness based on Theorem 2, i.e., a bigger forgetting factor \(\rho_i\) gives a smaller tracking variance but a bigger tracking time constant. To address this, we provide two remedies: a higher-order filter and a sliding window filter, which will be detailed below.

3.3.1 High Order Filter↩︎

Here we provide a general prior model. For the scale parameter update, we design \[\tau_{i,k|k-1}^2 = \sum_{m=1}^{N} \phi_m \tau_{i,k-m|k-m}^2, \label{momentump}\tag{29}\] with \(\sum_{m=1}^{N} \phi_m = 1\). Substituting it into 28 gives \[\tau_{i,k|k}^2 = \rho_i \sum_{m=1}^{N} \phi_m \tau_{i,k-m|k-m}^2 + (1-\rho_i) \mathcal{E}_{i,k}. \label{scale95update1}\tag{30}\]

The proof can be obtained by taking the Z-transform of 30 and applying standard linear system stability criteria [43]. We omitted it here for brevity. In the case \(N=1\), one has \(\phi_1=1\), which recovers the EWMA estimator. In the case \(N=2\), we obtain a velocity compensation prior by assigning \(\phi_1= 1+ \gamma\) and \(\phi_2=-\gamma\) so that \[\tau_{i,k|k-1}^2 = \tau_{i,k-1|k-1}^2 + \gamma (\tau_{i,k-1|k-1}^2- \tau_{i,k-2|k-2}^2).\] This explicitly calculates the first-order difference (trend) of the variance. Based on Proposition [BIBOstable], one has \(P(z) = z^2 - \rho_i(1+\gamma)z + \rho_i \gamma\). The system is strictly BIBO stable if the following condition holds: \[0 \le \gamma < \frac{1}{\rho_i}.\] In the case \(N=3\), we obtain an acceleration compensation prior. Denote \(\Delta \tau_{i,k-1}^2 \triangleq \tau_{i,k-1|k-1}^2- \tau_{i,k-2|k-2}^2\). We formulate the prior as \[\tiny \tau_{i,k|k-1}^2 = \tau_{i,k-1|k-1}^2 + \gamma_1 (\Delta \tau_{i,k-1}^2) + \gamma_2 \left[ \Delta \tau^2_{i,k-1} - \Delta \tau^2_{i,k-2} \right].\] By expanding the differences, the corresponding filter weights are exactly synthesized as \(\phi_1 = 1+\gamma_1+\gamma_2\), \(\phi_2 = -(\gamma_1+2\gamma_2)\), and \(\phi_3 = \gamma_2\). This 3rd-order design dramatically reduces dynamic tracking lag during parabolic or exponentially growing profiles by predicting the rate-of-change of the variance trend itself. However, the stability margin becomes tighter. The roots of \(P(z) = z^3 - \rho_i\phi_1 z^2 - \rho_i\phi_2 z - \rho_i\phi_3\) must be verified via the 3rd-order Jury test, requiring \(\gamma_2 < \gamma_1 < 1/\rho_i\) to prevent underdamped oscillatory divergence.

We visualize the trade-off between tracking speed and steady-state variance for the \(N=1\) and \(N=2\) estimators in Fig. 5. For the \(N=2\) case, setting \(\gamma = \frac{\rho^2}{1+\rho}\) yields two real poles: \(z_1 = \rho^2\) and \(z_2 = \frac{\rho}{1+\rho}\). The \(N=2\) time constant is dictated by the dominant pole (\(z_1 = \rho^2\) as \(\rho \ge 0.618\)), while the steady-state variance is derived based on Theorem 2. The results confirm that the \(N=2\) scenario possesses a better Pareto profile compared with \(N=1\). The detailed high-order HVKF (HOHVKF) is summarized in Algorithm 6.

3.3.2 Sliding Window Filter↩︎

By investigating 28 , we find that \(\mathcal{E}_{i,k}\) is influenced by the estimate of \(\chi_k\), which is coupled with the preceding scale parameter \(\tau_{i,k-1|k-1}^2\). This temporal dependency indicates that a refined historical estimate of \(\tau_{i,k-1|k-1}^2\) contributes to a tighter current scale estimate. Motivated by this, we formulate a sliding-window scale parameter estimator. Let \(L\) denote the length of the sliding window. At time step \(k\), we define the sequence of the localized scale parameters and the corresponding realized variational errors over the window as vectors: \[\notag \begin{align} \boldsymbol{\mathcal{T}}_{i,k} &\triangleq \begin{bmatrix} \tau_{i,k-L+1|k-L+1}^2, & \dots, & \tau_{i,k|k}^2 \end{bmatrix}^T, \\ \boldsymbol{\mathcal{E}}_{i,k} &\triangleq \begin{bmatrix} \mathcal{E}_{i,k-L+1}, & \dots, & \mathcal{E}_{i,k} \end{bmatrix}^T. \end{align}\] To obtain an optimal and smooth estimation of the sequence \(\boldsymbol{\mathcal{T}}_{i,k}\) given the measurement sequence \(\boldsymbol{\mathcal{E}}_{i,k}\), we cast the scale parameter update as a regularized localized optimization problem: \[\tiny \label{eq:sliding95window95opt} \hat{\boldsymbol{\mathcal{T}}}_{i,k} = \arg\min_{\boldsymbol{\mathcal{T}}_{i,k}} \left\{ \sum_{j=k-L+1}^{k} \frac{1}{2}\mathcal{D} \left( \mathcal{E}_{i,j}, \tau_{i,j|j}^2 \right) + \frac{\lambda}{2} \mathcal{R}(\boldsymbol{\mathcal{T}}_{i,k}) \right\}.\tag{31}\] Here, \(\mathcal{D}(\cdot, \cdot)\) is a data fidelity penalty (typically the squared Euclidean distance \(\left\| \mathcal{E}_{i,j} - \tau_{i,j|j}^2 \right\|_2^2\)), and \(\lambda > 0\) is the regularization parameter controlling the trade-off between data fidelity and tracking smoothness. The penalty function \(\mathcal{R}(\boldsymbol{\mathcal{T}}_{i,k})\) explicitly encodes our prior belief regarding the dynamics of the scale parameter. Depending on the physical nature of the noise characteristics, three distinct smoothing strategies can be adopted as follows.

• \(\ell_2\)-Regularization: \[\notag \mathcal{R}_{\ell_2}(\boldsymbol{\mathcal{T}}_{i,k}) = \sum_{j=k-L+1}^{k} \left( \tau_{i,j|j}^2 - \tau_{i,j-1|j-1}^2 \right)^2.\]

• \(\ell_1\)-Regularization: \[\notag \mathcal{R}_{\ell_1}(\boldsymbol{\mathcal{T}}_{i,k}) = \sum_{j=k-L+1}^{k} \left| \tau_{i,j|j}^2 - \tau_{i,j-1|j-1}^2 \right|.\]

• \(\ell_0\)-Regularization: \[\notag \mathcal{R}_{\ell_0}(\boldsymbol{\mathcal{T}}_{i,k}) = \sum_{j=k-L+1}^{k} \left\| \tau_{i,j|j}^2 - \tau_{i,j-1|j-1}^2 \right\|_0.\]

The proof is available in Appendix 6.7. The whole algorithm, termed as Sliding Window Hierarchical Variational Kalman Filtering (SWHVKF), is summarized in Algorithm 7.

4 Simulation↩︎

This section presents numerical examples to demonstrate the effectiveness of the proposed methods.

4.1 Example 1: Fast Measurement Covariance Tracking↩︎

We formulate the target tracking problem using the following discrete-time linear state-space model: \[\begin{align} x_{k+1} &= A x_k + B w_k \\ y_k &= C x_k + v_k \end{align}\] where the system matrices are defined as \(A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\), \(B = \begin{bmatrix} 0.5(\Delta t)^2 \\ \Delta t \end{bmatrix}\), and \(C = \begin{bmatrix} 1 & 0 \end{bmatrix}\), with the sampling interval \(\Delta t = 0.01\). The process noise is assumed to be \(w_k \sim \mathcal{N}(0, 1)\). To simulate abrupt changes in measurement quality, we introduce a piecewise-constant measurement noise profile \(v_k \sim \mathcal{N}(0, R_k)\), where \[R_k = \begin{cases} 1, & \text{if } k \in \mathbb{S}_1 \\ 100, & \text{if } k \in \mathbb{S}_2. \end{cases}\] Here, \(\mathbb{S} = \mathbb{S}_1 \cup \mathbb{S}_2\) represents the entire sequence of time steps. In the simulations, the root mean square error (RMSE) is calculated as \(\frac{1}{nT}\sum_{k=1}^{T}\sum_{i=1}^{n}e_{i,k}\), where \(T\) is the trajectory length and \(N\) is the state dimension.

During the simulation, we define \(\rho=[1,1,\rho_3]\) with \(\rho_3=0.99\). Note that \(\rho_i=1\) indicates that variational inference is not applied to that specific channel. For the HOHVKF, we set \(\gamma= 0.4925\), which yields the following scale update equations: \[\begin{align} \tau_{i,k|k-1}^2 &= \tau_{i,k-1|k-1}^2 + 0.4925 (\tau_{i,k-1|k-1}^2- \tau_{i,k-2|k-2}^2)\\ (\tau_{i,k|k})^2 &= \rho_i (\tau_{i,k|k-1})^{2} + (1-\rho_i) \mathcal{E}_{i,k}. \end{align}\] For the SWHVKF, we set the window length as \(L=5\). We use the same nominal parameters across all evaluated filters: KF, VBKF [4], MMAP, HVKF, HOHVKF, and the Ideal KF (which utilizes the ground truth covariance). The comparative results are visualized in Fig. 8, and the measurement covariance tracking trajectory is visualized in Fig. 9.

As shown in Fig. 9, the HOHVKF responds more than twice as fast as the HVKF, at the minor expense of slightly increased steady-state variance. This trade-off confirms the superiority of the proposed higher-order method. The detailed quantitative metrics are summarized in Table 1. The results indicate that both the proposed HOHVKF and SWHVKF perform better than the baseline HVKF, achieving accuracy levels closely approximating the ideal KF.

We further evaluate the performance of the proposed method over the entire measurement sequence under \(\ell_2\), \(\ell_1\), and \(\ell_0\) regularizations (solved via ADMM with hard thresholding). The results, depicted on the left side of Fig. 10, verify the equivalence between the \(\ell_2\) regularization optimization and the two-pass filter. Furthermore, we observe that \(\ell_1\) regularization achieves superior performance compared to \(\ell_2\) regularization, albeit at the cost of increased computational complexity.

4.2 Example 2: Rank Deficit Process Covariance Tracking↩︎

We consider a torsion load system [44]. The system dynamics are governed by the following equations: \[\begin{align} {x}_{k}&={A}_k {x}_{k-1}+{G}_{1,k} {u}_{k-1}+{G}_{2,k} {d}_{k-1}+{w}_{k}\\ {y}_k&={C}_k {x}_{k}+{v}_{k} \label{linearfun} \end{align}\tag{32}\] with \[\nonumber \tiny \begin{align} A_k&=\begin{bmatrix} 0.9205 & 0.0795 & 0.0085 & 0.0003\\ 0.2045 & 0.7955 & 0.0007 & 0.0085\\ -14.3468 & 14.3468 & 0.6872 & 0.0746\\ 37.5370 & -37.5370 & 0.1863 & 0.6405 \end{bmatrix}\\ G_{1,k}&=\begin{bmatrix} 0.0826, 0.0031, 15.5568, 1.2100 \end{bmatrix}^{T}\\ G_{2,k}&=\begin{bmatrix} 0.0031, 0.2076, 1.2100, 38.7470 \end{bmatrix}^{T} \\ C_k&=\begin{bmatrix} 1&0&0&0\\ 0&1&0&0 \\ 0&0&1&0\\ 0&0&0&1 \end{bmatrix} \end{align}\] where \(x_k=[\theta_m,\theta_t,v_m,v_t]^{T} \in \mathbb{R}^{4}\) denotes the state vector, \(u_k \in \mathbb{R}\) is the motor output torque, \(d_k\) is the external disturbance, and \(y_k\) is the measurement.

In the simulation, we set \(w_k \sim \mathcal{N}(0, 0.01\mathrm{I}_4)\) and \(v_k \sim \mathcal{N}(0, \mathrm{I}_4)\). Additionally, we model \(d_k\) as Gaussian noise with \(d_k \sim \mathcal{N}(0, 0.1\lambda_k)\), where \(\lambda_k\) follows \[\label{lambdak} \lambda_k = \begin{cases} 1, & \text{if } k \in \mathbb{S}_1 \\ 100, & \text{if } k \in \mathbb{S}_2 \end{cases}\tag{33}\] with \(\mathbb{S}_1 = [0, 100) \cup [400, 600) \cup [900, 1000]\) and \(\mathbb{S}_2 = [100, 400) \cup [600, 900)\). The time interval is \(\Delta t=0.01\) s. We use the same nominal covariances for all estimators, except for the ground-truth covariance in Ideal KF. Meanwhile, we set \(\nu=10\) for all process channels in STKF [37], apply a velocity-prior model with \(\gamma=0.5\) to HOHVKF, and use a sliding window length of 5 for SWHVKF. Additionally, we employ the same \(\rho=0.97\) for VBKF, HVKF, HOHVKF, and SWHVKF (specifically regarding all process channels for VBKF, and the \(G_2 d_k\) channel in its lifted form). The overall accuracy of the different estimators is illustrated in Fig. 11, with the covariance tracking visualization depicted in Fig. 12 and the corresponding RMSE values detailed in Table 2. The results demonstrate that the exact rank deficit process covariance estimation \(\mathbb{E} [(G_{2,k}d_{k-1})(G_{2,k}d_{k-1})^{T}]\) can be tracked by the proposed methods (but is failed by the other estimators). Notably, over 100 Monte Carlo runs, the proposed HOHVKF and SWHVKF achieve performance almost identical to that of the Ideal KF (overall RMSE of \(0.701 \pm 0.011\) for both HOHVKF and SWHVKF, compared to \(0.070 \pm 0.011\) for the Ideal KF).

4.3 Example 3: Disturbance Estimation↩︎

We consider a manipulator tracking example [9]. The augmented state-space model has \[\begin{align} {x}_{k}&={\Phi}_k {x}_{k-1}+{G}_k {u}_{k-1}+B_d{w}_{d,k}+B_\theta{w}_{\theta,k}\\ {y}_{k}&={H}_k{x}_{k}+B_v{v}_{k} \label{observer} \end{align}\tag{34}\] with \[\nonumber \tiny \begin{align} {\Phi}_k&=\left[\begin{array}{ccc} 1&0&0\\ \frac{T}{{I}_{m}}& 1-\frac{b_{m}T}{{I}_{m}} &-\frac{k_{m}T}{{I}_{m}}\\ 0&T&1\\ \end{array}\right], {G}_k =\left[\begin{array}{c} 0\\ \frac{T}{{I}_{m}}\\ 0 \end{array}\right]\\ {B}_{d} &=\left[\begin{array}{c} 1\\ \frac{T}{I_m}\\ 0 \end{array}\right], {B}_{\theta} =\left[\begin{array}{c} 0 \\ T \\ \frac{T^2}{2} \end{array}\right],{B}_{v} =\left[\begin{array}{c} 1 \\ T \\ \end{array}\right]\\ {H}_k&=\left[\begin{array}{ccc} 0 &1 &0\\ 0 &0 &0.1 \end{array}\right] \end{align}\] where \(x_{k}=[d_{k}, \dot{\theta}_{k},\theta_{k}]^{T}\) denotes the unknown external disturbance, the angular velocity and angle. \(T=0.01\) s is the sampling interval, \(u_k\) is the input torque driven by a computed torque controller (see [9]), \(B_d\) and \(B_\theta\) maps the disturbance noise \(w_{d,k}\) and the process noise \(w_{\theta,k} \sim \mathcal{N}(0,0.01)\) into the systems, and \(v_k \sim \mathcal{N}(0,0.01\mathrm{I}_2)\) is the measurement noise. In the simulation, we use \({I}_{m}=0.1\) Nm\(\cdot\)s\(^2/\deg\), \(b_m=1\) Nm\(\cdot\)s\(/\deg\), \(k_m=0.1\) Nm\(/\deg\). Moreover, we assume that the unknown disturbance has \[\nonumber {d}_{k}=\left\{\begin{array}{l} 50+{w}_{d1,k},400\le k \le 600 \\ {w}_{d2,k}, \operatorname{otherwise} \end{array}\right.\] where \(w_{d1,k}\sim \mathcal{N}(0,100)\) and \(w_{d2,k}\sim \mathcal{N}(0,1)\). The desired angle follows \(\theta_{d,k}=15 \sin (0.4\pi kT)\).

As for estimators, we apply the same nominal covariance for all estimators (if applicable), and tune the hyperparameters (controlling the robustness level) for STKF, MKCKF [42] by trial and error. As for VBKF, HVKF, and HOHVKF, we use the same forgetting factor \(\rho_d=0.96\), and apply \(\gamma_d=0.5\) for HOHVKF. The estimation and control performance by utilizing different estimators is visualized in Fig. 13 and summarized in Table 3, where the best values are highlighted in gray. We find the proposed methods are consistently better than KF, STKF, and VBKF.

5 Conclusion↩︎

In this paper, we introduced a hierarchical variational Kalman filtering (HVKF) that systematically decouples variational inference into sequential robust state filtering and hyperparameter updating. By lifting the system dimensions, this framework unifies the treatment of process and measurement noise covariances, proving particularly advantageous for inferring rank-deficient process covariances. Capitalizing on this decoupled architecture, we successfully accelerated covariance convergence by either modifying the prior model of the latent scale parameters or estimating them via a sliding window mechanism. Extensive simulations confirm the superiority of the proposed approaches compared to existing approaches.

6 Appendix↩︎

6.1 Proof of Lemma 2↩︎

The state \(\chi_k\) is uniquely identifiable if and only if the null space of the design matrix \(\mathcal{N}(W_k) = \{\mathbf{0}\}\). Let \(q_k = \begin{bmatrix} m_k \\ x_k \end{bmatrix} \in \mathcal{N}(W_k)\), implying \(W_k q_k = \mathbf{0}\). Expanding this expression yields: \[\tag{35} \begin{align} B_p^{-1} m_k &= \mathbf{0}, \tag{36} \\ B^\dagger (m_k - x_k) &= \mathbf{0}, \tag{37} \\ D^{-1} C x_k &= \mathbf{0}. \tag{38} \end{align}\] According to 7 and 6 , we know that \(B_p\) and \(D\) are non-singular. Thus, 36 guarantees \(m_k = \mathbf{0}\). Substituting this result into 37 gives \(B^\dagger x_k = \mathbf{0}\). Note that \(B^\dagger\) and \(B^{T}\) share the same null space, since \(B^\dagger= (B^T B)^{-1} B^T\) where \((B^T B)^{-1}\) is invertible. Thus, \(x_k \in \mathcal{N}(B^T)\). Meanwhile, 38 indicates \(x_k \in \mathcal{N}(C)\) since \(D\) is non-singular. Consequently, \(\mathcal{N}(W_k) = \{\mathbf{0}\}\) if and only if \(\mathcal{N}(B^T) \cap \mathcal{N}(C) = \{\mathbf{0}\}\).

6.2 The standard CAVI↩︎

We first focus on \(q(\chi_k)\). Based on 13 and denoting \(q(\chi_k) = \mathcal{N}(\mu_{\chi_k}, \Sigma_{\chi_k})\), the precision matrix determines the covariance and mean: \[\Sigma_{\chi_k} = \left( W_k^\top \mathbb{E}\left[ \Lambda^{-1} \right] W_k \right)^{-1}, \quad \mu_{\chi_k} = \Sigma_{\chi_k} W_k^\top \mathbb{E}\left[ \Lambda^{-1} \right] t_k \label{vbchi}\tag{39}\] where \(\mathbb{E}\left[ \Lambda^{-1} \right]\) is a diagonal matrix and \[\notag \mathbb{E}\left[ \Lambda^{-1} \right]_{ii} = \frac{\nu_i + 1}{\nu_i \tau_i^2 + \mathbb{E}_{q(\chi_k)} \left[ (t_{k,i} - W_{k,i}\chi_k)^2 \right]} \label{wei}\tag{40}\] with \[\scalebox{0.85}{\mathbb{E}_{q(\chi_k)} \left[ (t_{k,i} - W_{k,i}\chi_k)^2 \right] = (t_{k,i} - W_{k,i}\mu_{\chi_k})^2 + W_{k,i}\Sigma_{\chi_k}W_{k,i}^{T}}.\] We then focus on \(q(\lambda^{wv})\). Based on 14 , one can derive that \(q^*(\lambda_i)\) follows an Inverse-Gamma distribution \(\operatorname{IG}(\frac{\nu_i^{*}}{2}, \frac{\nu_i^{*}}{2}(\tau_i^{*})^{2})\) with \[\nu_i^{*} = \nu_i + 1,~(\tau_i^{*})^2 = \frac{\nu_i \tau_i^2 + \mathbb{E}_{q(\chi_k)} \left[ (t_{k,i} - W_{k,i}\chi_k)^2 \right]}{\nu_i + 1}.\]

6.3 Proof of Theorem 1↩︎

Let \(e_i(\chi_k) = [t_k - W_k\chi_k]_i\) denote the \(i\)-th residual element. The objective function in 19 can be reformulated as \(\mathcal{J}(\chi_k) = \sum_{i} \phi(e_i^2(\chi_k))\), where the scalar function \(\phi : \mathbb{R}_{\ge 0} \to \mathbb{R}\) is defined as \(\phi(u) = \frac{\nu_i+1}{2} \log(1 + \frac{u}{\nu_i\tau_i^2})\). Since \(\phi(u)\) is continuously differentiable and strictly concave on \(u \ge 0\) (verified by \(\phi''(u) < 0\)), it is bounded above by its first-order Taylor expansion at any expansion point \(u_0\). Specifically, \[\phi(u) \le \phi(u_0) + \phi'(u_0)(u - u_0).\] Let \(u_i^{(j)} \triangleq (e_i(\chi_k^{(j)}))^2\) be the squared residual at iteration \(j\). Applying the concavity property, we construct a surrogate function \(Q(\chi_k, \chi_k^{(j)})\) that serves as a global upper bound for \(\mathcal{J}(\chi_k)\): \[\scalebox{0.8}{ \mathcal{J}(\chi_k) \le Q(\chi_k, \chi_k^{(j)}) := \sum_{i} \left[ \phi(u_i^{(j)}) + \phi'(u_i^{(j)}) \left( e_i^2(\chi_k) - u_i^{(j)} \right) \right].} \label{eq:surrogate}\tag{41}\] Minimizing \(Q(\chi_k, \chi_k^{(j)})\) with respect to \(\chi_k\) is equivalent to minimizing the weighted least squares term \(\sum_{i} \omega_i^{(j)} e_i^2(\chi_k)\), as the other terms are constant with respect to \(\chi_k\). The weights are given by \(\omega_i^{(j)} = \phi'(u_i^{(j)}) = \frac{\nu_i+1}{\nu_i\tau_i^2 + u_i^{(j)}}\), which is identical to the diagonal elements of \(\Lambda_k(e_k)\) defined in 20 . The fixed-point iteration 21 computes the global minimizer of this surrogate: \[\chi_k^{(j+1)} = \arg\min_{\chi_k} Q(\chi_k, \chi_k^{(j)}).\] The monotonic descent property follows from the majorization-minimization principle: \[\mathcal{J}(\chi_k^{(j+1)}) \le Q(\chi_k^{(j+1)}, \chi_k^{(j)}) \le Q(\chi_k^{(j)}, \chi_k^{(j)}) = \mathcal{J}(\chi_k^{(j)}).\] Since \(\mathcal{J}(\chi_k)\) is bounded below by 0, the sequence of objective values \(\{\mathcal{J}(\chi_k^{(j)})\}\) converges. This completes the proof.

Some representative losses that can be solved by RWLS with guaranteed convergence are listed in Table 4.

6.4 An Alternative Expression of 22 and 23↩︎

By expanding \(\tilde{P}_{\chi k}^{-}\) in 22 , we arrive at \[\tiny \begin{align} x_{k,t}^{+} &= x_k^{-} + \tilde{K}_{xk,t}(y_k - C x_k^{-}) \\ \tilde{K}_{xk,t} &= \tilde{P}_{k}^{-} C^{T}(C \tilde{P}_{k}^{-} C^{T} + \tilde{R}_{k})^{-1} \\ m_{k,t}^{+} &= m_k^{-} + \tilde{K}_{mk,t}(y_k - C x_k^{-}) \\ \tilde{K}_{mk,t} &= \Sigma_{Pk} C^{T}(C \tilde{P}_{k}^{-} C^{T} + \tilde{R}_{k})^{-1} \\ \tilde{P}_{k}^{-} &= \Sigma_{Pk} + \tilde{Q}_{k} \\ \tilde{Q}_{k} &= B \Lambda_{w}^{-1} B^{T} \\ \Lambda_{w} &= \operatorname{diag}[d_{\nu_{n+1}}(e_{n+1}), \ldots, d_{\nu_{n+p}}(e_{n+p})]\\ \tilde{R}_{k} &= D \Lambda_{v}^{-1} D^{T} \\ \Lambda_{v} &= \operatorname{diag}[d_{\nu_{n+p+1}}(e_{n+p+1}), \ldots, d_{\nu_{n+p+m}}(e_{n+p+m})] \\ e_k &= t_k - W_k \chi_k^{t-1} \end{align} \label{update1}\tag{42}\] Correspondingly, at the final iteration, the posteriori error covariance can be update as \[\tiny \begin{align} P_{xk}^{+} &= (I - \tilde{K}_{xk,t} C) {P}_{k}^{-} (I - \tilde{K}_{xk,t} C)^T + \tilde{K}_{xk,t} {R}_{k} \tilde{K}_{xk,t}^{T}\\ P_{m_k}^{+} &= (I - \tilde{K}_{mk,t} C) \Sigma_{Pk} (I - \tilde{K}_{mk,t} C)^T + \tilde{K}_{mk,t} {R}_{k} \tilde{K}_{mk,t}^T\\ P_{m_k x_k}^+ &=(P_{ x_k x_k}^+)^{T}= \Sigma_{Pk} (I - C^T \tilde{K}_{xk,t}^T) \end{align} \label{posterior95covariance}\tag{43}\]

6.5 Proof of Proposition [scaleinv]↩︎

We proceed by induction. For the base case \(k=0\), \(\hat{x}'_{0|0} = \hat{x}_{0|0}\) and \(P'_{0|0} = \alpha P_0\) hold by definition. Assume \(\hat{x}'_{k-1|k-1} = \hat{x}_{k-1|k-1}\) and \(P'_{k-1|k-1} = \alpha P_{k-1|k-1}\). The recursive filter updates yield:\[\tiny \begin{align} \hat{x}'_{k|k-1} &= A\hat{x}'_{k-1|k-1} = A\hat{x}_{k-1|k-1} = \hat{x}_{k|k-1} \\ P'_{k|k-1} &= A(\alpha P_{k-1|k-1})A^T + \alpha Q_{k-1} = \alpha P_{k|k-1} \\ K'_k &= (\alpha P_{k|k-1})C^T \big[ C(\alpha P_{k|k-1})C^T + \alpha R_k \big]^{-1} = K_k \\ \hat{x}'_{k|k} &= \hat{x}'_{k|k-1} + K'_k(y_k - C\hat{x}'_{k|k-1}) = \hat{x}_{k|k} \\ P'_{k|k} &= (I - K'_k C)P'_{k|k-1} = (I - K_k C)(\alpha P_{k|k-1}) = \alpha P_{k|k} \end{align}\] By induction, the lemma holds for all \(k \ge 1\).

6.6 Proof of Theorem 2↩︎

Taking the expectation of 28 yields a recursive geometric series that asymptotically converges to \(\sigma_i^2\), satisfying Property 1. For Property 2, assuming temporal independence of \(\mathcal{E}_{i,k}\), by taking variance of 28 yields \[\operatorname{Var}[(\tau_{i,k|k})^2] = \rho_i^{2} \operatorname{Var}[(\tau_{i,k-1|k-1})^{2}] + (1-\rho_i)^2 \operatorname{Var}[ \mathbb{E}[e_i^2]].\] Subsequently, the steady-state variance of the EWMA filter is given by \(V_{\infty} = \frac{1-\rho_i}{1+\rho_i} \eta_i^4\). Substituting \(\rho_i = \frac{\nu_i}{\nu_i+1}\) gives \[V_{\infty} = \frac{1 - \frac{\nu_i}{\nu_i+1}}{1 + \frac{\nu_i}{\nu_i+1}} \eta_i^4 = \frac{1}{2\nu_i + 1} \eta_i^4.\] For Property 3, the EWMA time constant is \(T_i = \frac{-1}{\ln(\rho_i)}\) (by the definition of EWMA).

6.7 Proof of Theorem 3↩︎

Setting the gradient \(\partial \mathcal{J} / \partial \tau_{i,j}^2 = 0\) yields the symmetric tridiagonal stationarity condition for the interior points: \[\label{eq:stationarity95window} (1+2\lambda)(\tau_{i,j|k})^2 - \lambda (\tau_{i,j-1|k})^2 - \lambda (\tau_{i,j+1|k})^2 = \mathcal{E}_{i,j}.\tag{44}\] Substituting \(\lambda = \frac{\rho_i}{(1-\rho_i)^2}\) and multiplying by \((1-\rho_i)^2\), this condition becomes: \[\label{eq:stationarity95sub95window} (1+\rho_i^2)(\tau_{i,j|k})^2 - \rho_i (\tau_{i,j-1|k})^2 - \rho_i (\tau_{i,j+1|k})^2 = (1-\rho_i)^2 \mathcal{E}_{i,j}.\tag{45}\] To verify the proposed two-pass solution, we isolate the forward estimate from the backward pass: \[\label{eq:isolate95forward} (1-\rho_i)(\tau_{i,j|j})^2 = (\tau_{i,j|k})^2 - \rho_i (\tau_{i,j+1|k})^2.\tag{46}\] Rearranging the forward filter gives \((1-\rho_i)\mathcal{E}_{i,j} = (\tau_{i,j|j})^2 - \rho_i (\tau_{i,j-1|j-1})^2\). Multiplying by \((1-\rho_i)\) and substituting the relation from 46 for both steps \(j\) and \(j-1\) yields: \[\begin{align} (1-\rho_i)^2 \mathcal{E}_{i,j} &= \left[ (\tau_{i,j|k})^2 - \rho_i (\tau_{i,j+1|k})^2 \right] \\ &- \rho_i \left[ (\tau_{i,j-1|k})^2 - \rho_i (\tau_{i,j|k})^2 \right]. \end{align}\] Expanding the right side perfectly recovers the global optimality condition 45 . Because \(\mathcal{J}\) is strictly convex, satisfying this stationarity condition guarantees that the two-pass sequence is the unique exact minimizer.

Shilei Li received the B.E. degree in Detection Guidance and Control Technology and M.S. degree in Control Engineering both from Harbin Institute of Technology, Harbin, China, in 2015 and 2018 respectively, and the Ph.D. degree in Electronic and Computer Engineering from The Hong Kong University of Science and Technology, Hong Kong, China, in 2022. Currently, he is an assistant professor at the School of Automation, Beijing Institute of Technology, China. His research interests include robust filtering and smoothing, human-robot interaction, and exoskeleton robots.

Dawei Shi received the B.E. degree in Electrical Engineering and Automation from the Beijing Institute of Technology, Beijing, China, in 2008, the Ph.D. degree in Control Systems from the University of Alberta, Edmonton, AB, Canada, in 2014. In December 2014, he was appointed as an Associate Professor at the School of Automation, Beijing Institute of Technology. From February 2017 to July 2018, he was with the Harvard John A. Paulson School of Engineering and Applied Sciences, Harvard University, as a Postdoctoral Fellow in bioengineering. Since July 2018, he has been with the School of Automation, Beijing Institute of Technology, where he is a professor. His research focuses on the analysis and synthesis of complex sampled-data control systems with applications to biomedical engineering, robotics, and motion systems. He serves as an Associate Editor/Technical Editor for IEEE Transactions on Industrial Electronics, IEEE/ASME Transactions on Mechatronics, IEEE Control Systems Letters, and IET Control Theory and Applications. He is a member of the Early Career Advisory Board of Control Engineering Practice. He was a Guest Editor for European Journal of Control. He served as an associate editor for IFAC World Congress and is a member of the IEEE Control Systems Society Conference Editorial Board. He is a Senior Member of the IEEE.

Wei Zheng received his Bachelor of Engineering degree in Electronic Information Engineering from the Department of Electronic Engineering, Tsinghua University in 2002. He obtained his master’s degree in 2008 and his Ph.D. in 2013, both from the Intelligent Information Processing Laboratory, Institute of Computing Technology, Chinese Academy of Sciences. From 2013 to March 2016, he conducted postdoctoral research at the Visual Intelligence Laboratory of Samsung Research Institute (Beijing) Communication Technology. His research findings have been published in numerous renowned international conferences and journals, including CVPR, ACCV, IEEE Transactions, and Elsevier, and he holds a number of domestic and international invention patents. From 2009 to 2010, he conducted research on object recognition and multimedia retrieval at Panasonic Singapore Laboratories and Microsoft Research Asia (MSRA) respectively. From 2013 to 2017, he led the development of intelligent algorithms for mobile devices at Samsung Research Institute Beijing Communication Technology, where he led his team in upgrading recognition algorithms from traditional classification methods to deep learning approaches, ultimately delivering core algorithms for the intelligent photo album management engine in Samsung flagship smartphones. From 2017 to 2019, he served as an algorithm researcher at the joint laboratory of the Hong Kong University of Science and Technology and Youjia Innovation Co., Ltd., focusing on visual recognition algorithms for intelligent driving. Since 2019, he has been Vice President of Algorithm R&D at Youjia Innovation Co., Ltd., leading a R&D team of over 100 members to develop intelligent driving algorithms and designing core algorithm architectures for multiple autonomous driving products of the company.

Ling Shi(Fellow, IEEE) received the B.S. degree in electrical and electronic engineering from Hong Kong University of Science and Technology, Kowloon, Hong Kong, in 2002 and the Ph.D. degree in Control and Dynamical Systems from California Institute of Technology, Pasadena, CA, USA, in 2008. He is currently a Professor in the Department of Electronic and Computer Engineering, and the associate director of the Robotics Institute, both at the Hong Kong University of Science and Technology. His research interests include cyber-physical systems security, networked control systems, sensor scheduling, event-based state estimation, and exoskeleton robots. He served as an editorial board member for the European Control Conference 2013-2016. He was a subject editor for International Journal of Robust and Nonlinear Control (2015-2017). He has been serving as an associate editor for IEEE Transactions on Control of Network Systems from July 2016, and an associate editor for IEEE Control Systems Letters from Feb 2017. He also served as an associate editor for a special issue on Secure Control of Cyber Physical Systems in the IEEE Transactions on Control of Network Systems in 2015-2017. He served as the General Chair of the 23rd International Symposium on Mathematical Theory of Networks and Systems (MTNS 2018). He is a member of the Young Scientists Class 2020 of the World Economic Forum (WEF) and he is a fellow of IEEE (Class 2023).

References↩︎

---

1. Shilei Li and Dawei Shi are with School of Automation, Beijing Institute of Technology, Beijing 100081, China (e-mail: shileili@bit.edu.cn, daweishi@bit.edu.cn). Wei Zheng is with Shenzhen Youjia Innovation Technology Co., Ltd., Shenzhen, China (e-mail: zhengwei@minieye.cc). Ling Shi is with the Department of Electronic and Computer Engineering, The Hong Kong University of Science and Technology, Kowloon, Hong Kong (e-mail: eesling@ust.hk). (Corresponding author: Dawei Shi.)↩︎