A Factor-Graph-Based Approach to Vehicle Sideslip Angle Estimation

Sideslip angle is an important variable for understanding and monitoring vehicle dynamics, but there is currently no inexpensive method for its direct measurement. Therefore, it is typically estimated from proprioceptive sensors onboard using filtering methods from the family of the Kalman filter. As a novel alternative, this work proposes modeling the problem directly as a graphical model (factor graph), which can then be optimized using a variety of methods, such as whole-dataset batch optimization for offline processing or fixed-lag smoothing for on-line operation. Experimental results on real vehicle datasets validate the proposal, demonstrating a good agreement between estimated and actual sideslip angle, showing similar performance to state-of-the-art methods but with a greater potential for future extensions due to the more flexible mathematical framework. An open-source implementation of the proposed framework has been made available online.


Introduction
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 [1].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 [2].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 [3].
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 [4,5,6].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 [7,8,9,10,11], 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 [12,13] 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.[14] 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 [15] 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 quasistate 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 [16], 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, [17] 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.[18] 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 [19] the Magic Formula (or Pacejka tire model [20]) 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 [21], 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 [22] 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 work [23] 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) [24,25,26,27] 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 [28].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 [29] in Section 4. Finally, conclusion are drawn in Section 5.

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 u and v 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 [30]: 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 u, that is guaranteed within a small timestep ∆t.This model is characterized by two Degrees of Freedom (DoFs), namely the vehicle lateral velocity v and the yaw rate r.Hence, two equations of motion are sufficient to fully describe its behavior over time: where m is the vehicle mass, J z the moment of inertia with respect the yaw axis, C f and C r the front and rear cornering stiffness respectively, l f and l r 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 v, therefore one can consider as DoFs β and r in place of the couple v and r.From Figure 1, the relation between v and β is the following: where the approximation comes from the assumption of small angles, as stated above.Therefore: considering the constancy of u within each time-step ∆t.The equations (1) becomes: Figure 1: single-track model By considering equations ( 4), β and r are variables of the vehicle lateral dynamics and the other terms are considered known parameters.
It is worth noting that tha yaw rate r 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 a y is considered as a further measurement.Both a y and r are generally already available onboard common vehicles via the ESP system.Therefore, the following set of measurements is here considered: As a matter of practicality, the equations ( 4) and ( 5) in their continuoustime form need to be converted in the following discrete-time representation: The above equations are obtained from the forward Euler integration in the time step ∆t = t k − t k−1 .
3 Factor Graph for vehicle lateral dynamics

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 r 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 [22].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 k.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 e k−1 β and e k−1 r 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 r, whilst the binary factor minimizes the difference between the actual lateral acceleration a y 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 u 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 variables X k to minimize at the k − th instant are [β k r k ] T , equations ( 7) can be written in compact matrix form as: where T By defining the state update vector as The target is the estimation of the state update vector ∆ k 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 z k the k − th vector of measures, Q k 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 A is a large matrix collecting all matrices A k .From the calculation of the state update vector: Xk = Xk−1 + ∆k , where A, b and ∆ grow over time.
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 e 1 β and e 1 r for the estimation of the side slip angle and the yaw rate respectively, while factors e 2 ay and e 2 φ 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 A matrix and vectors b and ∆ are shown for the first three steps of Figure 3.
In Table 1 A matrix and b vector are reported, instead the ∆ vector for the Table 1: A matrix and b vector for the first three steps of estimation first three steps is that in equation (15).
By looking at the matrix A 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 β 1 and r 1 .Hence, the whitened matrix A 1 is a 4 × 2 matrix, where each row is associated to a specific factor of Figure 3.The whitened matrix A 2 is composed of a square matrix containing the first and second block together and so forth.Vectors b 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 A and vector b in Table 1, the interested reader can refer to Appendix.
Matrix Q k 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 σ φ = 10 −8 [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 σ ay = 10 −2 [m/s 2 ].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 σ β = 10 −5 [rad] and σ r = 10 −4 [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 M 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 M = 5 samples is heuristically found to be a a good trade-off between computational speed and goodness of estimation.By looking at Figure 3, 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 A matrix will be composed by six blocks following the rationale of Table 1 and vectors b and ∆ grow accordingly.

Results
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 [29].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., [13].Figure 4 shows the estimation of the vehicle side slip angle β and yaw rate r 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 r 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,  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.

Conclusions
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.
Vector b grows accordingly with A 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 b 2 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 b 6 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.

Figure 2 :
Figure 2: FG relative to the linear bicycle model

Figure 3 :Figure 3
Figure 3: FG relative to the first three steps of the estimation problem: into the black dashed window the first step with priors and into the gray dash-dot one the generic step, with involved factors 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: e 0 pβ = β 1 − β 0 (14a)

Figure 4 :
Figure 4: Vehicle side slip angle β estimate at the top and yaw rate r at the bottom by using a KF-based observer applied to the linear bicycle model

Figure 7 :
Figure 7: Vehicle side slip angle β estimation by considering both KF and FG estimators with a window of 5 samples

Figure 8 :
Figure 8: Vehicle side slip angle β estimation by considering both KF and FG estimators with a window of 5 samples, for another set of real data

Figure 9 :
Figure 9: Vehicle side slip angle β estimation by considering both KF and FG batch estimators Vehicle side slip angle β estimate at the top and yaw rate r at the bottom by using FG applied to the linear bicycle model