Real-time states estimation of a farm tractor using dynamic mode decomposition

Abstract

We present a pure data-driven method to estimate vehicle dynamics from the measurements of sideslip and yaw rate in the use of GPS and inertial navigation system. The GPS and INS configuration provides vehicle position, velocity vector, vehicle orientation, and yaw rate observations. A new dynamic mode decomposition with control (DMDc) method denoises the state observations by adopting the total least-squares algorithm. The total least-squares DMD with control (tlsDMDc) helps discover the underlying dynamics with the time-dependent observations of states and external control. The experiments of a simulated linear dynamic model with synthetic Gaussian noise illustrate that the solutions of tlsDMDc are more accurate than the standard DMDc to characterize underlying dynamics with imperfect measurements. We additionally investigate how the algorithm performs on vehicle motion deduction and sensor bias correction. It has been shown that the tlsDMDc-based state estimator with the couple of GPS and inertial sensor measurements provides accurate and robust observation in the presence of model error and measurement noise.

Introduction

The automation of agricultural machinery is considered to be one of the most efficient ways of improving productivity and quality of farming. Research and development are focused on developing full autonomy, with the ultimate goal of designing unmanned vehicles or field robots for farming work (Han et al. 2018). To keep an accurate and stable path tracking performance, significant research progress has been made in vehicle control techniques, including navigation sensors, vehicle models, and steering controllers.

There is abundant work on designing path tracking controllers, such as the geometric approach, the kinematic control laws, the optimal control, robust control and model predictive control (MPC), and so on (Backman et al. 2012; Li et al. 2016). Most controller designs often begin with vehicle modeling. The pure pursuit and its extension velocity independent controllers with a simple kinematic model, under nonholonomic constraints, control the motion of a vehicle reasonably well at low speed (< 4.5 m/s) (Wang and Noguchi 2016; Han et al. 2018). Sliding mode control (SMC) approaches with the kinematic models have been studied to overcome the lateral and/or longitudinal slippery (Eaton et al. 2009; Tu et al. 2019). With a two-dimensional dynamics model of a farm tractor, the linear quadratic regulator (LQR) controller provides 4 cm accuracy lateral control to a tractor at speeds up to 8 m/s (Bevly et al. 2002).

The identification of a vehicle model that accurately and efficiently characterize the vehicle dynamics, also known as system identification, is crucial to controller design. The vehicle model requires accurate knowledge of motion state parameters, such as velocity, heading, steering angle from the global positioning system (GPS), an inertial navigation system (INS), or both. However, some parameters, such as vehicle mass and center of gravity (COG), are challenging to measure, especially during the loading or unloading. Also, accurate vehicle state is not readily obtained from simple kinematic or dynamic models based on the Newtonian or Lagrangian. Machine learning is complementary, as it utilizes the measurements of the system to generate models, which can be improved with more data. These models ideally generalize to situations beyond the observed in the training dataset (Kaiser et al. 2018). There are many techniques to obtain data-driven models, including the eigensystem realization algorithm (ERA) and the observer–Kalman filter identification (OKID) for low dimension and linear system, and artificial neural networks (ANNs) for the complex and nonlinear system (Juang et al. 1993; Ma et al. 2011; Wang et al. 2016). Well-trained neuro-controller yields impressive results in vehicle control (Ishii et al. 1994). Regression methods are simple and easy to implement by characterizing the yaw dynamics from steer angle to yaw rate for the tractor at different velocities (Bevly et al. 2002; Wang and Noguchi 2018). However, many leading machine learning methods are limited in real-time modeling nonlinear and high-dimensional systems with disturbance. The dynamic mode decomposition (DMD) is a promising data-driven and equation-free method for identifying a linear or nonlinear dynamical system by time-dependent observations (Tu et al. 2014; Brunton and Kutz 2019). The DMD computes a temporal dynamics model using an Arnoldi-like method that estimates the infinite-dimensional Koopman operator (Dicle et al. 2016). It has been successfully applied to fluid dynamics, video processing, and robotics (Berger et al. 2015; Dicle et al. 2016). Because the Arnoldi algorithm is sensitive to measurement noise, several methods, such as noise-corrected DMD (ncDMD), total least-squares DMD (tlsDMD/TDMD), and extended-Kalman-filter-based DMD (EKFDMD), have been applied to promote DMD’s noise resistibility (Dicle et al. 2016; Dawson et al. 2016; Nonomura et al. 2019). DMD was extended as DMD with control (DMDc) to include actuation inputs and to disambiguate the effect of control and internal dynamics (Proctor et al. 2016). More recently, DMDc has been applied to heavily subsampled measurements of a system (Bai et al. 2020). However, few methods of noise reduction have been proposed for DMDc dealing with noisy measurements.

