Quantized State Estimation for Linear Dynamical Systems

Abstract

This paper investigates state estimation methods for dynamical systems when model evaluations are performed on resource-constrained embedded systems with finite precision compute elements. Minimum mean square estimation algorithms are reformulated to incorporate finite-precision numerical errors in states, inputs, and measurements. Quantized versions of least squares batch estimation, sequential Kalman, and square-root filtering algorithms are proposed for fixed-point implementations. Numerical simulations are used to demonstrate performance improvements over standard filter formulations. Steady-state covariance analysis is employed to capture the performance trade-offs with numerical precision, providing insights into the best possible filter accuracy achievable for a given numerical representation. A low-latency fixed-point acceleration state estimation architecture for optomechanical sensing applications is realized on Field Programmable Gate Array System on Chip (FPGA-SoC) hardware. The hardware implementation results of the estimator are compared with double-precision MATLAB implementation, and the performance metrics are reported. Simulations and the experimental results underscore the significance of modeling quantization errors into state estimation pipelines for fixed-point embedded implementations.

1. Introduction

Kalman filters were first introduced into onboard guidance and navigation systems during NASA’s Apollo Project in the 1960s [1]. However, digital simulations of the filter for onboard trajectory estimation using finite-precision arithmetic uncovered numerical stability problems. The filter simulations on the IBM 704 computer with 36-bit floating-point arithmetic were determined to be numerically unreliable. On the Apollo flight computer constrained with only 15-bit fixed-point arithmetic operations, onboard implementation of the filter was infeasible [2]. Since then, efforts to improve filter accuracy and stability under finite-precision hardware constraints have been a focus for practical realization of navigation filters. Square-root filtering was invented as a solution for execution on the Apollo guidance computer to solve the historic circumlunar navigation problem [3]. Square-root algorithms for state estimation were well-established by the 1970s [4,5] to address numerical stability issues due to quantization effects. With technological advancements in microprocessors and hardware accelerators, research on navigation filters continued to focus on high-speed, hard real-time calculations [6]. Recent research on quantized filtering algorithms has been enabled by optimized software packages, dedicated hardware for parallel computing, and purpose-built solutions [7,8,9].

The quantization problem in state estimation and optimal control has been widely studied in the literature [10,11,12]. Quantization is commonly utilized in wireless sensor networks (WSNs) with low-quality sensors with limited computation and communication capabilities, and where transmission bandwidth is severely constrained. A class of Kalman filters where the difference between measurements and its predictions (i.e., innovations) are quantized has been developed for decentralized state estimation [12,13]. In these quantized Kalman filters, measurement predictions are distributed from a fusion center to a sensor node, where innovations are quantized and sent back to the center for filter update. The sign of innovation Kalman filter (SOI-KF) [13] operates by quantizing innovations to only $1$-bit, which may give rise to large estimation errors. Filters are also proposed for multi-bit transmission, such as the multiple-level quantized innovation Kalman filter (MLQ-KF) [12]. It is shown that with multi-bit quantization, the performance of the quantized filter can be recovered to nearly match the mean squared errors of standard the Kalman filter [12]. However, to have such performance, uniformly distributed sensor nodes collectively convey their quantized information to a fusion center for processing into a combined target state estimate. Having just one isolated sensor significantly undermines estimator tracking performance, and a standard Kalman filter operating on high-resolution measurements from even one sensor is relatively more accurate. Work has also been reported in utilizing quantized measurements in the development and analysis of particle filters [14], unscented Kalman filters [15], and their fixed-point implementations [14,16]. These implementations are purpose-built for accelerating filter algorithms for specific applications and are not easily scalable for generic state estimation problems. To a great extent, the usual practice for high-speed onboard realization of filtering algorithms vastly depends upon the hardware resources and also on the choice of an optimal state estimator. This paper aims to bridge this gap by reformulating minimum mean square estimation algorithms to explicitly account for finite-precision numerical errors in states, inputs, and measurements. By integrating quantization noise models into the filter structure, the proposed approach enables the design of quantized filters that are better suited for fixed-point implementations on embedded systems.

This research focuses on optimal quantized filtering methods with an application focus on optomechanical acceleration sensors for inertial navigation. Optomechanical accelerometers rely on the coupling between the mechanical displacement of a test mass and light captured using an optical detection system [17]. Such a sensor requires optical data processing on high-speed hardware modules such as Field Programmable Gate Arrays (FPGAs) [18] for estimation of acceleration forces from test-mass displacement measurements. These precision force measuring units are deployed in geodesic applications, including the Gravity Recovery and Climate Experiment (GRACE) mission [19], the Laser Interferometer Space Antenna (LISA) [20], and the Laser Interferometer Gravitational-Wave Observatory (LIGO) [21].

For deployment on a spacecraft or resource-constrained hardware (e.g., FPGA [22,23]), dynamic range and sensor resolution limitations shall restrict the ability of standard filtering algorithms to precisely estimate states, especially when the dynamical system exhibits large variations in state variables. This necessitates the adoption of accurate error models and the use of novel filtering techniques to achieve sufficient estimation accuracy of desired state variables from the sensor measurements [10,12,24]. Furthermore, in scenarios where transmitting high-resolution measurements or performing double-precision operations is impossible or memory-intensive, quantization errors become significant. With increased performance requirements, the noise from the quantization effects has become an important aspect to analyze in efforts to maximize signal-to-noise ratio [12,13,25,26]. For the state estimation problem of dynamical stochastic processes, finite-precision implementation necessitates estimation to be based on quantized parameters of state, input, and observations. This requires revisiting the state estimation algorithms to include quantization errors in the filter implementation. Although using a sufficiently large word length for real-valued state variables can minimize the effects of quantization, even with a large number of bits, the system cannot be completely detached from finite word-length effects. In some instances, the luxury of large dynamic range for storage and computations may not be practicable. Consequently, the errors due to quantization have to be modeled as process and output noises, which can be accounted for in the system design using classical state-space and estimator modeling schemes.

The key contributions of this work are briefly summarized as follows. Given a finite-precision representation for system variables, linear state estimation filters are proposed. They consist of a (1) minimum variance estimator (least squares), quantized forms of (2) a discrete-time Kalman filter (QDKF), and (3) a square-root Kalman filter (QSRKF). The filter algorithms are applied to estimate states of an optomechanical oscillator model. The model formulation and assumptions are based upon the efforts of Kelly et al. [27] and the bench top experiments from Hines et al. [28]. Numerical simulations of the estimators are reported to show the prominence of modeling quantization errors in state estimation filters. The results offer much improved estimator performances over standard implementations of the respective filters when system variables are quantized. Steady-state filter performance is analyzed to provide insights into the best possible filter accuracy achievable for a given numerical representation. A least squares-based dual oscillator model is implemented on the FPGA board for the state estimation of acceleration forces from simulated measurements. A reliable estimator performance is reported by comparing the fixed-point implementation on the FPGA with the floating-point software implementation as a reference. Note that, in this work, quantization noise is from rounding off of numerical data to a desired number of bits. From here on, quantization errors and round-off errors are used interchangeably and refer to the same idea. Moreover, bit overflows are assumed to be negligible due to the careful selection of dynamic ranges for internal variable representation.

The rest of the article is organized as follows. The sensor model is described in Section 2. The quantization effects are incorporated into the model and state estimation filters are proposed in Section 3. Respective filter algorithms are derived in Appendix A, Appendix B and Appendix C. Numerical simulations are reported in Section 4 to illustrate the performance of the proposed filter structures. In Section 5, an FPGA architecture is proposed for on-board implementation of a dual-oscillator filter for acceleration estimation problem. Implementation results are reported. Concluding remarks are drawn in Section 6.

2. Description of Sensor Dynamics

The quantized filter performances are investigated for state estimation of the dynamical system described in this section. The generalized filtering algorithms are developed in Appendix A, Appendix B and Appendix C.

2.1. Discrete-Time Sensor Model

The one degree-of-freedom (1-DOF) accelerometer sensor dynamics are modeled as a second order spring-mass-damper system [29], equivalent to a perturbed linear harmonic oscillator. This formulation describes a direct conversion accelerometer, where the displacement of the proof mass $x\left(t\right)$ is directly measured using precise laser wavelength as a length reference [30]. Accounting for drift-causing optical and thermomechanical noise sources, through a white noise process and a biasing term, the dynamics for the proof mass displacement in response to the single-axis forcing function $g\left(t\right)$ are formulated using the following equations developed by Kelly [27] et al.:

$$\begin{array}{cc}\hfill \ddot{x}+2\omega \zeta \dot{x}+{\omega }^{2}x& =g\left(t\right)+b\left(t\right)+n_v\left(t\right)\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill \dot{b}\left(t\right)& =n_u\left(t\right)\hfill \end{array}$$

(2)

