Real-Time Numerical Differentiation of Sampled Data Using Adaptive Input and State Estimation

The dual operations of integration and differentiation provide the foundation for much of mathematics. From an analytical point of view, differentiation is simpler than integration; consider the relative difficulty of differentiating and integrating $\log(1+\sin^{2}x^{3}).$ In numerical analysis, integration techniques have been extensively developed in Davis \BBA Rabinowitz (\APACyear1984), whereas differentiation techniques have been developed more sporadically in Savitzky \BBA Golay (\APACyear1964); Cullum (\APACyear1971), Hamming (\APACyear1973, pp. 565, 566).

In practice, numerical integration and differentiation techniques are applied to sequences of measurements, that is, discrete-time signals composed of sampled data. Although, strictly speaking, integration and differentiation are not defined for discrete-time signals, the goal is to compute a discrete-time “integral” or “derivative” that approximates the true integral or derivative of the pre-sampled, analog signal.

In addition to sampling, numerical integration and differentiation methods must address the effect of sensor noise. For numerical integration, constant noise, that is, bias, leads to a spurious ramp, while stochastic noise leads to random-walk divergence. Mitigation of these effects is of extreme importance in applications such as inertial navigation as shown in Farrell (\APACyear2008); Grewel \BOthers. (\APACyear2020).

Compared to numerical integration, the effect of noise on numerical differentiation is far more severe. This situation is due to the fact that, whereas integration is a bounded operator on a complete inner-product space, differentiation is an unbounded operator on a dense subspace. Unboundedness implies a lack of continuity, which is manifested as high sensitivity to sensor noise. Consequently, numerical differentiation typically involves assumptions on the smoothness of the signal and spectrum of the noise as considered in Ahn \BOthers. (\APACyear2006); Jauberteau \BBA Jauberteau (\APACyear2009); Stickel (\APACyear2010); Listmann \BBA Zhao (\APACyear2013); Knowles \BBA Renka (\APACyear2014); Haimovich \BOthers. (\APACyear2022).

Numerical differentiation algorithms are crucial elements of many digital control algorithms. For example, PID control requires numerical differentiation to implement derivative action as presented in Vilanova \BBA Visioli (\APACyear2012); Astrom \BBA Hagglund (\APACyear2006). Flatness-based control is based on a finite number of derivatives as shown in Nieuwstadt \BOthers. (\APACyear1998); Mboup \BOthers. (\APACyear2009).

In practice, analog or digital filters are used to suppress the effect of sensor noise, thereby allowing the use of differencing formulae in the form of inverted “V” filters, which have the required gain and phase lead at low frequencies and roll off at high frequencies. These techniques assume that the characteristics of the signal and noise are known, thereby allowing the user to tweak the filter parameters. When the signal and noise have unknown and possibly changing characteristics, filter tuning is not possible and thus the problem becomes significantly more challenging. The recent work in Van Breugel \BOthers. (\APACyear2020) articulates these challenges and proposes a Pareto-tradeoff technique for addressing the absence of prior information. Additional techniques include high-gain observer methods, in which the observer approximates the dynamics of a differentiator as shown in Dabroom \BBA Khalil (\APACyear1999). An additional approach to this problem is to apply sliding-mode algorithms as shown in Levant (\APACyear2003); Reichhartinger \BBA Spurgeon (\APACyear2018); López-Caamal \BBA Moreno (\APACyear2019); Mojallizadeh \BOthers. (\APACyear2021); Alwi \BBA Edwards (\APACyear2013).

In feedback control applications, which require real-time implementation, phase shift and latency in numerical differentiation can lead to performance degradation and possibly instability. Phase shift arises from filtering, whereas latency arises from noncausal numerical differentiation, that is, numerical differentiation algorithms that require future data. For real-time applications, a noncausal differentiation algorithm can be implemented causally by delaying the computation until the required data are available. A feedback controller requires an estimate of the current derivative, however, and thus the delayed estimate provided by a noncausal differentiation algorithm may not be sufficiently accurate.

