Sequential Mini-Batch Noise Covariance Estimator

Abstract

Noise covariance estimation in an adaptive Kalman filter is a problem of significant practical interest in a wide array of industrial applications. Reliable algorithms for their estimation are scarce, and the necessary and sufficient conditions for identifiability of the covariances were in dispute until very recently. This chapter presents the necessary and sufficient conditions for the identifiability of noise covariances, and then develops sequential mini-batch stochastic optimization algorithms for estimating them. The optimization criterion involves the minimization of the sum of the normalized temporal cross-correlations of the innovations; this is based on the property that the innovations of an optimal Kalman filter are uncorrelated over time. Our approach enforces the structural constraints on noise covariances and ensures the symmetry and positive definiteness of the estimated covariance matrices. Our approach is applicable to non-stationary and multiple model systems, where the noise covariances can occasionally jump up or down by an unknown level. The validation of the proposed method on several test cases demonstrates its computational efficiency and accuracy.

1. Introduction

This chapter addresses the following learning problem: Given a vector time series and a library of models for the time evolution of the data (e.g., a Wiener process, a white noise acceleration model, also called nearly constant velocity model, or a white noise jerk model, also called nearly constant acceleration model), find suitable process and measurement noise covariances and select the best dynamic model for the time series. This problem is of considerable interest in a number of applications, such as fault diagnosis, robotics, signal processing, navigation, and target tracking, to name a few [1, 2].

The Kalman filter (KF) [3] is the optimal minimum mean square error (MMSE) state estimator for linear systems with mutually uncorrelated Gaussian white process and measurement noises, and is the best linear state estimator when the noises are non-Gaussian with known covariances. However, the noise covariances are unknown or only partially known in many practical applications.

We derived the necessary and sufficient conditions for the identifiability of unknown noise covariances, and presented a batch optimization algorithm for their estimation using the sum of the normalized temporal cross-correlations of the innovation sequence as the optimization criterion [4]. The motivation for this optimization metric stems from the fact that the innovations of an optimal Kalman filter are white, meaning that they are uncorrelated over time [2]. In [5], we proposed a sequential mini-batch stochastic gradient descent (SGD) algorithm that required multiple passes through the measurements for estimating noise covariances. We also presented its applicability to non-stationary systems by detecting changes in noise covariances. In this chapter, we present a practical single-pass stochastic gradient descent algorithm for noise covariance estimation in non-stationary systems. Extensions to multiple models where the system behavior can stem from a member of a known subset of models are discussed in [6].

1.1 Prior work

The key to noise covariance estimation is an expression for the covariance of the state estimation error and of the innovations of any stable, but not necessarily optimal, filter as a function of noise covariances. This expression serves as a foundational building block for the correlation-based methods for noise covariance estimation. Pioneering contributions using this approach were made by [7, 8, 9]. Sarkka and Nummenmaa [10] proposed a recursive noise-adaptive Kalman filter for linear state space models using variational Bayesian approximations. However, the variational methods generally require tuning hyper-parameters to converge to the correct covariance parameters and these algorithms often converge to local minima.

In [5], we presented a computationally efficient and accurate sequential estimation algorithm that is a major improvement over the batch estimation algorithm in [4]. The novelties of this algorithm stem from its sequential nature and the use of mini-batches, adaptive step size rules and dynamic thresholds for convergence in the stochastic gradient descent (SGD) algorithm. The innovation cross-correlations are obtained by a sequential fading memory filter. We applied a change-point detection algorithm described in [11] to extract the change points in noise covariances for non-stationary systems.

This chapter seeks to develop a streaming algorithm that reads measurements exactly once, thus making it real-time and practical. The only caveat is that the changes in noise covariances are assumed to occasionally jump up or down by an unknown magnitude. Extensions of this algorithm to a multiple model setting may be found in [6].

1.2 Organization of the chapter

The organisation of the chapter is as follows. Section 2 presents the mathematical formulation of the sequential mini-batch gradient descent algorithm for estimating the unknown noise covariances. In this section, we also present an overview of our approach based on a fading memory filter-based innovation correlation estimation, and an accelerated SGD update of the Kalman gain. In Section 3, we show that our single-pass method can track unknown noise covariances in non-stationary systems. Lastly, we conclude the chapter with a brief summary of the contributions in Section 4.

2. Sequential mini-batch SGD method for estimating process and measurement noise covariances

Consider a discrete-time linear dynamic system

$$x\left(k+1\right)=\mathit{Fx}\left(k\right)+\mathrm{\Gamma }v\left(k\right)$$E1

$$z\left(k\right)=\mathit{Hx}\left(k\right)+w\left(k\right)$$E2

where $x\left(k\right)$ is the $n_x$-dimensional state vector, $v\left(k\right)$ is the sequence of zero-mean white Gaussian process noise with unknown process noise covariance $Q\left(k\right)$ in the plant equation. The measurement equation, $z\left(k\right)$ with $n_z$-dimensional vector, is given in (2). Here, $w\left(k\right)$ is the sequence of zero-mean white Gaussian measurement noise with unknown measurement noise covariance $R\left(k\right)$. In the system, $F$ and $\mathrm{\Gamma }$ are the $n_x\times n_x$ state transition matrix and the noise gain matrix, respectively, and $H$ is the $n_z\times n_x$ measurement matrix. Here, the two noise sequences and the initial state error are assumed to be mutually uncorrelated. We assume that noise covariances $Q\left(k\right)$ and $R\left(k\right)$ are piecewise constant such that the filter reaches a steady-state between any two jumps of unknown magnitude.

Given $Q\left(k\right)$ and $R\left(k\right)$, the Kalman filter involves the consecutive processes of prediction and update given by [2, 3].

$$\widehat{x}\left(k+1|k\right)=F\widehat{x}\left(k|k\right)$$E3

$$\nu \left(k+1\right)=z\left(k+1\right)-H\widehat{x}\left(k+1|k\right)$$E4

$$\widehat{x}\left(k+1|k+1\right)=\widehat{x}\left(k+1|k\right)+W\left(k+1\right)\nu \left(k+1\right)$$E5