where $\omega$ is the natural frequency of the oscillator, $\zeta$ is its damping factor, and the bias term $b\left(t\right)$ is modeled as a Wiener process. In discrete time, the increments of this Wiener process can be represented as an independent and identically distributed Gaussian random sequence. The terms $n_v\left(t\right)$ and $n_u\left(t\right)$ are uncorrelated, zero-mean Gaussian white-noise processes with spectral densities ${\sigma }_v^2$ and ${\sigma }_u^2$, respectively.

In continuous-time state space description, the 1-DOF accelerometer model is

$$\begin{array}{c}\dot{\mathbf{X}}\left(t\right)=\left[\begin{array}{ccc}0& 1& 0\\ -{\omega }^2& -2\omega \zeta & 1\\ 0& 0& 0\end{array}\right]\mathbf{X}\left(t\right)+\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]g\left(t\right)+\left[\begin{array}{c}0\\ n_v\left(t\right)\\ n_u\left(t\right)\end{array}\right]\end{array}$$

(3)

$$\begin{array}{c}y\left(t\right)=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\mathbf{X}\left(t\right)+\nu \left(t\right)\end{array}$$

(4)

where the states of the system are $\mathbf{X}\left(t\right)={\left[x\left(t\right)\dot{x}\left(t\right)b\left(t\right)\right]}^T$ representing displacement, velocity, and bias, in that order. The measurement model implicitly assumes that the position state, $x\left(t\right)$, is observable and that the sensor dynamics do not consider off-axis accelerations.

The linear system in Equation (3) can be discretized using a zero-order hold (ZOH) approximation, assuming that inputs and noise change only at discrete sampling intervals $\mathsf{\Delta }t$. In this discretization, $g_k$ is assumed constant over each sampling interval. However, it’s crucial to note that if $g_k$ varies more rapidly than can be observed within the interval $\Delta t$, the Nyquist theorem limits our ability to estimate $g_k$ accurately. This highlights the importance of selecting an appropriate sampling rate in relation to the system’s dynamics. The discretized state space model is

$$\begin{array}{c}{\mathbf{X}}_{k+1}=\mathbf{\Phi }(t_{k+1},t_k){\mathbf{X}}_k+\mathbf{\Gamma }\left(t_{k+1},t_k\right)g_k+{\mathbf{w}}_k\end{array}$$

(5)

where the transition from a state ${\mathbf{X}}_{k+1}$ to ${\mathbf{X}}_k$ for the linear time-invariant system is determined by a matrix exponential of the system matrix in Equation (3) (denoted as A):

$$\begin{array}{c}\mathbf{\Phi }(t_{k+1},t_k)=e^{A\Delta t}\end{array}$$

(6)

Also,

$$\begin{array}{c}\begin{array}{cc}\hfill \mathbf{\Psi }(t_{k+1},t_k)& ={\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }(t_{k+1},\tau )d\tau \hfill \\ \hfill \mathbf{\Gamma }(t_{k+1},t_k)& ={\left[\begin{array}{ccc}{\mathbf{\Psi }}_{12}(t_{k+1},t_k)& {\mathbf{\Psi }}_{22}(t_{k+1},t_k)& {\mathbf{\Psi }}_{32}(t_{k+1},t_k)\end{array}\right]}^T\hfill \end{array}\end{array}$$

(7)

The stochastic process noise term ${\mathbf{w}}_k$, additive to the discrete-time state evolution, is described as

$$\begin{array}{c}\begin{array}{cc}\hfill {\mathbf{w}}_k& ={\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }(t_{k+1},\tau ){\left[\begin{array}{ccc}0& n_v\left(\tau \right)& n_u\left(\tau \right)\end{array}\right]}^{T}d\tau \hfill \\ \hfill \mathbf{Q}& =\mathbb{E}\left[{\mathbf{w}}_k{\mathbf{w}}_k^T\right]\hfill \end{array}\end{array}$$

(8)

For the linear system described here, the state transition matrix $\mathbf{\Phi }$ and the covariance matrix $\mathbf{Q}$ associated with ${\mathbf{w}}_k$ can both be analytically derived [27]. $\mathbb{E}[·]$ denotes the expected value operator.

Regarding the observation model, the displacement measurements are acquired at a fixed rate from an accurate one-dimensional interferometric displacement sensor. Following the state-space representation in Equation (4), the discrete-time measurement model is given by

$$y_k=x_k+{\nu }_k$$

(9)

This measurement model embeds the analog readout noise ${\nu }_k$ in the measurements. This observation error is treated as zero-mean white noise with an error variance of ${\sigma }_m^2$.

However, in practice, a change in the sensor’s output does not always translate exactly to a change in the mechanical input. As a result, the one-to-one association between the measured and the physical displacements in Equation (27) may not always hold true. The deviation in the sensor’s sensitivity is modeled through a scale factor and is estimated through sensor calibration and compensated during device operation [31]. The process of scale factor calibration and its associated uncertainties are not addressed in this paper.

However, the scale factor error arising from calibration uncertainty can be incorporated into the readout noise model for state estimation. To this extent, departure from unity scaling is modeled through scale factor error ${ϵ}_s$ to account for the total readout error:

$$\begin{array}{c}\hfill y_k=(1+{ϵ}_{s,k})x_k+{\nu }_k\end{array}$$

(10)

Although not rigorously treated, this direction is briefly noted in Section 3.3.

2.2. Calibrated Sensor Model

Accelerometer bias accumulates over time, as indicated in Equation (2). The bias model is

$$b_{k+1}=b_k+{\int }_{t_k}^{t_{k+1}}{n}_u\left(\tau \right)d\tau$$

(11)

A reasonable calibration step must be implemented periodically to correct for the sensor bias to prevent its build-up effect on state estimation. A calibration sequence shall be performed by applying a known input to the sensor and observing the system response (as described in previous work [27]). This calibration step provides us with an estimate of the bias $\widehat{b}$ and its error statistics as follows:

$$\begin{array}{cc}\hfill & b_0=\widehat{b}+n_{b_0}\mathrm{and}{b}_k=b_0+{\int }_{t_0}^{t_k}{n}_u\left(\tau \right)d\tau \hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill & \mathbb{E}[b_k-\widehat{b}]=0\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill & \mathbb{E}\left[(b_k-\widehat{b}){(b_k-\widehat{b})}^T\right]={\sigma }_{b0}^2+(t_k-t_0){\sigma }_u^2\hfill \end{array}$$

(14)

where, at the time of calibration $t_0$, the unbiased bias estimate $b_0$ is corrupted by a white noise source $n_{b0}$ and has a variance of ${\sigma }_{b0}^2$. The bias estimate is assumed to not significantly degrade between periodic calibrations, but its variance grows as the bias evolves with a dependence upon the zero-mean Gaussian process $n_u\left(t\right)$ as shown in Equation (2).

Assuming that an independent calibration step has been performed to estimate the sensor bias, the model dynamics in position and velocity (Equation (5)), influenced by the instantaneous bias term $b_k$, can be written as follows:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & \left[\begin{array}{c}{x}_{k+1}\\ {\dot{x}}_{k+1}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{\Phi }}_{11}& {\mathbf{\Phi }}_{12}\\ {\mathbf{\Phi }}_{21}& {\mathbf{\Phi }}_{22}\end{array}\right]\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\end{array}\right]+\left[\begin{array}{c}{\mathbf{\Psi }}_{12}\\ {\mathbf{\Psi }}_{22}\end{array}\right]g_k+\left[\begin{array}{c}{\mathbf{\Phi }}_{13}\\ {\mathbf{\Phi }}_{23}\end{array}\right]\widehat{b}+{\tilde{\mathbf{w}}}_k\hfill \\ & {\tilde{\mathbf{w}}}_k=\left[\begin{array}{c}{\mathbf{\Phi }}_{13}\\ {\mathbf{\Phi }}_{23}\end{array}\right](b_k-\widehat{b})+{\int }_{t_k}^{t_{k+1}}\left[\begin{array}{c}{n}_v\left(\tau \right){\mathbf{\Phi }}_{12}(t_{k+1},\tau )\\ n_v\left(\tau \right){\mathbf{\Phi }}_{22}(t_{k+1},\tau )\end{array}\right]d\tau \hfill \end{array}\end{array}$$

(15)

It can be shown that, to integrate the uncertainty in the instantaneous bias value into the system dynamics, the redefined process noise covariance $\tilde{\mathbf{Q}}$ is derived as:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill \tilde{\mathbf{Q}}& =\left[\begin{array}{cc}{\mathbf{\Phi }}_{13}^2& {\mathbf{\Phi }}_{13}{\mathbf{\Phi }}_{23}\\ {\mathbf{\Phi }}_{23}{\mathbf{\Phi }}_{13}& {\mathbf{\Phi }}_{23}^2\end{array}\right]({\sigma }_{b0}^2+{\sigma }_u^2(t_k-t_0))+\hfill \\ \hfill & {\sigma }_v^2\left[\begin{array}{cc}{\int }_{t_k}^{t_{k+1}}{\mathbf{\Phi }}_{12}^2(t_{k+1},\tau )d\tau & {\int }_{t_k}^{t_{k+1}}{\mathbf{\Phi }}_{12}(t_{k+1},\tau ){\mathbf{\Phi }}_{22}(t_{k+1},\tau )d\tau \\ {\int }_{t_k}^{t_{k+1}}{\mathbf{\Phi }}_{12}(t_{k+1},\tau ){\mathbf{\Phi }}_{22}(t_{k+1},\tau )d\tau & {\int }_{t_k}^{t_{k+1}}{\mathbf{\Phi }}_{22}^2(t_{k+1},\tau )d\tau \end{array}\right]\hfill \end{array}\end{array}$$

