Nonlinear Constrained Moving Horizon Estimation Applied to Vehicle Position Estimation

Abstract

The design of high–performance state estimators for future autonomous vehicles constitutes a challenging task, because of the rising complexity and demand for operational safety. In this application, a vehicle state observer with a focus on the estimation of the quantities position, yaw angle, velocity, and yaw rate, which are necessary for a path following control for an autonomous vehicle, is discussed. The synthesis of the vehicle’s observer model is a trade-off between modelling complexity and performance. To cope with the vehicle still stand situations, the framework provides an automatic event handling functionality. Moreover, by means of an efficient root search algorithm, map-based information on the current road boundaries can be determined. An extended moving horizon state estimation algorithm enables the incorporation of delayed low bandwidth Global Navigation Satellite System (GNSS) measurements—including out of sequence measurements—as well as the possibility to limit the vehicle position change through the knowledge of the road boundaries. Finally, different moving horizon observer configurations are assessed in a comprehensive case study, which are compared to a conventional extended Kalman filter. These rely on real-world experiment data from vehicle testdrive experiments, which show very promising results for the proposed approach.

1. Introduction

Many demanding mechatronic systems, like the German Aerospace Center’s (DLR’s) ROMO (short for ROboMObil—DLR’s robotic electric vehicle) [1], employ state dependent nonlinear optimization-based control (e.g., [2]), which needs an accurate knowledge of the system states. Often, these states cannot be gathered directly through sensors, as an appropriate measurement principle for the searched quantity is not available (e.g., determination of the state of charge of a battery), or the sensor is expensive and therefore it is desirable to be economized (e.g., vehicle over ground velocity sensing). Especially for future autonomous vehicles, it is necessary to determine an accurate vehicle position so as to guarantee a reliable vehicle path/trajectory generation, and to follow control functionality cf. [2,3,4].

Here, a novel nonlinear position estimator, relying on a moving horizon estimator approach, which fuses inertial navigation system (INS) and Global Navigation Satellite System (GNSS) sensor data based on a model-based observer framework [2] is proposed. This framework relies on multiphysical prediction model design in Modelica [5], and an automated tool chain to incorporate these by means of the Functional Mockup Interface (FMI) [6] technology, with well proven nonlinear Kalman filter-based algorithms (cf. e.g., [2,7]).

State of the Art in GNSS/INS Coupled Estimation

The topic of INS- and GNSS-based sensor fusion for different applications in ground or air vehicles was a large scope in the research of the last decades. A quiet comprehensive overview of methods such as dead reckoning can be found in the literature, for example, [8,9,10]. Moreover, in the literature [11], time delayed sensor fusion for sigma point Kalman filters (SPKF) is presented. In this approach, the system state is augmented with an $N$ samples delayed state, and a cross correlation of the lagged measurement with the covariance at $t_{k-N}$ and with the covariance of the actual time instance $t_k$ is performed. In the literature [12], the inertial and magnetometer measurements are delayed to correspond with the GNSS data, and then fused in an observer, resulting in delayed estimates. The current estimates are afterwards calculated integrating the IMU data with the delayed estimates as the start values. Both approaches try to incorporate the delayed information of the GNSS data with a postponed prediction [12] or correction [11].

On the contrary, here, a methodology is proposed that incorporates all of the past, delayed data in every estimation step by means of a nonlinear multiphysical vehicle prediction model and a real-time capable nonlinear moving horizon estimation (MHE) algorithm. Additionally, the knowledge of the road boundaries, for example, provided by a map system with a virtual horizon, are incorporated so as to restrict the vehicle position within the carriage way. A different approach incorporating a direct collocation technique in the MHE formulation was used for the estimation of an aircraft trajectory in the literature [13]. On the contrary, in [14] a Gauss–Newton method was used to solve the nonlinear least-squares problem of the MHE formulation of a Global Positioning System (GPS) and dead reckoning integrated navigation system, by linearizing the optimization problem along the previous state estimation in each iteration. Another method to compensate the missing data of the slow sampled measurements in a (linear) MHE formulation with bounded measurement noise is proposed by the authors of [15], by means of the introduction of prediction values in the missing data gaps. The interested reader can find examples of the recent research advances in the development of new moving horizon estimation techniques in the literature [16,17].

2. The Observer Setup–Problem Formulation of the MHE-Based GNSS/INS Fusion

In a realistic application with a consumer quality GPS sensor, a latency between 50–1000 ms [11,12] is usually considered. This implies a drastic delay in comparison to other in-vehicle sensor systems, for example, the inertial measurement unit (IMU). Here, this fact motivates the discussed observer example with highly delayed measurements in a vehicle position estimation application.

For the experimental evaluation of the constrained state estimation of DLR’s ROboMObil, several test campaigns were carried out at the ADAC (General German Automobile Club) vehicle testing ground in Kempten, Germany. Here, tests of an advanced version of the interactive vehicle path following control (PFC) [2,3] have been performed. In the top left of Figure 1, a portrait of the testing track is shown. The green line depicts the preplanned vehicle path, while the road boundaries are represented by the dotted-orange lines.

The ground-truth data was gathered with help of a differential GPS in order to guarantee high fidelity vehicle state measurements for the experimental validation. In Figure 2, the situation of the MHE estimator proposed here with delayed measurements is depicted. At the current time instance $t_k$, the estimator receives the measurements of the current vehicle yaw rate state ($y_k={\dot{\psi }}_{\mathrm{IMU}}^{\mathrm{C}}$) and the delayed measurements (${\mathit{y}}_k^{*}={\mathit{p}}_{{\mathrm{C}}_{\mathrm{GPS}}}^{\mathrm{I}}=\left\{x_{{\mathrm{c}}_{\mathrm{GPS}}}^{\mathrm{I}},y_{{\mathrm{c}}_{\mathrm{GPS}}}^{\mathrm{I}}\right\}$), which are delayed for $n_{\mathrm{d}}$ samples, and thus belong to the past vehicle state (${\mathit{x}}_{k-n_{\mathrm{d}}}$). Later, it will be shown that the incorporation of these delayed measurements in a conventional one-step estimation approach (i.e., a Kalman filter) leads to a poor performance or even instability.

2.1. The Extended Single Track Prediction Model

As the scope of the proposed observer (as implied in the introduction of this section) lies on the observation of the actual vehicle states, ${\mathit{p}}_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}},{\psi }_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}, {\mathit{v}}_{\mathrm{act}}^{\mathrm{C}}, {\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}$ for path following controllers in autonomous vehicles, as proposed by the authors of [2], an observer synthesis model reproduction depth has been chosen that meets the requirements of the computational efficiency and fidelity. An extended single track model (ESTM) was chosen for the forward driving operation mode $v\ge 0$, which incorporates vehicle standstill functionalities, rolling resistance, drag forces, and Pacejka’s Magic Formula [18] lateral characteristics of the tires. The following nomenclature is defined (cf. Figure 3):

• ${(·)}^{\mathrm{C}}$: Quantity expressed in the car coordinate system with origin in the center of gravity
• ${(·)}_{\mathrm{C}}$: Car quantity expressed in the inertial coordinate system—short for ${(·)}_{\mathrm{C}}^{\mathrm{I}}$

The details of the extended single track model equation derivation are given in the Appendix A. It is worth mentioning that with the Modelica modeling technology, it was easily possible to integrate “if-else” constructs, which are efficiently processed by the event handling features of the compiler, while experimenting with varying discretization methods and sample rates, as the model is in continuous-time formulation. The vehicle state vector is denoted as follows, and is graphically exemplified in Figure 3:

$${\mathit{x}}^{\mathrm{C}}=\left\{{\beta }^{\mathrm{C}},v^{\mathrm{C}},{\dot{\psi }}^{\mathrm{C}} ,{\psi }_{\mathrm{C}},x_{\mathrm{C}},y_{\mathrm{C}}\right\}$$

(1)

2.2. Road Boundaries Constraint Formulation and Evaluation

In this section, a methodology is described for incorporating advance-known street boundaries (e.g., from digital maps in combination with vision sensors) into the vehicle position estimation. It is assumed that the initial vehicle position (${\mathit{p}}_{\mathrm{C}}^{\mathrm{I}}$) is sufficiently precisely known, and the corresponding path parameter ($s^{*}$) (cf. Equation (2)) can be determined by means of a time independent path interpolation (TIPI) [2,3], which is explained in the following.

