Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
C
commonroad-vehicle-models
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Locked Files
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Iterations
Merge Requests
0
Merge Requests
0
Requirements
Requirements
List
Security & Compliance
Security & Compliance
Dependency List
License Compliance
Operations
Operations
Incidents
Analytics
Analytics
Code Review
Insights
Issue
Repository
Value Stream
Wiki
Wiki
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Commits
Issue Boards
Open sidebar
tum-cps
commonroad-vehicle-models
Commits
74908ce8
Commit
74908ce8
authored
Apr 24, 2019
by
Markus Koschi
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
update to Release 2018a
parent
fc628bd0
Changes
32
Hide whitespace changes
Inline
Side-by-side
Showing
32 changed files
with
1090 additions
and
273 deletions
+1090
-273
MATLAB/accelerationConstraints.m
MATLAB/accelerationConstraints.m
+47
-0
MATLAB/init_KS.m
MATLAB/init_KS.m
+6
-21
MATLAB/init_MB.m
MATLAB/init_MB.m
+6
-14
MATLAB/init_ST.m
MATLAB/init_ST.m
+3
-45
MATLAB/parameters_vehicle1.m
MATLAB/parameters_vehicle1.m
+14
-1
MATLAB/parameters_vehicle2.m
MATLAB/parameters_vehicle2.m
+14
-1
MATLAB/parameters_vehicle3.m
MATLAB/parameters_vehicle3.m
+14
-1
MATLAB/steeringConstraints.m
MATLAB/steeringConstraints.m
+40
-0
MATLAB/testVehicle.m
MATLAB/testVehicle.m
+16
-15
MATLAB/unitTests/test_derivatives.m
MATLAB/unitTests/test_derivatives.m
+71
-0
MATLAB/unitTests/test_zeroInitialVelocity.m
MATLAB/unitTests/test_zeroInitialVelocity.m
+183
-0
MATLAB/vehicleDynamics_KS.m
MATLAB/vehicleDynamics_KS.m
+8
-2
MATLAB/vehicleDynamics_MB.m
MATLAB/vehicleDynamics_MB.m
+54
-18
MATLAB/vehicleDynamics_ST.m
MATLAB/vehicleDynamics_ST.m
+43
-27
Python/accelerationConstraints.py
Python/accelerationConstraints.py
+47
-0
Python/init_KS.py
Python/init_KS.py
+5
-20
Python/init_MB.py
Python/init_MB.py
+6
-14
Python/init_ST.py
Python/init_ST.py
+3
-47
Python/longitudinalParameters.py
Python/longitudinalParameters.py
+8
-0
Python/parameters_vehicle1.py
Python/parameters_vehicle1.py
+16
-3
Python/parameters_vehicle2.py
Python/parameters_vehicle2.py
+14
-1
Python/parameters_vehicle3.py
Python/parameters_vehicle3.py
+14
-1
Python/steeringConstraints.py
Python/steeringConstraints.py
+41
-0
Python/steeringParameters.py
Python/steeringParameters.py
+7
-0
Python/testVehicle.py
Python/testVehicle.py
+4
-8
Python/unitTests/test_derivatives.py
Python/unitTests/test_derivatives.py
+82
-0
Python/unitTests/test_zeroInitialVelocity.py
Python/unitTests/test_zeroInitialVelocity.py
+214
-0
Python/vehicleDynamics_KS.py
Python/vehicleDynamics_KS.py
+10
-2
Python/vehicleDynamics_MB.py
Python/vehicleDynamics_MB.py
+56
-18
Python/vehicleDynamics_ST.py
Python/vehicleDynamics_ST.py
+36
-14
Python/vehicleParameters.py
Python/vehicleParameters.py
+8
-0
vehicleModels_commonRoad.pdf
vehicleModels_commonRoad.pdf
+0
-0
No files found.
MATLAB/accelerationConstraints.m
0 → 100644
View file @
74908ce8
function
acceleration
=
accelerationConstraints
(
velocity
,
acceleration
,
p
)
% accelerationConstraints - adjusts the acceleration based on acceleration
% constraints
%
% Syntax:
% acceleration = accelerationConstraints(velocity,acceleration,p)
%
% Inputs:
% acceleration - acceleration in driving direction
% velocity - velocity in driving direction
% p - longitudinal parameter structure
%
% Outputs:
% acceleration - acceleration in driving direction
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 15-December-2017
% Last update: ---
% Last revision:---
%------------- BEGIN CODE --------------
%positive acceleration limit
if
velocity
>
p
.
v_switch
posLimit
=
p
.
a_max
*
p
.
v_switch
/
velocity
;
else
posLimit
=
p
.
a_max
;
end
%acceleration limit reached?
if
(
velocity
<=
p
.
v_min
&&
acceleration
<=
0
)
||
(
velocity
>=
p
.
v_max
&&
acceleration
>=
0
)
acceleration
=
0
;
elseif
acceleration
<=-
p
.
a_max
acceleration
=
-
p
.
a_max
;
elseif
acceleration
>=
posLimit
acceleration
=
posLimit
;
end
%------------- END OF CODE --------------
MATLAB/init_KS.m
View file @
74908ce8
function
x0
=
init_KS
(
initState
,
p
)
function
x0
=
init_KS
(
initState
)
% init_KS - generates the initial state vector for the kinematic
% single-track model
%
...
...
@@ -7,7 +7,6 @@ function x0 = init_KS(initState, p)
%
% Inputs:
% initState - core initial states
% p - parameter vector
%
% Outputs:
% x0 - initial state vector
...
...
@@ -22,7 +21,7 @@ function x0 = init_KS(initState, p)
% Author: Matthias Althoff
% Written: 11-January-2017
% Last update:
---
% Last update:
15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -41,23 +40,9 @@ function x0 = init_KS(initState, p)
%obtain initial states from vector
sx0
=
initState
(
1
);
sy0
=
initState
(
2
);
vel0
=
initState
(
3
);
Psi0
=
initState
(
4
);
dotPsi0
=
initState
(
5
);
beta0
=
initState
(
6
);
%create equivalent bicycle parameters
g
=
9.81
;
%[m/s^2]
mu
=
p
.
tire
.
p_dy1
;
C_Sf
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
C_Sr
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
lf
=
p
.
a
;
lr
=
p
.
b
;
%initial steering angle from steady state of slip angle
delta0
=
vel0
*
(
lf
+
lr
)/(
C_Sf
*
g
*
lr
*
mu
)
*
dotPsi0
...
+
1
/(
C_Sf
*
lr
)
*
((
C_Sr
*
lf
+
C_Sf
*
lr
)
*
beta0
-
(
C_Sr
-
C_Sf
)
*
lr
*
lf
*
dotPsi0
/
vel0
);
delta0
=
initState
(
3
);
vel0
=
initState
(
4
);
Psi0
=
initState
(
5
);
%sprung mass states
x0
(
1
)
=
sx0
;
% s_x x-position in a global coordinate system
...
...
@@ -66,4 +51,4 @@ x0(3) = delta0; % steering angle of front wheels
x0
(
4
)
=
vel0
;
% velocity
x0
(
5
)
=
Psi0
;
% Ψ yaw angle
%------------- END OF CODE --------------
\ No newline at end of file
%------------- END OF CODE --------------
MATLAB/init_MB.m
View file @
74908ce8
...
...
@@ -21,7 +21,7 @@ function x0 = init_MB(initState, p)
% Author: Matthias Althoff
% Written: 11-January-2017
% Last update:
---
% Last update:
15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -69,23 +69,15 @@ function x0 = init_MB(initState, p)
%obtain initial states from vector
sx0
=
initState
(
1
);
sy0
=
initState
(
2
);
vel0
=
initState
(
3
);
Psi0
=
initState
(
4
);
dotPsi0
=
initState
(
5
);
beta0
=
initState
(
6
);
delta0
=
initState
(
3
);
vel0
=
initState
(
4
);
Psi0
=
initState
(
5
);
dotPsi0
=
initState
(
6
);
beta0
=
initState
(
7
);
%create equivalent bicycle parameters
g
=
9.81
;
%[m/s^2]
mu
=
p
.
tire
.
p_dy1
;
C_Sf
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
C_Sr
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
lf
=
p
.
a
;
lr
=
p
.
b
;
%initial steering angle from steady state of slip angle
delta0
=
vel0
*
(
lf
+
lr
)/(
C_Sf
*
g
*
lr
*
mu
)
*
dotPsi0
...
+
1
/(
C_Sf
*
lr
)
*
((
C_Sr
*
lf
+
C_Sf
*
lr
)
*
beta0
-
(
C_Sr
-
C_Sf
)
*
lr
*
lf
*
dotPsi0
/
vel0
);
%auxiliary initial states
F0_z_f
=
p
.
m_s
*
g
*
p
.
b
/((
p
.
a
+
p
.
b
))
+
p
.
m_uf
*
g
;
...
...
MATLAB/init_ST.m
View file @
74908ce8
function
x0
=
init_ST
(
initState
,
p
)
function
x0
=
init_ST
(
initState
)
% init_MB - generates the initial state vector for the single-track model
%
% Syntax:
...
...
@@ -6,7 +6,6 @@ function x0 = init_ST(initState, p)
%
% Inputs:
% initState - core initial states
% p - parameter vector
%
% Outputs:
% x0 - initial state vector
...
...
@@ -21,52 +20,11 @@ function x0 = init_ST(initState, p)
% Author: Matthias Althoff
% Written: 11-January-2017
% Last update:
---
% Last update:
15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
%states
%x1 = s_x x-position in a global coordinate system
%x2 = s_y y-position in a global coordinate system
%x3 = δ steering angle of front wheels
%x4 = u velocity in x-direction
%x5 = Ψ yaw angle
%x6 = Ψ yaw rate
%x7 = β slip angle at vehicle center
%u1 = v_delta steering angle velocity of front wheels
%u2 = ax longitudinal acceleration
%obtain initial states from vector
sx0
=
initState
(
1
);
sy0
=
initState
(
2
);
vel0
=
initState
(
3
);
Psi0
=
initState
(
4
);
dotPsi0
=
initState
(
5
);
beta0
=
initState
(
6
);
%create equivalent bicycle parameters
g
=
9.81
;
%[m/s^2]
mu
=
p
.
tire
.
p_dy1
;
C_Sf
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
C_Sr
=
-
p
.
tire
.
p_ky1
/
p
.
tire
.
p_dy1
;
lf
=
p
.
a
;
lr
=
p
.
b
;
%initial steering angle from steady state of slip angle
delta0
=
vel0
*
(
lf
+
lr
)/(
C_Sf
*
g
*
lr
*
mu
)
*
dotPsi0
...
+
1
/(
C_Sf
*
lr
)
*
((
C_Sr
*
lf
+
C_Sf
*
lr
)
*
beta0
-
(
C_Sr
-
C_Sf
)
*
lr
*
lf
*
dotPsi0
/
vel0
);
%sprung mass states
x0
(
1
)
=
sx0
;
% s_x x-position in a global coordinate system
x0
(
2
)
=
sy0
;
% s_y y-position in a global coordinate system
x0
(
3
)
=
delta0
;
% steering angle of front wheels
x0
(
4
)
=
vel0
;
% velocity
x0
(
5
)
=
Psi0
;
% Ψ yaw angle
x0
(
6
)
=
dotPsi0
;
% Ψ yaw rate
x0
(
7
)
=
beta0
;
% β slip angle at vehicle center
x0
=
initState
;
%------------- END OF CODE --------------
\ No newline at end of file
MATLAB/parameters_vehicle1.m
View file @
74908ce8
...
...
@@ -10,7 +10,7 @@ function p = parameters_vehicle1()
% ---
%
% Outputs:
% p - parameter
vector
% p - parameter
structure
%
% Example:
%
...
...
@@ -23,6 +23,7 @@ function p = parameters_vehicle1()
% Author: Matthias Althoff
% Written: 15-January-2017
% Last update: 05-July-2017
% 15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -31,6 +32,18 @@ function p = parameters_vehicle1()
p
.
l
=
4.298
;
%vehicle length [m]
p
.
w
=
1.674
;
%vehicle width [m]
%steering constraints
p
.
steering
.
min
=
-
0.910
;
%minimum steering angle [rad]
p
.
steering
.
max
=
0.910
;
%maximum steering angle [rad]
p
.
steering
.
v_min
=
-
0.4
;
%minimum steering velocity [rad/s]
p
.
steering
.
v_max
=
0.4
;
%maximum steering velocity [rad/s]
%longitudinal constraints
p
.
longitudinal
.
v_min
=
-
13.9
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_max
=
45.8
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_switch
=
4.755
;
%switching velocity [m/s]
p
.
longitudinal
.
a_max
=
11.5
;
%maximum absolute acceleration [m/s^2]
%masses
p
.
m
=
lb_sec2_ft_IN_kg
(
84
);
%vehicle mass [kg]; MASS
p
.
m_s
=
lb_sec2_ft_IN_kg
(
75
);
%sprung mass [kg]; SMASS
...
...
MATLAB/parameters_vehicle2.m
View file @
74908ce8
...
...
@@ -10,7 +10,7 @@ function p = parameters_vehicle2()
% ---
%
% Outputs:
% p - parameter
vector
% p - parameter
structure
%
% Example:
%
...
...
@@ -23,6 +23,7 @@ function p = parameters_vehicle2()
% Author: Matthias Althoff
% Written: 15-January-2017
% Last update: 05-July-2017
% 15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -31,6 +32,18 @@ function p = parameters_vehicle2()
p
.
l
=
4.508
;
%vehicle length [m] (with US bumpers)
p
.
w
=
1.610
;
%vehicle width [m]
%steering constraints
p
.
steering
.
min
=
-
1.066
;
%minimum steering angle [rad]
p
.
steering
.
max
=
1.066
;
%maximum steering angle [rad]
p
.
steering
.
v_min
=
-
0.4
;
%minimum steering velocity [rad/s]
p
.
steering
.
v_max
=
0.4
;
%maximum steering velocity [rad/s]
%longitudinal constraints
p
.
longitudinal
.
v_min
=
-
13.6
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_max
=
50.8
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_switch
=
7.319
;
%switching velocity [m/s]
p
.
longitudinal
.
a_max
=
11.5
;
%maximum absolute acceleration [m/s^2]
%masses
p
.
m
=
lb_sec2_ft_IN_kg
(
74.91452
);
%vehicle mass [kg]; MASS
p
.
m_s
=
lb_sec2_ft_IN_kg
(
66.17221
);
%sprung mass [kg]; SMASS
...
...
MATLAB/parameters_vehicle3.m
View file @
74908ce8
...
...
@@ -10,7 +10,7 @@ function p = parameters_vehicle3()
% ---
%
% Outputs:
% p - parameter
vector
% p - parameter
structure
%
% Example:
%
...
...
@@ -23,6 +23,7 @@ function p = parameters_vehicle3()
% Author: Matthias Althoff
% Written: 15-January-2017
% Last update: 05-July-2017
% 15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -31,6 +32,18 @@ function p = parameters_vehicle3()
p
.
l
=
4.569
;
%vehicle length [m]
p
.
w
=
1.844
;
%vehicle width [m]
%steering constraints
p
.
steering
.
min
=
-
1.023
;
%minimum steering angle [rad]
p
.
steering
.
max
=
1.023
;
%maximum steering angle [rad]
p
.
steering
.
v_min
=
-
0.4
;
%minimum steering velocity [rad/s]
p
.
steering
.
v_max
=
0.4
;
%maximum steering velocity [rad/s]
%longitudinal constraints
p
.
longitudinal
.
v_min
=
-
11.2
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_max
=
41.7
;
%minimum velocity [m/s]
p
.
longitudinal
.
v_switch
=
7.824
;
%switching velocity [m/s]
p
.
longitudinal
.
a_max
=
11.5
;
%maximum absolute acceleration [m/s^2]
%masses
p
.
m
=
lb_sec2_ft_IN_kg
(
101.3367
);
%vehicle mass [kg]; MASS
p
.
m_s
=
lb_sec2_ft_IN_kg
(
90.21635
);
%sprung mass [kg]; SMASS
...
...
MATLAB/steeringConstraints.m
0 → 100644
View file @
74908ce8
function
steeringVelocity
=
steeringConstraints
(
steeringAngle
,
steeringVelocity
,
p
)
% steeringConstraints - adjusts the steering velocity based on steering
% constraints
%
% Syntax:
% steeringVelocity = steeringConstraints(steeringAngle,steeringVelocity,p)
%
% Inputs:
% steeringAngle - steering angle
% steeringVelocity - steering velocity
% p - steering parameter structure
%
% Outputs:
% steeringVelocity - steering velocity
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 15-December-2017
% Last update: ---
% Last revision:---
%------------- BEGIN CODE --------------
%steering limit reached?
if
(
steeringAngle
<=
p
.
min
&&
steeringVelocity
<=
0
)
||
(
steeringAngle
>=
p
.
max
&&
steeringVelocity
>=
0
)
steeringVelocity
=
0
;
elseif
steeringVelocity
<=
p
.
v_min
steeringVelocity
=
p
.
v_min
;
elseif
steeringVelocity
>=
p
.
v_max
steeringVelocity
=
p
.
v_max
;
end
%------------- END OF CODE --------------
MATLAB/testVehicle.m
View file @
74908ce8
...
...
@@ -20,7 +20,7 @@ function testVehicle()
% Author: Matthias Althoff
% Written: 11-January-2017
% Last update:
---
% Last update:
16-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
...
...
@@ -33,27 +33,18 @@ g = 9.81; %[m/s^2]
tStart
=
0
;
%start time
tFinal
=
1
;
%start time
% vel0 = 15;
% Psi0 = 0.1;
% dotPsi0 = -0.1;
% beta0 = 0.05;
% sy0 = 1;
delta0
=
0
;
vel0
=
15
;
Psi0
=
0
;
dotPsi0
=
0
;
beta0
=
0
;
sy0
=
0
;
initialState
=
[
0
,
sy0
,
vel0
,
Psi0
,
dotPsi0
,
beta0
];
%initial state for simulation
x0_KS
=
init_KS
(
initialState
,
p
);
%initial state for kinematic single-track model
x0_ST
=
init_ST
(
initialState
,
p
);
%initial state for single-track model
initialState
=
[
0
,
sy0
,
delta0
,
vel0
,
Psi0
,
dotPsi0
,
beta0
];
%initial state for simulation
x0_KS
=
init_KS
(
initialState
);
%initial state for kinematic single-track model
x0_ST
=
init_ST
(
initialState
);
%initial state for single-track model
x0_MB
=
init_MB
(
initialState
,
p
);
%initial state for multi-body model
%--------------------------------------------------------------------------
% %specify continuous dynamics-----------------------------------------------
% carDyn = vehicleSys('vehicleDynamics',28,2,@vehicleDynamics_MB,[]); %initialize car
% carDyn_bicycle = vehicleSys('vehicleDynamics_bicycle',6,2,@DOTBicycleDynamics,[]); %initialize car
% %--------------------------------------------------------------------------
% %generate ode options
% stepsizeOptions = odeset('MaxStep',options.tStart-options.tFinal);
% opt = odeset(stepsizeOptions);
...
...
@@ -90,11 +81,21 @@ plot(t_brake,x_brake(:,9));
v_delta
=
0.15
;
acc
=
0.63
*
g
;
u
=
[
v_delta
acc
];
%simulate car
%simulate full car
[
t_acc
,
x_acc
]
=
ode45
(
getfcn
(
@
vehicleDynamics_MB
,
u
,
p
),[
tStart
,
tFinal
],
x0_MB
);
%simulate single-track model
[
~
,
x_acc_st
]
=
ode45
(
getfcn
(
@
vehicleDynamics_ST
,
u
,
p
),[
tStart
,
tFinal
],
x0_ST
);
%simulate kinematic single-track model
[
~
,
x_acc_ks
]
=
ode45
(
getfcn
(
@
vehicleDynamics_KS
,
u
,
p
),[
tStart
,
tFinal
],
x0_KS
);
figure
%position
hold
on
plot
(
x_acc
(:,
1
),
x_acc
(:,
2
))
plot
(
x_acc_st
(:,
1
),
x_acc_st
(:,
2
))
plot
(
x_acc_ks
(:,
1
),
x_acc_ks
(:,
2
))
figure
% velocity
plot
(
t_acc
,
x_acc
(:,
4
));
figure
% wheel spin
...
...
MATLAB/unitTests/test_derivatives.m
0 → 100644
View file @
74908ce8
function
res
=
test_derivatives
()
% test_derivatives - unit_test_function for checking whether the derivative
% is computed correctly
%
% Syntax:
% res = test_derivatives()
%
% Inputs:
% ---
%
% Outputs:
% res - boolean result (0/empty: test not passed, 1: test passed)
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 17-December-2017
% Last update: ---
% Last revision:---
%------------- BEGIN CODE --------------
% initialize result
res
=
[];
% load parameters
p
=
parameters_vehicle2
;
g
=
9.81
;
%[m/s^2]
% set state values
x_ks
=
[
...
3.9579422297936526
,
0.0391650102771405
,
0.0378491427211811
,
16.3546957860883566
,
0.0294717351052816
];
x_st
=
[
...
2.0233348142065677
,
0.0041907137716636
,
0.0197545248559617
,
15.7216236334290116
,
0.0025857914776859
,
0.0529001056654038
,
0.0033012170610298
];
x_mb
=
[
...
10.8808433066274794
,
0.5371850187869442
,
0.0980442671005920
,
18.0711398687457745
,
0.1649995631003776
,
0.6158755000936103
,
-
0.1198403612262477
,
-
0.2762672756169581
,
0.0131909920269115
,
-
0.0718483683742141
,
-
0.3428324595054725
,
0.0103233373083297
,
-
0.0399590564140291
,
-
0.0246468320579360
,
-
0.0551575051990853
,
0.5798277643297529
,
0.0172059354801703
,
0.0067890113155477
,
-
0.0184269459410162
,
-
0.0408207136116175
,
-
1.0064484829203018
,
0.0166808347900582
,
-
0.0049188492004049
,
53.6551710641082451
,
50.7045242506744316
,
56.7917911784219598
,
200.7079633169796296
,
-
0.0939969123691911
,
-
0.0881514621614376
];
% set input: (accelerating and steering)
v_delta
=
0.15
;
acc
=
0.63
*
g
;
u
=
[
v_delta
acc
];
% insert into vehicle dynamics to obtain derivatives
f_ks
=
vehicleDynamics_KS
(
x_ks
,
u
,
p
);
f_st
=
vehicleDynamics_ST
(
x_st
,
u
,
p
);
f_mb
=
vehicleDynamics_MB
(
x_mb
,
u
,
p
);
% ground truth
f_ks_gt
=
[
16.3475935934250209
,
0.4819314886013121
,
0.1500000000000000
,
5.1464424102339752
,
0.2401426578627629
];
f_st_gt
=
[
15.7213512030862397
,
0.0925527979719355
,
0.1500000000000000
,
5.3536773276413925
,
0.0529001056654038
,
0.6435589397748606
,
0.0313297971641291
];
f_mb_gt
=
[
17.8820162482414098
,
2.6300428035858809
,
0.1500000000000000
,
2.7009396644636450
,
0.6158755000936103
,
1.3132879472301846
,
-
0.2762672756169581
,
-
0.1360581472638375
,
-
0.0718483683742141
,
0.4909514227532223
,
-
2.6454134031927374
,
-
0.0399590564140291
,
0.0486778649724968
,
-
0.0551575051990853
,
0.0354501802049087
,
-
1.1130397141873534
,
0.0067890113155477
,
-
0.0886810139593130
,
-
0.0408207136116175
,
0.0427680029698811
,
-
4.3436374104751501
,
-
0.0049188492004049
,
0.1142109377736169
,
10.0527321757776047
,
0.0436512393438736
,
14.3044528684659404
,
590.5602192640882322
,
-
0.2105876149500405
,
-
0.2126005780977984
];
% comparison
res
(
end
+
1
)
=
all
(
abs
(
f_ks
'
-
f_ks_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
f_st
'
-
f_st_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
f_mb
'
-
f_mb_gt
)
<
1e-14
);
%--------------------------------------------------------------------------
% obtain final result
res
=
all
(
res
);
%------------- END OF CODE --------------
MATLAB/unitTests/test_zeroInitialVelocity.m
0 → 100644
View file @
74908ce8
function
res
=
test_zeroInitialVelocity
()
% test_zeroInitialVelocity - unit_test_function for starting with zero
% initial velocity
%
% Some vehicle models have a singularity when the vehicle is not moving.
% This test checks whether the vehicles can handle this situation
%
% Syntax:
% res = test_zeroInitialVelocity()
%
% Inputs:
% ---
%
% Outputs:
% res - boolean result (0/empty: test not passed, 1: test passed)
%
% Example:
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---
% Author: Matthias Althoff
% Written: 15-December-2017
% Last update: ---
% Last revision:---
%------------- BEGIN CODE --------------
% initialize result
res
=
[];
% load parameters
p
=
parameters_vehicle2
;
g
=
9.81
;
%[m/s^2]
% set options --------------------------------------------------------------
tStart
=
0
;
%start time
tFinal
=
1
;
%start time
delta0
=
0
;
vel0
=
0
;
Psi0
=
0
;
dotPsi0
=
0
;
beta0
=
0
;
sy0
=
0
;
initialState
=
[
0
,
sy0
,
delta0
,
vel0
,
Psi0
,
dotPsi0
,
beta0
];
%initial state for simulation
x0_KS
=
init_KS
(
initialState
);
%initial state for kinematic single-track model
x0_ST
=
init_ST
(
initialState
);
%initial state for single-track model
x0_MB
=
init_MB
(
initialState
,
p
);
%initial state for multi-body model
%--------------------------------------------------------------------------
% set input: rolling car (velocity should stay constant)
u
=
[
0
0
];
% simulate multi-body model
[
~
,
x_roll
]
=
ode45
(
getfcn
(
@
vehicleDynamics_MB
,
u
,
p
),[
tStart
,
tFinal
],
x0_MB
);
% simulate single-track model
[
~
,
x_roll_st
]
=
ode45
(
getfcn
(
@
vehicleDynamics_ST
,
u
,
p
),[
tStart
,
tFinal
],
x0_ST
);
% simulate kinematic single-track model
[
~
,
x_roll_ks
]
=
ode45
(
getfcn
(
@
vehicleDynamics_KS
,
u
,
p
),[
tStart
,
tFinal
],
x0_KS
);
% check correctness
% ground truth
x_roll_gt
=
[
...
0.0000000000000000
,
0.0000000000000000
,
0.0000000000000000
,
0.0000000000000000
,
0.0000000000000000
,
0.0000000000000000
,
-
0.0000000003174207
,
0.0000000848065981
,
-
0.0013834133396573
,
-
0.0020336367252011
,
-
0.0000000247286655
,
0.0176248518072475
,
0.0071655470428753
,
0.0000000006677358
,
-
0.0000001709775865
,
0.0000001839820148
,
0.0186763737562366
,
0.0003752526345970
,
0.0000000006728055
,
-
0.0000001734436431
,
0.0000001850020879
,
0.0154621865353889
,
0.0000251622262094
,
-
0.0000174466440656
,
-
0.0000174466440656
,
-
0.0000014178345014
,
-
0.0000014178345014
,
0.0000000008088692
,
0.0000000008250785
];
% comparison
res
(
end
+
1
)
=
all
(
abs
(
x_roll
(
end
,:)
-
x_roll_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
x_roll_st
(
end
,:)
==
x0_ST
);
res
(
end
+
1
)
=
all
(
x_roll_ks
(
end
,:)
==
x0_KS
);
%--------------------------------------------------------------------------
% set input: decelerating car ---------------------------------------------
v_delta
=
0
;
acc
=
-
0.7
*
g
;
u
=
[
v_delta
acc
];
% simulate multi-body model
[
~
,
x_dec
]
=
ode45
(
getfcn
(
@
vehicleDynamics_MB
,
u
,
p
),[
tStart
,
tFinal
],
x0_MB
);
% simulate single-track model
[
~
,
x_dec_st
]
=
ode45
(
getfcn
(
@
vehicleDynamics_ST
,
u
,
p
),[
tStart
,
tFinal
],
x0_ST
);
% simulate kinematic single-track model
[
~
,
x_dec_ks
]
=
ode45
(
getfcn
(
@
vehicleDynamics_KS
,
u
,
p
),[
tStart
,
tFinal
],
x0_KS
);
% check correctness
% ground truth
x_dec_gt
=
[
...
3.9830932439714242
,
-
0.0601543816394752
,
0.0000000000000000
,
-
8.0013986587693893
,
-
0.0026467910011601
,
-
0.0053025639381128
,
-
0.0019453336082831
,
-
0.0002270008481489
,
-
0.0431740570135472
,
-
0.0305313864800172
,
0.1053033709671266
,
0.0185102262795369
,
0.0137681838589760
,
-
0.0003400843778018
,
-
0.0000161129034342
,
0.0994502177784091
,
0.0256268504637763
,
0.0034700280714177
,
-
0.0002562443897593
,
-
0.0000034699487919
,
0.1128675292571417
,
0.0086968977905411
,
-
0.0020987862166353
,
-
0.0000183158385631
,
-
0.0000183158385631
,
-
0.0000095073736467
,
-
0.0000095073736467
,
-
0.0016872664171374
,
-
0.0012652511246015
];
x_dec_st_gt
=
[
...
-
3.4335000000000013
,
0.0000000000000000
,
0.0000000000000000
,
-
6.8670000000000018
,
0.0000000000000000
,
0.0000000000000000
,
0.0000000000000000
];
x_dec_ks_gt
=
[
...
-
3.4335000000000013
,
0.0000000000000000
,
0.0000000000000000
,
-
6.8670000000000018
,
0.0000000000000000
];
% comparison
res
(
end
+
1
)
=
all
(
abs
(
x_dec
(
end
,:)
-
x_dec_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
x_dec_st
(
end
,:)
-
x_dec_st_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
x_dec_ks
(
end
,:)
-
x_dec_ks_gt
)
<
1e-14
);
%--------------------------------------------------------------------------
% set input: accelerating car (wheel spin and velocity should increase; more wheel spin at rear)
v_delta
=
0.15
;
acc
=
0.63
*
g
;
u
=
[
v_delta
acc
];
% simulate multi-body model
[
~
,
x_acc
]
=
ode45
(
getfcn
(
@
vehicleDynamics_MB
,
u
,
p
),[
tStart
,
tFinal
],
x0_MB
);
% simulate single-track model
[
~
,
x_acc_st
]
=
ode45
(
getfcn
(
@
vehicleDynamics_ST
,
u
,
p
),[
tStart
,
tFinal
],
x0_ST
);
% simulate kinematic single-track model
[
~
,
x_acc_ks
]
=
ode45
(
getfcn
(
@
vehicleDynamics_KS
,
u
,
p
),[
tStart
,
tFinal
],
x0_KS
);
% check correctness
% ground truth
x_acc_gt
=
[
...
1.6869441956852231
,
0.0041579276718349
,
0.1500000000000001
,
3.1967387404602654
,
0.3387575860582390
,
0.8921302762726965
,
-
0.0186007698209413
,
-
0.0556855538608812
,
0.0141668816602887
,
0.0108112584162600
,
-
0.6302339461329982
,
0.0172692751292486
,
0.0025948291288222
,
-
0.0042209020256358
,
-
0.0115749221900647
,
0.4525764527765288
,
0.0161366380049974
,
-
0.0012354790918115
,
-
0.0023647389844973
,
-
0.0072210348979615
,
-
1.8660984955372673
,
0.0179591511062951
,
0.0010254111038481
,
11.1322413877606117
,
7.5792605585643713
,
308.3079237740076906
,
310.8801727728298374
,
-
0.0196922024889714
,
-
0.0083685253175425
];
x_acc_st_gt
=
[
...
3.0731976046859701
,
0.2869835398304387
,
0.1500000000000000
,
6.1802999999999999
,
0.1097747074946324
,
0.3248268063223300
,
0.0697547542798039
];
x_acc_ks_gt
=
[
...
3.0845676868494931
,
0.1484249221523043
,
0.1500000000000000
,
6.1803000000000026
,
0.1203664469224163
];
%comparison
res
(
end
+
1
)
=
all
(
abs
(
x_acc
(
end
,:)
-
x_acc_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
x_acc_st
(
end
,:)
-
x_acc_st_gt
)
<
1e-14
);
res
(
end
+
1
)
=
all
(
abs
(
x_acc_ks
(
end
,:)
-
x_acc_ks_gt
)
<
1e-14
);
%--------------------------------------------------------------------------