(16)

The objective of this work is to estimate the forcing acceleration from position measurements. To achieve this, the governing equations for the system dynamics can be leveraged to estimate the states using either a minimum variance or a Kalman filter approach. However, the practical implementation of an onboard state estimation filter also accounts for additional artifacts arising from implementation on finite-precision computing architecture. In particular, discrete-time systems are susceptible to numerical errors when finite word-length registers are used to represent the states and measurements [25]. For accurate estimation of the forcing acceleration, it is essential to account for these finite word-length effects due to quantization. Neglecting such effects will degrade the estimation accuracy, as will be shown through the numerical simulations that follow. Thus, quantization effects necessitate reformulating the state-space model described in Equation (5) to account for the corresponding numerical errors.

3. Model Reformulation and State Estimation

3.1. Dynamical System: Fixed-Point Realization

A fixed-point realization of a discrete-time system is a problem that considers the presence of quantization noise due to rounding off of products within the realization. Systematic approaches to deal with the adverse effects of fixed-point implementation in digital filters are developed by Mullis [26], Hwang [32], Williamson and Kadiman [33], Liu and Skelton [10]. These approaches involve formulating discrete-time models, such as those in Equations (5) and (9) where the system state ${\mathbf{X}}_k$, input $g_k$, and the measurement variable $y_k$ are quantized at each instance of computation. The discrete-time model in Equation (5) is now redefined as

$$\begin{array}{c}{\mathbf{X}}_{k+1}=\mathbf{\Phi }(t_{k+1},t_k)\mathbb{Q}\left[{\mathbf{X}}_k\right]+\mathbf{\Gamma }\left(t_{k+1},t_k\right)\mathbb{Q}\left[g_k\right]+{\mathbf{w}}_k\end{array}$$

(17)

$$\begin{array}{c}{y}_k=\left[100\right]\mathbb{Q}\left[{\mathbf{X}}_k\right]+{\nu }_k\end{array}$$

(18)

where $\mathbb{Q}[.]$ here represents the quantization by round-off. The additive property of the round-off errors enables their modeling into the system description as

$$\begin{array}{cc}\hfill & \mathbb{Q}\left[{\mathbf{X}}_k\right]={\mathbf{X}}_k+{\mathit{ϵ}}_{\mathbf{x},k}\left(\mathrm{state}\mathrm{quantization}\right)\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill & \mathbb{Q}\left[g_k\right]=g_k+{ϵ}_{g,k}\left(\mathrm{D}/\mathrm{A}\mathrm{conversion}\right)\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill & \mathbb{Q}\left[y_k\right]=y_k+{ϵ}_{y,k}\left(\mathrm{A}/\mathrm{D}\mathrm{conversion}\right)\hfill \end{array}$$

(21)

Here, ${\mathit{ϵ}}_{\mathbf{x},k}$ arises from quantization at the state nodes, ${ϵ}_{g,k}$ is from digital-to-analog (D/A) conversion of input, and ${ϵ}_{y,k}$ stems from rounding-off of the sampled measurements from an analog-to-digital (A/D) converter. This approach assumes that the state nodes are quantized after double-length accumulation. However, round-off errors in the coefficients are not independently treated in this model. It is assumed that coefficient round-off errors accumulate at the state nodes, and optimizing for state quantization tends to also account for coefficient quantization errors, as direct optimization of coefficient errors is not tractable [10].

Typically, round-off errors are characterized as zero-mean independent random variables that follow a uniform distribution [34]. This modeling approach accurately captures the inherent uncertainty associated with the precision of numerical computations. Therefore, the error statistics of round-off errors can be described as

$$\begin{array}{cc}\hfill & \mathbb{E}\left\{{\mathit{ϵ}}_{\mathbf{x},k}\right\}=\mathbf{0}\forall k\mathrm{and}{\mathbf{\Sigma }}_{\mathbf{x}}=\mathbb{E}\left\{{\mathit{ϵ}}_{\mathbf{x},k}{\mathit{ϵ}}_{\mathbf{x},k}^T\right\}=q_x{I}_{\mathbf{x}};q_x\triangleq \frac{2^{-2B_{\mathbf{x}}}}{12}\hfill \end{array}$$

(22)

$$\begin{array}{cc}\hfill & \mathbb{E}\left\{{ϵ}_{g,k}\right\}=0\forall k\mathrm{and}{\Sigma }_g=\mathbb{E}\left\{{ϵ}_{g,k}{ϵ}_{g,k}^T\right\}=q_g;q_q\triangleq \frac{2^{-2B_g}}{12}\hfill \end{array}$$

(23)

$$\begin{array}{cc}\hfill & \mathbb{E}\left\{{ϵ}_{y,k}\right\}=0\forall k\mathrm{and}{\Sigma }_y=\mathbb{E}\left\{{ϵ}_{y,k}{ϵ}_{y,k}^T\right\}=q_y;q_y\triangleq \frac{2^{-2B_y}}{12}\hfill \end{array}$$

(24)

where $B_{\mathbf{x}}$, $B_g$, and $B_y$ represent the word-lengths of state node registers and A/D and D/A converters. $I_{\mathbf{X}}$ is an identity matrix corresponding to the number of states. For a multi-output system where multiple measurements are available at an epoch, the measurement round-off covariance is $q{I}_{\mathbf{y}}$, as will be utilized in the subsequent derivation of the least squares estimator. By extension, for a multi-input system, the input round-off covariance could be defined as $q{I}_{\mathbf{g}}$. $I_{\mathbf{y}}$ and $I_{\mathbf{g}}$ have the dimensions that correspond to the number of inputs and outputs. The multi-input multi-output generalization is provided in Appendix A, Appendix B and Appendix C.

3.2. Formulation of Least Squares Estimator

The linear time-invariant system in Equation (15) implies that a measurement obtained at the $n\mathrm{th}$ instant from k ($n>k$) is related to the states and input at the instant $t_k$ as

$$\begin{array}{c}{y}_{k+n}=\left[\begin{array}{ccc}{\mathbf{\Phi }}_{11}^{\left(n\right)}& {\mathbf{\Phi }}_{12}^{\left(n\right)}& {\mathbf{\Psi }}_{12}^{\left(n\right)}\end{array}\right]\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\\ g_k\end{array}\right]+{\mathbf{\Phi }}_{13}^{\left(n\right)}\widehat{b}+{\tilde{\nu }}_{k+n}\end{array}$$

(25)

$$\begin{array}{c}{\tilde{v}}_{k+n}={\nu }_{k+n}+{\mathbf{\Phi }}_{13}^{\left(n\right)}(b_k-\widehat{b})+{\int }_{t_k}^{t_{k+n}}{\mathbf{\Phi }}_{12}(t_{k+n},\tau )n_{v}d\tau \end{array}$$

(26)

where ${\mathbf{\Phi }}_{ij}^{\left(n\right)}$ and ${\mathbf{\Psi }}_{ij}^{\left(n\right)}$ represent the respective elements of $\mathbf{\Phi }(t_{k+n},t_k)$ and $\mathbf{\Psi }(t_{k+n},t_k)$ (refer Equation (5)).

For a batch of $N+1$ position measurements during which the input acceleration and the bias are assumed unchanged, the instantaneous states and the acceleration input are linearly related as follows:

$$\begin{array}{c}\underset{\tilde{\mathbf{y}}}{\underbrace{\left[\begin{array}{c}{y}_k\\ y_{k+1}\\ ⋮\\ y_{k+N}\end{array}\right]}}=\underset{{\mathbf{H}}_k}{\underbrace{\left[\begin{array}{ccc}1& 0& 0\\ {\mathbf{\Phi }}_{11}^{\left(1\right)}& {\mathbf{\Phi }}_{12}^{\left(1\right)}& {\mathbf{\Psi }}_{12}^{\left(1\right)}\\ & ⋮& \\ {\mathbf{\Phi }}_{11}^{\left(N\right)}& {\mathbf{\Phi }}_{12}^{\left(N\right)}& {\mathbf{\Psi }}_{12}^{\left(N\right)}\end{array}\right]}}\underset{{\tilde{\mathbf{X}}}_k}{\underbrace{\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\\ g_k\end{array}\right]}}+\underset{{\mathit{\eta }}_k}{\underbrace{\left[\begin{array}{c}0\\ {\mathbf{\Phi }}_{13}^{\left(1\right)}\\ ⋮\\ {\mathbf{\Phi }}_{13}^{\left(N\right)}\end{array}\right]}}\widehat{b}+\underset{{\tilde{\mathit{\nu }}}_k}{\underbrace{\left[\begin{array}{c}{\tilde{\nu }}_k\\ {\tilde{\nu }}_{k+1}\\ ⋮\\ {\tilde{\nu }}_{k+N}\end{array}\right]}}\end{array}$$

