In the last 30 years a large amount of research and papers have been devoted to side-slip estimation, because it represents a fundamental feature for vehicle dynamics LenzoReview. Despite its central role, a direct measurement of the side-slip angle value over time during vehicle motion is often impractical, due to technical and economic reasons. For instance, a cheap but few practical solution is to use measures coming from Global Position System (GPS) that can provide the position of the receiver without any numerical integration and then derive the velocity of the vehicle using Doppler measurements LenzoReview68. Nevertheless, GPS receivers have intrinsic issues such as temporary signal unavailability due to surroundings such as trees, tall buildings, mountains and tunnels, as well as different working frequencies with respect to other sensors involved in vehicle dynamics control, such as accelerometers, gyroscopes and so on. On the other hand, a direct measurement of the vehicle side slip angle can be achieved by high precision optical sensors, but they are too much sophisticated, still in Research and Development (R&D) stage and too much expensive to be suitable for production vehicles OpticalSensor.
For these reasons, the side slip angle estimation continues to play a significant role in vehicle dynamics, attracting noticeable interest in the academic and industrial worlds LenzoReview2; Mastinu; LenzoReview4. Several methods have been developed and described throughout the scientific literature, which make use of different models and estimators. The most common way relies on model-based observers LenzoReview13; LenzoReview15; LenzoReview36; LenzoReview49; LenzoReview30, which make use of a vehicle reference model for state and parameters estimators. Different levels of sophistication can be obtained in order to achieve a phenomenon description as accurate as possible. One of the most common combinations that can be found in the literature is the bicycle (single-track) model as an observer for a Kalman Filter (KF). This arrangement allows the estimation of states and parameters in the same time. For instance in ReinaPaianoBlanco; ReinaMessina a linear bicycle model is used in combination with an Extended Kalman Filter (EKF) for the estimation of the side slip angle and vehicle parameters in the same time. The former, estimates lateral velocity and vehicle mass, by correcting also the bias of the gyroscope, instead the latter estimates the front and rear cornering stiffness and the side slip angle directly as a dynamic state. CKFlinear exploits the same linear model, but use a Cubature Kalman Filter (CKF) as estimator for obtaining the side slip angle and other states from common measures (i.e. accelerations, vehicle longitudinal velocity, etc.). In LenzoReview16 a combining approach between kinematic and dynamic model is carried out, since the kinematic model performs well during transient maneuvers but fails in the steady state conditions. Therefore, the information provided by the kinematic formulation are exploit to update the single-track model parameters (i.e. the tire cornering stiffness), while the dynamic state observer is used in nearly quasi-state condition. The steady state or transient conditions are discriminate via fuzzy-logic procedure. Furthermore, the bicycle model can be also coupled with nonlinear tire models, as in LeanzaVSD6, where the authors aim to estimate the side slip angle of a heavy-duty vehicle by using a Rational tire model and an EKF as estimator. This study showed good estimation performance, but at the cost of an overparametrised and complex model. On the other hand, the four-contact models provide a better description of vehicle dynamics, but obviously at the cost of a greater number of parameters and an increase in the complexity of the systems. As instance, LenzoReview18 uses an EKF applied to a four-contact vehicle model with a Dugoff tire model in order to estimate the side slip angle and the the tire/road forces. LenzoReview21 uses again a four-contact model, but with a semiphysical non-linear tire model “Unitire” and applies a redeuced-order Sliding Mode Observer (SMO), evaluating the performance of the proposed method by means of simulation and experiments. In LenzoReview55 the Magic Formula (or Pacejka tire model pacejka2002tyre) is exploited, where a preliminary filtering on vertical forces is performed via linear KF in order to estimate the roll angle and then an EKF is applied to the four-contact vehicle model to achieve the side slip angle. Again, Magic Formula with a four-contact vehicle model is used in LeanzaVSD8, where a dual estimation scheme was adopted: the side slip angle and tire/road forces by means of a Dual Unscented Kalman Filter (UKF) algorithm and the Pacejka tire parameters by solving a nonlinear Least Squares (LS) problem. However, the sophistication of these models might be unpractical in real applications. To overcome this issue, some authors, as LeanzaReinaMantriota
rely on direct causality equations without the need of any explicit tire model. In that research different estimators are developed, as the standard EKF, the CKF and Particle filtering (PF) and the results are evaluated by using a vehicle model with 14 Degrees Of Freedom (DOFs) which performs standard maneuvers. The workLenzoReview58
performs vehicle sideslip angle and road bank angle estimation via simple algebraic relationship in real time, based on two online parameters identification techniques, combining single-track model with roll and tire slip models and force due to bank angle. Moreover, with the use of a lateral G sensor signal and by including road bank angle effect, the front and rear cornering stiffness and vehicle side slip angle are identified, in the absence of any a priori knowledge on the road bank angle. In order to completely overcome the need for a vehicle model of any kind and its related complex set of parameters, different approaches based on Artificial Neural Networks (ANN)LenzoReview101; LenzoReview106; LenzoReview96; LenzoReview102 are widely regarded nowadays, since they are suitable to model complex systems just using their ability to identify relationships from input–output data.
In this paper a novel estimation technique grounded in the Factor Graphs (FGs) theory is presented. To the best of authors’ knowledge, it is the first time that a FG-based observer is proposed in the automotive field. A FG is a type of probabilistic graphical model that can be used to describe the structure of an estimation problem FGtheory1. Factor graphs are bipartite graphs comprising two kinds of nodes: variable nodes, and factor nodes. The variables nodes represent the unknown data to be estimated, while factors represent cost functions to be minimized, modeling relationships between the variables. Each factor node is only connected to the variables that appear in its cost function, giving the FG its characteristic aspect of sparsity. As shown below, by applying this estimation approach to the classical linear bicycle model, good estimation accuracy can be achieved even in the non-linear regions of the tyre behaviour compared to the celebrated Kalman filtering, while preserving the inherent simplicity of the linear model and the use of few parameters (i.e., the front and rear cornering stiffness).
The paper is organized as follows. In Section 2 the equations relative to the linear bicycle model are recalled and reorganized in order to directly obtain the side slip angle. Section 3 introduces the estimation problem with the use of FGs, applied to the case at hand with details on its implementation. Results obtained with the proposed method are evaluated by means of real data gathered in the Stanford database StanfordData in Section 4. Finally, conclusion are drawn in Section 5.
2 Vehicle dynamic model
Lateral dynamic represents a basic challenge in vehicle behavior, because lateral velocity is usually not directly measurable for practical and/or economic reasons. Nonetheless, it represents a fundamental information, since it influences the entire vehicle dynamics, especially the direction of the motion. As a matter of fact, the total velocity of the vehicle Center of Gravity (CoG) is the vectorial sum of the longitudinal and lateral velocities and respectively. Figure 1 shows the model used in this paper for dealing with the vehicle lateral behavior. It is known as “Bicycle” or “Single-Track” model and it is based on the following simplifications LinBicMod: equal internal and external dynamics so that tires of the same axle can be fused, leading to the model’s bicycle-like appearance; linear range of the tires, thus lateral forces are a linear function of slip angles by means of a specific coefficient named Cornering Stiffness; rear-wheel drive; negligible motion resistance; small angle approximation to preserve the linearity of the model; constant longitudinal velocity , that is guaranteed within a small time-step . This model is characterized by two Degrees of Freedom (DoFs), namely the vehicle lateral velocity and the yaw rate . Hence, two equations of motion are sufficient to fully describe its behavior over time:
where is the vehicle mass,
the moment of inertia with respect the yaw axis,and the front and rear cornering stiffness respectively, and the distance of the CoG from the front and rear axle and the front steering angle.
Usually, the side slip angle is of interest in the study of lateral dynamic, in place of the lateral velocity , therefore one can consider as DoFs and in place of the couple and . From Figure 1, the relation between and is the following:
where the approximation comes from the assumption of small angles, as stated above. Therefore:
considering the constancy of within each time-step . The equations (1) becomes:
By considering equations (4), and are variables of the vehicle lateral dynamics and the other terms are considered known parameters.
It is worth noting that tha yaw rate can be directly measured by means of a vertical gyroscope, instead the side slip angle usually has to be estimated. Although also can be measured via GPS and equation (2), often this type of measurement is unstable and unreliable due to the presence of shady areas of the GPS signal, such as tunnels or mountains. Hence, the need to estimate indirectly the side slip angle. To achieve this estimation the lateral acceleration is considered as a further measurement. Both and are generally already available onboard common vehicles via the ESP system. Therefore, the following set of measurements is here considered:
3 Factor Graph for vehicle lateral dynamics
3.1 The estimation problem
Discrete-time equations (6) refer to the vehicle system and to the measurements gathered for a correct estimate of the DoFs, in particular for the estimation of , since is directly measured. The most common way followed throughout the literature relies on a state-space representation of the above equations, writing two compacted matrix equations: one for the propagation of the model over time and one relating to the measures, both as a function of suitable state variables. Therefore, a recursive routine, often based on Kalman filtering, is performed to achieve the correct estimate of the unknown state variables LeanzaReinaMantriota.
In this paper, an alternative approach is suggested for the estimation of the unknown variables, without passing through the state-space form, namely the Factor Graphs (FGs), a method belonging to the family of probabilistic graphical models. The objective is to minimize given cost functions in each time step. The FG relative to the problem at hand is displayed in Figure 2 for a generic time-step . Each factor (filled circle in the figure) is connected with the unknown variables involved in the cost function that the factor minimizes. Cost functions come directly from equations (6):
The cost functions and are handled, respectively, by the black and gray ternary factors (Figure 2). On the other hand, the unary and binary factors are referred to the measurements: the unary factor minimizes the difference between the actual yaw rate and the associated unknown , whilst the binary factor minimizes the difference between the actual lateral acceleration and the estimated one.
In principle, the value of the error functions (7) has to be equal to zero, but actually an uncertainty is associated to each factor due to the stochastic nature of the estimation problems. First, measurements are collected through the use of sensors with inherent precision and accuracy, on the other hand, the uncertainties associated to the model include mathematical approximations and uncertainties of the inputs and .
Without loss of generality, with the assumption of zero-mean Gaussian probability density function (pdf) associated to the uncertainty in each factor, the minimization problem can be reduced to a linear least squares problem. Since the unknown variablesto minimize at the instant are , equations (7) can be written in compact matrix form as:
By defining the state update vector
state update vectoras , the next equation holds:
The target is the estimation of the state update vector via least squares. It is important to remark that the problem at hand is linear, therefore the following Weighted Least Squares (WLS) problem is set:
with the vector of measures, the error covariance matrix and the parenthesis is the prediction error. The argument of the sum is the Mahalanobis norm:
and from the next replacements:
one obtains the following simple least squares problem:
where is a large matrix collecting all matrices . From the calculation of the state update vector: , where , and grow over time.
Figure 3 displays the first three steps for the estimation problem at hand. The first step, surrounded by the closed dashed black line, includes two priors factors (the unitary gray ones) in place of the dynamic factors. The priors factors minimize the difference between the first estimates and the initial guess of the unknown variables, enforcing known initial conditions; therefore reliable initial guess is assumed, with values very close to the correct ones. Their error functions are:
The other steps follow the logic of the second one, contained in the closed dash-dot gray line. This closed line encloses four unknown variables and four factors: two belonging to the dynamic model and two for the measures. By looking at the first dynamic step enclosed in the gray line, the dynamic model is managed by the ternary factors and for the estimation of the side slip angle and the yaw rate respectively, while factors and introduce new measurements and minimize the difference between what is effectively measured and the what is expected from the model.
In order to understand how to move from the graphical representation of factors to the minimization problem, the matrix and vectors and are shown for the first three steps of Figure 3.
By looking at the matrix in Table 1, it is composed of blocks each one corresponding to a single time-step of the FG, as shown in Figure 3. In fact, the first block corresponds to the black dashed window containing the two priors, two measures and the first two unknown variables and . Hence, the whitened matrix is a matrix, where each row is associated to a specific factor of Figure 3. The whitened matrix is composed of a square matrix containing the first and second block together and so forth. Vectors and grow accordingly. It is worth noting that the resulting matrix is a sparse matrix, with elements concentrated near the diagonal. This characteristic can be exploited in the minimization problem of equation (13). For further details on the elements of matrix and vector in Table 1, the interested reader can refer to Appendix.
Matrix collects the uncertainty associated to each factor. Therefore,
contains the weights of the factors, representing the tuning parameters of this estimation problem. As a matter of fact, the greater the confidence given to a factor, the higher the values associated with its weight and then the lower the corresponding standard deviation. In this case, by looking at equations (7) it is clear that the factor associated to the yaw rate measurement has a large weight, being precisely measured, therefore, the standard deviation associated to the error of equation (7c) is very small. In this work a value of [rad/s] has been assumed. Instead, the error associated to the equation (7d) relative to the lateral acceleration measurement is taken larger because a linear model is compared with actual lateral acceleration, therefore a smaller weight is associated to that factor, by assuming a . Regarding the error relative to the model equations, a high weight is associated to the corresponding factors for the well-known reliability of the model, with standard deviations equal to [rad] and [rad/s] for the side slip angle model and yaw rate model respectively.
During vehicle motion, a huge amount of data can be collected, and performing a batch estimation on a too long time window would be impractical. Therefore, a fixed-lag-smoother is applied by considering a sliding window, which contains a fixed number of samples. Hence, the minimization is achieved on these samples and the most reliable estimate is retained as a set of priors for the next estimation. The length of the sliding window is another tuning parameter of this estimation approach. In this work, a window encasing
samples is heuristically found to be a a good trade-off between computational speed and goodness of estimation. By looking at Figure3, this window will be composed by one closed dashed black line with the priors consisting of the previous estimate (or of the initial conditions for the first window) and by five closed dash-dot gray line, where the indices of every factors change accordingly with the time step. Of course, the matrix will be composed by six blocks following the rationale of Table 1 and vectors and grow accordingly.
This section collects results obtained with the proposed approach and evaluated by means of real data acquired by an instrumented Ferrari 250 LM Berlinetta GT and made publicly available by Center for Automotive Research at Stanford StanfordData. A Global Navigation Satellite System (GNSS)-aided Inertial Navigation System provides overall vehicle body motion resulting in centimeter-level position accuracy and slip-angle direct measurement. The 2014 Targa Sixty-Six event served as the data collection venue that took place at the Palm Beach International Raceway, a 3.3 km-long track featuring of 10 turns and a 1 km straight.
For a thoroughly understanding of the advantages of the proposed method, the results obtained with the celebrated linear KF are first shown, which are well known in the automotive field. For the sake of brevity, the KF equations and the state-space form of the problem at hand is here omitted, since it is widespread in the literature, e.g., ReinaMessina. Figure 4 shows the estimation of the vehicle side slip angle and yaw rate obtained via linear Kalman filtering during a full lap of the Palm Beach International circuit. As expected, the unknown state associated to the yaw rate is estimated very well, since it is directly measured. In fact only the sensor noise is filtered out leading to an RMSE value of 0.27 deg/s. Note as well the relatively good estimation of (RMSE of 0.87 deg) despite the approximation of assuming a linearized model. This is explained by the relatively small range of values observed in practice and, in particular, for this race dataset.
In Figure 5 the estimation of the side slip angle and yaw rate is shown, but this time obtained from the FG-based observer. As seen from the figure, the estimator can track more closely and even for higher values than KF notwithstanding it is based on a linear model. A corresponding RMSE of 0.57 deg is achieved that is a 34% improvement over the KF implementation. Note that numerical estimators used for the FG are capable of optimally handle strongly non-linear model due to their iterative nature, but in this particular case this advantage is not exploited due to the linearity of the model. In fact, the adopted nonlinear solver (Gauss-Newton) only runs a single iteration before detecting it reached the optimum.
On the other hand, one fundamental difference between the FG solver and the KF is the use of a sliding-window estimator for the former, which is then capable of smoothing out sensor noise much more effectively than KF is able to by sequentially processing time steps one by one. However, for very small angles (see detailed view in Figure 6), KF seems to provide a less noisy output. This effect may be explained by the use of different tuning parameters in both approaches.
For completeness, another experiment performed on a different set of real data gathered in a different race session is displayed in Figure 8. Again, the KF shows the best performance in the linear region characterized by small values, but fails for higher values of side slip angles. Vice versa, the FG is noisier for small values but well estimates the actual side slip angle for higher values, that are the most interesting one for the vehicle handling and passengers safety.
In order to show the potential of the proposed method, the batch estimation is reported in Figure 9 on the total number of samples acquired. As one can see, both the advantages of KF and FG with sliding window method are obtained, consisting of a good estimation for high values of beyond the linear region and a very smooth estimate especially in the linear one, similarly to the KF, where a better visualization has been achieved at the bottom of the figure in a shorter time length.
In this paper an estimator grounded on the Factor Graph theory is applied for the first time for estimating the side slip angle of a vehicle during motion. A linear single-track model is considered in this research as the vehicle model the estimator. The performance of the proposed estimation approach are evaluated by using real data and contrasted with those obtained from the standard Kalman Filter. It was demonstrated that the proposed method estimates accurately the side slip angle even when its value exceeds the linearity range, which represents the more critical situation in terms of vehicle handling and passenger safety, and thus the knowledge of the correct value becomes more important. On the other hand, for small values of the side slip angle, the Kalman filtering preserves its superiority, as expected. Nonetheless, a batch estimation leads to a good estimate of the side slip angle with a level of noise similar to that achieved by using the KF. However, it is worth pointing out that higher values of the side slip angle can be correctly estimated by keeping a linear bicycle model with very few parameters with respect to other more complex vehicle models, representing an important aspect in terms of practical implementation on real commercial vehicles.
Next steps will take into account FGs applied on more complex (non-linear) vehicle models, in order to investigate the effectiveness and the ability to achieve even more accurate estimates and also other aspects related to vehicle dynamics that cannot be represented with the bicycle model.
The financial support of the projects: Agricultural inTeroperabiLity and Analysis System (ATLAS), H2020 (Grant No. 857125), and multimodal sensing for individual plANT phenOtypiNg in agrIculture rObotics (ANTONIO), ICT-AGRI-FOODCOFUND(GrantNo. 41946) is gratefully acknowledged.
In this Appendix, details about matrix and vector of Table 1 are provided, for the convenience of the interested reader. By starting with the matrix, two different kinds of block matrix can be found: one associated to the first step with priors and one associated to the others steps with the dynamic model. By recalling that:
the former kind of block matrix is a matrix where:
The longitudinal velocity is that measured at the specific time step on wich the priors are applied. Foe instance, for the first time step with priors associated to initial guess, . The other terms are constant in this work. The latter kind of block matrix is a matrix obtained as in the following:
Vector grows accordingly with matrix. By looking at equation (12b), the parentheses is the prediction error between actual value and estimated one. Therefore:
for priors the actual value is the guess and the function is the difference between these two values, hence:
and follows the same rationale;
for dynamic factors the actual value is zero, since it is the difference between the forward value and the same obtained by integrating the differential equation, for instance:
and follows the same rationale;
finally measures follow this rationale (only yaw rate is taken for the sake of brevity):
Of course, all indices change according to the time step.