$$P\left(k+1|k\right)=\mathit{FP}\left(k|k\right)F^{\prime }+\mathrm{\Gamma }Q\left(k\right){\mathrm{\Gamma }}^{\prime }$$E6

$$S\left(k+1\right)=\mathit{HP}\left(K+1|k\right)H^{\prime }+R\left(k\right)$$E7

$$W\left(k+1\right)=P\left(k+1|k\right)H^{\prime }S{\left(k+1\right)}^{-1}$$E8

$$P\left(k+1|k+1\right)=\left(I_{n_x}-W\left(k+1\right)H\right)P\left(k+1|k\right){\left(I_{n_x}-W\left(k+1\right)H\right)}^{\prime }$$

$$+W\left(k+1\right)R\left(k\right)W{\left(k+1\right)}^{\prime }$$E9

The Kalman filter predicts the next state estimate at time index $\left(k+1\right)$, given the observations up to time index $k$ in (3) and the concomitant predicted state estimation error covariance in (6), using system dynamics, the updated state error covariance $P\left(k|k\right)$ at time index $k$ and the process noise covariance, $Q\left(k\right)$. The updated state estimate at time $\left(k+1\right)$ in (5) incorporates the measurement at time $\left(k+1\right)$ via the Kalman gain matrix in (8), which depends on the innovation covariance $S\left(k+1\right)$ (which in turn depends on the measurement noise covariance $R\left(k\right)$, and the predicted state error covariance $P\left(k+1|k\right)$). The updated state error covariance $P\left(k+1|k+1\right)$ is computed via (9); this is the Joseph form, which is less sensitive to round-off error because it guarantees that the updated state covariance matrix will remain positive definite.

2.1 Identifiability conditions for estimating $Q$ and $R$

The necessary and sufficient conditions for identifiability of the covariances in adaptive Kalman filters were in dispute until very recently [4, 7, 8, 9, 12]. When $Q$ and $R$ are unknown, consider the innovations corresponding to a stable, suboptimal closed-loop filter matrix $\overline{F}=F\left(I_{n_x}-\mathit{WH}\right)$ given by [4, 13].

$$\begin{array}{c}\nu \left(k\right)=H{\overline{F}}^m\left[x\left(k-m\right)-\widehat{x}\left(k-m|k-m-1\right)\right]\\ +\left\{H\sum _{j=0}^{m-1}{\overline{F}}^{m-1-j}\left[\mathrm{\Gamma }v\left(k-m+j\right)-\mathit{FWw}\left(k-m+j\right)\right]\right\}+w\left(k\right)\end{array}$$E10

Given the innovation sequence (10), a weighted sum of innovations, $\xi \left(k\right)$, can be computed as

$$\xi \left(k\right)=\sum _{i=0}^m{a}_i\nu \left(k-i\right)$$E11

where the weights are the coefficients of the minimal polynomial of the closed-loop filter matrix $\overline{F}$, ${\sum }_{i=0}^m{a}_i{\overline{F}}^{m-i}=0,a_0=1.$ It is easy to see that $\xi \left(k\right)$ is the sum of two moving average processes driven by the process noise and measurement noise, respectively, given by [4].

$$\xi \left(k\right)=\sum _{l=1}^m{\mathcal{B}}_{l}v\left(k-l\right)+\sum _{l=0}^m{\mathcal{G}}_{l}w\left(k-l\right)$$E12

Here, ${\mathcal{B}}_l$ and ${\mathcal{G}}_l$ are given by

$${\mathcal{B}}_l=H\left(\sum _{i=0}^{l-1}{a}_i{\overline{F}}^{l-i-1}\right)\mathrm{\Gamma }$$E13

$${\mathcal{G}}_l=\left[a_l{I}_{n_z}-H\left(\sum _{i=0}^{l-1}{a}_i{\overline{F}}^{l-i-1}\right)\mathit{FW}\right];{\mathcal{G}}_0=I_{n_z}$$E14

Then, the cross-covariance between $\xi \left(k\right)$ and $\xi \left(k-j\right)$, ${\mathcal{L}}_j$, can be obtained as

$${\mathcal{L}}_j=E\left[\xi \left(k\right)\xi {\left(k-j\right)}^{\prime }\right]=\sum _{i=j+1}^m{\mathcal{B}}_{i}Q{{\mathcal{B}}_{i-j}}^{\text{'}}+\sum _{i=j}^m{\mathcal{G}}_{i}R{{\mathcal{G}}_{i-j}}^{\text{'}}$$E15

The noise covariance matrices $Q=\left[q_{\mathit{ij}}\right]$ of dimension $n_v\times n_v$ and $R=\left[r_{\mathit{ij}}\right]$ of dimension $n_z\times n_z$ are positive definite and symmetric. By converting the noise covariance matrices and the ${\mathcal{L}}_j$ matrices to vectors, Zhang et al. [4] show that they are related by the noise covariance identifiability matrix $\mathcal{I}$ given by

$$\mathcal{I}\left[\begin{array}{c}\mathrm{vec}\left(Q\right)\\ \mathrm{vec}\left(R\right)\end{array}\right]=\left[\begin{array}{l}{\mathcal{L}}_0\\ {\mathcal{L}}_1\\ ⋮\\ {\mathcal{L}}_m\end{array}\right]$$E16

As shown in [4], if the matrix $\mathcal{I}$ has full rank, then the unknown noise covariance matrices, $Q$ and $R$, are identifiable. Direct solution of linear equations in (16) for $Q$ and $R$ is highly ill-conditioned and is prone to numerical errors.

2.2 Recursive fading memory-based innovation correlation estimation

We compute the sample correlation matrix ${\widehat{C}}_{\mathit{seq}}^k\left(i\right)$ at sample $k$ for time lag $i$ as a weighted combination of the correlation matrix ${\widehat{C}}_{\mathit{seq}}^{k-1}\left(i\right)$ at the previous sample ($k-1$) and time lag $i$, and the samples of innovations $\nu \left(k-i\right)$ and $\nu \left(k\right)$. The tuning parameter $\lambda$, a positive constant between 0 and 1, is the weight associated with the previous sample correlation matrix. The current $M$ sample correlation matrices at time $k$ are used as the initial values for the next pairs of samples for the recursive computation. Let us define the number of measurement samples as $N$. Then,