(27)

where the forcing acceleration input $g_k$ is now treated as an additional state for estimation.

Additionally, incorporating round-off errors described in Equation (17) into the above model yields:

$$\begin{array}{c}\hfill \tilde{\mathbf{y}}={\mathbf{H}}_k({\tilde{\mathbf{X}}}_k+{\mathit{ϵ}}_{\tilde{\mathbf{X}},k})+{\mathit{\eta }}_k(\widehat{b}+{ϵ}_{\widehat{b},k})+{\tilde{\mathit{\nu }}}_k+{\mathit{ϵ}}_{\tilde{\mathbf{y}}}\end{array}$$

(28)

where the bias round-off error (${ϵ}_{\widehat{b},k}$) is assumed to have the same variance as that of state ($q_x$). The input round-off error from D/A, ${ϵ}_{g,k}$, is absorbed into the estimation state error ${ϵ}_{\tilde{\mathbf{X}},k}$. Finally, the uncorrelated errors due to quantization in the measurement batch are accumulated in a vector of length $N+1$ as ${\mathit{ϵ}}_{\tilde{\mathbf{y}}}$.

Grouping the error terms together in a new variable $\mathit{\mu }$, the above linear system of equations can be expressed as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & \tilde{\mathbf{y}}-{\mathit{\eta }}_k\widehat{b}={\mathbf{H}}_k{\tilde{\mathbf{X}}}_k+\mathit{\mu }\hfill \\ \hfill & \mathit{\mu }={\mathbf{H}}_k{ϵ}_{\tilde{\mathbf{X}},k}+{\mathit{\eta }}_k{ϵ}_{\widehat{b},k}+{\tilde{\mathit{\nu }}}_k+{\mathit{ϵ}}_{\tilde{\mathbf{y}}}\hfill \end{array}\end{array}$$

(29)

The measurement error mean and the covariance (${\mathbf{P}}_{\mathit{\mu }\mathit{\mu }}$) can be obtained as

$$\begin{array}{cc}\hfill & \mathbb{E}\left[\mathit{\mu }\right]={\mathbf{H}}_k\mathbb{E}\left[{ϵ}_{\tilde{\mathbf{X}},k}\right]+{\mathit{\eta }}_k\mathbb{E}\left[{ϵ}_{\widehat{b},k}\right]+\mathbb{E}\left[{\tilde{\mathit{\nu }}}_k\right]+\mathbb{E}\left[{\mathit{ϵ}}_{\tilde{\mathbf{y}}}\right]=\mathbf{0}\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{\mathit{\mu }\mathit{\mu }}=\mathbb{E}\left[\mathit{\mu }{\mathit{\mu }}^T\right]={\mathbf{H}}_k{\mathbf{\Sigma }}_{\tilde{\mathbf{X}}}{\mathbf{H}}_k^T+{\mathit{\eta }}_k{\Sigma }_{\widehat{b}}{\mathit{\eta }}_k^T+{\mathbf{P}}_{\tilde{\mathit{\nu }}\tilde{\mathit{\nu }}}+{\mathbf{\Sigma }}_{\tilde{\mathbf{y}}}\hfill \end{array}$$

(31)

where the quantization noise covariances (${\mathbf{\Sigma }}_{\tilde{\mathbf{X}}},{\Sigma }_{\widehat{b}}$, and ${\mathbf{\Sigma }}_{\tilde{\mathbf{y}}}$) are obtained from Equations (22)–(24) as

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{\tilde{\mathbf{X}}}=\mathrm{diag}(q_x,q_x,q_g);{\Sigma }_{\widehat{b}}=q_x;\mathrm{and}{\mathbf{\Sigma }}_{\tilde{\mathbf{y}}}=q_y{\mathbf{I}}_{\tilde{\mathbf{y}}}\end{array}$$

(32)

Moreover, the elements of the measurement noise covariance matrix ${\mathbf{P}}_{\mathit{\nu }\mathit{\nu }}$ have explicit dependence on time and will require periodic calibration to prevent measurement degradation [27]. The covariance elements can be computed in indicial notation as

$$\begin{array}{c}\hfill {\mathbf{P}}_{\mathit{\nu }\mathit{\nu },ij}={\sigma }_m^2{\delta }_{ij}+{\mathbf{\Phi }}_{13}^{\left(i\right)}{\mathbf{\Phi }}_{13}^{\left(j\right)}\left({\sigma }_{b0}^2+{\sigma }_u^2(t_k-t_0)\right)+{\sigma }_v^2{\int }_{t_k}^{t_{k+i}}{\mathbf{\Phi }}_{12}^2(t_{k+i},\tau )d\tau \end{array}$$

(33)

Ultimately, an optimal state estimate using the batch of measurements $\tilde{\mathbf{y}}$, are obtained by solving the normal equations as

$$\begin{array}{c}\hfill \widehat{\tilde{\mathbf{X}}}\left(k\right)={\left[{\mathbf{H}}_k^T{\mathbf{P}}_{\mathit{\mu }\mathit{\mu }}^{-1}{\mathbf{H}}_k\right]}^{-1}{\mathbf{H}}_k^T{\mathbf{P}}_{\mathit{\mu }\mathit{\mu }}^{-1}(\tilde{\mathbf{y}}-{\mathit{\eta }}_k\widehat{b})\end{array}$$

(34)

The above expression is a stochastic moving average filter, and it consumes at least three running measurements ($N\ge 3$) to provide the estimates for the single-axis position, velocity, and acceleration states at every estimation epoch. Evidently, the filter averages the measurements from two future time events, causing the filter to lag the measurement sequence by at least two measurement cycles. The minimum variance estimation with quantized states and measurements for a linear system is derived in Appendix A.

3.3. Note on State Estimation with Scale Factor Errors

  The readout error in Equation (26) can be modified to account for the scale factor error described in Equation (10). This modification starts with acknowledging that the measurements are contaminated by measurement noise and scale factor errors as

$$\begin{array}{c}\hfill y_{k+n}=(1+{ϵ}_{s,k+n})x_{k+n}+{\nu }_{k+n}\end{array}$$

(35)

wherein the scale factor error is modeled as an uncorrelated zero-mean Gaussian white noise with variance ${\sigma }_{{ϵ}_s}^2$.

As a consequence, the total readout error in Equation (26) has an additional term ${ϵ}_{s,k+n}{x}_{k+n}$ such that

$$\begin{array}{c}\hfill {\tilde{\nu }}_{k+n}={ϵ}_{s,k+n}{x}_{k+n}+{\mathit{\nu }}_{k+n}+{\mathbf{\Phi }}_{13}^{\left(n\right)}(b_k-\widehat{b})+{\int }_{t_k}^{t_{k+n}}{\mathbf{\Phi }}_{12}(t_{k+n},\tau )n_{v}d\tau \end{array}$$

(36)

Notice that the measurement noise is now linearly related to the displacement state. This problem can be addressed by using an a priori estimate of displacement state (${\widehat{x}}_{a,k+n}$) from the propagation of model dynamics (Equation (15)) [35].

Now, using ${\widehat{x}}_{a,k+n}$ for evaluating the scale factor error contribution, it can be shown that the additional variance that contributes to the measurement noise variance is

$$\begin{array}{c}\hfill {\sigma }_{{\tilde{\nu }}_{k+n}}^2={\sigma }_{{\nu }_{k+n}}^2+{\sigma }_{{ϵ}_{s,k+n}}^2{\left({\widehat{x}}_{a,k+n}\right)}^2\end{array}$$

(37)

Following a procedure similar to the one described in Section 3.2, this updated measurement noise variance can now be used to compute ${\mathbf{P}}_{\tilde{\mathit{\nu }}\tilde{\mathit{\nu }}}$ in Equation (31). Note that the incorporation of a priori state information and corresponding a priori error covariance ($\tilde{\mathbf{Q}}$) allows us to extend the least squares filter to update the state estimates as described in Appendix A.3. Additionally, since acceleration is an estimated state, its contribution to the process noise is evaluated as a tunable parameter (details in Section 3.5).

3.4. Multi-Oscillator Problem

