LAB 2: Lateral Dynamics – State

estimation

SD2231– Applied vehicle dynamics control

May 2024

Louis Colette - Aymane Moutchou

Postal address Visiting address Telephone Internet

Royal Institute of Technology Teknikringen 8 +46 8 790 6000 www.ave.kth.se
KTH Vehicle Dynamics Stockholm Telefax
SE-100 44 Stockholm +46 8 790 9304
Sweden
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Contents
1 Task 1: Washout filtering approach of side-slip estimation. . . . . . . . . . . . 1
1.1 Task 1.a: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Task 1.b: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Task 1.c: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 Task 1.d: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.5 Task 1.e: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
1.6 Extra task 1.f . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2 Task 2: Unscented K ALMAN filter estimation. . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1 Task 2.a: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.2 Task 2.b: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Task 2.c: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2.4 Task 2.d: . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

ii
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

1 Task 1: Washout filtering approach

of side-slip estimation
1.1 Task 1.a:
A S IMULINK architecture including three body side-slip estimators was built as re-
quested: a model-based, integration of lateral acceleration based, and a washout
filtered based. The main equations of the different methods are summarized below
and their implementation is shown in Figures 1, 2 and 3.

vy mod,COG
• Bicycle model: βy mod = vx , where:

vx lr l f + lr C12C34 − l f C12 mv2x

mod,CoG
vy =
2
 δ
l f + lr C12C34 + mv2x lrC34 − l f C12

δ being the tyre to road steering angle. This model gives the results at the
Center of Gravity so no transfer was done here.

Figure 1: Model-based body side-slip estimator.

vy kin,COG
 
• Integration of lateral acceleration: βy kin = arctan vx , where:

ay kin,COG = ay kin,IMU + rx ψ̈z − ry ψ̈z

Z T 
kin,COG
vy = ay kin,COG (1 − Kroll ) − ψ̇z vx dt
0

1
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 2: Integration of lateral acceleration based body side-slip estimator.

• Wash-out filter: this filter takes out the best results of each of the
two previous

methods, the lateral speed is expressed by vy wo,COG = LP vy mod + HP vy kin
where LP is a low pass filter and HP is a high pass one. The body side slip is
written as:  wo,COG 
wo vy
βy = arctan
vx

Figure 3: Wash-out filtered based body side-slip estimator.

1.2 Task 1.b:

Some of the parameters involved in the different estimating procedures need tuning.
The tuning is performed by comparing results from the designed body side-slip es-
timators from the reference measurement system (VBOX). Since fine-tuning has to
be carried out once and for all for future tasks, a good compromise has to be found
between the results of the various tests. The region or state of validity of each model
is also considered to choose which data to rely on.

Bicycle model: For this estimator, five parameters can be identified for possible
fine-tuning. These are the lengths from COG to axles, the mass, the longitudinal
velocity and the tyre cornering stiffness.
First, since the lengths from COG to axles and the mass are facts, there is no way to
change them
For the longitudinal speed, since the tests are conducted with different ranges of
speeds (for instance slow speeds for cornering and almost three times faster ones for
the frequency sweep), it was difficult to choose a fixed value for them all. Looking
at the measured longitudinal speed however, the variations for each test are not so

2
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

significant, hence the assumption that speed can be considered constant. As a result,
the longitudinal speed measurements (from VBOX) are used for the model-based
estimator.
The tyre cornering stiffness for the front and rear axles (C12 and C34 ) provided by
the handout are obviously not the correct ones. Consequently, these need also to be
tuned. Since this model is valid for the steady state, the aim is to tune them to get
closer results to the measurements for what could be considered steady state tests.
While the slalom test, and the beginning of both the cornering and frequency sweep
tests can be seen as steady-state situations, the high speed step steer procedure is def-
initely not in steady-state. After trying different values for both constants, the values
in Table 1 are chosen. The figures 4, 5 and 6 show that, after the initial noisy oscilla-
tions, the results from the bicycle model match the experimental results as expected,
i.e. the slalom test, and the beginning of both the cornering and frequency sweep
tests. When tuning the parameters, it was noticed that increasing them significantly
causes the model results to deviate more from the experimental ones although they
still follow the general tendencies.

C f = C12 (kN/m) Cr = C34 (kN/m)