$${\widehat{C}}_{\mathit{seq}}^k\left(i\right)=\left(1-\lambda \right)\nu \left(k-i\right)\nu {\left(k\right)}^{\prime }+\lambda {\widehat{C}}_{\mathit{seq}}^{k-1}\left(i\right),$$E17

$${\widehat{C}}_{\mathit{seq}}^0\left(i\right)=0,i=\mathrm{0,1,2},\cdots ,M-1;k=M,\cdots ,N$$E18

2.3 Objective function and the gradient

The ensemble cross-correlation of a steady-state suboptimal Kalman filter is related to the closed-loop filter matrix $\overline{F}=F\left(I_{n_x}-\mathit{WH}\right)$, the matrix $F$, the measurement matrix $H$, the steady-state predicted covariance matrix $\overline{P}$, steady-state filter gain $W$ and the steady-state innovation covariance, $C\left(0\right)$ via [8, 9].

$$C\left(i\right)=E\left[\nu \left(k\right)\nu {\left(k-i\right)}^{\prime }\right]=H{\overline{F}}^{i-1}F\left[\overline{P}{H}^{\prime }-\mathit{WC}\left(0\right)\right]$$E19

To avoid the scaling effects of measurements, the objective function $\mathrm{\Psi }$ formulated in [4] involves a minimization of the sum of normalized $C\left(i\right)$ with respect to the corresponding diagonal elements of $C\left(0\right)$ for $i>0$. Formally, we can define the objective function $\mathrm{\Psi }$ to be minimized with respect to $W$ as

$$\mathrm{\Psi }=\frac{1}{2}\mathit{tr}\left\{\sum _{i=1}^{M-1}{\left[\mathit{diag}\left(C\left(0\right)\right)\right]}^{-\frac{1}{2}}C{\left(i\right)}^{\prime }{\left[\mathit{diag}\left(C\left(0\right)\right)\right]}^{-1}C\left(i\right){\left[\mathit{diag}\left(C\left(0\right)\right)\right]}^{-\frac{1}{2}}\right\}$$E20

where $\mathit{diag}\left(C\right)$ denotes the diagonal matrix of $C$. We can rewrite the objective function by substituting (20) into (19) as

$$\mathrm{\Psi }=\frac{1}{2}\mathit{tr}\left\{\sum _{i=1}^{M-1}\varphi \left(i\right)\mathit{X\phi }{X}^{\prime }\right\}$$E21

where

$$\varphi \left(i\right)={\left[H{\overline{F}}^{i-1}F\right]}^{\prime }\phi \left[H{\overline{F}}^{i-1}F\right]$$E22

$$X=\overline{P}{H}^{\prime }-\mathit{WC}\left(0\right)$$E23

$$\phi ={\left[\mathit{diag}\left(C\left(0\right)\right)\right]}^{-1}$$E24

The gradient of objective function, ${\nabla }_W\mathrm{\Psi }$, can be computed as [4].

$${\nabla }_W\mathrm{\Psi }=-\sum _{i=1}^{M-1}{\left[H{\overline{F}}^{i-1}F\right]}^{\prime }\mathit{\phi C}\left(i\right)\mathit{\phi C}\left(0\right)-F^{\prime }\mathit{ZFX}-\sum _{l=0}^{i-2}{\left[C\left(l+1\right)\mathit{\phi C}{\left(i\right)}^{\prime }\mathit{\phi H}{\overline{F}}^{i-l-2}\right]}^{\prime }$$E25

where