A popular approach to numerical differentiation is to apply state estimation with integrator dynamics, where the state estimate includes an estimate of the derivative of the measurement as shown in Kalata (\APACyear1984); Bogler (\APACyear1987). This approach has been widely used for target and vehicle tracking in Jia \BOthers. (\APACyear2008); H. Khaloozadeh (\APACyear2009); Lee \BBA Tahk (\APACyear1999); Rana \BOthers. (\APACyear2020). As an extension of this approach, the present paper applies input estimation to numerical differentiation, where the goal is to estimate the input as well as the state, as in Gillijns \BBA De Moor (\APACyear2007); Orjuela \BOthers. (\APACyear2009); Fang \BOthers. (\APACyear2011); Yong \BOthers. (\APACyear2016); Hsieh (\APACyear2017); Naderi \BBA Khorasani (\APACyear2019); Alenezi \BOthers. (\APACyear2021).

The present paper is motivated by the situation where minimal prior information about the signal and noise is available. This case arises when the spectrum of the signal changes slowly or abruptly in an unknown way, and when the noise characteristics vary due to changes in the environment, such as weather. With this motivation, adaptive input estimation (AIE) was applied to target tracking in Ansari \BBA Bernstein (\APACyear2019), where it was used to estimate vehicle acceleration using position data. In particular, the approach of Ansari \BBA Bernstein (\APACyear2019) is based on retrospective cost input estimation (RCIE), where recursive least squares (RLS) is used to update the coefficients of the estimation subsystem. The error metric used for adaptation is the residual (innovations) of the state estimation algorithm, that is, the Kalman filter. This technique requires specification of the covariances of the process noise, input-estimation error, and sensor noise.

The present paper extends the approach of Ansari \BBA Bernstein (\APACyear2019) by replacing the Kalman filter with an adaptive Kalman filter in which the input-estimation error and sensor-noise covariances are updated online. Adaptive extensions of the Kalman filter to the case where the variance of the disturbance is unknown are considered in Yaesh \BBA Shaked (\APACyear2008); Shi \BOthers. (\APACyear2009); Moghe \BOthers. (\APACyear2019); Zhang \BOthers. (\APACyear2020). Adaptive Kalman filters based on the residual for integrating INS/GPS systems are discussed in Mohamed \BBA Schwarz (\APACyear1999); Hide \BOthers. (\APACyear2003); Almagbile \BOthers. (\APACyear2010). Several approaches to adaptive filtering, such as bayesian, maximum likelihood, correlation, and covariance matching, are studied in Mehra (\APACyear1972). A related algorithm involving a covariance constraint is developed in Mook \BBA Junkins (\APACyear1988).

The adaptive Kalman filter used in the present paper as part of AIE/ASE is based on a search over the range of input-estimation error covariance. This technique has proven to be easy to implement and effective in the presence of unknown signal and noise characteristics. The main contribution of the present paper is a numerical investigation of the accuracy of AIE combined with the proposed adaptive state estimation (ASE) in the presence of noise with unknown properties. The accuracy of AIE/ASE is compared to the backward-difference differentiation, Savitzky-Golay differentiation (Savitzky \BBA Golay (\APACyear1964); Schafer (\APACyear2011); Staggs (\APACyear2005); Mboup \BOthers. (\APACyear2009)), and numerical differentiation based on high-gain observers (Dabroom \BBA Khalil (\APACyear1999)).

The present paper represents a substantial extension of preliminary results presented in Verma \BOthers. (\APACyear2022). In particular, the algorithms presented in the present paper extend the adaptive estimation component of the approach of Verma \BOthers. (\APACyear2022) in Section 5, and the accuracy of these algorithms is more extensively evaluated and compared to prior methods in Section 6.

The contents of the paper are as follows. Section 2 presents three baseline numerical differentiation algorithms. Section 3 discusses the delay in the availability of the estimated derivative due to the computation time and non-causality. This section also defines an error metric for comparing the accuracy of the algorithms considered in this paper. Section 4 describes the adaptive input estimation algorithm. Section 5 provides the paper’s main contribution, namely, adaptive input estimation with adaptive state estimation. Section 6 applies three variations of AIE using harmonic signals with various noise levels. Finally, Section 7 applies the variations of AIE to simulated vehicle position data generated by CarSim.

This section presents three algorithms for numerically differentiating sampled data. These algorithms provide a baseline for evaluating the accuracy of the adaptive input and state estimation algorithms described in Section 5.