Ideally, the actual path position and the position of the car coincide, ${\mathit{p}}_{\mathrm{P}}\left(s^{*}\right)={\mathit{p}}_{\mathrm{C}}$ (cf. Figure 4). To approximate this condition, it is necessary to minimize the displacement ($\mathit{e}\left(s\right)$) between the vehicle and reference path, as depicted in the following:

The geometrical interpretation of this minimization objective is that ${\mathit{p}}_{\mathrm{P}}\left(s^{*}\right)$ can be determined by projecting ${\mathit{p}}_{\mathrm{C}}$ orthogonally on the path ${\mathit{p}}_{\mathrm{P}}\left(s\right)$. For the TIPI this condition implies that the inverse of the maximal curvature of the demanded vehicle path defines the maximum lateral displacement for which $s^{*}$ exists: $e_y^{\mathrm{P}}\left(s^{*}\right)\le 1/{\kappa }_{\mathrm{P}}\left(s^{*}\right).$

$$s^{*}=\underset{s}{\mathrm{argmin}}{\Vert \underset{\mathit{e}\left(s\right)}{\underbrace{{\mathit{p}}_{\mathrm{P}}\left(s\right)-{\mathit{p}}_{\mathrm{C}}}}\Vert }_{\mathbf{2}}$$

(2)

The interested reader can find details on how this optimization problem is solved in the literature [3].

The parametric path ${\mathit{\lambda }}_{\mathrm{c}}\left(s\right)$ contains the following quantities for the calculation of the road boundaries constraints: the position $x_{\mathrm{P}}^{\mathrm{I}}\left(s\right)$ and $y_{\mathrm{P}}^{\mathrm{I}}\left(s\right)$ of the road middle lane, the positions of the left ($l_x^{\mathrm{I}}\left(s\right),l_y^{\mathrm{I}}\left(s\right))$ and the right ($r_x^{\mathrm{I}}\left(s\right),r_y^{\mathrm{I}}\left(s\right)$) border, and the corresponding path orientation ${\psi }_{\mathrm{P}}^{\mathrm{I}}\left(s\right)$ and its curvature ${\kappa }_{\mathrm{P}}\left(s\right)$.

The extension of the vehicle prediction model (cf. Appendix A) with the roadway boundaries interpolation yields an extra state ($\dot{s}$), which is a non-physical state that belongs to the estimation task. The experimental tests considering $\dot{s}$ as an estimated state showed that this configuration leads to unsatisfactory results in the overall observer performance. Therefore, this state is separated from the state correction step in the Kalman algorithms (cf. Section 3.2).

In Figure 5, the calculation of the roadway border constraints is graphically shown. By means of the above described algorithm, a path parameter ($s_i$) can be found for which the longitudinal derivation error of the vehicle’s actual position (${\mathit{p}}_{\mathrm{C}}^{\mathrm{I}}$) tends to zero.

By means of the path normal vector (${\mathit{n}}_{\mathrm{P}}^{\mathrm{I}}\left(s_i\right)$), represented in the inertial coordinate system (Equation (2)), an inequality function $\mathit{c}\left(\mathit{x}\right)\le 0$ is calculated that penalizes the positions outside of the roadway borders, as follows:

$${\mathit{n}}_{\mathrm{P}}^{\mathrm{I}}\left(s_i\right)=\left\{-\sin \left({\psi }_{\mathrm{P}}^{\mathrm{I}}\left(s_i\right)\right),\cos \left({\psi }_{\mathrm{P}}^{\mathrm{I}}\left(s_i\right)\right)\right\}$$

(3)

$$\mathit{c}\left(\mathit{x}\right)=\left[\begin{array}{cc}-l_x^{\mathrm{I}}\left(s_i\right)+x_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}& -l_y^{\mathrm{I}}\left(s_i\right)+y_{\mathrm{C}}^{\mathrm{I}}{}_{\mathrm{act}}\\ r_x^{\mathrm{I}}\left(s_i\right)-x_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}& r_y^{\mathrm{I}}\left(s_i\right)-y_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}\end{array}\right]\cdot {\mathit{n}}_{\mathrm{P}}^{\mathrm{I}}\left(s_i\right)$$

(4)

This nonlinear inequality function, $\mathit{c}\left(\mathit{x}\right)$, can be handled directly by a posteriori constraint projection method for an (extended) Kalman filter. Details on the simplified Newton descent search algorithm used here are given in Appendix B. For the real-time capable moving horizon estimation algorithm in Section 3. it is necessary to linearize $\mathit{c}\left(\mathit{x}\right)$ at all of the time instances $t_k$, where it is likely that a constraint may be violated by the estimator $\mathit{c}\left({\mathit{x}}_k\right)>-ϵ$, as follows:

$$\mathit{c}\left(\mathit{x}\right)\cong \mathit{c}\left({\mathit{x}}_k\right)+{\frac{\partial \mathit{c}}{\partial \mathit{x}}|}_{{\mathit{x}}_{\mathit{k}}}\cdot \left(\mathit{x}-{\mathit{x}}_k\right)$$

(5)

By rearranging Equation (4), the linearized inequality description in Equation (5) can be formulated as follows:

$${\mathit{C}}_k\cdot \mathit{x}\le {\mathit{d}}_k \stackrel{\mathrm{yields}}{\to }\underset{{\mathit{C}}_k}{\underbrace{{\frac{\partial \mathit{c}}{\partial \mathit{x}}|}_{{\mathit{x}}_{\mathit{k}}}}}\cdot \mathit{x}\le \underset{{\mathit{d}}_k}{\underbrace{{\frac{\partial \mathit{c}}{\partial \mathit{x}}|}_{{\mathit{x}}_{\mathit{k}}}\cdot {\mathit{x}}_k-\mathit{c}\left({\mathit{x}}_k\right)}}$$

(6)

Figure 6 shows an example for the calculation of the nonlinear constraint function. In the left plot, a street is marked with the left $\mathit{l}\left(s\right)$ and the right $\mathit{r}\left(s\right)$ street boundaries, while the car (red line with direction arrows) crosses the right boundary in the hairpin curve. This leads to a violation of the right border constraint condition $c_1\left(\mathit{x}\right)>0, \forall t\in \left[19.6 22.1\right] \wedge$ $\left[29.4 30\right]$, as can be seen in the right plot of Figure 6.

Assuming that only the vehicle yaw rate (${\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}$) is available to the (extended) Kalman filter, the position estimate would drift away, as shown in Figure 7 dark green line). Making use of the proposed boundary estimation approach used here, in combination with the inequality handling feature from the author of [2], yields a bounded and valid result (light green line), as follows:

3. Real-Time Nonlinear Moving Horizon Estimation

In this section, a nonlinear moving horizon estimator (MHE), which utilizes multiple past measurements to estimate the state at the current time instance, is introduced. In addition to the state constraints, delayed measurements can be directly incorporated into the problem formulation.

Referring to the literature [19], the approach of the moving horizon estimation is the reformulation of the general optimization objective of the Kalman filter theory, also known as the full-information filter. In the nonlinear case, the minimization problem can be written as follows:

$$\begin{array}{l}{\displaystyle \underset{{\mathit{\xi }}_k}{\min }\parallel {\mathit{x}}_0-{\widehat{\mathit{x}}}_0{\parallel }_{{\mathit{I}}_0^{+}}^2+{\displaystyle \sum }_{i=1}^N\parallel {\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right){\parallel }_{{\mathit{R}}^{-1}}^2+{\displaystyle \sum }_{i=0}^{N-1}\parallel {\mathit{x}}_{i+1}-\mathit{f}\left({\mathit{x}}_i,{\mathit{u}}_i\right){\parallel }_{{\mathit{Q}}^{-1}}^2}\\ {\mathit{\xi }}_k={\left({\mathit{x}}_0^T, {\mathit{x}}_1^T,\dots ,{\mathit{x}}_N^T\right)}^T\end{array}$$

(7)