$$Z={\overline{F}}^{\text{'}}Z\overline{F}+\frac{1}{2}\sum _{i=1}^{M-1}{\left(H{\overline{F}}^{i-1}F\right)}^{\prime }\mathit{\phi C}\left(i\right)\mathit{\phi H}+{\left({\left(H{\overline{F}}^{i-1}F\right)}^{\prime }\mathit{\phi C}\left(i\right)\mathit{\phi H}\right)}^{\prime }$$E26

The $Z$ term in (26) is computed by a Lyapunov equation; it is often small and can be neglected in (25) for computational efficiency.

In computing the objective function and the gradient, we replace $C\left(i\right)$ by their sample estimates, ${\widehat{C}}_{\mathit{seq}}\left(i\right)$. With this replacement, the noise covariance estimation becomes a data-dependent stochastic optimization/learning problem.

2.4 Estimation of $Q$ and $R$

2.4.1 Estimation of $R$

We define $\mu \left(k\right)$ as the post-fit residual sequence of the Kalman filter, which is related to the innovations $\nu \left(k\right)$ via

$$\mu \left(k\right)=z\left(k\right)-H\widehat{x}\left(k|k\right)=\left(I_{n_z}-\mathit{HW}\right)\nu \left(k\right);k=1,2,\cdots ,N$$E27

From the joint covariance of the innovation sequence $\nu \left(k\right)$ and the post-fit residual sequence $\mu \left(k\right)$, and the Schur determinant identity [14, 15], one can show that [4].

$$G=E\left[\mu \left(k\right)\mu {\left(k\right)}^{\prime }\right]={\mathit{RS}}^{-1}R$$E28

where $S$ is the innovation covariance. Knowing the sampled estimates of $G$ and $S$=${\widehat{C}}_{\mathit{seq}}\left(0\right)$, the measurement noise covariance $R$ is estimated. Because (28) can be interpreted as a continuous-time algebraic Riccati equation or as a simultaneous diagonalization problem in linear algebra [15], the measurement noise covariance $R$ can be estimated by solving a continuous-time Riccati equation as in [4, 16] or by solving the simultaneous diagonalization problem via Cholesky decomposition and eigen decomposition.

2.4.2 Estimation of $Q$

Since the process noise covariance $Q$ and the steady-state updated covariance $P$ are generally coupled, $Q$ and $P$ can be obtained via a Gauss–Seidel type iterative computation given the estimated $R$. Wiener process is an exception where an explicit non-iterative solution $Q=\mathit{WS}{W}^{\prime }$ is possible [4]. Let $t$ and $l$ denote the indices of iteration starting with $t$ = 0 and $l$ = 0. The initial steady-state updated covariance, $P^0$, can be computed as the solution of the Lyapunov equation given by

$$P^{\left(0\right)}=\tilde{F}{P}^{\left(0\right)}{\tilde{F}}^{\prime }+\mathit{WR}{W}^{\prime }+\left(I_{n_x}-\mathit{WH}\right)\mathrm{\Gamma }{Q}^{\left(t\right)}{\mathrm{\Gamma }}^{\prime }{\left(I_{n_x}-\mathit{WH}\right)}^{\prime };Q^{\left(0\right)}=\mathit{WS}{W}^{\prime }$$E29

where $\tilde{F}=\left(I_{n_x}-\mathit{WH}\right)F$. We iteratively update $P$ as in (30) until convergence

$$P^{\left(l+1\right)}={\left[{\left({\mathit{FP}}^{\left(l\right)}{F}^{\prime }+\mathrm{\Gamma }{Q}^{\left(t\right)}{\mathrm{\Gamma }}^{\prime }\right)}^{-1}+H^{\prime }{R}^{-1}H\right]}^{-1}$$E30

Given the converged $P$, $Q$ will be updated in the $t$-loop until the estimate of $Q$ converges.

$$Q^{\left(t+1\right)}={\mathrm{\Gamma }}^{†}\left[{\left(P+\mathit{WS}{W}^{\prime }-\mathit{FP}{F}^{\prime }\right)}^{\left(t+1\right)}+{\lambda }_Q{I}_{n_x}\right]{\left({\mathrm{\Gamma }}^{\prime }\right)}^{†}$$E31

where ${\lambda }_Q$ is a regularization parameter used for ill-conditioned estimation problems.

2.5 Updating the gain $W$ sequentially

The estimation algorithm sequentially computes the $M$ sample covariance matrices at every measurement sample $k$ as in (17). Let $B$ be the mini-batch size for updating the Kalman filter gain $W$ in the SGD. Our proposed method updates the gain $W$ when the sample index $k$ is divisible by the mini-batch size $B$. When compared to the batch estimation algorithm, the sequential mini-batch SGD algorithm allows more opportunities to converge to a better local minimum of (20) by frequently updating the filter the gain [5]. The generic form of the gain update is given by

$$W^{\left(r+1\right)}=W^{\left(r\right)}-{\alpha }^{\left(r\right)}{\nabla }_{W^{\left(r\right)}}\mathrm{\Psi }$$E32

where $r$ is the updating index starting with $r=0$. In our previous research [5], we explored the performance of accelerated SGD methods (e.g., bold driver [17], constant, subgradient [18], RMSProp [19], Adam [20], Adadelta [21]) for updating adaptive step size ${\alpha }^{\left(r\right)}$ in (32). The root mean square propagation (RMSProp) method is applied for the estimation procedure in this chapter. The RMSProp keeps track of the moving average of the squared incremental gradients for each gain element by adapting the step size element-wise as in the following.

$${\tau }_{r,\mathit{ij}}={\mathit{\gamma \tau }}_{r-1,\mathit{ij}}+\left(1-\gamma \right){\left[{\left({\nabla }_{W^{\left(r\right)}}\mathrm{\Psi }\right)}_{\mathit{ij}}\right]}^2;{\tau }_0=0$$E33

$${\alpha }_{\mathit{ij}}^{\left(r\right)}=\frac{{\alpha }^{\left(0\right)}}{\sqrt{{\tau }_{r,\mathit{ij}}+\epsilon }}$$E34

Here, $\gamma =0.9$ is the default value and $\epsilon ={10}^{-8}$ to prevent division by zero.

The pseudocode for the sequential mini-batch SGD estimation algorithm for a non-stationary system is included as Algorithm 1.

Algorithm 1 Pseudocode of sequential mini-batch SGD algorithm.

1: input: $W^0$, $Q^0$, $R^0$, ${\alpha }^0$, $B$$⊳$$W^0$: initial gain, $Q^0$: initial $Q$, $R^0$: initial $R$, ${\alpha }^0$: initial step size, $B$: batch size.

2: r = 0 {$⊳$ Initialize the updating index $r$_}.

3: fork = 1 to Ndo {$⊳$$N$: Number of samples}.

4:  compute innovation correlations $\nu \left(k\right)$.

5:  if$k>N_b+M$then {$⊳$$N_b$: Number of burn-in samples}.

6:   compute ${\widehat{C}}_{\mathit{seq}}^k\left(i\right)$, i = 0,1,2,...M-1.

7:   if$\mathit{Mod}\left(k,B\right)=0$then.

8:    compute the objective function $\mathrm{\Psi }$.

9:    compute the gradient ${\nabla }_W\mathrm{\Psi }$.

10:   update the step size ${\alpha }^{\left(r\right)}$.

11:   update the gain $W_{\mathit{ij}}^{\left(r+1\right)}=W_{\mathit{ij}}^{\left(r\right)}-{\alpha }_{\mathit{ij}}^{\left(r\right)}\left[{\nabla }_{W^{\left(r\right)}}\mathrm{\Psi }\right)]{}_{\mathit{ij}}$.

12:   update $R^{\left(r+1\right)}$ and $Q^{\left(r+1\right)}$.

13:   r = r + 1.

14:   end if.

15:  end if.

16: end for.

3. Numerical examples

In [5], we provided the evidence that the multi-pass sequential mini-batch stochastic gradient descent (SGD) algorithms improve the computational efficiency of the batch estimation algorithm via a number of test cases used in [2, 7, 8, 9, 12], and also showed their applicability to non-stationary systems when coupled with a change-point detection algorithm [11]. In [22], we proposed a single-pass sequential mini-batch SGD estimation algorithm by accessing measurements exactly once for non-stationary systems by modifying the example used in [12] to periodically change the process and measurement noise covariances.

In this section, we illustrate the utility of our proposed single-pass sequential mini-batch SGD estimation algorithm by applying it to general diverse examples involving detectable (but not completely observable) systems, non-stationary systems and a bearings-only tracking problem.

For the non-stationary systems, we assumed the process and measurement noise covariances occasionally change by an unknown level. Here “occasionally” implies the jumps are infrequent enough that the Kalman filter is in the steady-state prior to a jump in the noise covariance. We define the number of subgroups in which the noise covariances are not changing as $N_{\mathit{sg}}$. Given the number of observation samples, $N$, each subgroup has constant noise covariances with $N/N_{\mathit{sg}}$ samples. In this section, we consider two non-stationary scenarios for tracking time-varying $Q$ and $R$ with $N_{\mathit{sg}}=5$ subgroups. We also consider the bearings-only tracking problem where $Q$ changes continuously.

Note that the number of “burn-in” samples and the number of lags are $N_b=50$ and $M=5$, respectively in the estimation procedure. The root mean square propagation (RMSProp) method is applied to update the filter gain. All Monte Carlo simulations were run using a computer with an Intel Core i7-8665U processor and 16 GB of RAM.

We used the averaged normalized innovation squared (NIS) metric [2] to measure the consistency of the proposed algorithm.

$${\overline{\epsilon }}_{\nu }\left(k\right)=\frac{1}{N_{\mathit{mc}}}\sum _{i=1}^{N_{\mathit{mc}}}\nu {\left(k\right)}^{\prime }{S}^{-1}\nu \left(k\right)$$E35

where $N_{\mathit{mc}}$ is the number of Monte Carlo runs. The root mean square error (RMSE) in resultant position and velocity is computed using

$$\mathit{\text{RMSE}}\left(k\right)=\sqrt{\frac{1}{N_{\mathit{mc}}}\sum _{i=1}^{N_{\mathit{mc}}}{\left(x{\left(k\right)}_{\mathit{\text{true}}}-\widehat{x}\left(k\right)\right)}^2}$$E36

3.1 Case 1: A detectable (but not completely observable) system that satisfies the identifiability conditions

Mehra [8] stated, without proof, that a necessary and sufficient condition for noise covariance estimation is that the system satisfies the observability property. This example, due to Odelson et al. [7], demonstrates that this condition is not necessary. The example does satisfy the full column rank condition for the identifiability matrix in (16).

Odelson et al. [7] proposed a noise covariance estimation method based on the autocovariance least-squares formulation by using the Kronecker operator $\delta$. This method computes the covariances from the residuals of the state estimation. Note that the incompletely observable (but detectable ¹) system used in [7] is described by

$$F=\left[\begin{array}{cc}0.1& 0\\ 0& 0.2\end{array}\right],H=\left[10\right],\mathrm{\Gamma }=\left[\begin{array}{c}1\\ 2\end{array}\right]$$E37

where $F$ is the non-singular transition matrix, $H$ is the constant output matrix, and $\mathrm{\Gamma }$ is the constant input matrix in (1) and (2). Note that this system is a hypothetical numerical example. The process noise $v\left(k\right)$ and the measurement noise $w\left(k\right)$ are supposed to be uncorrelated Gaussian white noise sequences with zero-mean and covariances as in the following

$$E\left[v\left(k\right)v{\left(j\right)}^{\prime }\right]=Q{\delta }_{\mathit{kj}}$$E38

$$E\left[w\left(k\right)w{\left(j\right)}^{\prime }\right]=R{\delta }_{\mathit{kj}}$$E39

In this scenario, the true $R$ values for the five subgroups are [0.30, 0.81, 0.49, 0.72, 0.42], and the true $Q$ values for the five subgroups are [0.16, 0.49, 0.25, 0.36, 0.20]. The values are changed every 10,000 samples. Table 1 shows the results of 100 Monte Carlo simulations based on the single-pass SGD algorithm in estimating $Q$ and $R$. As can be seen, the estimated $Q$ and $R$ are close to their corresponding true values. In this scenario, the single-pass SGD estimation method has a speedup factor of 31 over the batch and multiple-pass SGD estimation methods (not shown).

Subgroup index	$R$			$Q$			${\overline{P}}_{11}$			${\overline{P}}_{22}$			$W_{11}$			$W_{21}$
	Truth	Mean	RMSE	Truth	Mean	RMSE	Truth	Mean	RMSE	Truth	Mean	RMSE	Truth	Mean	RMSE	Truth	Mean	RMSE
$1^{\mathit{st}}$	0.30	0.31	0.06	0.16	0.15	0.04	0.16	0.15	0.04	0.66	0.62	0.18	0.35	0.33	0.03	0.70	0.67	0.03
$2^{\mathit{nd}}$	0.81	0.86		0.49	0.42		0.49	0.43		2.01	1.74		0.38	0.34		0.76	0.67
$3^{\mathit{rd}}$	0.49	0.49		0.25	0.24		0.25	0.24		1.03	0.97		0.34	0.34		0.68	0.67
$4^{\mathit{th}}$	0.72	0.71		0.36	0.35		0.36	0.35		1.48	1.44		0.33	0.34		0.67	0.67
$5^{\mathit{th}}$	0.42	0.41		0.20	0.20		0.20	0.21		0.83	0.84		0.33	0.34		0.66	0.67

Table 1.

Single-pass SGD estimation for Case 1 (100 MC Runs; 50,000 samples; M = 5; RMSProp; Batch size = 64).

Figure 1 demonstrates that the sequential mini-batch gradient descent algorithm can track $Q$ and $R$ correctly. Here, the trajectories of $Q$ and $R$ estimates are smoothed by a simple first order fading memory filter with a smoothing weight of 0.7. Figure 1e shows the averaged NIS of SGD (RMSProp; batch size of 64) method with the 95% probability region [0.74, 1.30], and shows that the SGD-based Kalman filter is consistent. The only place at which the NIS values are large are immediately after the jump in the noise variances. This is because adaptation requires a few samples.

Figure 1.

Trajectories of $Q$ and $R$ estimates without signal smoothing and with a smoothing weight of 0.7 for Case 1.

3.2 Case 2: a five-state inertial navigation system with diagonal $Q$ and $R$

For estimating the unknown noise covariance parameters and the optimal Kalman filter gain for part of an inertial navigation system (INS), Mehra [8] proposed an iterative innovation correlations-based method starting from an arbitrary initial stabilizing gain. Inertial navigation [25] involves tracking the position and orientation of an object relative to a known starting orientation and velocity and it uses measurements provided by accelerometers and gyroscopes. These systems have found universal use in military and commercial applications [26].

Since the earth is not flat, the inertial navigation systems need to keep tilting the platform (with respect to inertial space) to keep the axes of the accelerometers horizontal. Here, small error sources that drive the Schuler-loop cause the navigation errors, and these errors are” damped” by making use of external velocity measurements, such as are furnished by a Doppler radar [27, 28].

In this problem, Mehra [8] used a system based on the damped Schuler-loop error propagation forced by exponentially correlated as well as white noise input. The system matrices for this navigation system are given by

$$\begin{array}{l}F=\left[\begin{array}{ccccc}0.75& -1.74& -0.3& 0& -0.15\\ 0.09& 0.91& -0.0015& 0& -0.008\\ 0& 0& 0.95& 0& 0\\ 0& 0& 0& 0.55& 0\\ 0& 0& 0& 0& 0.905\end{array}\right];H=\left[\begin{array}{ccccc}1& 0& 0& 0& 1\\ 0& 1& 0& 1& 0\end{array}\right];\\ \mathrm{\Gamma }=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 24.64& 0& 0\\ 0& 0.835& 0\\ 0& 0& 1.83\end{array}\right]\end{array}$$E40