85 180
Table 1: Tuned cornering stiffness for bicycle model

Figure 4: Body side-slip against time for the cornering test - Model-based.

Figure 5: Body side-slip against time for the slalom test - Model-based.

3
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 6: Body side-slip against time for the frequency sweep test - Model-based.

Integration of lateral acceleration: For this method, and to avoid size problems
later in the code and when plotting figures, the integration was done over the length
of the Time vector made available by the VBOX measurements. This estimator
needs to provide decent results for the transient tests, i.e. especially the high speed
step steer test. The results for the high speed step steer test are shown in the figure
7. As predicted, there is a drift in the results, but the behaviour is similar.

Figure 7: Body side-slip against time for high speed step steer test - Integration of
lateral acceleration based.

One of the reason why there is a drift is the IMU misalignment. One potential ad-
justment involves subtracting the average acceleration value determined outside the
maneuver from the total signal initially. Then, you reset the measured acceleration
to zero both before and after the maneuver itself. The beginning of the maneuver
is detected when the steer angle overcomes a specified threshold (0.03 rad). The
outcome of this additional feature is depicted in the Figure 8.
One parameter is identified for possible fine-tuning which is the vehicle roll gra-
dient. While the provided value already makes sense since it is not hard to mea-
sure the vehicle roll gradient, it goes through a fine-tuning process which yields
Kroll = 1.8deg/g. But the outcome, which is shown in the Figure 9, might not reflect
the physical reality since it might only compensate the inherent shift of the IMU
measurements. As a result, the initial vehicle roll gradient value will be kept.
The solution to obtain more accurate results and to get rid of this shift without mod-
ifying the vehicle roll gradient would be to perform sensors and physical model
fusion, using a K ALMAN filter. This is studied in Task 2 2.

4
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 8: Body side-slip against time for high speed step steer test with IMU mis-
alignment correction - Integration of lateral acceleration based.

Figure 9: Body side-slip for high-speed steer test with IMU misalignment and Kroll
corrections - Integration of lateral acceleration based.

1.3 Task 1.c:

For the wash-out filter, the adjustable parameter is the coefficient T, which deter-
mines the level of contributions from model-based estimator and lateral acceleration
integration based estimator.

Figure 10: Body side-slip against time for the cornering test - Wash-out.

5
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 11: Body side-slip against time for the slalom test - Wash-out.

Figure 12: Body side-slip against time for the high-speed steer test - Wash-out.

Figure 13: Body side-slip against time for the frequency sweep test - Wash-out.

T is tuned by trial-and-error to give the best compromise of results for the dif-
ferent test maneuvers. Values ranging from 0.1 to 100 are tested in the four test
scenarios. Bigger values around 5 give the best results for the high speed step test,
however, smaller values around 1 give better values for the three other scenarios.
Again, it can be identified that the high speed step steer test should predominantly
utilize the transient estimator, while the other tests should lean more towards the
steady-state estimator. Eventually, the chosen value T = 3 provides reasonably good
behaviors. The results for this value are shown in Figures 10 - 13. However, dis-
crepancies are evident, particularly in the high-speed step steer and frequency sweep

6
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

tests. This is mostly because the model-based estimation diverges significantly from
the true value in the high-speed step steer test, while the integration of lateral accel-
eration based estimation deviates notably from the true value in the frequency sweep
test. Still, these results are the fruit of an inherently imperfect compromise.

1.4 Task 1.d:

In order to evaluate the quality of the body side-slip estimates from the three im-
plemented methods and for all the test drive scenarios, the Mean Squared Error and
Max error towards the reference side-slip were calculated. The results are shown in
Table 2, for which data was sometimes cropped to delete spikes at the beginning or
the end of the time frame.
Before continuing, it should be kept in mind that the errors are computed only for
the time interval during which the maneuver occurs. The time dependent signal are
cropped accordingly to delete spikes at the beginning or at the end of the time frame
and to limit integration shift. Moreover, the integration of lateral acceleration based
estimate is calculated with the adjusted lateral acceleration signal obtained in Task
1.b (1.2).

