Skip to content

C
commonroad-vehicle-models

11.08., 9:00 - 11:00: Due to updates GitLab will be unavailable for some minutes between 09:00 and 11:00.

Commit 74908ce8 authored by Markus Koschi's avatar Markus Koschi
Browse files
Options

update to Release 2018a

parent fc628bd0
Hide whitespace changes
Inline Side-by-side
Showing with 1090 additions and 273 deletions
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);
%--------------------------------------------------------------------------
% steering to left---------------------------------------------------------
v_delta = 0.15;
u = [v_delta 0];
% simulate multi-body model
[~,x_left] = ode45(getfcn(@vehicleDynamics_MB,u,p),[tStart, tFinal],x0_MB);
% simulate single-track model
[~,x_left_st] = ode45(getfcn(@vehicleDynamics_ST,u,p),[tStart, tFinal],x0_ST);
% simulate kinematic single-track model
[~,x_left_ks] = ode45(getfcn(@vehicleDynamics_KS,u,p),[tStart, tFinal],x0_KS);
% check correctness
% ground truth
x_left_gt = [ ...
0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0003021160057306, 0.0115474648881108, -0.0013797955031689, -0.0019233204598741, -0.0065044050021887, 0.0176248291065725, 0.0071641239008779, 0.0001478513434683, 0.0092020911982902, -0.0178028732533553, 0.0186751057310096, 0.0003566948613572, 0.0001674970785214, 0.0015871955172538, -0.0175512251679294, 0.0154636630992985, 0.0000482191918813, -0.0000173442953338, -0.0000174708138706, -0.0000014178345014, -0.0000014178345014, 0.0002293337149155, 0.0003694012334077];
x_left_st_gt = [ ...
0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000, 0.0000000000000000];
x_left_ks_gt = [ ...
0.0000000000000000, 0.0000000000000000, 0.1500000000000000, 0.0000000000000000, 0.0000000000000000];
% comparison
res(end+1) = all(abs(x_left(end,:) - x_left_gt) < 1e-14);
res(end+1) = all(abs(x_left_st(end,:) - x_left_st_gt) < 1e-14);
res(end+1) = all(abs(x_left_ks(end,:) - x_left_ks_gt) < 1e-14);
%--------------------------------------------------------------------------
% obtain final result
res = all(res);
end
% add input and parameters to ode
function [handle] = getfcn(fctName,u,p)
function dxdt = f(t,x)
dxdt = fctName(x,u,p);
end
handle = @f;
end
%------------- END OF CODE --------------
MATLAB/vehicleDynamics_KS.m
View file @ 74908ce8
......@@ -7,7 +7,7 @@ function f = vehicleDynamics_KS(x,u,p)
% Inputs:
% x - vehicle state vector
% u - vehicle input vector
% p - vehicle parameter vector
% p - vehicle parameter structure
%
% Outputs:
% f - right-hand side of differential equations
......@@ -22,7 +22,7 @@ function f = vehicleDynamics_KS(x,u,p)
% Author: Matthias Althoff
% Written: 12-January-2017
% Last update: ---
% Last update: 15-December-2017
% Last revision:---
%------------- BEGIN CODE --------------
......@@ -40,6 +40,12 @@ l = p.a + p.b;
%u1 = v_delta steering angle velocity of front wheels
%u2 = ax longitudinal acceleration
%consider steering constraints
u(1) = steeringConstraints(x(3),u(1),p.steering);
%consider acceleration constraints
u(2) = accelerationConstraints(x(4),u(2),p