where the system is discretized using a time step of 0.1 seconds. In this five-state system, the first two states represent the a velocity damping term and velocity error, respectively, and the other three states model the correlated noise processes. The noise corresponding to states 3 and 5 impacts both the velocity error and the velocity damping term; the fourth state impacts the sensor error in state 2 only.

In this problem, the true values corresponding to each subgroup with $N_{\mathit{sg}}=5$ subgroups and $N=\mathrm{100,000}$ samples are as in (41). Each parameter of $Q$ and $R$ changes every 20,000 samples.

$$\begin{array}{l}\left[\begin{array}{l}{R}_{11}\\ R_{22}\end{array}\right]=\left[\begin{array}{l}\left[0.25,0.56,0.64,0.42,0.36\right]\\ \left[0.25,0.25,0.49,0.16,0.04\right]\end{array}\right];\left[\begin{array}{l}{Q}_{11}\\ Q_{22}\\ Q_{33}\end{array}\right]=\left[\begin{array}{l}\left[0.25,0.64,0.49,0.25,0.49\right]\\ \left[0.25,0.36,0.56,0.16,0.04\right]\\ \left[0.36,0.49,0.64,0.25,0.09\right]\end{array}\right]\end{array}$$E41

Table 2 shows the results of 100 Monte Carlo simulations for estimating the noise parameters using RMSProp update. The estimated parameters are close to the corresponding true values. Given 100,000 samples, the proposed method with a batch-size of 64 requires 1,891 seconds for 100 Monte Carlo simulations, i.e., 18.91 seconds per run. The batch and multi-pass SGD estimation methods need more than 3,000 seconds for a single MC run (not shown); the single-pass SGD algorithm has a speedup factor of 158.6.