The dynamic response of a harmonic oscillator depends on the oscillator’s natural frequency, damping, and the driving signal frequency. To attain a wide dynamic range for precise inertial measurements, multiple oscillators may be deployed in a multiplexed sensing network [28]. The estimates of forcing accelerations from multiple sensing nodes are appropriately fused to obtain an estimate with high confidence. If the estimates are calculated from the observations sampled by the same hardware system at the same instance, a covariance-weighted average of all the arriving estimates can be implemented to reduce noise over a wide working range of measurable accelerations using the multi-oscillator system’s response.

Assuming that the estimation errors (from least squares filter independently applied to measurements from different oscillators) are independent and unbiased (zero-mean), the acceleration estimates are fused by using a weighted average with the reciprocal variance values as weights [36]:

$$\begin{array}{c}\overline{g}\left(k\right)=\frac{{\displaystyle \sum _{i=0}^{i=N}}{\widehat{g}}_i\left(k\right).\frac{1}{{\mathbf{P}}_{33}^i}}{{\displaystyle \sum _{i=0}^{i=N}}\frac{1}{{\mathbf{P}}_{33}^i}}\end{array}$$

(38)

where N represents the number of independent estimates and ${\mathbf{P}}_{33}^i$ is the estimated non-zero variance of ${\widehat{g}}_i\left(k\right)$. The variance ${\sigma }_{\overline{g}}^2$ of the fused estimate is always lower or equal to the best individual estimate and is given by

$${\sigma }_{\overline{g}}^2=\sum _{i=0}^{i=N}\frac{1}{{\mathbf{P}}_{33}^i}$$

(39)

Note that the independence of error in estimates is a loosely constructed term and difficult to fulfill, especially when the oscillators are of the same type. If the individual estimation errors are not independent, the covariance-weighted average still yields a correct estimate, but its assigned confidence is overestimated.

3.5. Quantized Discrete-Time Kalman Filter (QDKF)

The least squares moving average filter described in Section 3.2 uses an $N+1$ measurement batch to estimate the states at every epoch. In this section, a Kalman filter formulation is briefly described where the state variables are sequentially estimated by fusing predictions of the state variables from the oscillator dynamical model with noisy position measurements.

The discrete-time dynamics given in Equation (15) can be remodeled to accommodate the instantaneous forcing acceleration input as a state. However, since the forcing acceleration is not directly observed through a model, it is estimated from new measurements. The process covariance is augmented with an acceleration model uncertainty parameter $\alpha$ that is appropriately scaled to indicate the confidence in the evolution of $g_k$. After all, the acceleration input cannot be perfectly delivered to the system and is affected by a noise process that is denoted here as $w_{g,k}$. An independent periodic calibration step prevents accumulation of errors in acceleration estimates. The dynamical model therefore can be reformulated with the evolution of the modified states, ${\mathbf{X}}_k={\left[x_k{\dot{x}}_k{g}_k\right]}^T$, as given by the discrete-time dynamics as

$$\begin{array}{c}\underset{{\mathbf{X}}_{k+1}}{\underbrace{\left[\begin{array}{c}{x}_{k+1}\\ {\dot{x}}_{k+1}\\ g_{k+1}\end{array}\right]}}=\underset{\tilde{\mathbf{\Phi }}}{\underbrace{\left[\begin{array}{ccc}{\mathbf{\Phi }}_{11}& {\mathbf{\Phi }}_{12}& {\mathbf{\Psi }}_{12}\\ {\mathbf{\Phi }}_{21}& {\mathbf{\Phi }}_{22}& {\mathbf{\Psi }}_{22}\\ 0& 0& 1\end{array}\right]}}\underset{{\mathbf{X}}_k}{\underbrace{\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\\ g_k\end{array}\right]}}+\underset{\tilde{\mathbf{\Gamma }}}{\underbrace{\left[\begin{array}{c}{\mathbf{\Phi }}_{13}\\ {\mathbf{\Phi }}_{23}\\ 0\end{array}\right]}}\widehat{b}+\underset{{\overline{\mathbf{w}}}_k}{\underbrace{\left[\begin{array}{c}{\tilde{\mathbf{w}}}_k\\ w_{g,k}\end{array}\right]}}\end{array}$$

(40)

Finally, the dynamical and the measurement models, under the influence of quantization noises, can be reformulated as

$$\begin{array}{cc}\hfill {\mathbf{X}}_{k+1}& ={\tilde{\mathbf{\Phi }}}_k({\mathbf{X}}_k+{ϵ}_{\mathbf{X},k})+{\tilde{\mathbf{\Gamma }}}_k(\widehat{b}+{ϵ}_{\widehat{b},k})+{\overline{\mathbf{w}}}_k\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill y_k& ={\mathbf{H}}_k({\mathbf{X}}_k+{ϵ}_{\mathbf{X},k})+{\tilde{\mathit{\nu }}}_k+{ϵ}_y\hfill \end{array}$$

(42)

where ${\mathbf{H}}_k=\left[100\right]$ is the observation model matrix indicating that the position of the proof mass is directly observable through measurements. The round-off errors included in the model follow the same definitions as described in the least squares estimator (Section 3.2).

With the reformulated dynamics and the observation models described in Equations (41) and (42), the round-off errors are incorporated into the Kalman filter formulation for sequential state estimation. Starting from an initial value of state and corresponding error covariance, a quantized form of Kalman filter, QDKF, is thus realized. Algorithm 1 describes the recursive operations involved in the implementation of the QDKF. The derivation of the QDKF is presented in Appendix B.

Algorithm 1: Quantized discrete-time Kalman filter (QDKF).

1: Initialize $$\begin{array}{l}{\widehat{\mathbf{X}}}_0^{+}=\mathbb{E}\left[{\mathbf{X}}_0\right]\hfill \end{array}$$ (43) $$\begin{array}{c}{\mathbf{P}}_0^{+}=\mathbb{E}\left[({\widehat{\mathbf{X}}}_0-{\mathbf{X}}_0){({\widehat{\mathbf{X}}}_0-{\mathbf{X}}_0)}^T\right]\hfill \end{array}$$ (44)   2: Propagate $$\begin{array}{c}{\widehat{\mathbf{X}}}_{k+1}^{-}={\tilde{\mathbf{\Phi }}}_k{\widehat{\mathbf{X}}}_k^{+}+{\tilde{\mathbf{\Gamma }}}_k\widehat{b}\hfill \end{array}$$ (45) $$\begin{array}{c}{\mathbf{P}}_{k+1}^{-}={\tilde{\mathbf{\Phi }}}_k({\mathbf{P}}_k^{+}+{\mathbf{\Sigma }}_{\mathbf{X},k}){\tilde{\mathbf{\Phi }}}_k^T+{\tilde{\mathbf{\Gamma }}}_k{\mathbf{\Sigma }}_{\widehat{b},k}{\tilde{\mathbf{\Gamma }}}_k^T+\left[\begin{array}{cc}\tilde{\mathbf{Q}}& 0\\ 0& \alpha \end{array}\right]\hfill \end{array}$$ (46)   3: Update $$\begin{array}{c}{\mathbf{K}}_k={\mathbf{P}}_k^{-}{\mathbf{H}}_k^T{[{\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{H}}_k{\mathbf{\Sigma }}_{\mathbf{X},k}{\mathbf{H}}_k^T+{\mathbf{R}}_k+{\mathbf{\Sigma }}_{\mathbf{y},k}]}^{-1}\hfill \end{array}$$ (47) $$\begin{array}{c}{\widehat{\mathbf{X}}}_k^{+}={\widehat{\mathbf{X}}}_k^{-}+{\mathbf{K}}_k({\mathbf{y}}_k-{\mathbf{H}}_k{\widehat{\mathbf{X}}}_k^{-})\hfill \end{array}$$ (48) $$\begin{array}{c}{\mathbf{P}}_k^{+}=[\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k]{\mathbf{P}}_k^{-}\hfill \end{array}$$ (49)

   An equivalent expression for the covariance update in Equation (49) can be written in a symmetric form, as shown below. This symmetric version is often used in software implementation as it guarantees positive semi-definiteness of ${\mathbf{P}}_k^{+}$ in the presence of round-off errors.

$${\mathbf{P}}_k^{+}=[\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k]{\mathbf{P}}_k^{-}{[\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k]}^T+{\mathbf{K}}_k[{\mathbf{H}}_k{\mathbf{\Sigma }}_{\mathbf{X},k}{\mathbf{H}}_k^T+{\mathbf{R}}_k+{\mathbf{\Sigma }}_{\mathbf{y},k}]{\mathbf{K}}_k^T$$

(50)

3.6. Quantized Square-Root Kalman Filter (QSRKF)

For onboard implementation with limited computational word-length, the standard Kalman filter algorithm is susceptible to numerical instability. Round-off errors can cause loss of positive definiteness in error covariance matrices during computation [2,37]. Square-root Kalman filters (SRKFs) mitigate this numerical degradation by computing and propagating the square roots of the error covariance matrices for both time and measurement updates. The quantized version of square-root filters, termed QSRKF here, incorporates quantization errors into the SRKF formulation, thereby improving performance as well as the filter’s numerical stability under fixed-point implementation.