In this work, the DMD with control approach is applied to characterize the dynamics of a farm tractor. There has been other research on modeling off-road vehicles including the agricultural tractor, but this is the first effort in the context of DMDc. The configuration of measurement sensors is the integration of the real-time kinematic (RTK) carrier phase differential GPS, also termed RTK-GPS, and the inertial navigation system (INS), which is a common technology in an automatic guidance system. First, we present a traditional dynamic model of the tractor and the standard DMDc algorithm. Second, the total least-squares DMDc (tlsDMDc) has been proposed to improve the estimation accuracy of dynamics. The improvement of tlsDMDc is demonstrated in a dynamic corrupted by synthetic Gaussian noise. We additionally investigate how the proposed tlsDMDc performs on vehicle sates estimation and sensor noise correction. The identified vehicle model integrated with GPS measurements through sensor fusion methods, such as Kalman filter (FK) and extended Kalman filter (EKF) (Chindamo et al. 2018; Jin et al. 2019), can provide higher states estimate and also correct accumulative errors of INS in the long run.

A linear vehicle dynamic model

The tractor for analyzation is simplified as a bicycle model (also called the single-track model (Jin et al. 2019)) depicted in Fig. 1. The vehicle model used to describe lateral dynamics is the two-dimensional bicycle model in Kise et al. (2002)