(a) $R$, $Q$ and $\overline{P}$ Estimates for Case 2
Subgroup index	$R_{11}$		RMSE	$R_{22}$		RMSE	$Q_{11}$		RMSE	$Q_{22}$		RMSE	$Q_{33}$		RMSE
	Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean
$1^{\mathit{st}}$	0.25	0.23	0.34	0.25	0.24	0.04	0.25	0.24	0.05	0.25	0.23	0.05	0.36	0.35	0.06
$2^{\mathit{nd}}$	0.56	0.52		0.25	0.25		0.64	0.62		0.36	0.31		0.49	0.44
$3^{\mathit{rd}}$	0.64	0.85		0.49	0.50		0.49	0.50		0.56	0.52		0.64	0.71
$4^{\mathit{th}}$	0.42	0.44		0.16	0.16		0.25	0.26		0.16	0.16		0.25	0.25
$5^{\mathit{th}}$	0.36	0.30		0.04	0.04		0.49	0.48		0.04	0.04		0.09	0.08
Subgroup index	${\overline{P}}_{11}$		RMSE	${\overline{P}}_{22}$		RMSE	${\overline{P}}_{33}$		RMSE	${\overline{P}}_{44}$		RMSE	${\overline{P}}_{55}$		RMSE
	Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean
$1^{\mathit{st}}$	19.46	18.84	4.12	0.33	0.31	0.05	306.26	297.69	73.27	0.23	0.22	0.05	4.02	3.86	0.71
$2^{\mathit{nd}}$	43.76	42.11		0.43	0.40		766.59	744.98		0.34	0.30		5.45	4.99
$3^{\mathit{rd}}$	37.81	40.15		0.66	0.67		600.91	621.04		0.52	0.49		7.33	8.00
$4^{\mathit{th}}$	18.49	18.96		0.22	0.22		303.75	312.24		0.15	0.15		2.78	2.78
$5^{\mathit{th}}$	29.31	28.55		0.07	0.07		574.37	562.90		0.04	0.04		0.97	0.93
(b) $W$ Estimates for Case 2
Subgroup index	$W_{11}$		RMSE	$W_{21}$		RMSE	$W_{31}$		RMSE	$W_{41}$		RMSE	$W_{51}$		RMSE
	Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean
$1^{\mathit{st}}$	0.94	0.96	0.02	$2.78\cdot {10}^{-3}$	$-3.56\cdot {10}^{-3}$	0.01	−2.80	−2.75	0.04	$-2.22\cdot {10}^{-5}$	$6.50\cdot {10}^{-3}$	0.01	0.04	0.04	0.01
$2^{\mathit{nd}}$	0.96	0.98		$3.22\cdot {10}^{-3}$	$-4.32\cdot {10}^{-3}$		−2.91	−2.88		$-8.76\cdot {10}^{-4}$	$8.37\cdot {10}^{-3}$		0.03	0.01
$3^{\mathit{rd}}$	0.94	0.95		$2.84\cdot {10}^{-3}$	$-3.84\cdot {10}^{-3}$		−2.79	−2.83		$-1.19\cdot {10}^{-4}$	$8.79\cdot {10}^{-3}$		0.04	0.03
$4^{\mathit{th}}$	0.94	0.95		$3.84\cdot {10}^{-3}$	$2.08\cdot {10}^{-4}$		−2.80	−2.84		$-6.80\cdot {10}^{-4}$	$3.89\cdot {10}^{-4}$		0.03	0.02
$5^{\mathit{th}}$	0.98	1.00		$3.50\cdot {10}^{-3}$	$-1.33\cdot {10}^{-4}$		−3.03	−3.00		$-1.09\cdot {10}^{-3}$	$2.10\cdot {10}^{-4}$		0.01	0.01
Subgroup index	$W_{12}$		RMSE	$W_{22}$		RMSE	$W_{32}$		RMSE	$W_{42}$		RMSE	$W_{52}$		RMSE
	Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean		Truth	Mean
