Nonlinear Constrained Moving Horizon Estimation Applied to Vehicle Position Estimation

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.


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]).

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 = . ψ C IMU ) and the delayed measurements (y * k = p I C GPS = x I c GPS , y I c GPS ), which are delayed for n d samples, and thus belong to the past vehicle state (x k−n 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.

The Extended Single Track Prediction Model
As the scope of the proposed observer (as implied in the introduction of this chapter) lies on the observation of the actual vehicle states, , , , 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 ≥ 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. Figrue 3): • (•) : Quantity expressed in the car coordinate system with origin in the center of gravity • (•) : Car quantity expressed in the inertial coordinate system-short for (•)

The Extended Single Track Prediction Model
As the scope of the proposed observer (as implied in the introduction of this chapter) lies on the observation of the actual vehicle states, , , , 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 ≥ 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. Figrue 3): • (•) : Quantity expressed in the car coordinate system with origin in the center of gravity • (•) : Car quantity expressed in the inertial coordinate system-short for (•)

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, p I C act , ψ I C act , v C act , . ψ C act 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 ≥ 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.

The Extended Single Track Prediction Model
As the scope of the proposed observer (as implied in the introduction of this chapter) lies on the observation of the actual vehicle states, , , , 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 ≥ 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. Figrue 3): • (•) : Quantity expressed in the car coordinate system with origin in the center of gravity • (•) : Car quantity expressed in the inertial coordinate system-short for (•)   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:

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 (p I C ) 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, p P (s * ) = p C (cf. Figure 4). To approximate this condition, it is necessary to minimize the displacement (e(s)) between the vehicle and reference path, as depicted in the following: The details of the extended single track model equation derivation are given in the Appendix in Chapter 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 Figrue 3:

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 ( ) is sufficiently precisely known, and the corresponding path parameter ( * ) (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, ( * ) = (cf. Figure 4). To approximate this condition, it is necessary to minimize the displacement ( ( )) between the vehicle and reference path, as depicted in the following: The geometrical interpretation of this minimization objective is that ( * ) can be determined by projecting orthogonally on the path ( ). 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 * exists: The interested reader can find details on how this optimization problem is solved in the literature [3].
The parametric path ( ) contains the following quantities for the calculation of the road boundaries constraints: the position ( ) and ( ) of the road middle lane, the positions of the left ( ( ), ( )) and the right ( ( ), ( )) border, and the corresponding path orientation ( ) and its curvature ( ).
The extension of the vehicle prediction model (cf. Appendix A) with the roadway boundaries interpolation yields an extra state ( ), which is a non-physical state that belongs to the estimation task. The experimental tests considering 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). The geometrical interpretation of this minimization objective is that p P (s * ) can be determined by projecting p C orthogonally on the path p P (s). 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 P y (s * ) ≤ 1/κ P (s * ).
The interested reader can find details on how this optimization problem is solved in the literature [3]. The parametric path λ c (s) contains the following quantities for the calculation of the road boundaries constraints: the position x I P (s) and y I P (s) of the road middle lane, the positions of the left (l I x (s), l I y (s)) and the right (r I x (s), r I y (s)) border, and the corresponding path orientation ψ I P (s) and its curvature κ P (s).
The extension of the vehicle prediction model (cf. Appendix A) with the roadway boundaries interpolation yields an extra state ( . s), which is a non-physical state that belongs to the estimation task. The experimental tests considering . 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 (p I C ) tends to zero. In Figure 5, the calculation of the roadway border constraints is graphically shown. By means of the above described algorithm, a path parameter ( ) can be found for which the longitudinal derivation error of the vehicle's actual position ( ) tends to zero. By means of the path normal vector ( ( )), represented in the inertial coordinate system (Equation (2)), an inequality function ( ) ≤ 0 is calculated that penalizes the positions outside of the roadway borders, as follows: This nonlinear inequality function, ( ), 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 ( ) at all of the time instances , where it is likely that a constraint may be violated by the estimator ( ) > −ϵ, as follows: By rearranging Equation (4), the linearized inequality description in Equation (5) can be formulated as follows: Figure 6 shows an example for the calculation of the nonlinear constraint function. In the left plot, a street is marked with the left ( ) and the 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 ( ) > 0, ∀ ∈ [19.6 22.1] ∧ [29.4 30], as can be seen in the right plot of Figure 6. By means of the path normal vector (n I P (s i )), represented in the inertial coordinate system (Equation (2)), an inequality function c(x) ≤ 0 is calculated that penalizes the positions outside of the roadway borders, as follows: This nonlinear inequality function, c(x), 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 c(x) at all of the time instances t k , where it is likely that a constraint may be violated by the estimator c(x k ) > − , as follows: By rearranging Equation (4), the linearized inequality description in Equation (5) can be formulated as follows: Figure 6 shows an example for the calculation of the nonlinear constraint function. In the left plot, a street is marked with the left l(s) and the right r(s) 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 [29.4 30], as can be seen in the right plot of Figure 6.  Assuming that only the vehicle yaw rate ( . ψ C act ) 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: Assuming that only the vehicle yaw rate ( ) 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:

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: where in the nomenclature of min‖ ( )‖ is denoted a with , and is a weighted least squares minimization problem of a function with respect to . The functions and are the nonlinear state functions, resprectivley, of the model output equations. The matrix = ( ) and the vector represent the initial guess values of the covariance and the system state. The matrix 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 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 [m] [m] Observer act C corrected Observer act C corrected & constrained

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: where in the nomenclature of min and the vectorx 0 represent the initial guess values of the covariance and the system state. The matrix 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 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 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.  . At the filter initialization ( ), only one measurement is available, and therefore the measurement storage must be filled with < steps before the window starts to move. Afterwards, the window length is kept constant and all of the past measurements are taken into account (the window with 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. Chapter 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 realtime 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 chapter. 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.
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 (ξ k ) is assembled with M subsequent discrete state vectors within the current estimation window. The optimization cost function g(ξ k ) in Equation (9) is composed of the following three parts.
The first argument · 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 (x k−M ) is coincident with the corrected Kalman filter state estimation (x + k−M ) weighted with the information matrix (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, I + k−M , is calculated via the inverse of the covariance matrix P + k−M −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, P = L · U. The inverse of the lower triangular (L) can be efficiently calculated by the LAPACK routine DTRTRI, and therefore, the propagated information matrix results in I + k−M = L −1 · L. The second argument, · R −1 is the, over the time instances (k − M to M) summarized and with R −1 weighted, difference between the available measurements y m i and the output equations of the underlying prediction model h(x i ) as function of the states of the current optimization vector ξ k .
Finally, the third argument, · Q −1 , denotes the, over the time instances (k − M to M) summarized and with Q −1 weighted, difference between the optimized states x i and the open loop integrated prediction model states x int in Equation (10). x int is calculated only once per optimization step by a simulation usingx + k−M as start vector and 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).
It is introduced so as to guarantee that the oldest estimation ( ) is coincident with the corrected Kalman filter state estimation ( ) weighted with the information matrix ( ). Note that in every moving horizon estimation step, a Kalman filter step from − − 1 to − is performed to fulfill the Kalman state estimation theory-Equation (7). The information matrix, , is calculated via the inverse of the covariance matrix ( ) . 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, = ⋅ . The inverse of the lower triangular ( ) can be efficiently calculated by the LAPACK routine DTRTRI, and therefore, the propagated information matrix results in = ⋅ .
The second argument, ‖⋅‖ is the, over the time instances ( − to ) summarized and with weighted, difference between the available measurements and the output equations of the underlying prediction model ( ) as function of the states of the current optimization vector .
Finally, the third argument, ‖⋅‖ , denotes the, over the time instances ( − to ) summarized and with weighted, difference between the optimized states and the open loop integrated prediction model states in Equation (10). is calculated only once per optimization step by a simulation using as start vector and as input vector. In Figure 9, a qualitative graphical interpretation of the optimization problem for a scalar problem with = = 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 are available, the measurements and the model inputs are appended to the buffer. 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
Fill ring buffer with measurements and system inputs: if k < M then append u k to u and y m k to y m else left shift one entry of u and y m and append u k respectively y m k end if Optimize over stored measurement window (Equation (8)

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
In step 1 (line 1), an initial solution, ξ 0 k , is needed. Well-proven strategies for its calculation are an open loop integration of the prediction model from x k−M to x k , or a left shift of the last optimization vector ξ k−1 and appending an open loop integration from x k−1 to x k . In the unconstrained (lines 3-4) case of the second step, the gradient ∇g ξ j k of the descent direction can be directly calculated, as shown in Algorithm 3.
Also, if equality constraints (lines 5-7) should be incorporated, A · ξ = b, the descent direction must be projected on these by means of the Moore-Penrose pseudoinverse [25] In most of the MHE applications, this step can be neglected (i.e., P † (A) = 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, G pa,i , in the null space of the linear constraints must be determined. The descent direction is projected on the active constraints G pa,i r pro,j G T pa,i . More algorithm implementation details can be found in the literature [24].

Algorithm 3. MHE calculation by means of functional mockup unit (FMU) evaluations
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 ξ j k 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 ξ j k + ∆s 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 x int,i is only performed once in this approach.
In step 4 (line 10), the actual optimization step, ξ j+1 k , 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 g ξ j k − g ξ j+1 k < 10 · , 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 (ξ 0 k ), 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 (ξ j k ).

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 (ξ 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 (x int ) in segments where no measurements are available.

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 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 ESTM s = 200 ms, T BC s = 4 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 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. Z denotes the permutation matrix between the inputs of the BC FMU and the states of the ESTM FMU. The first implementations of the constrained observer were based on a single prediction FMU that combined the ESTM prediction model (Chapter 3) and the boundary constraint evaluation (Chapter 2.2). To separate the estimated states from the state of the constraint evaluation (the path parameter ), 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 = 4 ms. This is due to the fast dynamics of the control loop to determine  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 to , it is checked whether any constraint may be potentially activated ( ) 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 and (orange-dotted). In this example, the state propagation of the Kalman filter (step 3) causes a violation of , 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, , 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 is limited via the simplified Newton descent search, as described in Appendix B.  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 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,x + cstr k−M , 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 x cstr int is limited via the simplified Newton descent search, as described in Appendix B.

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 = ( , , … , ) are not coupled with each other between the sample points ( ). 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 = 200 ms, this may lead to a physically unfeasible set , 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 to the current time instance is denoted as . The set of optimized state vectors in the -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 , denoted with a red circle, as follows: In the depicted qualitative example (cf. Figure 13), one can see, that through the evolution of the optimization process a displacement , ≠ 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:

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 = ( , , … , ) are not coupled with each other between the sample points ( ). 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 = 200 ms, this may lead to a physically unfeasible set , 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 to the current time instance is denoted as . The set of optimized state vectors in the -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 , denoted with a red circle, as follows: In the depicted qualitative example (cf. Figure 13), one can see, that through the evolution of the optimization process a displacement , ≠ 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:

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 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 ms, this may lead to a physically unfeasible set ξ 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 x 0 int . The set of optimized state vectors ξ j k 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 x L int,i denoted with a red circle, as follows: In the depicted qualitative example (cf. Figure 13), one can see, that through the evolution of the optimization process a displacement x L int,i 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:  Figure 13. Schematic representation of a moving measurement window.
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 ( , ) and the optimized stated vector ( ) by means of the user tunable weighting matrix ( ). Besides performing the integration ( , ) by means of the FMU (Equation (12)), it is necessary to approximate the integration rule for the gradient ( ) 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 , 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: 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 , in which the operator ± denotes that all of the values are additively added to the existing entries from the earlier loop. 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 (x L int,i ) and the optimized stated vector (x i ) by means of the user tunable weighting matrix (Q MS ). Besides performing the integration (x L int,i ) by means of the FMU (Equation (12)), it is necessary to approximate the integration rule for the gradient g(ξ k ) 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 x L int,i 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: 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.

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.
In Equation (15), all of the active measurements are denoted with an additional subscript ((·) a ) for the active sensors. In the construction of the active measurement matrix (y m a ), 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, P + k−M . In the case that less sensor information is available, larger values of det(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 I + k−M and the initial guessx + k−M also needs to be considered in the Kalman step.

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 y m a 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.

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

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 (τ (ST),W ), angular velocities (ω (ST),W ), and steering angles (δ W ) quantities, as well as the Correvit optical measurement unit (used to measure the vehicle's longitudinal and

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 = 200 ms. Moreover, the usage of the vehicle yaw rate ( ) can actively contribute to improve the measurement, whereas the incorporation of the vehicle lateral acceleration ( ) downgrades the observer performance. The output equation, = ⋅ ( + ) , shows clearly that the lateral acceleration is algebraically cross-coupled to the vehicle yaw rate ( ). 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

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 ms. Moreover, the usage of the vehicle yaw rate ( . ψ C ) can actively contribute to improve the measurement, whereas the incorporation of the vehicle lateral acceleration (a C y ) downgrades the observer performance. The output equation, , shows clearly that the lateral acceleration is algebraically cross-coupled to the vehicle yaw rate ( . ψ 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 u-and the position is strongly drifting away from the planned trajectory, caused by the evolving position integration error (compare equations for dx C /dt, dy C /dt-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.
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 -and the position is strongly drifting away from the planned trajectory, caused by the evolving position integration error (compare equations for d d ⁄ , d d ⁄ -in Equation (A17)). 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. Observer act C corrected [m] [m] Observer act C , C act I corrected Figure 16. Comparison of different measurement incorporations.

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, 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 d = 0 . . . 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 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 T mean in [ms] 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 T mean = 136.8 ms), the algorithm can be executed faster than in real-time (T mean 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 ( Unfortunately, the SR-EKF algorithm could not be stabilized to give a feasible estimate for all of the measures, especially the side slip angle (β 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. Unfortunately, the SR-EKF algorithm could not be stabilized to give a feasible estimate for all of the measures, especially the side slip angle ( ) 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 = 2 is the largest section, which correlates to a delay of 400 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 = 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 ms, the results are still reliable.   The section with n d = 2 is the largest section, which correlates to a delay of 400 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). Unfortunately, the SR-EKF algorithm could not be stabilized to give a feasible estimate for all of the measures, especially the side slip angle ( ) 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 = 2 is the largest section, which correlates to a delay of 400 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 = 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 ms, the results are still reliable.   In the last two rows of the simulation study, two configurations with n 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 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 = 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 (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.

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 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. With initial reference refreshing 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 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). 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 = 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 (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.

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 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. With initial reference refreshing 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.

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 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. 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 , estimate fit of about 98%, and by mean of the delay compensation technique, this value is, even with a delay of = 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 ( ) quality for all of the configurations is very high, whereas the yaw rate ( ) and side slip angle ( ) do vary about 5%, but still have reasonable estimates. Figure 21. Experiment with vehicle standstill estimation (red) vs. reference (black). test drives and data acquisition. Moreover, the author's special thanks go to BMBF for supporting the research on the Modelica-based observer framework in the ITEA project MODRIO (BMBF support code 01IS12022G), and the current research on the embedded FMI realization and standardization in the ITEA project EMPHYSIS (BMBF support code 01IS17023B).

Conflicts of Interest:
The author declares no conflict of interest.

Appendix A. The Extended Nonlinear Single Track Model Equations
The set of ordinary differential equations of the ordinary differential equation form ψ C , ψ C , x C , y C can be given as follows: The vehicle side slip (β C ) is the angle between the vehicle origin coordinate system and the actual vehicle velocity vector (v C ). The yaw angle (ψ C ) and yaw angle rate ( . ψ C ) describe the rotation with respect to a fixed inertial system (x I , y I ), in which the vehicle positions x C , y C are also expressed. Additional quantities are the vehicle mass (m), the yaw moment around the center of gravity (M C Z ), and the corresponding vehicle yaw inertia (J C Z ). To prevent division by zero, the denominator of the vehicle side slip state ( . β C ) is calculated by use of Equation (A2), which limits the minimum velocity to v C min (only valid for small values of v C min ) .
The inputs to the model are the commanded torque set-points to the in-wheel motors and the per axle averaged steering angles u C = τ W 1 , τ W 2 , τ W 3 , τ W 4 , δ W f , δ W r . The forces (marked blue in Figure 3) to the vehicle body center of gravity are calculated by considering the geometric dependencies to the instantaneous steering angles δ (·) , as follows: The lateral forces of the tire are calculated by trigonometric functions based on Pacejka's tire model [18], whose input variable is the wheel's side slip angle α W (·) , as follows: Herein, the factor B denotes the tire's stiffness, C is an aspect ratio for the calculated force, D the maximum value of the curve, and E is the modulus of bending rupture. For the longitudinal tire dynamics, a simplified modeling without the consideration of the actual wheel slip was used, as experiments in combination with observer algorithms yielded poor and unstable results, and the vehicle's acceleration range of the maneuvers considered here is limited, as follows: Finally, the speed dependent rolling resistance ( f rv ( and the longitudinal air drag (F C Air x ) are calculated according to Equation (A6). By use of the "if-statement", the vehicle is prevented from rolling backwards in case of standstill on a planar surface, and no braking torque is applied, as follows: In the following the result of the STM parameter, identification by means of a nonlinear model based optimization is given (cf. Table A1). The derived model is implemented in Modelica and later exported as an FMU for the observer, described in Section 3.2. For the optimization process, the DLR Optimization Library [29] has been utilized. As a tuning algorithm, the pattern search method was chosen. For training and validation, data real world experiments on vehicle test tracks with ROboMObil have been used. ROMO'S ESTM optimized parameters are given in Table A1.

Appendix B. A Constraint Projection Method for Kalman Filters
In the literature [2], a survey on different methods for incorporating inequality constraints to Kalman filter algorithms is given, and three methods for nonlinear Kalman filters that fit for constraints of the type c(x) ≤ 0, which are neither contradictory nor redundant, and only one constraint is active at the same time, are discussed. The algorithm applied here is based on a simplified Newton descent search (i.e., without considering the second order derivative), which projects the a posteriori estimation (x + k ) of a Kalman filter on the constrained surface. The optimization objective is formulated as follows: The idea behind this algorithm is to perform a descent search along the gradient (∇c x + k ) calculated at the pointx + k , until the constraint equation c x P k ≤ 0 holds again. Graphically this can be interpreted as shown in Figure A1. The idea behind this algorithm is to perform a descent search along the gradient ( ( )) calculated at the point , until the constraint equation ≤ 0 holds again. Graphically this can be interpreted as shown in Figure A1. This optimization task can be formulated with the method of Lagrange multipliers. Equation (A8) denotes the above formulated optimization objective in a general description, as follows: Therefore, the optimal solution of this unrestricted minimization problem is formulated as follows: This approach is now applied to the estimation projection optimization problem (Equation (A7)) and the searched restricted estimate (x P k ) is calculated using the Lagrange multiplier (µ) and the gradient of the active constraint (c(x)), as follows:x P k =x + k + µ · ∇c(x) s.t.c(x) = 0 c x + k + µ · ∇c(x) = 0 (A10) To fulfill Equation (A10), the approach is expressed with a simplified Newton descent search algorithm by means of a scalar zero search F(µ) = 0, with respect to the Lagrange variable µ, as follows: This can be implemented as an iterative search algorithm, which determines µ such that the condition F(µ) = 0 holds within a predefined maximum number of calculation steps. The pseudo code for the algorithm is given in Algorithm A1. Algorithm A1. Simplified Newton descent search algorithm for constrained estimation projection.
With W weighted least squares minimization of function e with respect to x"subject to" if the problem is restricted