$$\left\{ {\begin{array}{*{20}l} {\dot{X} = { }\left[ {\begin{array}{*{20}c} {\dot{\beta }} \\ {\dot{\omega }} \\ \end{array} } \right] = A_{c} \left[ {\begin{array}{*{20}c} \beta \\ \omega \\ \end{array} } \right] + B_{c} u} \hfill \\ {A_{c} = \left[ {\begin{array}{*{20}c} {{\raise0.7ex\hbox{${ - 2\left( {k_{f} + k_{r} } \right)}$} \!\mathord{\left/ {\vphantom {{ - 2\left( {k_{f} + k_{r} } \right)} {mv}}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${mv}$}}} & {{\raise0.7ex\hbox{${\left[ { - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right) - mv^{2} } \right]}$} \!\mathord{\left/ {\vphantom {{\left[ { - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right) - mv^{2} } \right]} {mv^{2} }}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${mv^{2} }$}}} \\ {{\raise0.7ex\hbox{${ - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right)}$} \!\mathord{\left/ {\vphantom {{ - 2\left( {k_{f} l_{f} - k_{r} l_{r} } \right)} I}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{$I$}}} & {{\raise0.7ex\hbox{${ - 2\left( {k_{f} l_{f}^{2} + k_{r} l_{r}^{2} } \right)}$} \!\mathord{\left/ {\vphantom {{ - 2\left( {k_{f} l_{f}^{2} + k_{r} l_{r}^{2} } \right)} {Iv}}}\right.\kern-\nulldelimiterspace} \!\lower0.7ex\hbox{${Iv}$}}} \\ \end{array} } \right]_{2 \times 2} } \hfill \\ {B_{c} = \left[ {\begin{array}{*{20}c} {2k_{f} /mv} \\ {2k_{f} l_{f} /I} \\ \end{array} } \right]_{2 \times 1} } \hfill \\ \end{array} } \right.$$

(1)

where $\beta$ is the slip angle of the vehicle body, $\omega$ is the yaw rate, and the system input $u$ is the front wheel steering angle $\delta$. ${k}_{f}$, and ${k}_{r}$ are called the cornering stiffness of the front and rear tires, respectively. $m$ is the mass of the vehicle body, $v$ is the vehicle speed in the longitudinal direction. $I$ is the vehicle moment of the inertia around the z-axis. ${l}_{f}$ is the distance of the front wheel from the COG of the vehicle, and ${l}_{r}$ is the distance of the rear wheel from the COG of the vehicle.

Fig. 1

Dynamic model of a vehicle at low speed

Parameters related to the position of COG are not convenient to measure directly as the vehicle size increasing or while loading or unloading. In addition, the lateral tire force is assumed to be proportional to the slip angle. However, if the tires–terrain interactions in the ground vehicles are involved. The assumption will not be valid at large slip angles. In the case of large slip angles, the lateral tire force depends on the tire load, slip angle, tire-road friction coefficient, and so on (Rajesh Rajamani 2006).

Total least-squares dynamic mode decomposition with control

Identifying the dynamic model can be formulated in the discrete-time domain since the control input and state measurement are often implemented at discrete instances in time. Assuming the vehicle state at time $t +\ 1$ depends only on the state and actions from the previous state at time $t$ given by

$$X_{{{\text{t}} + 1}} = \left[ {\begin{array}{*{20}c} {\beta_{{{\text{t}} + 1}} } \\ {\omega_{{{\text{t}} + 1}} } \\ \end{array} } \right] = A\left[ {\begin{array}{*{20}c} {\beta_{t} } \\ {\omega_{t} } \\ \end{array} } \right] + Bu_{t}$$

(2)

The system is assumed to be fully observed, that is, both of the two state-variables are measurable. This section presents the mathematical description of the total least-squares dynamic mode decomposition with control. The DMDc method helps disambiguate the effect of control from internal dynamics and get the numerical solutions of system matrix $A$ and actuation matrix $B$, instead of computing each variable in (1), such as cornering stiffness, inertia moment, and so on.

Dynamic mode decomposition with control

For the standard DMDc algorithm, the first step is to collect a number of pairs of the state and the system inputs as they evolve in time. The discrete-time system with the actuation input can be written in matrix form as

$$\left\{ {\begin{array}{*{20}c} {\begin{array}{*{20}c} {X^{\prime} = AX + BU} \\ {X = \left[ {\begin{array}{*{20}c} \vdots & \vdots & \vdots \\ {x_{1} } & \ldots & {x_{m - 1} } \\ \vdots & \vdots & \vdots \\ \end{array} } \right]_{{n \times \left( {m - 1} \right)}} } \\ \end{array} } \\ {X^{\prime} = \left[ {\begin{array}{*{20}c} \vdots & \vdots & \vdots \\ {x_{2} } & \ldots & {x_{m} } \\ \vdots & \vdots & \vdots \\ \end{array} } \right]_{{n \times \left( {m - 1} \right)}} } \\ {U = \left[ {\begin{array}{*{20}c} \vdots & \vdots & \vdots \\ {u_{1} } & \ldots & {u_{m - 1} } \\ \vdots & \vdots & \vdots \\ \end{array} } \right]_{{l \times \left( {m - 1} \right)}} } \\ \end{array} } \right.$$

(3)

where ${x}_{\mathrm{j}}\in {\mathbb{R}}^{n}, {u}_{\mathrm{j}}\in {\mathbb{R}}^{l}, A\in {\mathbb{R}}^{n\times n}, B\in {\mathbb{R}}^{n\times l}$, $n$ is 2, and $l$ is 1 for a two-dimensional vehicle model in (2). The time-series set of vehicle state observation is referred to as a snapshot. The matrix $X^{\prime}$ is one step forward-shifted from the snapshot matrix $X$. The total number is $m$. Data matrices include the snapshots of vehicle state measurements and control sequence, evenly spaced in the time domain. The time step should be sufficiently small to resolve the high frequencies in the dynamics. In most cases of the vehicle system, neither A nor B is unknown; the dynamics in (3) is recast as

$$X^{\prime} = \left[ {A B} \right]\left[ {\begin{array}{*{20}c} X \\ U \\ \end{array} } \right] = G\Omega$$

(4)

where$\mathrm{G}\in {\mathbb{R}}^{n\times \left(n+l\right)}$, and $\Omega \in {\mathbb{R}}^{(n+l)\times \left(m-1\right)}$ contains both the state and control snapshots. For the case where the number of snapshots is greater than the size of each snapshot, the solution of matrix $\mathrm{G}$ can be obtained by using the least-squares regression to the overdetermined system. The singular value decomposition (SVD) is performed on the augmented data matrix giving $\Omega = U\Sigma V^{*}$. Then, the estimation of matrices A and B can be given by the following

$$\begin{aligned} \left[ {\overline{A}} {\overline{B}} \right] &= X^{\prime} \Omega^{\dag } \\ & = X^{\prime} V\Sigma^{ - 1} U \\ & = \left[ {\begin{array}{*{20}c} { X^{\prime} V\Sigma^{ - 1} U_{1} } & {X^{\prime}V\Sigma^{ - 1} U_{2} } \\ \end{array} } \right] \\ \end{aligned}$$

(5)

where ${\Omega }^{\dag }$ is called the right Moore–Penrose generalized inverse or the pseudoinverse of matrix ${\Omega }$. $U_{1}$ is a $2 \times 2$ matrix from the beginning two rows of matrix $U \in {\mathbb{R}}^{{n \times \left( {n + l} \right)}}$, and $U_{2}$ is the third row of matrix $U$. In this research, the number of snapshots is much larger than observable states, that is, $\left( {m - 1} \right) > \left( {n + l} \right)$. Therefore, only the first $n + l$ columns of $V$ are computed and ${\Sigma }$ is $\left( {n + l} \right)$-by-$\left( {n + l} \right)$. The computational complexity is ${\text{O}}\left( {\left( {n + l} \right)^{2} \left( {m - 1} \right)} \right)$. For large-dimensional systems where $n \gg 1$, it has significant complexity because $X^{\prime }$ will be a very big matrix. In this case, additional SVD is required to find the reduced-order subspace of the matrix $X^{\prime }$ in the original DMDc algorithm (Proctor et al. 2016). This research focuses on the approximation of $A$ and $B$, which have maximum two columns.

Correcting the measurement noise and bias of standard DMDc using the total least-squares method

The above discussion on the vehicle dynamics identification using standard DMDc focuses on the ideal case with precise snapshots. For the over-constrained cases, in which the number of snapshot matrix is greater than that of observables, Eq. (4) gives the minimum Frobenius solution

$$\mathop {\min }\limits_{{G, E_{X^{\prime} } }} \left\| {E_{X^{\prime}} } \right\|_{F} ,subject\;to\;X^{\prime} + E_{X^{\prime} } = G\Omega$$

(6)

where ${\text{E}}_{{{\text{x}}^{\prime } }}$ accounts for the noise estimation in the shifted snapshot data and $||\cdot||_{F}$ denotes the Frobenius norm. Indeed, both the snapshot data ($X$ and $X^{\prime }$) and the control input $U$ are measured with error. In such a case, adopting the classic total least square (TLS) algorithms (Markovsky and Van Huffel 2007), the approximation of (4) can be made equality by accounting for the noise in $X$, $X^{\prime }$, and $U$

$$\mathop {\min }\limits_{{\tilde{G},E_{{X^{\prime } }} ,E_{\Omega } }} \left\| {\begin{array}{*{20}c} {E_{{X^{\prime } }} } \\ {E_{\Omega } } \\ \end{array} } \right\|_{F} ,\,{\text{subject}}\,{\text{to}}\,X^{\prime } + E_{{X^{\prime } }} = \tilde{G}[\Omega + E_{\Omega } ]$$

(7)

where ${\text{E}}_{{\Omega }}$ is an augmented error matrix of $X$ and $U$. Note that the TLS is a simplification of the general approach called mixed adjustment model in geodesy (Leick et al. 2015). It is apparent that the DMDc implicitly neglects the error in ${\Omega }$ and introduces a bias dependent on ${\text{E}}_{{\Omega }}$, but not $E_{{X^{\prime } }}$ . In contrast, the bias is removed by incorporating the errors in both $X^{\prime }$ and ${\Omega }$ as in (7). To solve this minimization problem, the equation can be rearranged as

$$\left[ {\begin{array}{*{20}c} {\tilde{G}} & { - I} \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\Omega + E_{\Omega } } \\ {X^{\prime} + E_{x^{\prime}} } \\ \end{array} } \right] = 0$$

(8)

where $\left[ {\begin{array}{*{20}c} {{\tilde{\text{G}}}} & { - {\text{I}}} \\ \end{array} } \right] \in {\mathbb{R}}^{{n \times \left( {2n + l} \right)}}$ and $\left[ {\begin{array}{*{20}c} {{\Omega } + {\text{E}}_{{\Omega }} } \\ {x^{\prime} + {\text{E}}_{{x{^{\prime}}}} } \\ \end{array} } \right] \in {\mathbb{R}}^{{\left( {2n + l} \right) \times \left( {m - 1} \right)}} .$ For the solution ${\tilde{\text{G}}}$ to be unique, the matrix $\left[ {\begin{array}{*{20}c} {\Omega + E_{\Omega } } \\ {X^{\prime} + E_{x^{\prime}} } \\ \end{array} } \right]$ must have exactly $n + l$ independent rows. Since this matrix has $2n + l$ rows in all, it must be rank deficient by $n.$ Therefore, the optimal solution of (7) can be restated as the goal of estimating the optimal error matrix $\left[ {\begin{array}{*{20}c} {E_{\Omega } } \\ {E_{x^{\prime}} } \\ \end{array} } \right]$ that changes the snapshots matrices $\left[ {\begin{array}{*{20}c} \Omega \\ {X^{\prime}} \\ \end{array} } \right]$ with full-rank $2n + l$ to a “noise-free” matrix $\left[ {\begin{array}{*{20}c} {\Omega + E_{\Omega } } \\ {X^{\prime} + E_{x^{\prime}} } \\ \end{array} } \right]$ with rank $n + l.$ By the Eckart–Young Theorem, the best rank-$\left( {n + l} \right)$ approximation to $\left[ {\begin{array}{*{20}c} \Omega \\ {X^{\prime}} \\ \end{array} } \right]$ can be given by dropping the last $n$ smallest singular value

$$\begin{aligned} \left[ {\begin{array}{*{20}c} {\Omega + E_{\Omega } } \\ {X^{\prime } + E_{{x^{\prime } }} } \\ \end{array} } \right] & = U\Sigma V^{*} \approx \left[ {\begin{array}{*{20}c} {U_{{11}} } & {U_{{12}} } \\ {U_{{21}} } & {U_{{22}} } \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\Sigma _{{1:\left( {n + l} \right)}} } & 0 \\ 0 & 0 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {V_{1}^{*} } \\ {V_{2}^{*} } \\ \end{array} } \right] \\ & = \left[ {\begin{array}{*{20}c} {U_{{11}} \Sigma _{{1:\left( {n + l} \right)}} V_{1}^{*} } \\ {U_{{21}} \Sigma _{{1:\left( {n + l} \right)}} V_{1}^{*} } \\ \end{array} } \right] \\ \end{aligned}$$

(9)

The computational complexity of the truncated SVD is ${\text{O}}\left( {\left( {n + l} \right)^{2} \left( {m - 1} \right)} \right)$, which is the same as the original DMDc in (5). Combing (8) and (9), the total least-squares estimation of ${\tilde{\text{G}}}$ is given by

$${\tilde{\text{G}}} = U_{21} U_{11}^{ - 1}$$

(10)

where $U_{11} \in {\mathbb{R}}^{{\left( {n + l} \right) \times \left( {n + l} \right)}}$ and $U_{21} \in {\mathbb{R}}^{{n \times \left( {n + l} \right)}}$. If the $U_{11}$ is invertible, the matrix $A$ of a two-dimensional vehicle model is the beginning two rows of the matrix ${\tilde{\text{G}}}$ and the matrix $B$ is the third row.

Experiments and discussion

The following section describes a series of examples for the evaluation of the proposed tlsDMDc method. The input–output dynamics of the tractor are expected to be identified in real time since the vehicle states and dynamics are time variant. Experiments start from simulated vehicle dynamics and additional noise with known characteristics. We analyze how the number of snapshots, the strength of the input signal, and the noise level affect the performance of both tlsDMDc and the standard DMDc. Other experiments are to identify the dynamics of a farming tractor using the tlsDMDc and show its potential in applying vehicle navigation.

Vehicle dynamics simulation

The parameters for simulating the dynamics of a farming tractor are listed in Table 1. The definitions of the listed variables are illustrated in Fig. 1. The values indicating the characteristics of the vehicle are observed by other researchers (Bevly et al. 2002).

Table 1 Approximate parameters for the vehicle model

According to (1), the dynamic matrix $A$ has discrete time eigenvalues $\lambda = e^{{\left( { - 0.39 \pm 0.13i} \right)\Delta t}}$, with the time step $\Delta t = 0.1$ s. Note that the system is slightly damped. We stimulate the system with the input $U = 3 \times \sin t$, with $t \in \left[ {0, 9.9} \right]$. A total of 100 snapshots of vehicle states are collected from the initial condition $X_{0} = \left[ {\begin{array}{*{20}c} 0 & 0 \\ \end{array} } \right]^{T}$. The identified eigenvalues of the matrix $A$ from both standard DMDc and the tlsDMDc are shown in Fig. 2, for 1000 trials corrupted with 40 dB signal to noise ratio (SNR) white Gaussian noise. Note that only the positive complex eigenvalues with the 95% confidence ellipse are shown here. Results in Fig. 2 show that the distribution of estimated eigenvalues from tlsDMDc is more concentrated on the true eigenvalue, which means that tlsDMDc works better than DMDc in correcting the bias of the estimated eigenvalues.

Fig. 2

Calculated eigenvalues of matrix ${\text{A}}$ from 1000 trials. Only the positive complex eigenvalues with the 95% confidence ellipse are shown

The results for the noise-corrupted measurements when adjusting the SNR are discussed. The input is $U = 3 \times \sin t$. A total of 70 snapshots are collected from the initial condition $X_{0} = \left[ {\begin{array}{*{20}c} 0 & 0 \\ \end{array} } \right]^{T}$. As shown at the top panel of Fig. 3, rather than checking the error in eigenvalues, we instead consider the Frobenius norm of the difference between the true and the identified matrices, $||A - \bar{A}||_{F}$ and $||B - \bar{B}||_{F}$ (Dawson et al. 2016). For large values of SNR, both algorithms can identify vehicle dynamics precisely. The accuracy of the standard DMDc algorithm degrades with the increase in noise level. The mean and 95% confidence ellipse of 1000 trails, instead of individual data points of identified eigenvalues of matrix $A$, are given for each dataset at the bottom panel in Fig. 3. For 60 dB SNR, DMDc and tlsDMDc give little difference in the mean and area of 95% confidence ellipse. As the noise level increases, the mean of eigenvalues identified from DMDc deviates from the true value. On the contrary, the tlsDMDc gives bias-free eigenvalues estimates, represented by the center of 95% confidence ellipse coinciding with the true eigenvalue.

Fig. 3

Errors in the estimated vehicle dynamics on noisy data while changing the noise level. In the top panel, the errors arising from performing DMDc and tlsDMDc are given as $||A - \bar{A}||_{F}$ and $||B - \bar{B}||_{F}$, respectively. The bottom panel shows the mean and 95% confidence ellipses with the positive imaginary part

The effects of the number of snapshots and the amplitude of control input are investigated. The control input is $U = 5 \times \sin t$. These errors are evaluated by 1000 trails with different random seeds. In Figs. 4 and 5, the errors in the estimated matrices $A$ and $B$, and estimated eigenvalues of matrix $A$ for DMDc and tlsDMDc are calculated by changing the number of snapshots and input amplitude, respectively. The top panel in Fig. 4 shows the error decays, while the number of snapshots increases. The error of estimated matrices $A$ and $B$ decays faster with small numbers of snapshots. The reason is that the system has not yet completed a full period of oscillation. As the number of snapshots increases, internal dynamics are fully observed and so are the error of the estimate decays. However, the error saturation for the DMDc can be observed clearly in the bottom panel of Fig. 4. On the contrary, the tlsDMDc can prevent the error saturation present in DMDc for all trails. The bottom panel of Fig. 5 exhibits the same saturation phenomena as the increase in input signal magnitude. Further to this, the top panel of Fig. 5 shows the error decays with the increase in amplitude of the input signal. For the small input signal, the system is not well excited, so the state is overwhelmed by the noise when the ${\text{U}} = 2 \times \sin t$ as shown in the bottom panel of Fig. 5. In both Fig. 4 and Fig. 5, the distribution of estimated eigenvalues from tlsDMDc is more concentrated to the mean of the identified values that the results from standard DMDc. It indicates that the tlsDMDc is more likely to attain a closer approximation to the true eigenvalue than the DMDc method on the same dataset.

Fig. 4

Effects of the number of snapshots on errors in estimated matrices $A$ and $B$ and eigenvalues for the vehicle system with 30 dB measurement noise

Fig. 5

Effects of the control signal’s amplitude on errors in estimated matrices $A$ and $B$ and eigenvalues for the vehicle system with 30 dB measurement noise

The above discussions on the noise-induced linear dynamic system reveal that both the DMDc and tlsDMDc can estimate the vehicle dynamics, especially in cases for which the number of snapshots is large enough and sensor noise is minimal. Also, tlsDMDc yields an unbiased characterization of the vehicle system in the context of noisy measurements, whereas the error saturates when DMDc is used.

To further assess the performance and capabilities of predictive modeling, tlsDMDc is compared with ANN, which is a sophisticated data-driven state estimation method (Jin et al. 2019). Three typical NN architectures are selected in this research, that is, the multilayer perceptron (MLP), long short-term memory (LSTM), and nonlinear autoregressive neural network with external input (NARXNET). Boussaada et al. (2018) explain the architecture of the NARXNET in detail and apply the model in solving prediction issues. The trained inputs are vehicle states $x_{{\text{t}}}$ and control signal $u_{{\text{t}}}$. Outputs are the estimates of vehicle states at next time step, $x_{{{\text{t}} + 1}}$. The MLP has 2 hidden layers of 5 neurons at each layer. The hidden layers use the log-sigmoid transfer function, while the output layer uses the linear transfer function. The LSTM model has 27 hidden units. The NARXNET has 10 hidden units, and its input and feedback delays are set as 2. The input is $U = 5 \times \sin t$. The number of collected snapshots ranges from 30 to 100 corrupted with 30 dB SNR Gaussian noise. The accuracy of estimated vehicle states is measured by mean square error (MSE). The MSE listed in Table 2 is scaled by $10^{4}$ for comparison. NN models cannot perform as stable as tlsDMDc with limited training data. The reduction of MSE following the increasing snapshots reveals that a comprehensive training dataset is necessary for the NNs to obtain a good estimation. Kaiser et al. (2018) have proven that the training time required by a DMDc model is two orders of magnitude less time than that of NN models. But the training time of a DMDc model increases slightly with the amount of training data. This means that tlsDMDc is more suitable for identifying a vehicle’s internal dynamics with limited state measurement.

Table 2 Comparison of NNs and tlsDMDc with different training datasets

Application of tlsDMDc for real-time vehicle state estimation

In this section, tlsDMDc is applied to estimate the dynamics of a real farming tractor based on the measurements from an RTK-GPS and an inertial measurement unit (IMU). The RTK–GPS receiver (Trimble SPS855, Trimble Navigation, California, USA) is used to measure vehicle position to centimeter accuracy. The IMU (VN100, VectorNav Technologies, USA) is used to measure the heading angle ($\varphi$) and yaw rate (${\upomega }$) of the tractor. The heading accuracy of the IMU is 2.0 deg. root mean square (RMS) with 5–7 deg/hr. bias instability. Integrating the vehicle model and GPS/INS, we intend to improve the heading measurement accuracy and limit the drift error of the heading angle. The following experiments are conducted at a farm at Hokkaido University using a wheel-type tractor with a rotary. The performance of tlsDMDc is evaluated by the error of reconstructed and predicted states. Another experiment is to further explore its potential usage in navigation by integrating the identified vehicle model and sensor measurement.

Vehicle states reconstruction and prediction using an identified vehicle model from tlsDMDC

Figure 6 shows the time histories of steering angle and vehicle speed during the experiment. The limitation of the steering angle is $\pm 30$ deg. (negative sign implies turning left). Anderson and Bevly (2010) estimate the vehicle sideslip β from the difference between the speed direction from GPS ($\varphi_{GPS}$) and the heading of the vehicle from IMU ($\varphi_{IMU}$)

$${\upbeta } = \varphi_{GPS} - \varphi_{IMU}$$

(11)

Fig. 6

Time series steering angle and vehicle speed

Calculated slip angles and measured yaw rates from the IMU are shown in Fig. 7. The time interval of each measurement is $\Delta t = 0.2$ s. The number of snapshots is 80, that is, we collect the previous 16 s data, i.e., the duration of 31–47 s before the red vertical line in Fig. 7, for identifying the vehicle dynamics. Using the identified vehicle model, we reconstruct the vehicle states and predict the states from 47 s to the end of the experiment, as shown in Fig. 7. The results from an MLP with 2 hidden layers of 5 neurons, an LSTM network with 27 hidden neurons, and a NARXNET with 10 hidden neurons are also figured as comparisons. All the methods can reconstruct vehicle states. Among them, NARXNET, LSTM, DMDc, and tlsDMDc fit the measurements of yaw rate and slip angle well. For all the methods, except for DMDc, the slip angle error becomes significant at the prediction period. One reason is that the magnitude of the slip angle is so small as to be easily overwhelmed by the measurement noise. The other reason is that the vehicle dynamics in (1) is related to the vehicle speed. However, speed and vehicle dynamics are simply assumed to be constant during prediction. The accuracy of estimated vehicle states listed in Table 3 is all measured by MSE for comparison. Using the limited training data, which is 80 in this experiment, NN models cannot get stable state estimation like tlsDMDc or DMDc. Note that the performance of the NN is highly affected by the initial network weights.

Fig. 7

Time histories of measured and estimated vehicle states, slip angles in the top panel, and yaw rates in the bottom panel. The measurements before the red vertical line are for training, which means the number of snapshots for system identification is 80

Table 3 Error analysis of state estimation using different methods

Precise vehicle state estimations can be used to update position and orientation estimates between GPS measurement updates for vehicle navigation. Especially during the short GPS outages periods, the dead reckoning method is widely used to deduce the vehicle position and heading following (Schubert et al. 2011)

$$\left[ {\begin{array}{*{20}c} x \\ y \\ \varphi \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x_{0} + \sum \frac{v}{{\upomega }}\left[ {\cos \left( {\varphi + \beta } \right) - \cos \left( {\varphi + \beta + {\upomega }\Delta t} \right)} \right]} \\ {y_{0} + \sum \frac{v}{{\upomega }}\left[ {\sin \left( {\varphi + \beta + {\upomega }\Delta t} \right) - \sin \left( {\varphi + \beta } \right)} \right]} \\ {\varphi_{0} + \sum {\upomega }\Delta t} \\ \end{array} } \right]$$