$1^{\mathit{st}}$	0.94	0.94	0.08	0.38	0.39	0.02	−1.63	−1.65	0.25	0.23	0.23	0.02	−0.94	−0.93	0.05
$2^{\mathit{nd}}$	1.02	1.00		0.40	0.42		−1.65	−1.70		0.27	0.25		−1.02	−0.98
$3^{\mathit{rd}}$	0.86	0.96		0.36	0.37		−1.56	−1.68		0.26	0.25		−0.86	−0.93
$4^{\mathit{th}}$	0.99	0.98		0.39	0.39		−1.57	−1.69		0.23	0.23		−0.98	−0.99
$5^{\mathit{th}}$	1.19	1.06		0.44	0.46		−1.26	−1.77		0.20	0.22		−1.17	−1.11

Table 2.

Single-pass SGD estimation for Case 2 (100 MC Runs, 100,000 samples; M = 5; RMSProp; Batch size = 64).

Figure 2 shows the trajectories of the estimated $Q$ and $R$ with a signal smoothing with a smoothing weight of 0.7. For this example, it is known that accurate estimation of $R_{11}$ is hard as shown in Figure 2d. The reason is that $R_{11}$ is dominated by the state uncertainty, i.e., the measurement noise is “buried” in a much larger innovation [4]. In spite of the difficulty in estimating $R_{11}$, the filter is consistent as measured by NIS as shown in Figure 2f.

Figure 2.

Trajectories of $Q$ and $R$ estimates with a signal smoothing at smoothing weight = 0.7 for Case 2.

3.3 Case 3: Bearings-only tracking problem

In many practical situations, it is generally hard to get a closed-form solution for state estimation because the noise covariances are often unknown and the dynamics are nonlinear. Arasaratnam et al. [29] proposed a nonlinear filter using bearings-only measurements for estimating the position and velocity of a target in a high-dimensional state. This method is based on the measurements from a passive sensor that measures only the direction of arrival of a signal emitted by the target [2]. This so-called bearings-only tracking problem arises in a variety of practical applications, such as air traffic control, underwater sonar tracking and aircraft surveillance [2, 30, 31].

In this example, we consider a two-dimensional bearings-only tracking problem of a nearly-constant velocity target from a single moving observer used in [32]. The dynamics of the target (relative to the observer) are described by

$$x\left(k+1\right)=\mathit{Fx}\left(k\right)+\mathrm{\Gamma }v\left(k\right)-U\left(k\right)$$E42

$$z\left(k\right)=h\left(x\left(k\right)\right)+w\left(k\right)$$E43

Formally, if the state vector of the target is $x^t\left(k\right)={\left[{\zeta }^t,{\eta }^t,{\dot{\zeta }}^t,{\dot{\eta }}^t\right]}^{\text{'}}$, and the state vector of the observer is $x^o\left(k\right)={\left[{\zeta }^o,{\eta }^o,{\dot{\zeta }}^o,{\dot{\eta }}^o\right]}^{\text{'}}$ for position and velocity along the $\zeta$ and $\eta$ axes, $x\left(k\right)=x^t\left(k\right)-x^o\left(k\right)$ represents the relative state vector of the target with respect to the observer and the input vector $U\left(k\right)=x^o\left(k\right)-{\mathit{Fx}}^o\left(k-1\right)$; $w\left(k\right)$ is a zero-mean white Gaussian noise with variance ${\sigma }_{\theta }^2$. The nonlinear measurement involves the bearing of the target from the observer’s platform, given by $h\left(x\left(k\right)\right)={tan}^{-1}\left(\zeta /\eta \right)$. Here, $\mathrm{\Gamma }$ is the identity matrix with ones on the diagonal and zeros elsewhere.

The system matrices for this problem are given by

$$F=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],\mathrm{\Gamma }=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],Q=\left[\begin{array}{cccc}{T}^3/3& 0& T^2/2& 0\\ 0& T^3/3& 0& T^2/2\\ T^2/2& 0& T& 0\\ 0& T^2/2& 0& T\end{array}\right]\tilde{q}\left(k\right)$$E44

where the sampling interval, $T$ is 1 second. The zero-mean white process noise intensity $\tilde{q}\left(k\right)$ is ${\tilde{q}}_0=9\cdot {10}^{-12}$${\mathit{km}}^2/s^3$, except for the interval where it starts to increase rapidly to 1.5 $\cdot {\tilde{q}}_0$ around the sample index $k$ = 480 and then decreases again rapidly around $k$ = 960 as below:

