$29
Lab 5: Vehicle Dynamics and Extended Kalman Filter Solution
In this lab you will model and simulate a nonlinear vehicle dynamics model. You will then implement a standard and an extended Kalman lter to estimate the system states.
Vehicle Dynamics Model
In this lab we will model the vehicle using a bicycle model similar to that presented in class and shown in Figure 1:
vx = vy!z
2
Ff ( f ) sin( ) +
Fx
m
m
2
vy =
vx!z +
[Ff ( f ) cos( ) + Fr( r)]
m
!z =
2
[aFf ( f ) cos( )
bFr( r)]
Jz
_
)
vy sin(
)
X = vx cos(
_
) + vy cos(
)
Y = vx sin(
_ = !z
where the longitudinal force Fx and the steering angle are inputs. We will refer to the dynamics as
vector.
v
x
v
y
z
F
x
is the input
x = f(x; u) where x =
!
X Y
is the state vector and u =
The tire forces are given by
Ff ( f ) = ma +a bA sin(C arctan(B f ))
Fr( r) = ma +b bA sin(C arctan(B r))
and the front and rear slip angles are
f = arctan
vy + a!z
vx
vy b!z
r = arctan
vx
Note that if vx = 0 the slip angles are unde ned. In this case set the slip angles to 0.
1 of 5
Due: April 20 ME C231B/EECS C220C
Figure 1: Bicycle Model.
The vehicle parameters are
m = 2237 kg
Jz = 5112 kg m2
a = 1:46 m
b = 1:55 m
and the tire model parameters are
A = 6:8357
B = 0:0325
= 238:9874
We assume that we measure longitudinal velocity vx, lateral acceleration ay, and yaw rate !z. Therefore,
2 of 5
Due: April 20 ME C231B/EECS C220C
the measurement equation is
2 3
vx
= h(x; u) = 4m2 [Ff ( f ) cos( ) + Fr( r)]5 : !z
Using the Kalman lter we will attempt to estimate the sideslip angle using these measurements and the system dynamics. The sideslip angle is de ned as
:= arctan vy vx
and is the direction of travel of the center of mass of the vehicle.
Simulate the nonlinear continuous time system with the following sets of di erent inputs for 20 seconds:
Constant Velocity Skidpad: Let the initial longitudinal velocity be 20 m=s, the longitudinal force Fx = 2500N, and the steering angle = 10 degrees.
Constant Acceleration Skidpad: Let the initial longitudinal velocity be 5 m=s, the longi-tudinal force Fx = 8000 N, and the steering angle = 10 degrees.
Lane Change: With Fx = 0 N and an initial velocity of 20 m=s apply zero steering input for 5 seconds, then apply a step steering input of = 5 degrees for two seconds, apply
zero steering for 5 seconds, and then apply a step steering input of = 5 degrees for two seconds.
For each test plot the input signals Fx and , the resulting position X vs. Y , and the lateral and longitudinal velocities vy and vx.
2. Discretize the nonlinear dynamics by a forward Euler approximation with sampling time t = 0:01:
xk+1 = xk + tf(xk; uk)
yk = h(xk; uk)
Simulate the Lane Change scenario with the discretized dynamics and plot the resulting position X vs. Y , and the lateral and longitudinal velocities vy and vx to make sure that the discretized system is a good approximation of the continuous time system.
Perform a symbolic linearization of the discretized system dynamics and measurement equations. You will use these results to implement an extended Kalman Filter in the following problems so you will need to get expressions for the (A; B; C; D) matrices as a function of the states and inputs. Report the (A; B; C; D) matrices of the linearized system at the operating point:
vx = 10 m=s
vy = !z = X = Y = = 0
Fx = = 0
3 of 5
Due: April 20 ME C231B/EECS C220C
Note that the value of the tire model parameter A was originally incorrect and has been changed to 6:8357. Make sure you update this value so the simulation and Kalman lter estimates will be re ective of the measured data.
Download the VehicleData.mat le from bCourses. This le contains real data from a vehicle test. It includes the inputs:
Fx in - longitudinal force delta in - steering angle
and measurements:
ay meas - lateral acceleration vx meas - longitudinal velocity
yawRate meas - yaw rate
beta meas - sideslip angle
Implement a standard Kalman lter on the discrete time linearized system that uses the measure-ments of lateral acceleration, longitudinal velocity, and yaw rate to estimate the sideslip angle. Plot the estimates of the lateral acceleration, yaw rate, and sideslip angle compared to the actual measurements.
To do this the discrete time dynamics should be linearized about the operating point:
vx = 10 m=s
vy = !z = X = Y = = 0
Fx = = 0
The initial state mean and error covariance are
203
200:1
0:1
0
0
0
0 3
3
7
6
0
0
0
0
0
7
m0
=
60
and 0
=
0
0
0
0:1
0
0
:
6 7
6
0
0
0:1
0
0
0
7
0
6
7
6 7
60
7
6
0
0
0
0
0:1
0
7
6 7
6
7
60
7
6
0
0
0
0
0
0:1
7
4 5
4
5
We assume the process and measurement noise w e ect the system through the matrices
20
1
0
0
0
0
0
0
03
0
0
0
0
0
0
1
0
0
1
0
0
0
0
0
0
0
0
E =
60
0
0
1
0
0
0
0
0
7
and F =
2
0
0
0
0
0
0
0
1
0
3
6
0
1
0
0
0
0
0
0
7
0
0
0
0
0
0
0
0
1
0
7
6
4
5
60
0
0
0
1
0
0
0
0
7
6
7
60
0
0
0
0
1
0
0
0
7
4
5
4 of 5
Due: April 20
ME C231B/EECS C220C
and has covariance
2
1
0
0
0
0
0
0
0
0
3
0
1
0
0
0
0
0
0
0
6
0
0
0
1
0
0
0
0
0
7
6
0
0
0:1
0
0
0
0
0
0
7
W =
6
7
:
0
0
0
0
1
0
0
0
0
6
7
6
0
0
0
0
0
0:1
0
0
0
7
6
7
6
7
6
0
0
0
0
0
0
1
0
0
7
6
7
6
0
0
0
0
0
0
0
0:1
0
7
6
7
6
0
0
0
0
0
0
0
0
0:05
7
4
5
Using the same data and noise characteristics implement an extended Kalman lter to estimate the sideslip angle of the vehicle. Plot the estimates of the lateral acceleration, yaw rate, and sideslip angle compared to the actual measurements and the estimates from the standard Kalman lter.
How does the extended Kalman lter estimates compare to the standard Kalman lter? Are there speci c times where the extended Kalman lter performs signi cantly better? Explain why this would happen.
5 of 5