where in the nomenclature of $\underset{\mathit{x}}{\min }\Vert \mathit{e}{\left(\mathit{x}\right)\Vert }_{\mathit{W}}$ is denoted a with $\mathit{W}$, and is a weighted least squares minimization problem of a function $\mathit{e}$ with respect to $\mathit{x}$. The functions $\mathit{f}$ and $\mathit{h}$ are the nonlinear state functions, resprectivley, of the model output equations. The matrix ${\mathit{I}}_0^{+}={\left({\mathit{P}}_0^{+}\right)}^{-1}$ and the vector ${\widehat{\mathit{x}}}_0$ represent the initial guess values of the covariance and the system state. The matrix $\mathit{Q}$ denotes the covariance of the system states, and its entries constitute the confidence in the underlying prediction model and can be tuned by the application engineer. The second tuning matrix $\mathit{R}$ represents the confidence in the actual measurements. The well-known Kalman filter algorithms (e.g., extended Kalman filter (EKF) and unscented Kalman filter (UKF)) are special cases of this optimization objective, in the case that only the measurements at the current time instance ($t_k$) are available [19]. As the dimension, $N$, of the optimization problem would grow tremendously with proceeding time, in MHE theory, the time span is limited to a predefined length of previous time instances and is shifted in every sample step.

Therefore, a sliding window with $M$ steps back from the actual time instance ($t_k$), is considered to smooth the state estimation. In every sample step ($T_s$), this window is shifted one-step ahead—in Figure 8 this is illustrated for one measurement variable, $y^{\mathrm{m}}$. At the filter initialization ($t_0$), only one measurement is available, and therefore the measurement storage must be filled with $r<M$ steps before the window starts to move. Afterwards, the window length is kept constant and all of the past $M$ measurements are taken into account (the window with $M$ measurements is colored in green in the subsequent figures). In other words, MHE can be seen as a real-time calculable approximation of the full-information filter.

In this way, the estimate gets more robust against external disturbances, delayed measurements can be incorporated (cf. Section 3.2.3), and also constraints can be imposed directly [19].

By means of the proposed estimation framework presented in [2], it is also possible to incorporate complex nonlinear Modelica-based prediction models to moving the horizon estimation. In the context of toolchains for observer code generation and nonlinear constraint MHE, different research studies were recently published that utilize the ACADO toolbox for embedded [20] and real-time moving horizon estimation [21,22]. They make use of the real-time iteration (RTI) scheme transferred to the MHE approach in he literature [23]. The basic strategy is to discretize the estimation problem with a multiple shooting discretization using numerical integration. Then, the main idea of the RTI scheme is to use the shifted state variables of the previous optimization run as the new linearization point, and to perform only one SQP step per sample time [20].

In this work, the problem formulation in Equation (7) is extended for nonlinear systems incorporating linear state constraints, tailored to meet real-time application restrictions by means of a nonlinear gradient descent search. Details on the reasons for this approach are given in the following section.

$$\begin{array}{cc}\hfill \underset{{\mathit{\xi }}_k}{\min } g({\mathit{\xi }}_k& ={\left({\mathit{x}}_{k-M}^T, {\mathit{x}}_{k-M+1}^T,\dots ,{\mathit{x}}_k^T\right)}^T)\hfill \\ \hfill \mathrm{s}.\mathrm{t}.\mathit{A}\cdot {\mathit{\xi }}_k& =\mathit{b}\hfill \\ \hfill \mathit{C}\cdot {\mathit{\xi }}_k& \le \mathit{d}\hfill \end{array}$$

(8)

$$g({\mathit{\xi }}_k)=\parallel {\mathit{x}}_{k-M}-{\widehat{\mathit{x}}}_{k-M}^{+}{\parallel }_{{\mathit{I}}_{k-M}^{+}}^2+{\displaystyle \sum }_{i=k-M}^k\parallel {\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right){\parallel }_{{\mathit{R}}^{-1}}^2+{\displaystyle \sum }_{i=k-M+1}^k\parallel {\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}{\parallel }_{{\mathit{Q}}^{-1}}^2$$

(9)

$$\begin{array}{c}{\mathit{x}}_{\mathrm{int},k-M}={\widehat{\mathit{x}}}_{k-M}^{+}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{\mathit{x}}_{\mathrm{int},i}={\mathit{f}}_{i|i-1}\left({\mathit{x}}_{\mathrm{int},i-1},{\mathit{u}}_{i-1}\right)\\ \left(i=k-M+1,\dots ,k\right)\end{array}$$

(10)

All of the system function evaluations were done by the FMI [6] (cf. also Section 3.2) and are marked in red in the following section. The optimization vector (${\mathit{\xi }}_k$) is assembled with $M$ subsequent discrete state vectors within the current estimation window. The optimization cost function $g\left({\mathit{\xi }}_k\right)$ in Equation (9) is composed of the following three parts.

The first argument ${\Vert \cdot \Vert }_{{\mathit{I}}_{k-M}^{+}}$ is the arrival cost, which summarizes all of the available information prior to the estimation window; this can also be seen as a regulation term on the states at $t_{k-M}$ [21]. It is introduced so as to guarantee that the oldest estimation (${\mathit{x}}_{k-M}$) is coincident with the corrected Kalman filter state estimation (${\widehat{\mathit{x}}}_{k-M}^{+}$) weighted with the information matrix (${\mathit{I}}_{k-M}^{+}$). Note that in every moving horizon estimation step, a Kalman filter step from $k-M-1$ to $k-M$ is performed to fulfill the Kalman state estimation theory—Equation (7). The information matrix, ${\mathit{I}}_{k-M}^{+}$, is calculated via the inverse of the covariance matrix ${\left({\mathit{P}}_{k-M}^{+}\right)}^{-1}$. As direct matrix inversion should be avoided, because of the numerical stability and accuracy, the author proposes using a square root (SR)-UKF or SR-EKF Kalman filter algorithm that uses square-root decomposition and rank 1 updates to propagate the covariance matrix in a lower triangular form, $\mathit{P}=\mathit{L}\cdot \mathit{U}$. The inverse of the lower triangular ($\mathit{L}$) can be efficiently calculated by the LAPACK routine DTRTRI, and therefore, the propagated information matrix results in ${\mathit{I}}_{k-M}^{+}={\mathit{L}}^{-\mathbf{1}}\cdot \mathit{L}$.

The second argument, ${\Vert \cdot \Vert }_{{\mathit{R}}^{-1}}$ is the, over the time instances ($k-M$ to $M$) summarized and with ${\mathit{R}}^{-1}$ weighted, difference between the available measurements ${\mathit{y}}_i^{\mathrm{m}}$ and the output equations of the underlying prediction model $\mathit{h}\left({\mathit{x}}_i\right)$ as function of the states of the current optimization vector ${\mathit{\xi }}_k$.

Finally, the third argument, ${\Vert \cdot \Vert }_{{\mathit{Q}}^{-1}}$, denotes the, over the time instances ($k-M$ to $M$) summarized and with ${\mathit{Q}}^{-1}$ weighted, difference between the optimized states ${\mathit{x}}_i$ and the open loop integrated prediction model states ${\mathit{x}}_{\mathrm{int}}$ in Equation (10). ${\mathit{x}}_{\mathrm{int}}$ is calculated only once per optimization step by a simulation using ${\widehat{\mathit{x}}}_{k-M}^{+}$ as start vector and ${\mathit{u}}_i$ as input vector.

In Figure 9, a qualitative graphical interpretation of the optimization problem for a scalar problem with $n_x=n_y=1$ is shown. The circle points of the quantities denote the particular values at a discrete-time instance, which is evaluated in the objective function Equation (9).

The complete algorithm is summarized in Algorithm 1. In the first step, all of the past measurements are stored in a first in first out (FIFO) ring buffer. As long as not enough measurements for the complete window $M$ are available, the measurements and the model inputs are appended to the buffer.

Algorithm 1. Algorithm for a nonlinear MHE

Set $k=0$ $\left(k\in {\mathbb{N}}^{+}\right)$ and set ${\mathit{x}}_k={\mathit{x}}_0$

while (brake==false) do

Fill ring buffer with measurements and system inputs:

if $k<M$ then