(12)

when the instant yaw rate ${\upomega } = 0$, Eq. (12) can be simplified as

$$\left[ {\begin{array}{*{20}c} x \\ y \\ \varphi \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x_{0} + \sum v\sin \left( {\varphi + \beta } \right)\Delta t} \\ {y_{0} + \sum v\cos \left( {\varphi + \beta } \right)\Delta t} \\ {\varphi_{0} + \sum {\upomega }\Delta t} \\ \end{array} } \right]$$

(13)

The estimated trajectory and vehicle heading using the dead reckoning method are depicted in Fig. 8. Ground truth of the vehicle position and orientation is measured by an RTK-GPS. The estimated vehicle trajectories and heading angles deviate from the ground truth because of accumulated errors derived from the dead reckoning method. The lateral deviation of each method is summarized in Table 4. The positioning accuracy is consistent with the accuracy of estimated states in Fig. 7. The tlsDMDc gets better position estimation than the DMDc because its yaw rate estimate is more accurate than the DMDc’s. Moreover, position accuracy is highly affected by the yaw rate as shown in (12). Therefore, we can partially conclude that DMD-based system identification methods, DMDc and tlsDMDc, surpass NN models in this situation, which is with low-speed movement and limited system observation.

Fig. 8

Comparison of ground truth and estimated vehicle trajectories in the top panel and orientation in the bottom panel