In QSRKF, error covariance matrices are factored into square-root matrices computed using QR decomposition. The square roots of the initial state error, process noise, measurement noise, and quantization noise covariance matrices are calculated once using the Cholesky method. For linear time-invariant systems, these can often be predetermined and stored onboard. For the dynamical system described in the QDKF formulation (Section 3.5), Algorithm 2 presents the operations involved in QSRKF realization. The filter algorithm is described in detail in Appendix C.

Algorithm 2: Quantized Square-Root Kalman filter (QSRKF).

1: Initialize $$\begin{array}{cc}\hfill & {\widehat{\mathbf{X}}}_0^{+}=\mathbb{E}\left[{\mathbf{X}}_0\right]\hfill \end{array}$$ (51) $$\begin{array}{cc}\hfill & {\mathbf{S}}_0^{+}=\sqrt{\mathbb{E}\left[({\widehat{\mathbf{X}}}_0-{\mathbf{X}}_0){({\widehat{\mathbf{X}}}_0-{\mathbf{X}}_0)}^T\right]}\hfill \end{array}$$ (52)   2: Propagate $$\begin{array}{cc}\hfill & {\widehat{\mathbf{X}}}_{k+1}^{-}={\tilde{\mathbf{\Phi }}}_k{\widehat{\mathbf{X}}}_k^{+}+{\tilde{\mathbf{\Gamma }}}_k\widehat{b}\hfill \end{array}$$ (53) $$\begin{array}{cc}\hfill & {\mathbf{S}}_{k+1}^{-}=\mathrm{qr}{\left\{{\left[{\mathbf{\Phi }}_k{\mathbf{S}}_k^{+}|{\mathbf{\Phi }}_k{\mathsf{\Lambda }}_{\mathbf{x},k}\left|{\mathbf{\Gamma }}_k{\mathsf{\Lambda }}_{\mathbf{u},k}\right|{\gamma }_k{\mathbf{S}}_{\mathbf{w},k}\right]}^T\right\}}^T\hfill \end{array}$$ (54)   3: Update $$\begin{array}{cc}\hfill & {\mathbf{S}}_{\mathbf{z}\mathbf{z},k}=\mathrm{qr}{\left\{{\left[{\mathbf{H}}_k{\mathbf{S}}_k^{-}|{\mathbf{H}}_k{\mathsf{\Lambda }}_{\mathbf{x},k}\left|{\mathbf{S}}_{\mathbf{v},k}\right|{\mathsf{\Lambda }}_{\mathbf{y},k}\right]}^T\right\}}^T\hfill \end{array}$$ (55) $$\begin{array}{cc}\hfill & {\mathbf{K}}_k={\mathbf{S}}_k^{-}{\left({\mathbf{H}}_k{\mathbf{S}}_k^{-}\right)}^T{\left({\mathbf{S}}_{\mathbf{z}\mathbf{z},k}{\mathbf{S}}_{\mathbf{z}\mathbf{z},k}^T\right)}^{-1}\hfill \end{array}$$ (56) $$\begin{array}{cc}\hfill & {\widehat{\mathbf{X}}}_k^{+}={\widehat{\mathbf{X}}}_k^{-}+{\mathbf{K}}_k({\mathbf{y}}_k-{\mathbf{H}}_k{\widehat{\mathbf{X}}}_k^{-})\hfill \end{array}$$ (57) $$\begin{array}{cc}\hfill & {\mathbf{S}}_k^{+}=\mathrm{qr}\{{\left[[\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k]{\mathbf{S}}_k^{-}|{\mathbf{K}}_k\left[{\mathbf{H}}_k{\mathsf{\Lambda }}_{\mathbf{x},k}\left|{\mathbf{S}}_{\mathbf{v},k}\right|{\mathsf{\Lambda }}_{\mathbf{y},k}\right]\right]}^T{\}}^T\hfill \end{array}$$ (58)

The following is true for the above algorithm:

• ${\mathbf{S}}_0^{+}$ is the square-root factor of the initial estimation error covariance.
• ${\mathbf{S}}_{k+1}^{-}$ and ${\mathbf{S}}_k^{+}$ represent the prior and posterior estimation error covariance square-root factors.
• ${\mathbf{S}}_{\mathbf{v},k}$ and ${\mathbf{S}}_{\mathbf{w},k}$ are the square-roots of the measurement and process noise covariance matrices.
• ${\mathsf{\Lambda }}_{\mathbf{x},k}$, ${\mathsf{\Lambda }}_{\mathbf{y},k}$, and ${\mathsf{\Lambda }}_{\mathbf{u},k}$ represent the matrix square-roots of the quantization error covariances for state, measurement, and output noises.
• $\mathrm{qr}\{·\}$ indicates QR decomposition operation.

4. Numerical Simulations

The optomechanical inertial sensor parameters and the corresponding noise processes modeled in this simulation are highlighted in

Table 1. Simulated sensor model and noise parameters.

Parameter	Value	Units
Oscillators
Sampling frequency ($f_s$)	$30.5$	Hz
Oscillator-1 frequency (${\omega }_1$)	$3.76$	Hz
Oscillator-2 frequency (${\omega }_2$)	$8.5$	Hz
Damping ratios (${\zeta }_1,{\zeta }_2$)	$4.386\times {10}^{-6}$
Modeling processes
${\sigma }_v$	$1\times {10}^{-9}$	$\mathrm{m}/\mathrm{s}\sqrt{\mathrm{Hz}}$
${\sigma }_u$	$1\times {10}^{-8}$	${\mathrm{m}/\mathrm{s}}^2\sqrt{\mathrm{Hz}}$
${\sigma }_m$	$1\times {10}^{-11}$	m
${\sigma }_{b0}$	$1\times {10}^{-8}$	${\mathrm{m}/\mathrm{s}}^2$
Quantization processes
$q_x$	$\sqrt{2^{-2B_x}/12}$	unit of corresponding state
$q_y$	$\sqrt{2^{-2B_y}/12}$	m
$q_{\widehat{b}}$	$\sqrt{2^{-2B_{\widehat{b}}}/12}$	m/s2

. These parameters are derived from laboratory experiments and sensor benchtop prototypes described in references [28,31,38]. The contributing process noise sources, including thermal, mechanical, and cavity drifts, have been thoroughly investigated in the cited works. The oscillator parameters, process noise terms (${\sigma }_v$, ${\sigma }_u$) driving the discrete-time state evolution, calibrated sensor bias as well as the readout noise floor [27], are set to values consistent with the experimental observations.

Noisy position measurements are simulated from true position states corrupted by a stochastic measurement noise process (${\sigma }_m$) along with an additive quantization process ($q_y$). The stochastic processes (${\sigma }_v,{\sigma }_u$) driving the discrete-time state evolution, the state quantization processes ($q_x$), the standard deviation of the calibrated bias estimate (${\sigma }_{b0}$), and its corresponding quantization process ($q_{\widehat{b}}$) are also presented in Table 1.

In this simulation, sensor bias is assumed to be estimated through an independent calibration step that is briefly described in Section 2.2 and detailed in [27]. The bias estimate $\widehat{b}$ is randomly drawn from $\mathcal{N}(0,{\sigma }_{b0}^2)$, and the acceleration input is simulated as a sinusoidal signal of frequency $0.01$ Hz and amplitude $1\times {10}^{-5}$ g. In this analysis, the state and bias values are stored with fractional word length of internal registers configured to signed 16 bits ($B_x=B_{b0}=16$). This means that the fractional parts of the states and input nodes are rounded off to 16 bits, with the word-length of the measurements being the only variable. The measurements are quantized to fractional lengths (FLs) of 8 to 16 bits ($B_y$) to emulate different ADC resolutions available on development boards. To prevent numerical underflows and to maintain precision, the covariance matrix elements are assigned higher word lengths compared with the state and measurement variables. For FPGA implementation, dedicated digital signal processing (DSP) blocks can be utilized to efficiently multiply the covariance matrix elements with the corresponding state or measurement variables. Modern FPGAs typically provide DSP blocks that support multiplication of 18-bit and $25$-bit operands. To handle higher word lengths of the covariance matrix elements, multiple DSP blocks can be cascaded to perform pipelined multiply-and-accumulate operations, enabling accurate computations while maintaining high throughput.

Figure 1 shows the results for the errors in the estimated acceleration and the corresponding $3\sigma$ estimation error bounds from the moving average least squares method. In this figure, the errors and the $3\sigma$ bounds for the least squares implementation are compared for measurements of 12 versus $16$-bit fractional length. The least squares filter structure defined in Equation (34) supplements the measurement noise covariance with the quantization error covariance that reflects the bit resolution. Hence, the simulation in Figure 1 demonstrates additional errors in the acceleration estimates due to less precise representation of measurements. The simulation also signifies the expansion of the $3\sigma$ bounds, indicative of added uncertainties in the estimated errors due to increased quantization noise in the measurements. For instance, acceleration estimates with the measurements rounded off to a fractional length of $12$-bits have larger errors and covariance bounds than those of $16$-bit measurements.