append ${\mathit{u}}_k$ to $\mathit{u}$ and ${\mathit{y}}_k^{\mathrm{m}}$ to ${\mathit{y}}^{\mathrm{m}}$

else

left shift one entry of $\mathit{u}$ and ${\mathit{y}}^{\mathrm{m}}$

and append ${\mathit{u}}_k$ respectively ${\mathit{y}}_k^{\mathrm{m}}$

end if

Optimize over stored measurement window (Equation (8)):

if $k>M$ then

Propagate ${\widehat{\mathit{x}}}_{k-M-1}^{+}$ via a Kalman Filter step (e.g., EKF or UKF cf. 2):

xKF(${\widehat{\mathit{x}}}_{k-M-1}^{+})\to {\widehat{\mathit{x}}}_{k-M}^{+}, {\mathit{I}}_{k-M}^{+}$

Project states on the constrained area (c.f. Appendix B)

$\underset{\mathit{x}}{\min }\Vert \mathit{x}-{\widehat{\mathit{x}}}_{k-M}^{+}\Vert \mathrm{s}.\mathrm{t}. \mathit{c}\left(\mathit{x}\right)\le 0$

end if

$k=k+1$

end while

3.1. A Nonlinear Gradient Descent Opimization Algorithm for MHE

For the solution of the proposed MHE problem formulation, Equation (8) to Equation (10), a nonlinear gradient descent search algorithm (NG) was chosen, because for this solver, only the first derivatives of the minimization objective (Equations (8)–(10)) are needed, which is an important constraint for the available interfaces of the extended FMI 2.0 co-simulation interface (see [2]—Section 4.2.2). Furthermore, linear equality and inequality constraints can be incorporated easily, and the method can be stopped after every optimization step, still offering a reliable sub-optimal solution. The latter is important if the optimization is not finalized at the next sample instant. The algorithm of the constrained NG is given in Algorithm 2, for more details see, for example [24]. This gradient descent search algorithm is already successfully being used on a rapid-prototyping real-time system. For example, it was employed—similar to the one proposed in this manuscript—in previous research on path planning tasks [4]. This gradient descent optimization has been implemented and successfully tested on a dSpace real-time control system with a sample time of 100 ms. The gradient descent algorithm has a fixed number of maximum iterations so as to guarantee the real-time constraint. Even if the global optimum is not found, the suboptimal solution will be better than the initial start solution. Here, the implemented NG algorithm for the observer framework is tailored for the nonlinear moving horizon estimator to meet the maximum flexibility in the calculation of the gradient, and the objective function calculation. That is, it is possible for the user to modify the calculation in Modelica with replaceable function pointers, without modifying the optimization algorithm itself. This will be shown later in an example in Section 3.2.

Algorithm 2. Nonlinear gradient search algorithm for MHE

Set $j=0$ $\left(j\in {\mathbb{N}}^{+}\right)$ and ${\mathit{\xi }}_k^0={\left({\mathit{x}}_{\mathrm{int},k-M}^T,\dots ,{\mathit{x}}_{\mathrm{int},k}^T\right)}^T$

while$\left|g\left({\mathit{\xi }}_k^j\right)-g\left({\mathit{\xi }}_k^{j+1}\right)\right|>10\cdot ϵ$do

if unconstrained then

${\mathit{r}}_j=-\mathbf{\nabla }g\left({\mathit{\xi }}_k^j\right)$

elseif constrained then

${\mathit{r}}_{\mathrm{pro},j}=-{\mathit{P}}^{†}\left(\mathbf{A}\right)\cdot \mathbf{\nabla }g\left({\mathit{\xi }}_k^j\right)$

${\mathit{r}}_{\mathrm{f},j}={\mathit{r}}_{\mathrm{pro},j}-\left({\mathit{G}}_{\mathrm{pa},i}{\mathit{r}}_{\mathrm{pro},j}\right){\mathit{G}}_{\mathrm{pa},i}^T$

end if

Determine step size via line search: ${\eta }_j=\underset{0\le \eta }{\mathrm{argmin}} g\left({\mathit{\xi }}_k^j+\eta \cdot {\mathit{r}}_j\right)$

Optimization step: ${\mathit{\xi }}_k^{j+1}={\mathit{\xi }}_k^j+{\eta }_j{\mathit{r}}_j$

$j=j+1$

end while

In step 1 (line 1), an initial solution, ${\mathit{\xi }}_k^0$, is needed. Well-proven strategies for its calculation are an open loop integration of the prediction model from ${\mathit{x}}_{k-M}$ to ${\mathit{x}}_k$, or a left shift of the last optimization vector ${\mathit{\xi }}_{\mathit{k}-\mathbf{1}}$ and appending an open loop integration from ${\mathit{x}}_{k-1}$ to ${\mathit{x}}_k$. In the unconstrained (lines 3–4) case of the second step, the gradient $\mathbf{\nabla }g\left({\mathit{\xi }}_k^j\right)$ of the descent direction can be directly calculated, as shown in Algorithm 3.

Algorithm 3. MHE calculation by means of functional mockup unit (FMU) evaluations

Define: ${\mathit{R}}^{*}={\left(\mathit{R}\cdot {\mathit{R}}^T\right)}^{-1}$ and ${\mathit{Q}}^{*}={\left(\mathit{Q}\cdot {\mathit{Q}}^T\right)}^{-1}$

$\mathit{\nabla }{g}_{1:n}=\left({\mathit{I}}_{k-M}^{+}+{({\mathit{I}}_{k-M}^{+})}^T\right)\left({\mathit{x}}_{k-M}-{\widehat{\mathit{x}}}_{k-M}^{+}\right) -2{\frac{\partial h\left({\mathit{x}}_{k-M}\right)}{\partial {\mathit{x}}_{k-M}}}^T{\mathit{R}}^{*}\left({\mathit{y}}_{k-M}^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_{k-M}\right)\right)$

for$i=k-M+1 \mathrm{to} k$do

$\mathbf{\nabla }{g}_{1+\left(i-k+M\right)\cdot n:\left(i-k+M+1\right)\cdot n}=\frac{\partial g\left({\mathit{\xi }}_k\right)}{\partial {\mathit{x}}_i}=2{\mathit{Q}}^{*}\left({\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}\right)-2\frac{\partial \mathit{h}{\left({\mathit{x}}_i\right)}^T}{\partial {\mathit{x}}_i}{\mathit{R}}^{*}\left({\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right)\right)$

end for

Also, if equality constraints (lines 5–7) should be incorporated, $\mathit{A}\cdot \mathit{\xi }=\mathit{b}$, the descent direction must be projected on these by means of the Moore–Penrose pseudoinverse [25] ${\mathit{r}}_{\mathrm{pro},j}={\mathit{P}}^{†}\left(\mathbf{A}\right)\cdot \mathbf{\nabla }g\left({\mathit{\xi }}_k^j\right)$. In most of the MHE applications, this step can be neglected (i.e., ${\mathit{P}}^{†}\left(\mathbf{A}\right)=\mathit{I}$), as only inequality constraints are of mayor interest in order to limit the boundaries of the states. To guarantee that the descent direction does not violate the inequality constraints, a set of possible active constraints, ${\mathit{G}}_{\mathrm{pa},i}$, in the null space of the linear constraints must be determined. The descent direction is projected on the active constraints $\left({\mathit{G}}_{\mathrm{pa},i}{\mathit{r}}_{\mathrm{pro},j}\right){\mathit{G}}_{\mathrm{pa},i}^T$. More algorithm implementation details can be found in the literature [24].

In step 3 (line 9), an iterative free line search via quadratic approximation by a second order Taylor series polynomial is performed (Equation (11)). Unfortunately, the derivatives of $g\left({\mathit{\xi }}_k^j\right)$ must be calculated via numerical differences, as the extended FMU 2.0 co-simulation interface only supports directional derivatives with respect to the system states and inputs (see [6]—Section 2.1.9). Note that the necessary determination of $g\left({\mathit{\xi }}_k^j+\mathsf{\Delta }s\right)$ in the differential quotient causes only evaluations of the algebraic output equations of the FMI (cf. Equation (9)), as the calculation of the initial guess ${\mathit{x}}_{\mathrm{int},i}$ is only performed once in this approach.