Several noteworthy differences exist among BD, SG, and HGO. First, BD differentiation operates on adjacent pairs of data points, whereas SG differentiation operates on a moving window of data points. Consequently, SG differentiation is potentially more accurate than BD differentiation. Because of the time $T_{\rm c}$ needed for computation, all numerical differentiation entails an unavoidable delay in the availability of the estimated derivative. In addition, BD differentiation and HGO differentiation do not require future data to estimate the derivative, thus they are causal algorithms. On the other hand, SG differentiation requires future data, and thus it is noncausal. For causal differentiation, the delay is $\delta=1$ step due to computation; for noncausal differentiation, $\delta\geq 2$. Assuming $T_{\rm c}\leq T_{\rm s},$ Figure 1 illustrates the case $\delta=1$. Note that $\delta=1$ for BD and HGO, whereas $\delta=\ell+1$ for SG with window size $2\ell+1.$

To quantify the accuracy of each numerical differentiation algorithm, for all $k\geq\delta$, we define the relative root-mean-square error (RMSE) of the estimate of the $q$th derivative as

Note that numerator of (22) accounts for the effect of the delay $\delta.$ For real-time implementation, the relevant error metric depends on the difference between the true current derivative and the currently available estimate of the past derivative, as can be seen in the numerator of (22). When the derivative estimates are exact, (22) determines an RMSE value that can be viewed as the delay floor for the $q$th derivative, that is, the error due solely to the fact that a noncausal differentiation algorithm must be implemented with a suitable delay. Note that the delay floor depends on $\delta$ and is typically positive.

The true values of $y^{(q)}_{k}$ are the sampled values of $y^{(q)}$ in the absence of sensor noise. Of course, the true values of $y^{(q)}_{k}$ are unknown in practice and thus cannot be used as an online error criterion. However, these values are used in (22), which is computable in simulation for comparing the accuracy of the numerical differentiation algorithms.

To compare the various baseline algorithms presented in Section 2, we consider numerical differentiation of the continuous-time signal $y(t)=\sin(20t)$, where $t$ is time in seconds. The signal $y(t)$ is sampled with sample time $T_{\rm s}=0.01$ sec. The measurements are assumed to be corrupted by noise, and thus the noisy sampled signal is given by $y_{k}=\sin(0.2k)+Dv_{k}$, where $v_{k}$ is standard (zero-mean, unit-variance, Gaussian) white noise. The value of $D$ is chosen to set the desired signal-to-noise ratio (SNR).

For single differentiation with SG, let $\ell=2$ and $p_{d}=3$. For single differentiation with HGO, let HGO/1 denote HGO with $r=2$, $\alpha_{1}=2$, $\alpha_{2}=1$, and $\varepsilon=0.2$, and let HGO/2 denote HGO/1 with $\varepsilon=0.2$ replaced by $\varepsilon=0.7$. Figure 2 shows the relative RMSE $\rho_{k_{\rm f}}^{(1)}$ of the estimate of the first derivative for SNR ranging from $20$ dB to $60$ dB, where $k_{\rm f}=2000$ steps.

For double differentiation with SG, let $\ell=2$ and $p_{d}=2$. For double differentiation with HGO, let HGO/1 denote HGO with $r=3$, $\alpha_{1}=8$, $\alpha_{2}=24$, $\alpha_{3}=32$, and $\varepsilon=1$, and let HGO/2 denote HGO/1 with $\varepsilon=1$ replaced by $\varepsilon=2$. Figure 3 shows the relative RMSE $\rho_{k_{\rm f}}^{(2)}$ of the estimate of the second derivative for SNR ranging from $40$ dB to $60$ dB, where $k_{\rm f}=2000$ steps. The delay floors in Figures 2 and 3 are determined by computing the RMSE between the true value $y_{k}^{(q)}$ and the $\delta$-step-delayed true value $y_{k-\delta}^{(q)}$.

The comparison between HGO/1 and HGO/2 in Figure 2 and 3 shows that the performance of HGO differentiation depends on the noise level, and thus tuning is needed to achieve the best possible performance. When the noise level is unknown, however, this tuning is not possible. Hence, we now consider a differentiation technique that adapts to the actual noise characteristics.

This section discusses adaptive input estimation (AIE), which is a specialization of retrospective cost input estimation (RCIE) derived in Ansari \BBA Bernstein (\APACyear2019). This section explains how AIE specializes RCIE to the problem of causal numerical differentiation.

Consider the linear discrete-time system