Figure 2 shows the estimation error results for the Kalman filter formulation, where the process noise associated with the acceleration channel is modeled as a zero-mean process with a variance of $0.1$$(\alpha =0.1)$. As in the least squares method, the errors and error bounds from the Kalman filter are sensitive to the precision in the measurements. This sensitivity is illustrated in Figure 2a,b, which respectively compare 8 and $12$-bit measurements against measurements with $16$-bit fractional resolution. The analysis suggests that as the measurement precision increases, the errors and the error bounds tend to align statistically with those of a floating-point implementation of the discrete-time Kalman filter, despite finite-precision hardware constraints. In another observation, the least squares filter has lower variance than that of the QDKF for this application. This is because the least squares filter need not account for the process noise associated with the acceleration channel.

Furthermore, accounting for finite word-length effects in the filter implementation not only accurately predicts the uncertainty in the estimation errors but also reduces these errors. Shown in Figure 3 is discrete-time Kalman filter (DKF) implementation without considering round-off errors in the filter design versus the quantized discrete-time Kalman filter (QDKF) proposed in this work (Algorithm 1). In this comparison, the measurements are quantized to $12$-bits in both the DKF and the QDKF implementations, but the filter formulation in DKF does not incorporate quantization error covariances as the proposed QDKF algorithm does. Evidently, the QDKF filter resulted in lower error values than the DKF and also higher $3\sigma$ bounds to represent increased covariance due to quantization errors. Moreover, the DKF errors are numerically inconsistent with the corresponding error covariances, as its formulation does not account for quantization noise statistics. The same phenomenon is also evident in the least squares estimation, as observed in Figure 1. Furthermore, numerical round-off errors exacerbate the lack of observability in the bias state, resulting in growing uncertainty in acceleration estimation errors.

4.1. Steady-State Performance

In practice, the Kalman filter is often run for long periods of time. As $k\to \infty$ and if given input remains within reasonable magnitude, the error covariance (${\mathbf{P}}_k^{-}$) converges to a bounded steady-state value $\mathbf{P}$. For a large k, ${\mathbf{P}}_{k+1}^{-}={\mathbf{P}}_k^{-}\triangleq \mathbf{P}$, and Equation (46) satisfies the discrete-time algebraic Riccati equation [24]:

$$\begin{array}{cc}\hfill & \mathbf{P}={\tilde{\mathbf{\Phi }}}_k[\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k]\mathbf{P}{\tilde{\mathbf{\Phi }}}_k^T+{\tilde{\mathbf{\Phi }}}_k{\mathbf{\Sigma }}_{\mathbf{X},k}{\tilde{\mathbf{\Phi }}}_k^T+{\tilde{\mathbf{\Gamma }}}_k{\mathbf{\Sigma }}_{\widehat{b},k}{\tilde{\mathbf{\Gamma }}}_k^T+\left[\begin{array}{cc}\tilde{\mathbf{Q}}& 0\\ 0& \alpha \end{array}\right]\hfill \end{array}$$

(59)

with ${\mathbf{K}}_k$ given in Equation (47).

Figure 4 shows the standard deviation contours for steady-state acceleration estimation errors. These contours illustrate how the steady-state error bounds change as a function of the fractional length in measurement quantization and the process noise associated with the acceleration channel. In the absence of quantization errors, the contour isolines maintain constant values for given levels of process noise. As the numerical precision of measurements increases, the contours asymptotically approach the steady-state performance achieved by floating-point implementation. The sensitivity analysis curves that graphically illustrate the impact of quantization noise on the state estimation accuracy are an important contribution of this work.

The steady-state behavior reflects the filter performance over an extended period, depicting characteristics of the estimation errors once the transient effects have diminished and the error dynamics have stabilized. The steady-state analysis offers a quantitative measure of the best possible accuracy achievable by the filter and is based on sensor parameters, process, and measurement noise characteristics. Consequently, it serves as a valuable tool in sensor design and parameter tuning. In interferometric sensing, the design and modeling of mechanical elements, signal processing, and estimation filters can be tailored for accuracy and reliability using steady-state covariance analysis.

Furthermore, in the context of quantized Kalman filtering, the Mahalanobis distance is chosen as a metric to quantify the impact of quantization noise on the consistency of estimation errors. The Mahalanobis distance (d) between the quantized observation (${\mathbf{y}}_k$) and its prediction (${\mathbf{H}}_k{\widehat{\mathbf{X}}}_k^{-})$) reflects the variance between them, scaled by the inverse of the associated covariance matrix (${\mathbf{P}}_{\mathbf{z}\mathbf{z},k}^{-1}$). That is,

$$\begin{array}{c}\hfill d=\sqrt{{\mathbf{z}}_k^T{\mathbf{P}}_{\mathbf{z}\mathbf{z},k}^{-1}{\mathbf{z}}_k}\end{array}$$

(60)

where, from Algorithm 1,

$$\begin{array}{cc}\hfill {\mathbf{z}}_k& ={\mathbf{y}}_k-{\mathbf{H}}_k{\widehat{\mathbf{X}}}_k^{-}\hfill \end{array}$$

(61)

$$\begin{array}{cc}\hfill {\mathbf{P}}_{\mathbf{z}\mathbf{z},k}& ={\mathbf{H}}_k{\mathbf{P}}_k^{-}{\mathbf{H}}_k^T+{\mathbf{H}}_k{\mathbf{\Sigma }}_{\mathbf{X},k}{\mathbf{H}}_k^T+{\mathbf{R}}_k+{\mathbf{\Sigma }}_{\mathbf{y},k}\hfill \end{array}$$

(62)

Figure 5 illustrates the Mahalanobis distance of the estimates from the QDKF method for observations with varying fractional lengths over time. The Mahalanobis distance quantifies the dissimilarity between the estimated and true states, considering the covariance of the estimation errors. A smaller Mahalanobis distance indicates better filter performance, as the estimated states are closer to the true states. To visualize the trend, a moving average of 10,000 Mahalanobis distance samples is computed. The results demonstrate that higher quantization noise in the observations leads to greater disparities between the observed and predicted measurements. While sequential filtering helps the filter learn the quantization noise statistics over time, it cannot fully match the lower bound achieved in double-precision simulations. Notably, this metric indicates that 16-bit measurements closely track the lower bound, suggesting an optimal word length for sensor data transmissions in this application.

4.2. Square Root Kalman Filter Simulations

For $12$-bit measurements, the variances from the quantized DKF were observed to take negative values, which is theoretically nonviable. The SRKF, by construction, avoids loss of positive definiteness of the error covariance matrices and thereby offers resistance to numerical overflows and underflows. For covariance matrices that are appropriately scaled and quantized to avoid numerical degeneracy for this application, the performance of the QSRKF proposed in Algorithm 2 is comparable to that of the QDKF (Algorithm 1). Figure 6 compares the estimation errors and the corresponding $3\sigma$ bounds from QDKF and the QSRKF filters. Although this result indicates that the errors are not significantly reduced using the QSRKF version, the loss of positive definiteness in QDKF is a weakness of the standard implementation. Therefore, the square-root filters are preferable for onboard implementation even though they are computationally burdensome.

At the same time, it is evidently important to model quantization noise sources into the standard SRKF algorithm for reliable filter performance. Figure 7 compares the performance of the square-root filters with the quantization noise modeled into the filter structure (QSRKF in Algorithm 2) versus a standard SRKF implementation that does account for the round-off error statistics. In this comparison, the measurements are quantized to $12$-bits in both the QSRKF and the SRKF runs.

Table 2. Average Mahalanobis distance for different filters and measurement fractional lengths (FLs). DP represents the ideal filter performance with double-precision measurements.

Filter	FL = 8	FL = 10	FL = 12	FL = 16	DP
DKF	36.3042	9.2255	2.4355	0.4855	0.4018
QDKF	0.9839	0.8647	0.6923	0.4584
SRKF	36.3031	9.1854	2.4278	0.4827	0.4016
QSRKF	0.9839	0.8647	0.6923	0.4588

presents the time-averaged Mahalanobis distance for each filter (DKF, QDKF, SRKF, and QSRKF) as a function of measurement resolutions. The results show that the quantized filters (QDKF and QSRKF) proposed in this work outperform filter implementations that do not account for round-off errors (DKF and SRKF). The quantized filters maintain a lower average Mahalanobis distance, indicating better estimation accuracy and robustness to the effects of measurement quantization. Furthermore, the average Mahalanobis distance decreases as the fractional length increases for all filters. This trend suggests that higher measurement resolutions lead to improved filter performance, as more precise measurements provide better information for state estimation. The last column in the table (DP) represents the average Mahalanobis distance for the ideal filter implementations using double-precision measurements. These values serve as a benchmark for the best achievable performance without quantization effects. The quantized filters (QDKF and QSRKF) approach the performance of the ideal filters, demonstrating their effectiveness in mitigating the impact of measurement quantization.