$$\begin{array}{cc}\hfill g\left({\mathit{\xi }}_k^j+\eta \cdot {\mathit{r}}_j\right)& \approx G\left(\eta \right)≔g\left({\mathit{\xi }}_k^j\right)+g^{\prime }\left({\mathit{\xi }}_k^j\right)\eta +\frac{1}{2}{g}^{″}\left({\mathit{\xi }}_k^j\right){\eta }^2\hfill \\ \hfill \frac{\partial G\left(\eta \right)}{\partial \eta }& \stackrel{!}{=}0=g^{\prime }\left({\mathit{\xi }}_k^j\right)+g^{″}\left({\mathit{\xi }}_k^j\right)\eta ,\hfill \\ \hfill \Rightarrow {\eta }_j& =-\frac{g^{\prime }\left({\mathit{\xi }}_k^j\right)}{g^{″}\left({\mathit{\xi }}_k^j\right)}\hfill \end{array}$$

(11)

In step 4 (line 10), the actual optimization step, ${\mathit{\xi }}_k^{j+1}$, is computed. Finally, in the last step, it is checked whether the stop criterion has been reached. This can be, on the one hand, with a criterion that controls whether the change in the last iteration, $j$, is small $\left|g\left({\mathit{\xi }}_k^j\right)-g\left({\mathit{\xi }}_k^{j+1}\right)\right|<10\cdot \mathsf{ϵ}$, and, on the other hand, a real-time constraint that stops the search in order to guarantee the cycle-time of the real-time system. In this case, it is assumed that the initial guess (${\mathit{\xi }}_k^0$), calculated by the high fidelity Modelica model, has been already a valid suboptimal solution, and the iterations of the NG did further improve it (cf. Algorithm 2), before the stop (${\mathit{\xi }}_k^j$).

3.2. Moving Horizon Estimation Algorithm Extensions

Here, for the analyzed ESTM MHE observer, different extensions are discussed, in comparison to the nominal MHE algorithm formulation in Section 3. First, a computationally reliable method is introduced for the calculation of the prediction model and the observer constraints, by means of two multi-rate extended FMUs 2.0 for co-simulation (cf. [2,6,26]). It enables enlarging the sampling time of the observer, inspite of the fact that the constraint calculation needs to be executed in a 50 times faster sampling rate, which gives large benefits to the real-time capability. Second, advanced methods for the coupling of discrete optimization variables are proposed. These enforce the physical coupling of the discrete tuner states (${\mathit{\xi }}_k$) to overcome an unrealistic solution of the state trajectory. Third, a heuristic method is discussed to prevent optimization freezing through intelligent recalculation of the reference trajectory (${\mathit{x}}_{\mathrm{int}}$) in segments where no measurements are available.

3.2.1. Constraint Evaluation with a Multi-Rate FMU Model Splitting Concept

The first implementations of the constrained observer were based on a single prediction FMU that combined the ESTM prediction model (Section 3) and the boundary constraint evaluation (Section 2.2). To separate the estimated states from the state of the constraint evaluation (the path parameter $s$), Modelica’s logical vector indexing feature was used. However, a simulation experiment analysis showed that in this configuration, it is necessary to run the whole estimator with a fast sampling rate of $T_s=4 \mathrm{ms}$. This is due to the fast dynamics of the control loop to determine the current path parameter ($s$) in the constraint calculation module (cf. Section 2.2). To overcome this issue, the ESTM and the boundary constraint (BC) model were split into two separate FMUs with different sample times ($T_s^{\mathrm{ESTM}}= 200 \mathrm{ms}, T_s^{\mathrm{BC}}=4 \mathrm{ms}$). The connection of the multi-rate FMUs and the estimation algorithm are sketched in Figure 10. Through the model splitting, the states of the ESTM FMU ($\mathit{x}=\{{\beta }^{\mathrm{C}},v^{\mathrm{C}},{\dot{\psi }}^{\mathrm{C}},{\psi }_{\mathrm{C}},x_{\mathrm{C}},y_{\mathrm{C}}\})$ are the inputs of the boundary constraints of FMU, which only has one state, the corresponding path parameter $s$. The sample time of the constrained FMU is $50$ times higher than the one of the ESTM, so as to guarantee numerical stability. $\mathit{Z}$ denotes the permutation matrix between the inputs of the BC FMU and the states of the ESTM FMU.

With this implementation, all of the necessary quantities for the constrained MHE are nested within the multi-rate FMU block, whose interfaces to the outside (denoted with blue arrows) are the same as if no inner model separation was performed. This gives a large benefit in the matter of computational effort, not only caused by the larger integration step, but it also enables the possibility to calculate the constraint only if it is necessary for the estimation algorithm. This benefit is shown in the following simplified flow diagram of the extended MHE algorithm proposed here (see Figure 11). By means of a forward integration in step 1 from $t_{k-M}$ to $t_k$, it is checked whether any constraint may be potentially activated ($c^{\mathrm{a}}$) and if so, the constraints are linearized along the open loop state trajectory. In step 2, the nonlinear gradient algorithm performs the optimization over the estimation window, incorporating the linearized constraints if necessary. In the last step, step 3, the Kalman filter is updated with the consideration of the system constraints to guarantee that the initial state of the moving window in the next iteration step lies within the feasible region.