where $k\geq 0$ is the step, $x_{k}\in\mathbb{R}^{n}$ is the state, $d_{k}\stackrel{{\scriptstyle\triangle}}{{=}}d(kT_{\rm s})\in\mathbb{R}$, $v_{k}\in\mathbb{R}$ is standard white noise, and $D_{2}v_{k}\in\mathbb{R}$ is the sensor noise. The matrices $A\in\mathbb{R}^{n\times n}$, $B\in\mathbb{R}^{n\times 1}$, $C\in\mathbb{R}^{1\times n}$, and $D_{2}\in\mathbb{R}$ are assumed to be known. Define the sensor-noise covariance $V_{2}\stackrel{{\scriptstyle\triangle}}{{=}}D_{2}D_{2}^{\mathsf{T}}$. The goal of AIE is to estimate $d_{k}$ and $x_{k}$.

AIE consists of three subsystems, namely, the Kalman filter forecast subsystem, the input-estimation subsystem, and the Kalman filter data-assimilation subsystem. First, consider the Kalman filter forecast step

where $\hat{d}_{k}$ is the estimate of $d_{k}$, $x_{\rm da,k}\in\mathbb{R}^{n}$ is the data-assimilation state, $x_{{\rm fc},k}\in\mathbb{R}^{n}$ is the forecast state, $z_{k}\in\mathbb{R}$ is the residual, and $x_{{\rm fc},0}=0$.

Next, to obtain $\hat{d}_{k}$, the input-estimation subsystem of order $n_{\rm e}$ is given by the exactly proper dynamics

where $P_{i,k}\in{\mathbb{R}}$ and $Q_{i,k}\in{\mathbb{R}}$. AIE minimizes $z_{k}$ by using recursive least squares (RLS) to update $P_{i,k}$ and $Q_{i,k}$ as shown below. The subsystem (28) can be reformulated as

where the regressor matrix $\Phi_{k}$ is defined by

the coefficient vector $\theta_{k}$ is defined by

and $l_{\theta}\stackrel{{\scriptstyle\triangle}}{{=}}2n_{{\rm e}}+1$. In terms of the backward-shift operator ${\bf q}^{-1}$, (28) can be written as

where

To update the coefficient vector $\theta_{k},$ we define the filtered signals

where, for all $k\geq 0$,

and $\overline{A}_{k}\stackrel{{\scriptstyle\triangle}}{{=}}A(I+K_{{\rm da},k}C)$, where $K_{{\rm da},k}$ is the Kalman filter gain given by (47) below. Furthermore, define the retrospective variable

where the coefficient vector $\hat{\theta}\in{\mathbb{R}}^{l_{\theta}}$ denotes a variable for optimization, and define the retrospective cost function

where $R_{z}\in(0,\infty)$, $R_{d}\in(0,\infty),$ and $R_{\theta}\in{\mathbb{R}}^{l_{\theta}\times l_{\theta}}$ is positive definite. Then, for all $k\geq 0$, the unique global minimizer $\theta_{k+1}$ of (43) is given by the RLS update as shown in Islam \BBA Bernstein (\APACyear2019)

where

Using the updated coefficient vector given by (45), the estimated input at step $k+1$ is given by replacing $k$ by $k+1$ in (29). We choose $\theta_{0}=0,$ and thus $\hat{d}_{0}=0.$ Implementation of AIE requires that the user specify the orders $n_{\rm e}$ and $n_{\rm f},$ as well as the weightings $R_{z},$ $R_{d},$ and $R_{\theta}.$ These parameters are specified for each example in the paper.

In practice, $V_{1,k}$ and $V_{2,k}$ may be unknown in (49) and (47). To address this problem, three versions of AIE are presented. In each version, $V_{1,k}$ and $V_{2,k}$ may or may not be adapted. These versions are summarized in Table 1.

To adapt ${V}_{1,k}$ and ${V}_{2,k}$, at each step $k$ we define the computable performance metric

where $\widehat{S}_{k}$ is the sample variance of $z_{k}$ over $[0,k]$ given by

and ${S}_{k}$ is the variance of the residual $z_{k}$ given by the Kalman filter, that is,

Note that (51) is the difference between the theoretical and empirical variances of $z_{k}$, which provides an indirect measure of the accuracy of $V_{1}$ and $V_{2}.$