Numerical simulations show that the Kalman filters appear to be under-confident because of the uncertainty in the evolution of the acceleration state. The least squares filter, however, does not require handling process noise associated with the acceleration state and is therefore much more confident about the estimation errors. Moreover, the process noise of the acceleration input is well studied to have low uncertainty [28]. Therefore, a least-squares based moving average filter is considered for hardware implementation.

5. Architecture for FPGA Implementation

In this section, an FPGA-based embedded architecture designed for estimating input acceleration force for a dual-oscillator system is described. The estimation algorithm consumes simulated measurements from two distinct oscillator models and delivers a covariance-weighted average of the estimated accelerations as illustrated in Figure 8.

5.1. Implementation Overview

The system architecture comprises an FPGA accelerator designed to estimate the forcing accelerations from each of the oscillators within an FPGA sensor node and then to compute a covariance-weighted average of these estimates, as detailed in Section 3.4. The proposed architecture is a hardware-software co-design illustrated in Figure 8. Xilinx Zynq 7020 SoC is targeted for hardware evaluation of the proposed system. This SoC integrates a host processor, also known as the processing system (PS), featuring ARM Cortex-A9 MPCore. The PS performs the filter-specific operations that involve discrete-time state propagation and measurement model evaluation (Equations (27) and (40)). For a linear time-invariant system like the one under consideration, the state propagation from time-step $t_k$ to $t_{k+1}$ involves a constant state transition matrix and consequently a constant measurement model matrix (Equation (27)). Additionally, the processor manages the flow of simulated observations to be transmitted to the programmable logic (PL) via the memory-mapped register space indicated in Figure 9.

5.2. State Machine

The matrix operations in the intellectual property (IP) core are managed by a state machine shown in Figure 10. Initially, the IP core waits in an IDLE state, awaiting initialization by the processor. Upon receiving the initialization signal through a control register in the memory-mapped register space, the core transitions to the INIT state. Here, the processor initializes the internal memory (FIFO) of the IP core with values of the measurement model matrix augmented with the measurement noise covariance matrix. Once initialization is complete, signified by an init_done signal, the core progresses to the LS_OPS state, where least squares operations are executed. Upon completion of these operations, the core returns to the IDLE state, ready for the next batch of least squares operations, triggered by the arrival of another set of measurement model parameters relayed by the processor (for the second oscillator model in this case). This state machine governs the matrix operations within this pipelined least squares filter, with the hardware modules instantiated once and reused between different oscillator models.

In the subsequent stages of the implementation illustrated in Figure 8, the processor writes the simulated vector measurements into a data register, and the core performs the least squares estimation. Once estimates and corresponding variances from two such computations are available, the core calculates the covariance-weighted average of these estimates (Equation (38)). This result is then written by the core to a FIFO, ultimately to be read by the processor through another memory-mapped register.

5.3. Least Squares Module

This module implements a moving average filter that computes the normal equations using the direct expression for the least squares solution [23]. Operationally, three measurements are accumulated in a measurement vector ${\tilde{\mathbf{y}}}_i$ to compute the acceleration estimate. This implies that the dimension of matrix operations is three, with the estimates having a time delay equivalent to two measurement samples.

The estimation process involves a series of fixed-point matrix operations, including matrix transpose, inversion, matrix-matrix, and matrix-vector multiplications, to arrive at the least squares solution. Matrix-matrix multiplications are performed using systolic array architecture (SAA) [39], which is a pipelined two-dimensional network of multiply and accumulate (MAC) units, effective for low-latency matrix multiply operations. Matrix-vector multiplications also utilize MAC units to operate on the time-aligned input streams of matrix and measurement vector channels. The matrix inverse is computed using direct inversion expressions for the involved $3\times 3$ matrices. The model and measurement data required for the computation are stored in the memory buffer of the IP core.

While the least squares solution in this example is tractable because the measurement model matrices are scaled to be well-conditioned, this approach can be computationally expensive. If ${\mathbf{H}}^T\mathbf{H}$ is ill-conditioned, the solution can be numerically unstable [40]. In practice, Cholesky (or) QR factorization [41], singular value decomposition, and, to a reasonable extent, LU decomposition [42] are efficient and accurate methods for solving normal equations.

The least squares module is instantiated only once, and the operations are pipelined to reuse the same module for estimating accelerations from two oscillators. This ensures effective resource management on the Zynq 7020 FPGA SoC.

5.4. Covariance Weighted Average Module

This module reads the buffered estimates and their corresponding variances to compute a weighted average of the acceleration estimate from two oscillator units, as described in Equation (38). Fixed-point division is performed using Xilinx’s Divider Generator LogiCORE IP employing the radix-2 non-restoring integer division algorithm.

5.5. Implementation Results

The FPGA implementation of the estimation logic is validated using simulated inputs corresponding to the measurement models and the measurements themselves. To ensure accurate representation of values and avoid bit overflows and underflows, the inputs are scaled appropriately to maintain a consistent dynamic range. A MATLAB implementation of the same algorithm serves as a golden reference for comparison. The estimation results from the fixed-point FPGA implementation closely align with MATLAB’s double-precision estimates and the ground truth, as depicted in Figure 11a. Although fewer, finite-precision numerical errors as high as $10\%$ (up to 1 $\mathsf{\mu }$g) are observed in the FPGA output. This is illustrated in Figure 11b. These round-off errors appear to propagate through the numerous matrix operations involved, resulting in a few outlier estimates. Adaptive scaling, a higher number of fractional bits for data representation, and mixed-point (fixed and floating point) implementations are some techniques that could help reduce these errors. Additionally, solving normal equations through numerically robust methods such as Cholesky decomposition should further enhance estimation accuracy and potentially minimize the numerical errors.

5.6. Latency and Resource Utilization

The implementation results are shown in

Table 3. Post-implementation FPGA resource utilization results.

Resource	Available	Utilization
LUTs	$\mathrm{53,200}$	$19\%$
LUTRAM	$\mathrm{17,400}$	$5\%$
Flip-Flops	$\mathrm{106,400}$	$14.78\%$
BRAM	140	$0.71\%$
DSP	220	$85.45\%$

. This represents the implementation of state estimation logic deployed using SoC technology, where covariance-weighted acceleration estimation is implemented on an FPGA coupled to an ARM processor controlling the data flow. The hardware-software co-design required 188 digital signal processing (DSP) elements of the Zynq 7020 FPGA, running at 100 MHz. The low-latency co-design fits within $19\%$ of the available FPGA look-up tables (LUTs), leaving the remaining resources for other potential application requirements such as sensor data processing. DSP usage can be reduced by further pipelining the operations or by strategically using LUTs for multiplications. The hardware-accelerated execution of pipelined acceleration estimation logic has an approximate latency of 3.62 $\mathsf{\mu }$s for an estimation epoch from a dual-oscillator setup.

6. Conclusions

In aerospace applications, system states are filtered on onboard embedded compute elements using measurements from a sensor network. The sensors and embedded flight computing systems are resource-constrained, limiting the precision of stored or transmitted data and consequently impacting the signal-to-noise ratio of the filter output. Accurate state estimation in finite-precision embedded implementations depends on the precision of the measurements and the word lengths of the state and input variables stored on the embedded computer.

This work presents an optomechanical sensor model for estimating forcing accelerations from simulated displacement measurements of a proof mass. The state estimation algorithms are reformulated to incorporate rounding errors into classical estimator frameworks. A least squares estimator, a discrete-time Kalman filter, and a square-root Kalman filter are developed for optimal state estimation with quantized measurement, state, and input variables. Numerical simulations demonstrate that the modified filter frameworks account for finite-precision effects, ensuring proper management of errors and uncertainties in the acceleration estimates. This approach maximizes the performance of filters implemented on fixed-point hardware architectures. Steady-state performance analysis shows that the best possible accuracy achievable by the filter is tightly coupled with the numerical precision of the internal variables. Metrics such as Mahalanobis distance give concrete insights into the word-length versus performance trade-offs.

Additionally, a dual-oscillator system for estimating acceleration states from independent measurements belonging to two oscillator models is proposed for hardware implementation. A covariance-weighted average of independent acceleration estimates is realized on an FPGA-SoC using a finite-precision implementation of the least squares method. The pipelined FPGA realization with simulated model and measurements reasonably tracks a double-precision MATLAB implementation of the same least squares-based estimation. In summary, this article addresses the realization of state estimation on embedded architectures, emphasizing the importance of managing finite word-length implementation errors to design and implement high-performance, resource-efficient estimation algorithms on memory-constrained computing systems.

It is worth noting that while this work thoroughly addresses quantization effects, the potential impacts of bit overflows are neglected. Scaling digital filter realizations to prevent overflow errors remains an avenue for future investigation.