Cornering Slalom
Model-based MSE 8.22 × 10−4 4.51 × 10−6
Max Error 1.08 × 10−1 5.51 × 10−3
MSE 7.41 × 10−3 2.48 × 10−4
LatAccInt based
Max Error 2.66 × 10−1 3.38 × 10−2
MSE 5.08 × 10−4 3.44 × 10−5
Wash-out
Max Error 5.90 × 10−2 1.25 × 10−2

Step steer Frequency sweep

Model-based MSE 8.94 × 10−3 7.87 × 10−5
Max Error 1.70 × 10−1 3.54 × 10−2
MSE 1.70 × 10−3 4.41 × 10−4
LatAccInt based
Max Error 1.01 × 10−1 3.80 × 10−2
MSE 4.40 × 10−4 3.41 × 10−5
Wash-out
Max Error 4.58 × 10−2 1.51 × 10−2
Table 2: Body side-slip errors from different estimators for all driving scenarios

Since the Washout filter is supposed to represent the best out of the results of
each other estimator, it is the most relevant to look at. The errors are observed to be
reasonable, the max error is thought to be reducible with a more optimization of the
value of the filter coefficient T. It is however difficult, as explained in Task 1.c (1.3),
to find a value that works for all scenarios, and looking at the table, it would probably
be interesting to have an additional logical reset for the integration part. This would
make the integration of lateral acceleration based estimator more accurate and enable
an increase of the wash-out filter coefficient T.

7
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

1.5 Task 1.e:

The three estimators tend to drift over time. This tendency is especially observed at
the beginning and at end of each test. The first possible cause could be the strong
acceleration and deceleration phases where data fluctuates a lot and the models strug-
gle to perfectly follow these rapid changes. The second probable cause could be the
non-validity of the linear tyre model during these phases as well since the tyres can
reach full slip and exceed the maximum possible friction.

It was noted that for all the tests the integration method was the most exposed to
drift. This is predictable since it relies on measurements from the sensors which can
be inaccurate. This inaccuracy could be due for instance to the effects of road in-
clination, banking, or of the sensors. However, the side-slip curve from this method
still follows the general shape of the real measured values.

The accuracy of the estimators also depends on steady-state or transient ma-

neuvers and on vehicle speed. For maneuvers with moderate to medium friction
utilizations, such a steady state cornering maneuver, all of the estimating approaches
yielded satisfying estimations of vehicle side-slip. The results were less accurate at
low speeds but the correct general shape was obtained in most cases. However, for
maneuvers with a slightly high friction utilization and at high speed like the high-
speed steering test, the estimators produced larger errors and less accurate results as
such a maneuver corresponds to a critical driving condition.

1.6 Extra task 1.f

This task consists of making the filter coefficient T be dependent on whether the
vehicle is under a transient or steady-state motion, and, putting more weight on the
model-based term in the washout filter when the manoeuvre is less transient.
As suggested in the handout, attempts were made to make T a linear function
of the vehicle signals that describe the rate in which the vehicle is cornering. These
signals can be the steering angle, the lateral acceleration or the yaw rate. The focus
was on the lateral acceleration.
Many methods were tried to detect if the manoeuver is transient or in steady-
state. For instance, a frequency analysis was carried by calculating the Fourier
Transform of the lateral acceleration signal, and processing it to determine if the
signal exhibits rapid variations indicative of transient maneuvers, either by calcu-
lating the mean, the variance, or looking for rappidly appearing and disappearing
peaks. Another method tried was the analysis of the rate of change by calculating
the derivative and comparing the different test scenarios. However, these attempts
weren’t entirely successful. In fact, in total, during this first part of the lab, two ar-
chitectures were tested, the first one is entirely on MATLAB, and the second one is
using SIMULINK. The SIMULINK model was kept because it gave more accurate
results for the kinematic model and because it allowed gaining more hands-on expe-
rience on using SIMULINK. However, while the tested methods for this task could
be applied to the MATLAB model by applying threshholds that separate the tran-
sient from the steady state manoeuvres, it was not the case for the Simulink model, it
was very challenging to find a threshhold because for example the slalom and high-

8
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

speed steer had similar maximum frequency peak wvalues although they represent a
steady-state and a transient manoeuver respectively.
It is certain that with a little more time the issues would have been solved, but
we unfortunately could not finish.

2 Task 2: Unscented K ALMAN fil-