Table 4 Average positioning error of each method at reconstruction and prediction period

Integration of GPS/INS and vehicle model with sensor fusion methods

This section presents the integration of the GPS and INS measurements with state estimate from the tlsDMDc-based vehicle model to eliminate accumulative errors in vehicle position and orientation deduction. The scheme of the sensor fusion method is shown in Fig. 9. The system input includes the steering angle and speed. Measurements include vehicle speed, yaw angle and yaw rate from the INS, and position and yaw angle from GPS. Because of the low update rate and high measurement noise, the yaw angle from GPS is only used for calculating the slip angle. KF and EKF are two simple and typical sensor fusion methods that integrate the vehicle model with GPS and INS measurement. A KF built on the identified vehicle model can process significant estimation errors. The standard KF combines measurements and estimations of parameters with Bayesian inference and iterates over the two probabilistic process steps, which are the prior prediction step and the posterior update step (Thrun et al. 2006; Anderson and Bevly 2010). Further, to eliminate the cumulative errors in the standard dead reckoning method, an EKF is adopted to improve the position and orientation estimation based on the position measurement from GPS and heading angle from IMU. In EKF, we approximate the nonlinear dead reckoning transformation function (13) by a Jacobian matrix $J$

$$J_{k} = \left[ {\begin{array}{*{20}c} 1 & 0 & {v\cos \left( {\varphi_{k} + \beta_{k} } \right)\Delta t} \\ 0 & 1 & { - v\sin \left( {\varphi_{k} + \beta_{k} } \right)\Delta t} \\ 0 & 0 & 1 \\ \end{array} } \right]$$