$$\tilde{q}\left(k\right)=\left\{\begin{array}{c}{\tilde{q}}_0+0.25{\tilde{q}}_0\left(1+tanh\left(0.015\left(k-480\right)\right)\right),k\le 720\\ {\tilde{q}}_0+0.25{\tilde{q}}_0\left(1+tanh\left(0.015\left(960-k\right)\right)\right),\text{otherwise}\end{array}\right.$$E45

The linearized measurement matrix, $H\left(k\right)$, is the Jacobian of the measurement function given by

$$H_k=\frac{\partial h\left(x\left(k\right)\right)}{\partial x\left(k\right)}=\left[\begin{array}{cccc}\frac{\eta \left(k\right)}{{\zeta }^2\left(k\right)+{\eta }^2\left(k\right)}& \frac{-\zeta \left(k\right)}{{\zeta }^2\left(k\right)+{\eta }^2\left(k\right)}& 0& 0\end{array}\right]$$E46

A total of 1920 measurement samples were generated for this scenario. The observer moves straight with a speed of 5 $\mathit{\text{knots}}$, except for 480 seconds (between $k$ = 480 and $k$ = 960), where it turns with ${2.4}^{\circ }/s$ as shown in Figure 3 (these times are marked by cross sign).

Figure 3.

Observer and target trajectory (100 MC runs).

For a fair comparison of the estimation algorithms, we initialized all filters with the same mean and covariance using the prior knowledge of the initial target range and the initial bearing measurement [33, 34]. Here, the initial target range and the initial bearing measurement are generated as $\overline{r}\sim N\left(r,{\sigma }_r^2\right)$ and ${\overline{\theta }}_0\sim N\left(\theta ,{\sigma }_{\theta }^2\right)$, respectively, where $r$ is the true initial target range and $\theta$ is the true initial bearing measurement. The initial target speed is initialized as $\overline{s}\sim N\left(s,{\sigma }_s^2\right)$, where $s$ is the true initial target speed. Let ${\sigma }_{<\cdot >}$ denotes the standard deviation of the parameter. Assuming that the target is moving towards the observer, the initial course estimate can be obtained as $\overline{c}={\overline{\theta }}_0+\pi$. The initial state vector and the initial covariance are given by

$${\widehat{x}}_0=\left[\begin{array}{c}\widehat{\zeta }\\ \widehat{\eta }\\ \widehat{\dot{\zeta }}\\ \widehat{\dot{\eta }}\end{array}\right]=\left[\begin{array}{c}\overline{r}cos\left({\overline{\theta }}_0\right)\\ \overline{r}sin\left({\overline{\theta }}_0\right)\\ \overline{s}sin\left(\overline{c}\right)-{\dot{\zeta }}_0^o\\ \overline{s}cos\left(\overline{c}\right)-{\dot{\eta }}_0^o\end{array}\right];P_0=\left[\begin{array}{cccc}{P}_{\mathit{\zeta \zeta }}& P_{\mathit{\zeta \eta }}& 0& 0\\ P_{\mathit{\eta \zeta }}& P_{\mathit{\eta \eta }}& 0& 0\\ 0& 0& P_{\dot{\zeta }\dot{\zeta }}& P_{\dot{\zeta }\dot{\eta }}\\ 0& 0& P_{\dot{\eta }\dot{\zeta }}& P_{\dot{\eta }\dot{\eta }}\end{array}\right]$$E47

where

$$P_{\mathit{\zeta \zeta }}={\overline{r}}^2{\sigma }_{\theta }^2{cos}^2\left({\overline{\theta }}_0\right)+{\sigma }_r^2{sin}^2\left({\overline{\theta }}_0\right);P_{\mathit{\eta \eta }}={\overline{r}}^2{\sigma }_{\theta }^2{sin}^2\left({\overline{\theta }}_0\right)+{\sigma }_r^2{cos}^2\left({\overline{\theta }}_0\right)$$E48

$$P_{\mathit{\zeta \eta }}=P_{\mathit{\eta \zeta }}=\left({\sigma }_r^2-{\overline{r}}^2{\sigma }_{\theta }^2\right)sin\left({\overline{\theta }}_0\right)cos\left({\overline{\theta }}_0\right);P_{\dot{\zeta }\dot{\zeta }}={\overline{s}}^2{\sigma }_c^2{cos}^2\left(\overline{c}\right)+{\sigma }_s^2{sin}^2\left(\overline{c}\right)$$E49

$$P_{\dot{\eta }\dot{\eta }}={\overline{s}}^2{\sigma }_c^2{sin}^2\left(\overline{c}\right)+{\sigma }_s^2{cos}^2\left(\overline{c}\right);P_{\dot{\zeta }\dot{\eta }}=P_{\dot{\eta }\dot{\zeta }}=\left({\sigma }_s^2-{\overline{s}}^2{\sigma }_c^2\right)sin\left(\overline{c}\right)cos\left(\overline{c}\right)$$E50

where $r$ and $s$ are 5 km and 4 $\mathit{\text{knots}}$, respectively, and the target course is $-{140}^{\circ }$. Here, ${\sigma }_r$ is 2 $\mathit{km}$, ${\sigma }_{\theta }$ is 1.5 ${}^{\circ }$, ${\sigma }_s$ is 2 $\mathit{\text{knots}}$ and ${\sigma }_c=\pi /\sqrt{12}$ for this problem.

Figure 4 shows a comparison of algorithms for the bearings-only tracking problem. The cubature Kalman filter (CKF) uses a third-degree spherical-radial cubature rule that provides the set of cubature points scaling linearly with the state-vector dimension [29]. The cubature Kalman filter and our single-pass SGD extended KF (EKF) method can track the target well, but our proposed method shows better computational efficiency compared to CKF by a factor of 2.5 (not shown). Root mean square error (RMSE) in position and velocity over 100 Monte Carlo runs are shown in Figure 4c and Figure 4d. During the whole maneuver, the RMSE of the proposed single-pass SGD-EKF algorithm was slightly lower than that with the CKF method.

Figure 4.

Comparison of estimation algorithms for the bearings-only tracking problem (100 MC runs).

4. Conclusions

In this chapter, we derived a single-pass sequential mini-batch SGD algorithm for estimating the noise covariances in an adaptive Kalman filter. We demonstrated the utility of the method using diverse examples involving a detectable (but not completely observable) system, a non-stationary system, and a nonlinear bearings-only tracking problem. The evaluation showed that the proposed method has acceptable state estimation root mean square error (RMSE) and exhibits filter consistency as measured by the normalized innovation squared (NIS) criterion.

Acknowledgments

This work was supported in part by the U.S. Office of Naval Research (ONR), in part by the U.S. Naval Research Laboratory (NRL) under Grant N00014-18-1-1238, N00014-21-1-2187 and Grant N00173-16-1-G905.

Abbreviations

CKF	Cubature Kalman filter
EKF	Extended Kalman filter
INS	Inertial navigation system
KF	Kalman filter
MMSE	Minimum mean square error
NIS	Normalized innovation squared
RMSE	Root mean square error
RMSProp	Root mean square propagation
SGD	Stochastic gradient descent