ter estimation
2.1 Task 2.a:
A K ALMAN filter is a powerful mathematical tool used for state estimation in dy-
namic systems. It combines noisy sensor measurements with a predictive model to
produce an optimal estimate of the system’s true state. By recursively updating es-
timates, it refines predictions over time, providing accurate and efficient tracking of
system states. The Unscented K ALMAN Filter (UKF) is implemented here since
the predictive model is non linear. It was chosen to perform this implementation on
M ATLAB, which is described below in seven steps. While the car’s states are the
longitudinal speed vx , the lateral speed vy and the yaw rate ψ̇z , the measurements
variables are the longitudinal speed vx , the lateral acceleration ay and the yaw rate
ψ̇z . Eventually, the input variable is δ .

Step 1: Coordinate Transformation

The UKF relies on the IMU and GPS measurements to generate the optimal esti-
mate of the car’s true states. However, the IMU location differs from the COG. As
a result, measured velocities and accelerations need to be moved to be utilized:

ayCOG = ay IMU + rx ψ̈z − ry ψ̈z

vyCOG = vy IMU + rx ψ̇z − ry ψ̇z
Step 2: Noise covariances
It also needed to estimate the noise contained in the car’s states and in the measure-
ments variables. Those estimates account for a part of the adjustable parameters of
the system and are stored respectively in the matrix Q and R.
 
noise(vx,state ) 0 0
Q= 0 noise(vy,state ) 0 
0 0 noise(ψ̇z,state )
 
noise(vx,meas ) 0 0
R= 0 noise(ay,meas ) 0 
0 0 noise(ψ̇z,meas )
As an example, the noise of the acceleration measurement is much greater than the
one of the longitudinal velocity, hence noise(ay,meas ) > noise(vx,meas ). The measure-
ments noises can be derived by calculating the standard deviation of the signals. If
the noise is set too high, the algorithm will not give too much credibility to the mea-
surements and vice versa. In this first trial, the Q and R noises are respectively set to

9
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

0.1 and 0.01.

Step 3: Initialization
The initial state X = (vx , vy , ψ̇z )T is straightforwardly initialized using sensor mea-
surements. However, the state estimation covariance, P, is assigned a zero value due
to the absence of any indication regarding suitable values for it.

Step 4: State Function Definition

This function is basically used in the prediction part of the UKF algorithm to update
the car’s states based on the equations of the physical model denoted in the handout
(the non linear bicycle model).
 
vy + ψ̇z · l f
α12 = arctan −δ;
vx
 
vy − ψ̇z · lr
α34 = arctan ;
vx
F12 = −C f · α12 ;
F34 = −Cr · α34 ;
1
v˙x = − · F12 · sin(δ ) + ψ̇z · vy ;
m
1
v˙y = · (F34 + F12 · cos(δ )) − ψ̇z · vx ;
m
1
ψ̈z = · (l f · F12 · cos(δ ) − lr · F34);
Iz
The RUNGE -K UTTA 4 method is used here to integrate the time derivatives of the
car’s states over a timestep to obtain the new car’s states.

Step 5: Measurement Function definition

This function allows to estimate the measurements from the car’s states based on the
selected physical model. It is used in the update part of the UKF algorithm.

1
ay = · (F34 + F12 · cos(δ ));
m
The estimated measurements of vx and vy are just equal to the corresponding
car’s states.

Step 6: UKF loop

The UKF algorithm deals with each time step measurement. Considering the time
step i, it first performs the prediction part and then the update part. The functions
ukf predict1 and ukf update1 are respectively used for these purposes. It is also
important to provide the corresponding input (here δ (i) stored in predictParam) to
make these stages possible.

[xPre, PPre] = uk f predict1(x, P, state f unc UKF, Q, predictParam)

[X, P, K, MU, S] = uk f update1(xPre, PPre,Y (:, i), meas f unc UKF, R, predictParam)

10
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

where X is the true car’s states, P the state estimation covariance, MU the estimated
car’s states, all being outputs of the UKF algorithm, and Y the measurements at time
step i (from VBOX).

Step 7: Body side-slip

Once the UKF outputs have been recovered, one can combine them to derive the
body side-slip, as expected.

2.2 Task 2.b:

The outcomes from the UKF filter are showcased in the forthcoming Figures 14-
17 for every maneuver, alongside results from previous estimators. It is important
to mention that the figures are cropped to eliminate the spikes and thus, may not
display the complete graph.