Qualitatively, the incorporation of the constraints is depicted in Figure 12. The feasible region of the constraint is limited through the function of $c_1$ and $c_2$ (orange-dotted). In this example, the state propagation of the Kalman filter (step 3) causes a violation of $c_2$, and therefore, the a posteriori propagated state must be constrained by a method (e.g., state constraint projection in Appendix B. The boundary constraints control (step 1) now starts with the corrected a posteriori estimate, ${\widehat{\mathit{x}}}_{k-M}^{{+}_{\mathrm{cstr}}}$, and detects a constraint violation between the third and fourth sample point. This is only possible as the BC model is integrated fifty times between every ESTM evaluation and correction step. To guarantee that the initial solution of the NG solver lies in the feasible region, the ${\mathit{x}}_{\mathrm{int}}^{\mathrm{cstr}}$ is limited via the simplified Newton descent search, as described in Appendix B.

3.2.2. Multiple Shooting Inspired Optimization Objective Extension

In the original MHE problem formulation (Equation (8) to Equation (10)), the discrete system states within the moving window ${\mathit{\xi }}_k={\left({\mathit{x}}_{k-M}^T, {\mathit{x}}_{k-M+1}^T,\dots ,{\mathit{x}}_k^T\right)}^T$ are not coupled with each other between the sample points ($t_k$). This implies that the optimization algorithm does not have any information about the dynamic behavior of the ESTM prediction model between the sample points within the estimation window. In the case of the ESTM, with a large sample time $T_s=200 \mathrm{ms}$, this may lead to a physically unfeasible set ${\mathit{\xi }}_k$, which however minimizes the optimization criteria. A consideration to overcome this weak point is the introduction of coupling penalty terms between the time instances in the minimization criterion in Equations (9) and (10). Figure 13 exemplifies the approach developed here, namely: the initial open loop integration from the time instance $t_{k-M}$ to the current time instance $t_k$ is denoted as ${\mathit{x}}_{\mathrm{int}}^0$. The set of optimized state vectors ${\mathit{\xi }}_k^j$ in the $j$-th NG descent step (cf. Algorithm 2) is marked with green circles.

The temporal evolution from these discrete states by means of the FMU yields a set of system states ${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}$ denoted with a red circle, as follows:

$${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}={\mathit{f}}_{i|i-1}\left({\mathit{x}}_{i-1},{\mathit{u}}_{i-1}\right),\left(i=k-M,\dots ,k\right)$$

(12)

In the depicted qualitative example (cf. Figure 13), one can see, that through the evolution of the optimization process a displacement ${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}\ne {\mathit{x}}_p$ is caused in the j-th iteration step of the NG algorithm (see Algorithm 2). To minimize this gap, the MHE optimization objective is changed to the following:

$$\begin{array}{c}g\left({\mathit{\xi }}_k\right)=\parallel {\mathit{x}}_{k-M}-{\widehat{\mathit{x}}}_{k-M}^{+}{\parallel }_{{\mathit{I}}_{k-M}^{+}}^2\\ +{{\displaystyle \sum }}_{i=k-M}^k\parallel {\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right){\parallel }_{{\mathit{R}}^{-1}}^2\\ +{{\displaystyle \sum }}_{i=k-M+1}^k\parallel {\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}{\parallel }_{{\mathit{Q}}^{-1}}^2\\ +\underset{\mathrm{Additional} \mathrm{penalty}}{\underbrace{{\displaystyle {\displaystyle \sum }_{i=k-M}^k}\parallel {\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}{\parallel }_{{\mathit{Q}}_{\mathrm{MS}}}^2}}\end{array}$$

(13)

In comparison to the original formulation, the quality functional is extended with an additional weighted least squares expression so as to enforce a stronger coupling of the piecewise integration (${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}$) and the optimized stated vector (${\mathit{x}}_i$) by means of the user tunable weighting matrix (${\mathit{Q}}_{\mathrm{MS}}$).

Besides performing the integration (${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}$) by means of the FMU (Equation (12)), it is necessary to approximate the integration rule for the gradient $g\left({\mathit{\xi }}_k\right)$ calculation. It is proposed that it is more important to generate a good approximated descent direction for the optimizer than the exact reproduction of the integration method ${\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}$ used in the FMU.

In the simplest case, this is achieved by the consideration of the directional derivative at the past instance (later called V1). In the second version (V2), the integrator is approximated as an Euler 1 integration rule. The last optimization variable coupling approximation is formulated by a trapezoid integration rule (V3), as follows:

$$\begin{array}{l}\mathrm{V}2\to {\mathit{x}}_{\mathrm{E}1,i}^{\mathrm{L}}={\mathit{f}}_{i|i-1}\left({\mathit{x}}_{i-1},{\mathit{u}}_{i-1}\right)\approx {\mathit{x}}_{i-1}+{\mathit{f}}_{i-1}\cdot T_s\\ \mathrm{V}3\to {\mathit{x}}_{\mathrm{Tr},i}^{\mathrm{L}}={\mathit{f}}_{i|i-1}\left({\mathit{x}}_{i-1},{\mathit{u}}_{i-1}\right)\approx {\mathit{x}}_{i-1}+\frac{1}{2}\cdot ({\mathit{f}}_{i-1}+{\mathit{f}}_i)\cdot T_s\end{array}$$

(14)

With these three versions, the complete extended MHE gradient computation is given in Algorithm 4. Comparing this gradient calculation to the original formulation in Algorithm 3, the main difference is the additional “for loop” with the index $j$, in which the operator ± denotes that all of the values are additively added to the existing entries from the earlier loop.

Algorithm 4. Multiple shooting MHE gradient calculation

Define: ${\mathit{R}}^{*}={\left(\mathit{R}\cdot \mathit{R}\right)}^{-1}; {\mathit{Q}}^{*}={\left(\mathit{Q}\cdot \mathit{Q}\right)}^{-1}$; $\mathit{E}=\mathrm{eye}\left(n\right)$

Define: $i_1\left(i\right):= 1+\left(i-k+M\right)n:\left(i-k+M+1\right)n$

$\mathit{\nabla }{g}_{1:n}=\left({\mathit{I}}_{k-M}^{+}+{({\mathit{I}}_{k-M}^{+})}^T\right)\left({\mathit{x}}_{k-M}-{\widehat{\mathit{x}}}_{k-M}^{+}\right) -2{\frac{\partial \mathit{h}\left({\mathit{x}}_{k-M}\right)}{\partial {\mathit{x}}_{k-M}}}^T{\mathit{R}}^{*}\left({\mathit{y}}_{k-M}^m-\mathit{h}\left({\mathit{x}}_{k-M}\right)\right)$

for$i=k-M+1$ to $k$ do

$\mathit{\nabla }{g}_{i_1\left(i\right)}=\frac{\partial \mathit{g}\left({\mathit{\xi }}_k\right)}{\partial {\mathit{x}}_i}=2{\mathit{Q}}^{*}\left({\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}\right)-2\frac{\partial \mathit{h}{\left({\mathit{x}}_i\right)}^T}{\partial {\mathit{x}}_i}{\mathit{R}}^{*}\left({\mathit{y}}_i^{\mathrm{m}}-\mathit{h}\left({\mathit{x}}_i\right)\right)$

if version == V3 then

$\mathit{\nabla }{g}_{i_1\left(i\right)}-={\left(\frac{1}{2}\cdot T_s\cdot \frac{\partial f_i}{\partial x_i}\right)}^T\cdot 2\cdot {\mathit{Q}}_{\mathrm{MS}}\cdot \left({\mathit{x}}_i-{\mathit{x}}_{\mathrm{int},i}^{\mathrm{L}}\right)$

end if

end for

Define: $j_1\left(j\right):=1+\left(j-k+M\right)n:\left(j-k+M+1\right)n$

for$j=k-M$ to $k-1$ do

$\mathit{\nabla }{g}_{j_1\left(j\right)}+=\frac{\partial \mathit{g}\left({\mathit{\xi }}_k\right)}{\partial {\mathit{x}}_j}$

switch(version)

case V1:

$\mathit{\nabla }{g}_{j_1\left(j\right)}+=-\frac{\partial {\mathit{f}}_j}{\partial {\mathit{x}}_j}\cdot 2\cdot {\mathit{Q}}_{\mathrm{MS}}\cdot \left({\mathit{x}}_{j+1}-{\mathit{x}}_{\mathrm{int},j+1}^{\mathrm{L}}\right)$

case V2:

$\mathit{\nabla }{g}_{j_1\left(j\right)}+=-{\left(\frac{1}{2}\cdot T_s\cdot \frac{\partial f_j}{\partial x_j}\right)}^T\cdot 2\cdot {\mathit{Q}}_{\mathrm{MS}}\cdot \left({\mathit{x}}_{j+1}-{\mathit{x}}_{\mathrm{int},j+1}^{\mathrm{L}}\right)$

case V3:

$\mathit{\nabla }{g}_{j_1\left(j\right)}-={\left(\mathit{E}+\frac{1}{2}\cdot T_s\cdot \frac{\partial {\mathit{f}}_j}{\partial {\mathit{x}}_j}\right)}^T\cdot 2\cdot {\mathit{Q}}_{\mathrm{MS}}\cdot \left({\mathit{x}}_{j+1}-{\mathit{x}}_{\mathrm{int},j+1}^{\mathrm{L}}\right)$

end switch

end for

3.2.3. Multi-Rate, Triggered, or Delayed Measurements in MHE

For the nonlinear MHE approach of Section 3, it is proposed to keep the lagged sensor data in a ring buffer, to interconnect them with past measurements of non-lagged sensors, and to index the active sensors at the particular time instance. Comparing this with the formulation of Equation (9), all of the expressions that are connected with the measured output (the middle part) have to be calculated separately in each time step so as to incorporate the changing amount of available sensor information at the dedicated time step.

$$\begin{array}{cc}\hfill g\left({\mathit{\xi }}_k\right)& {\displaystyle =\cdots +{\displaystyle \sum }_{i=k-M}^k\parallel {\mathit{y}}_{i,\mathrm{a}}^{\mathrm{m}}-{\mathit{h}}_{\mathrm{a}}\left({\mathit{x}}_i\right){\parallel }_{{\mathit{R}}_k^{-1}}^2+\cdots }\hfill \\ \hfill {\overline{\mathit{\sigma }}}_{r_{\mathrm{a}}}& = \left\{{\mathit{\sigma }}_{r_i}|i\in {\mathit{y}}_k\right\}\hfill \\ \hfill {\mathit{R}}_k& =\mathrm{diag}\left({\overline{\mathit{\sigma }}}_{r_{\mathrm{a}}}\right)\hfill \\ \hfill {\mathit{h}}_{\mathrm{a}}& =\left\{{\mathit{h}}_i|i\in {\mathit{y}}_k\right\}\hfill \\ \hfill {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}& =\left[\begin{array}{ccc}{y}_{k-M,1}^{\mathrm{m}}& \dots & y_{k,1}^{\mathrm{m}}\\ ⋮& \dots & ⋮\\ y_{k-M,n_y}^{\mathrm{m}}& \dots & y_{k,n_y}^{\mathrm{m}}\end{array}\right]\hfill \end{array}$$

(15)

In Equation (15), all of the active measurements are denoted with an additional subscript (${(\cdot )}_{\mathrm{a}}$) for the active sensors. In the construction of the active measurement matrix (${\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}$), the time-delays of the single sensors are already considered. To cope with the varying dimensions in the optimization problem, the implementation could utilize the vector indexing feature in the Modelica language. The same technique is used in the Kalman filter propagation (cf. Algorithm 1) for the reduced indexed measurements. This procedure is valid according to the Kalman theory, and can be seen in an analogy to sequential Kalman filtering (see [10] – Section 6.1). In these implementations, the matrix vector notation of the Kalman filter algorithm is replaced by sequentially solving a scalar problem for each measurement. Therefore, more sensor information can improve the estimation in the sense of minimizing the covariance, ${\mathit{P}}_{k-M}^{+}$. In the case that less sensor information is available, larger values of $\det ({\mathit{P}}_{k-M}^{+}$) occur, but the validity of the Kalman theory still holds. In the same manner, the dimension of the available measurements at $t_{k-M}$ for the calculation of the information matrix ${\mathit{I}}_{k-M}^{+}$ and the initial guess ${\widehat{\mathit{x}}}_{k-M}^{+}$ also needs to be considered in the Kalman step.

3.2.4. Adaptive Initial Reference Refreshing for Delayed Measurements

In Section 3.2.3, a theory extension to MHE is given that enables the assignment of measurements to their particular time instance by intelligent measurement storage and temporal activation indexing. In Figure 14, the measurement signal ${\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}$ is schematically sketched, which is only available at the time instances highlighted with a yellow flash. For example, between time instances $t_{k-M+1}$ and $t_{k-M+3}$, no new measurement information is available.

Different algorithm experiments have shown that this may force the NG optimizer to tune the variable ${\mathit{\xi }}_{k,3}$ towards the initial guess of the open loop state trajectory (${\mathit{x}}_{\mathrm{int}}$). To overcome this very conservative solution, a heuristic method is introduced in Equation (16) to refresh ${\mathit{x}}_{\mathrm{int}}^{j+1}$ after the optimization step (line 10 in Algorithm 2). It determines the gaps in the logical vector indexing matrix of the active measurements ${\mathit{y}}_{{\mathrm{index}}_{\mathrm{a}}}^{\mathrm{m}}$ and integrates from the last time instance where all of the measurements are available, as follows:

$${\mathit{x}}_{\mathrm{int},i}^{j+1}=\{\begin{array}{c}{\mathit{x}}_{\mathrm{int},i}^0, \forall \left\{i|\left(i\in {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\wedge \left(i+1\in {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\right\}\\ {\mathit{f}}_{i|i-1}\left({\mathit{\xi }}_{k,i-1}^j,{\mathit{u}}_{i-1}\right), \forall \left\{i|\left(i\in {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\wedge \left(i+1\notin {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\right\}, \left(i=k-M,\dots ,k\right)\hfill \\ {\mathit{f}}_{i|i-1}\left({\mathit{x}}_{\mathrm{int},i-1}^{j+1},{\mathit{u}}_{i-1}\right),\forall \left\{i|\left(i\notin {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\wedge \left(i+1\notin {\mathit{y}}_{\mathrm{a}}^{\mathrm{m}}\right)\right\}\hfill \end{array}$$

(16)

4. Experimental Results

The following experimental results rely on the test measurement data that were recorded during the driving experiments using ROboMObil, as described in Section 2. Two real-time rapid prototyping systems were used to record all of the variables from the wheel robot controllers as well as the senor system of ROboMObil. The arrangement, shown in Figure 15, is designed to meet the robotics inspired central control architecture with two synchronized rapid prototyping controllers (dSPACE MABX2 and ABX), which represent ROboMObil’s central control unit. The wheel robots with the traction and steering drive torques (${\tau }^{\left(\mathrm{ST}\right),\mathbf{W}}$), angular velocities (${\omega }^{\left(\mathrm{ST}\right),\mathbf{W}}$), and steering angles (${\delta }^{\mathbf{W}}$) quantities, as well as the Correvit optical measurement unit (used to measure the vehicle’s longitudinal and lateral speed ${\mathit{v}}_{\mathrm{act}}^{\mathrm{C}}=\left\{v_{{\mathrm{act}}_x}^{\mathrm{C}},v_{{\mathrm{act}}_y}^{\mathrm{C}}\right\}$), are connected via high-speed controller area network (CAN) busses. The inertial measurement system with a differential corrected Global Navigation Satellite System (GNSS) to measure the vehicle states, such as the positon ${\mathit{p}}_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}$, yaw angle ${\psi }_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}$ and yaw rate ${\dot{\psi }}_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}$, or the velocity ${\mathit{v}}_{\mathrm{act}}^{\mathrm{C}}$, is wired to the central control by an Ethernet bus. Additionally, the wheel (${\mathit{a}}^{\mathbf{W}}$) and chassis body (${\mathit{a}}_{\mathrm{body}}$) accelerations, as well as wheel travel sensors ($s^{\mathbf{W}}$) are tethered via analog inputs. Both the inertial measurement unit from Oxford Technical Solutions Ltd. (OxTS) measurement unit and the optical odometry sensor are suited for the experimental evaluation of the driving experiments.

4.1. Observer Configuration

The preliminary observer case studies showed that with the modeling approach chosen here (cf. Section 2.1), in combination with a Runge–Kutta 4 integrator, the vehicle position estimator can be run with a cycle time of $T_s=200 \mathrm{ms}$. Moreover, the usage of the vehicle yaw rate (${\dot{\psi }}^{\mathrm{C}}$) can actively contribute to improve the measurement, whereas the incorporation of the vehicle lateral acceleration ($a_y^{\mathrm{C}}$) downgrades the observer performance. The output equation, $a_y^{\mathrm{C}}=v^{\mathrm{C}} \cdot ({\dot{\psi }}^{\mathrm{C}}+{\dot{\beta }}^{\mathrm{C}})$, shows clearly that the lateral acceleration is algebraically cross-coupled to the vehicle yaw rate (${\dot{\psi }}^{\mathrm{C}}$). In fact, this causes the aforementioned observations, as it is, to the best knowledge of the author, impossible to find a covariance configuration—even by optimization—which makes reasonable use of both the sensor information and overcomes the negative effects of, for example, minimal jittering relative delays between both signals.

In Figure 16, different simulation results of the same vehicle position estimator are given. Again, the road boundaries are marked in a dotted-orange color. The vehicle completed three rounds through the circuit. The red trajectory denotes the open loop result—the ESTM model is simulated with the actual system inputs $\mathit{u}$—and the position is strongly drifting away from the planned trajectory, caused by the evolving position integration error (compare equations for $\mathrm{d}{x}_{\mathrm{C}}/\mathrm{d}t, \mathrm{d}{y}_{\mathrm{C}}/\mathrm{d}t$—in Equation (A1)). The green and the blue curves denote the results with a single step Kalman filter, under the assumption that no delays are in the measurements.

The best observation results could be achieved by the use of a square root extended Kalman filter (SR-EKF) algorithm (cf. e.g., [2]), whereas the square root unscented Kalman filter (SR-UKF) (cf. e.g., [2]) was less robust. The reason for this is the sigma point propagation through the Pacejka tire model, which evidently leads to a wrong state propagation caused by its high sensitivity around the actual state estimation point.

4.2. Moving Horizon Estimation Algorithm Assessment

In this last subSection, the complete MHE ESTM algorithm with its extensions to couple the optimization variables (cf. Section 3.2), refreshment of the initial guess, and efficient road boundary constraint handling are demonstrated (cf. Figure 7). In

Table 1. Comparison of the vehicle position observer performance indicators. SR-EKF—square-root extended Kalman filter; MHE—moving horizon estimation/estimator; MS—multiple shooting; SS—single shooting.

Algorithm	${\overline{\mathit{T}}}_{\mathbf{mean}}$	${\mathit{x}}_{{\mathbf{C}}_{\mathbf{Fit}}}$	${\mathit{y}}_{{\mathbf{C}}_{\mathbf{Fit}}}$	${\mathit{\psi }}_{{\mathbf{C}}_{\mathbf{Fit}}}$	${\dot{\mathit{\psi }}}_{\mathbf{Fit}}^{\mathbf{C}}$	${\mathit{v}}_{\mathbf{Fit}}^{\mathbf{C}}$	${\mathit{\beta }}_{\mathbf{Fit}}^{\mathbf{C}}$
${\mathit{n}}_{\mathbf{d}}=-$
Open Loop	$6.4$	75.5	62.2	96.4	80.2	77.7	81.0
Open Loop Cstr.	$6.4$	87.7	87.5	96.4	80.2	77.7	81.0
${\mathit{n}}_{\mathbf{d}}=0$
$\mathrm{SR}\text{-}\mathrm{EKF} {\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}$	$4.4$	78.9	95.2	99.1	95.5	77.3	87.4
$\mathrm{SR}\text{-}\mathrm{EKF} {\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}$ Cstr.	$4.8$	88.2	89.2	99.1	95.5	77.3	87.4
SR-EKF	6.4	97.9	97.2	99.2	93.3	82.4	85.2
MHE SS	67.2	97.3	96.5	99.4	95.5	83.1	81.3
MHE MS V1	$46.4$	98.5	97.8	99.3	86.9	86.4	80.4
$n_{\mathbf{d}}=1$
MHE SS up	$78.8$	98.2	97.8	99.4	98.4	84.6	80.9
MHE MS up V1	93.6	98.3	97.8	99.4	86.8	85.3	81.0
$n_{\mathbf{d}}=2$
MHE SS fix	$77.6$	98.4	97.8	99.2	98.9	85.9	80.1
MHE SS up	$75.2$	98.7	98.1	99.2	98.2	85.9	80.3
MHE MS up V1	61.6	98.8	98.2	99.3	86.9	86.4	80.4
MHE MS fix V2	$64.8$	98.5	98.1	99.2	86.3	86.7	80.0
MHE MS up V2	$110.0$	98.7	98.2	99.2	86.8	86.7	80.0
MHE MS fix V3	$67.2$	98.5	98.0	99.2	86.5	86.6	80.3
MHE MS up V3	136.8	98.7	98.3	99.2	86.6	86.6	80.3
${\mathit{n}}_{\mathbf{d}}=3$
MHE SS up	56.8	97.7	97.1	98.9	97.3	86.8	79.6
MHE MS up V1	79.6	97.7	97.1	99.1	87.9	87.3	80.1

, the outcome of a comprehensive simulation case study is summarized. The window length has been set to $M=4$ and the GPS time delay varies between $n_{\mathrm{d}}=0\dots 3$ steps. All of the parameters for the respective observer configuration have been optimized with the DLR multi-objective parameter synthesis (MOPS) optimizer framework [27] to guarantee comparable results.

The results in Table 1 are sorted with respect to the number of delayed samples ($n_{\mathrm{d}}$) of the GPS signal. The results that differ tremendously (good or bad) in a group of the same number of delays are highlighted in green or red, respectively. The first column denotes the used observer setup, in which the acronyms for SS (single shooting; standard formulation), MS (multiple shooting), fix (constant initial guess), and up (anti optimization freezing) are used. The second column gives the average execution time per estimation step ${\overline{T}}_{\mathrm{mean}}$ in $\left[\mathrm{ms}\right]$ of the experiment executed on a standard 64-bit Windows-based system (Intel i7-4600U 2,7 GHz, 8 GB Ram, SSD), and can be interpreted as a measure for the increase of computational complexity in comparison to the improvement of the estimation. It can be seen that even in the most computationally demanding configuration (MHE MS up V3 ${\overline{T}}_{\mathrm{mean}}=136.8 \mathrm{ms}$), the algorithm can be executed faster than in real-time (${\overline{T}}_{\mathrm{mean}}\ll T_s$). In the third to eighth columns, the percentage goodness of fit (normalized root mean square error between to time series) in comparison to the reference measures of the experiment (cf. Section 2) are summarized.

The first two rows in Table 1 give reference of the ESTM without any observer correction, but in the second row has the roadway constraint incorporation. The next two rows are the first results using an observer that only incorporates the measured vehicle yaw rate (${\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}$). Both still have a large positional deviation, albeit the estimation of the vehicle yaw rate, and the angle and side slip angle are improved in comparison with the open loop tests. The next section highlights three versions of the ESTM observer, with all measurements ${\dot{\psi }}_{\mathrm{act}}^{\mathrm{C}}, {\mathit{p}}_{{\mathrm{C}}_{\mathrm{act}}}^{\mathrm{I}}$ available and not delayed. The best results can be achieved with the multiple shooting objective V1. The following two experiments incorporate a delay of one sample step.

Unfortunately, the SR-EKF algorithm could not be stabilized to give a feasible estimate for all of the measures, especially the side slip angle (${\beta }^{\mathrm{C}}$) is heavily oscillating (cf. Figure 17—bottom left). Here, for the first time, the delayed measurement compensation in the MHE formulation can show its advantage in the single shooting as well as in the multiple shooting objective formulations.

The section with $n_{\mathrm{d}}=2$ is the largest section, which correlates to a delay of $400 \mathrm{ms}$. All of the modifications introduced in Section 3.2 are tested here for their performance. Even though all of the results are very close to each other, the multiple shooting V1 with reference updating gives the best performance in positioning accuracy and computational reliability (cf. state plots in Figure 18).

In the last two rows of the simulation study, two configurations with $n_{\mathrm{d}}=3$ are assessed. The single shooting (see Figure 19) and multiple shooting do benefit from the proposed update mechanism, and even if the GPS signal is delayed by $600 \mathrm{ms}$, the results are still reliable.

In total, it can be stated that the time delay incorporation in a MHE is very effective and gives good stability, especially when the sample steps are large. The competitive SR-EKF algorithm failed already with a delay of $n_{\mathrm{d}}=1$, although its computational time is up to $20$ times lower. The influence of the extension of the objective function with multiple shooting penalties needs to be analyzed from case to case, whereas the anti-freezing feature in the optimizer can be seen as a good improvement to the solution at time instance $t_k$ (cf. Figure 20).

Future investigations will be performed with methods other than the NG solver methods, as recently proposed in the literature [28], which are capable of handling nonlinear (in-)equality constraints in real-time. With this extension, it is likely that the multiple shooting approaches might be even more effective.

5. Conclusions

The outcome of the ESTM MHE observer can be summarized as follows:

• A continuous-time Modelica vehicle model with event handling for vehicle standstill (cf. Section 2.1) could be derived (see estimation experiment in Figure 21, starting from standstill and coming back to standstill for about $20 \mathrm{s}$) and automatically discretized via Dymola, the extended FMU 2.0 for co-simulation technology and the model-based observer framework.
• The derivation of the roadway limit constraint has been designed by extending the principle of the path interpolation introduced in Section 2.2.
• The nominal MHE algorithm of Section 3 was augmented with a multiple shooting formulation in the objective function, a heuristic optimization freezing prevention, a multi-rate model, and a constraint calculation splitting methodology.
• For the experimental investigations, real test data from ROMO was selected, and additionally, the GPS position measures were delayed for the experimental setup.
• All together with the technique for delayed measurements in the MHE application (Section 3.2.3), a comprehensive simulative assessment with the different objective configuration and anti-freezing features, as well as varying delays in comparison to a standard Kalman filter, have been given.
• The proposed estimation approach could achieve a position $x_{\mathrm{C}},y_{\mathrm{C}}$ estimate fit of about 98%, and by mean of the delay compensation technique, this value is, even with a delay of $n_{\mathrm{d}}=3$, only reduced to about 97%. The estimate of the vehicle velocity is even 4% improved by the use of the MHE technique in comparison to the EKF. The yaw angle estimate (${\psi }_{\mathrm{C}}$) quality for all of the configurations is very high, whereas the yaw rate (${\dot{\psi }}^{\mathrm{C}}$) and side slip angle (${\beta }^{\mathrm{C}}$) do vary about 5%, but still have reasonable estimates.