(14)

Fig. 9

Implement of tlsDMDc for online vehicle state estimate

The measurement update step in EKF is the same as the KF. The posterior update step gives a better estimation of the position and orientation, $\widehat{{Z_{k} }}$.

When GPS and INS are available, we can identify and update the vehicle model based on the real-time measurements. Note that the vehicle speed is not constant and the vehicle dynamics are time variant. Therefore, the identified model update rate should be adjusted to get a better vehicle state estimation. The number of snapshots is set as 30 for real-time system identification, and the vehicle model updates every sampling time interval. The overall procedure is termed “EKF + tlsDMDc” in this research. When the GPS or INS measurements are not available, the observation matrices of EKF and KF are then set to zero, and integration of the inertial sensors and the experimental vehicle model is performed to update the state estimation.

The results of Fig. 8 are repeated with the integration of GPS and INS measurement and vehicle model with the sensor fusion method. The MSE of state estimation in Table 5 is as small as 0.05. We use the filtered states and estimate vehicle position using (12) and (13). The average positioning error is 0.35 m, which is caused by accumulated state estimation and speed measurement errors.

Table 5 Accuracy of positioning and state estimation with and without sensor fusion

The field experiment is conducted once again using the same platform and sensors as the previous section to evaluate the accuracy improvement of applying senor fusion methods to the tlsDMDc-based vehicle model. The steering angle and vehicle speed are shown in Fig. 10. Note that the vehicle speed is not constant. In the top panel of Fig. 11, the heading angles depicted in the blue and orange lines are measurements from GPS and IMU, respectively. The update rate of GPS is not high enough to measure the yaw angle during 90˚ turns. Also, the estimated position deriving from the IMU measurement deviates from the reference trajectory shown in the bottom panel of Fig. 11. On the contrary, the EKF + tlsDMDc eliminates IMU measurement bias and gives a smooth heading estimate during turning, such as the dashed line in time interval for 144–150 s and from 206 to 209 s in the top panel of Fig. 11. In addition, accumulative positioning errors are neglectable during the 230 s term of experiment with continuous curves. Accurate positioning results in the bottom panel of Fig. 11 proves that integrating the tlsDMDc with GPS and INS measurement can correct the measurement noise and improve the positioning accuracy, even during the turning or continuous curve path. The previous two experiments show the effectiveness of the sensor fusion method in correcting the yaw measurement drift and vehicle state estimation.