Figure 14: Body side-slip against time for the cornering test - UKF.

Figure 15: Body side-slip against time for the slalom test - UKF.

11
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 16: Body side-slip against time for the high-speed steer test - UKF.

Figure 17: Body side-slip against time for the frequency sweep test - UKF.

The UKF true states don’t match accurately the VBOX measurements for the
high speed step steer and frequency sweep tests. These are decent for the two other
tests. As a result, the UKF adjustable parameters would need a fine-tuning process.

2.3 Task 2.c:

The UKF outcome can be enhanced by fine-tuning its parameters which are the
coefficients responsible for the sigma points genesis (α, β , κ), the covariance and
process noises. The focus is made on the high speed steer test.
First, the measurement covariance noises, which describe the amount of credibility
put in the measurement, are derived by assuming that the standard deviations (std)
will not change over time. Since the three measurements are assumed to be indepen-
dent, non-diagonal coefficients are equal to zero. Hence,

std(vx,meas )2
   
0 0 47.4 0 0
R= 0 std(ay,meas )2 0 = 0 12.3 0 
2 0 0 0.0243
0 0 std(ψ̇z,meas )

12
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

The enhancement is depicted in the Figure 18. The noise is much lower since the
measurements covariance noises increased, meaning that less credibility was given
to the measurements.

Figure 18: Body side-slip against time for the high speed steer test - UKF, adjusted
measurement covariance noises.

Then, the fine-tuning of the other coefficients relies on a trial-and-error method. The
effect of each coefficient will be assessed one by one to try to reach a combination
that provides an optimal configuration for the UKF.
Regarding the process covariance noises, it seems that the lateral velocity one has
the more significant contribution. By reducing its values, a smoother behavior is
reached, meaning that more weight is given to the physical model. While the be-
havior looks better, it still does not match the measured body side-slip. The other
process noises don’t affect much the shape of the signal except for excessive values
( 100), providing totally different behavior compared to measured body side-slip.
Again, since the three measurements are assumed to be independent, non-diagonal
coefficients are equal to zero. As a result, the following process noise covariance
matrix Q is chosen:

   
noise(vx,state ) 0 0 0.1 0 0
Q= 0 noise(vy,state ) 0  =  0 0.0075 0 
0 0 noise(ψ̇z,state ) 0 0 0.1

The result is shown in the Figure 19. Several (α, β , κ) combinations are tested with
different orders of magnitude. However, it seems to barely change the outcome of
the UKF. Thus, the initial combination is kept.
Initially, it can be imagined that the UKF outcome behavior could be modified in
order to match the corresponding measurement. However, the fine-tuning of the
UKF seems to solely allow to correct the smoothness and the quality of the outcome.
As a result, the UKF appears not to be the most optimal solution for all the tests.
For example, the wash-out filter outperforms the UKF for the high speed steer and
frequency sweep tests.

13
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 19: Body side-slip against time for the high speed steer test - UKF, adjusted
measurement and process covariance noises.

2.4 Task 2.d:

The vehicle dynamics model is extended to take the roll and pitch dynamics (φy and
θx respectively) and the rolling resistance fr into account. The equations along the x
and y directions are thus modified:

m(v˙x − ψ̇z · vy ) = −F12 · sin(δ ) − fr · m · g

m(v˙y + ψ̇z · vx + g · sin(φx ) · cos(θy )) = F34 + F12 · cos(δ )
As a consequence, two new inputs need to be introduced in the states and measure-
ments functions. The results for the four tests are presented in the Figure 20-23.

Figure 20: Body side-slip against time for the high speed steer test - UKF, extended
vehicle dynamics model.

14
SD2231 – LAB 2: Lateral Dynamics – State estimation May 2024

Figure 21: Body side-slip against time for the high speed steer test - UKF, extended
vehicle dynamics model.

Figure 22: Body side-slip against time for the high speed steer test - UKF, extended
vehicle dynamics model.

Figure 23: Body side-slip against time for the high speed steer test - UKF, extended
vehicle dynamics model.

The outcomes of the UKF are slightly improved for the two tests for which it
was already working before. Regarding the others, there are bare changes. Since the
extend of the model is very light, it might explain why there are so few modifications.

15