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

init_ST.m 1.74 KB
Newer Older
Markus Koschi's avatar
Markus Koschi committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
function x0 = init_ST(initState, p)
% init_MB - generates the initial state vector for the single-track model
%
% Syntax:  
%     x0 = init_ST(initState, p)
%
% Inputs:
%     initState - core initial states
%     p - parameter vector
%
% Outputs:
%     x0 - initial state vector
%
% Example: 
%
% Other m-files required: none
% Subfunctions: none
% MAT-files required: none
%
% See also: ---

% Author:       Matthias Althoff
% Written:      11-January-2017
% Last update:  ---
% 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

%------------- END OF CODE --------------