Car testdrive (lane change manoeuvre) (Muscod)

This page contains the Muscod code for the Car testdrive (lane change manoeuvre) problem.

The differential equations in C code:

// Controls
double C_steer = u[0];
double C_brake = u[1];
double C_acc   = u[2];
 
// Differential states
double X_v     = xd[2];
double X_beta  = xd[3];
double X_psi   = xd[4];
double X_wz    = xd[5];
double X_delta = xd[6];
 
// Intermediate values
double alpha_f, alpha_r, v_km_h, v_km_h2;
double F_Ax, F_Ay, F_Bf, F_Br, F_Rf, F_Rr, F_sf, F_sr, F_lr, F_lf;
double f_R, f_1, w_mot, f_2, f_3, M_mot, M_wheel;
double X_v_cos_X_beta, X_v_sin_X_beta;
 
X_v_cos_X_beta = X_v * cos ( X_beta );
X_v_sin_X_beta = X_v * sin ( X_beta );
alpha_f        = X_delta - atan( ( P_l_f * X_wz - X_v_sin_X_beta ) / X_v_cos_X_beta );
alpha_r        =           atan( ( P_l_r * X_wz + X_v_sin_X_beta ) / X_v_cos_X_beta );
 
F_sf    = P_D_f * sin( P_C_f * atan( P_B_f*alpha_f - P_E_f*(P_B_f*alpha_f - atan(P_B_f*alpha_f)) ) );
F_sr    = P_D_r * sin( P_C_r * atan( P_B_r*alpha_r - P_E_r*(P_B_r*alpha_r - atan(P_B_r*alpha_r)) ) );
 
F_Ax    = 0.5 * P_c_w * P_rho * P_A * X_v*X_v;
F_Ay    = 0.0;
F_Bf    = 2.0/3.0 * C_brake;
F_Br    = 1.0/3.0 * C_brake;
 
v_km_h  = X_v / 100.0 * 3.6;
v_km_h2 = v_km_h * v_km_h;
f_R     = P_f_R0 + P_f_R1 * v_km_h + P_f_R4 * v_km_h2 * v_km_h2;
F_Rf    = f_R * P_F_zf;
F_Rr    = f_R * P_F_zr;
 
f_1     = 1.0 - exp( -3.0 * C_acc );
w_mot   = X_v * P_ig * P_i_t / P_R;
f_2     = -37.8 + (1.54 - 0.0019 * w_mot_i) * w_mot;
f_3     = -34.9 - 0.04775 * w_mot;
M_mot   = f_1 * f_2_i + ( 1.0 - f_1 ) * f_3_i;
M_wheel = P_ig * P_i_t * M_mot_i;
 
F_lr    = M_wheel / P_R - F_Br - F_Rr;
F_lf    = - F_Bf - F_Rf;
 
// 0 Horizontal position x
rhs[0] = X_v * cos( X_psi - X_beta );
// 1 Vertical position y
rhs[1] = X_v * sin( X_psi - X_beta );
// 2 Velocity v
rhs[2] = 1.0 / P_m * (
	  (F_lr - F_Ax) * cos(X_beta) + F_lf * cos(X_delta + X_beta)
	- (F_sr - F_Ay) * sin(X_beta) - F_sf * sin(X_delta + X_beta) );
// 3 Side slip angle beta
rhs[3] = X_wz - 1.0 / (P_m * X_v) * (
	  (F_lr - F_Ax) * sin(X_beta) + F_lf * sin(X_delta + X_beta)
	+ (F_sr - F_Ay) * cos(X_beta) + F_sf * cos(X_delta + X_beta) );
// 4 Yaw angle psi
rhs[4] = X_wz;
// 5 Velocity of yaw angle w_z
rhs[5] = 1.0 / P_I_zz * (
	  F_sf * P_l_f * cos(X_delta) - F_sr * P_l_r
	+ F_lf * P_l_f * sin(X_delta) - F_Ay * P_e_SP );
// 6 Steering angle delta
rhs[6] = C_steer;