Fig. 10

Time series steering angle and speed of the tractor

Fig. 11

Reconstructed vehicle heading and vehicle position using (12) and (13)

Conclusions

In this work, the DMDc approach is applied to characterize the input–output model of an agricultural tractor and to estimate vehicle states of slip angle and yaw rate in real time. The DMDc requires less amount of data for the training, compared with other machine learning methods, such as neural networks. It is essential to perform system identification online with noisy or imprecise snapshot measurements, which is often important for state prediction and control purposes.

We analyze that DMDc is biased to sensor noise and present a total least-squares-inspired algorithm to remove the bias of DMDc. The solution of the total least-squares method minimizes errors in both the dependent and independent variables. A simulated linear vehicle dynamic system with additional noise demonstrates how the bias of DMDc depends on the size of snapshots, strength of the input signal, and the noise level. Also, future system behavior forecasts based on the identified model from tlsDMDc are more representative than those based on standard DMDc models. Experiments on the noise-induced linear dynamic system reveal that both the DMDc and tlsDMDc can estimate the vehicle dynamics, especially in situations for which the number of snapshots is large enough and sensor noise is minimal. In addition, tlsDMDc yields an unbiased characterization of the vehicle system in the context of noisy measurements, whereas the error saturates when using DMDc.

While the demonstration of tlsDMDc on a linear system showcases the advantages of the unbiased formulation over standard DMDc, the tlsDMDc shows its potential in the application of vehicle navigation. Moreover, it outperforms NNs, such as MLP, LSTM, and NARXNET, in prediction accuracy and robustness at the limited measurement conditions. It has been proved that the tlsDMDc-based state estimator with GPS and INS measurements provides accurate observation in the presence of model error and noise.

The standard DMDc and proposed tlsDMDc both become ill-conditioned in the case of identifying dynamics with feedback control. If the actuation corresponds to feedback states, it is impossible to decouple the effect of actuation from internal dynamics. Therefore, the identification of actuation as a function of the system state will be explored in further research. Further exploration also includes enforcing known physics in the tlsDMDc method to improve the robustness of the model. Based on the estimate of vehicle states as well as the vehicle dynamics, diagnosing unpredicted disturbance and forecasting harmful conditions of a vehicle are also meaningful applications related to vehicle safety.