[
  {
    "path": ".gitignore",
    "content": "# all movies\n*.avi\n\n# rosbag data\n/model_fitting/data"
  },
  {
    "path": "README.md",
    "content": "# trajectory_tracking_simulation\nFor developing trajectory tracking algorithm with MATLAB. \n\nRun /simulation/main.m to compute trajectory tracking simulation.\n\nReference path is designed in /path_design/path_design.m using spline from used-defined reference points.\n\n・MPC\n\n![mpc](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/mpc.gif)\n\n・PID\n\n![pid](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/pid.gif)\n\n・pure pursuit\n\n![purepursuit](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/purepursuit.gif)\n\n## Currently available controller\n* pure-pursuit\n* PID\n* MPC (use Optimization Toolbox)\n* MPC without constraint\n\n## Parameters\nparameters are all set in /simulation/main.m\n\n### simulation\n* simulation_time : overall simulation time\n* simulation_rk4_time_step : time span for integrate dynamics\n\n### path\n* vel_ref : reference speed [m/s]\n\n### simulated vehicle model\n* param.tau : steering dynamics 1d-approximated time constant [s]\n* param.wheelbase : wheelbase length [m]\n* param.steer_lim : tire angle limit [rad]\n* param.vel_max : max velocity limit [m/s]\n* param.vel_min : min velocity limit [m/s]\n* param.input_delay : input delay [s]\n* param.measurement_noise_stddev :  measurement noise standard deviation for [pos x[m], pos y[m], heading[rad], steering[rad]]\n* param.steering_steady_state_error_deg : steady state error for steering model\n\n### controllers\nfor all\n* param.control_dt : control time span (zero order hold) [s]\n\nfor pure-pursuit\n* param.pure_pursuit_lookahead : lookahead distance for pure-pursuit controller [m]\n\nfor mpc\n* param.mpc_dt : time span for prediction\n* param.mpc_n : step numbers for prediction\n* param.mpc_constraint_steering_deg : steering limit constraint for optimization problem\n* param.mpc_constraint_steer_rate_deg : steering rate limit constraint for optimization problem\n* param.mpc_model_dim : mpc state model dimension\n* param.mpc_Q : state weight matrix for optimization\n* param.mpc_R : input weight matrix for optimization\n* param.mpc_sensor_delay : for sensor delay compensation\n"
  },
  {
    "path": "model_fitting/func/quat2elu.m",
    "content": "function rpy = quat2elu(qw, qx, qy, qz)\r\n\r\n\r\nsinr_cosp = 2 * qw .* qx + qy .* qz;\r\ncosr_cosp = 1 - 2 * qx .* qx + qy .* qy;\r\nroll = atan2(sinr_cosp, cosr_cosp);\r\n\r\nsinp = 2 * (qw .* qy - qz .* qx);\r\n% remove out of range \r\nsinp(sinp > 1) = 1;\r\nsinp(sinp < -1) = -1;\r\n\r\npitch = asin(sinp);\r\n\r\nsiny_cosp = 2 * (qw .* qz + qx .* qy);\r\ncosy_cosp = 1 - 2 * (qy .* qy + qz .* qz);\r\nyaw = atan2(siny_cosp, cosy_cosp);\r\n\r\nrpy = [roll, pitch, yaw];"
  },
  {
    "path": "model_fitting/main.m",
    "content": "set(0, 'defaultAxesFontSize', 14);\r\nset(0, 'defaultTextFontSize', 14);\r\nset(0, 'DefaultAxesLineWidth', 1.2, 'DefaultLineLineWidth', 1.5);\r\n\r\nrad2deg = 180 / pi;\r\ndeg2rad = pi / 180;\r\n\r\nlexus_param.tread_rear = 1.63;\r\nlexus_param.tread_front = 1.64;\r\nlexus_param.wheelbase = 2.79;\r\n\r\naddpath func\r\n\r\n\r\n%% Ԃ낦\r\nimuraw.t = imuraw.headerstampsecs + imuraw.headerstampnsecs * 1e-9;\r\nwheelspeedrpt.t = wheelspeedrpt.headerstampsecs + wheelspeedrpt.headerstampnsecs * 1e-9;\r\nyawraterpt.t = yawraterpt.headerstampsecs + yawraterpt.headerstampnsecs * 1e-9;\r\nsteerrpt.t = steerrpt.headerstampsecs + steerrpt.headerstampnsecs * 1e-9;\r\nndtpose.t = ndtpose.headerstampsecs + ndtpose.headerstampnsecs * 1e-9;\r\nshiftrpt.t = shiftrpt.headerstampsecs + shiftrpt.headerstampnsecs * 1e-9;\r\n\r\ntmp = max([imuraw.t(1), wheelspeedrpt.t(1), yawraterpt.t(1), steerrpt.t(1), ndtpose.t(1), shiftrpt.t(1)]);\r\nimuraw.t = imuraw.t - tmp;\r\nwheelspeedrpt.t = wheelspeedrpt.t - tmp;\r\nyawraterpt.t = yawraterpt.t - tmp;\r\nsteerrpt.t = steerrpt.t - tmp;\r\nndtpose.t = ndtpose.t - tmp;\r\nshiftrpt.t = shiftrpt.t - tmp;\r\nts = max([imuraw.t(1), wheelspeedrpt.t(1), yawraterpt.t(1), steerrpt.t(1), ndtpose.t(1), shiftrpt.t(1)]);\r\ntf = min([imuraw.t(end), wheelspeedrpt.t(end), yawraterpt.t(end), steerrpt.t(end), ndtpose.t(end), shiftrpt.t(end)]);\r\n\r\n\r\n%% ԑf[^yawrate߂\r\n\r\nwheelspeed_all = [wheelspeedrpt.front_left_wheel_speed, wheelspeedrpt.front_right_wheel_speed, ...\r\n            wheelspeedrpt.rear_left_wheel_speed, wheelspeedrpt.rear_right_wheel_speed];\r\n        \r\nfigure;\r\nplot(wheelspeedrpt.t, wheelspeed_all);grid on\r\nxlabel('t [s]'); ylabel('vel [m/s]'); legend('front-left','front-right','rear-left','rear-right');\r\n\r\n\r\n\r\nwheelspeedrpt.velsign = round(interp1q(shiftrpt.t, shiftrpt.output, wheelspeedrpt.t));\r\nwheelspeedrpt.velsign(isnan(wheelspeedrpt.velsign) == 1) = 0;\r\nwheelspeedrpt.velsign(wheelspeedrpt.velsign == 1) = -1;\r\nwheelspeedrpt.velsign(wheelspeedrpt.velsign == 3) = 1;\r\n\r\nwheelspeedrpt.yaw_from_rear_wheelspeeds = (wheelspeedrpt.rear_left_wheel_speed - wheelspeedrpt.rear_right_wheel_speed) /4 / lexus_param.tread_rear .* wheelspeedrpt.velsign;\r\nwheelspeedrpt.yaw_from_front_wheelspeeds = (wheelspeedrpt.front_left_wheel_speed - wheelspeedrpt.front_right_wheel_speed) /4 / lexus_param.tread_front .* wheelspeedrpt.velsign;\r\nwheelspeedrpt.vel_from_reat_wheelspeeds = (wheelspeedrpt.rear_left_wheel_speed + wheelspeedrpt.rear_right_wheel_speed) /2 .* wheelspeedrpt.velsign;\r\n\r\n\r\n% figure;plot(imuraw.t, [imuraw.angular_velocityx, imuraw.angular_velocityy, imuraw.angular_velocityz]); grid on;\r\n\r\n%% XeAOkinematicsfpĊpxvZ\r\n\r\n% Ƃ肠vbg\r\nfigure;plot(steerrpt.t, [steerrpt.manual_input, steerrpt.command], steerrpt.t, steerrpt.output, '-.');grid on;\r\nlegend('manual', 'command','output');\r\nxlabel('t [s]'), ylabel('steering [deg]');\r\n\r\n\r\n% `⊮  `B֐F萔1/8.7=0.115[s]\r\ndt = 0.03;\r\ntp = (ts:dt:tf)';\r\nsteer_p.cmd = interp1q(steerrpt.t, steerrpt.command, tp);\r\nsteer_p.out = interp1q(steerrpt.t, steerrpt.output, tp);\r\n\r\n\r\n% XeAOpx{֎ԑpxvZodompxAimupxƔr\r\nwheelspeed_p.vel_from_rear = interp1q(wheelspeedrpt.t, wheelspeedrpt.vel_from_reat_wheelspeeds, tp);\r\nomega_steer_p = wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad) / lexus_param.wheelbase;\r\n\r\n\r\n\r\n%% NDT̔lpxvZ\r\n\r\nfigure;plot(ndtpose.posepositionx, ndtpose.posepositiony)\r\n\r\neul = quat2elu(ndtpose.poseorientationw, ndtpose.poseorientationx, ndtpose.poseorientationy, ndtpose.poseorientationz);\r\nfor i = 2:length(eul)\r\n    if eul(i,3) - eul(i-1,3) > pi\r\n        eul(i:end,3) = eul(i:end,3) - 2 * pi;\r\n    elseif eul(i,3) - eul(i-1,3) < -pi\r\n        eul(i:end,3) = eul(i:end,3) + 2 * pi;\r\n    end\r\nend\r\n\r\neul_p = interp1q(ndtpose.t, eul, tp);\r\neul_p_f = filtfilt(SOS, G, eul_p);\r\nyawrate_from_ndt = (eul_p_f(2:end,3) - eul_p_f(1:end-1,3)) / dt;\r\nyawrate_from_ndt = [yawrate_from_ndt; yawrate_from_ndt(end)];\r\n\r\n%% ndt狁߂pxɑ΂wheelbasẽp[^tBbeBO\r\n\r\nwheelbase_fitting = yawrate_from_ndt \\ (wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad));\r\nomega_steer_p_fitting = wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad) / wheelbase_fitting;\r\n\r\n\r\n%% Spxf[^vbg\r\n\r\nfigure;\r\nplot(imuraw.t, imuraw.angular_velocityz * (-rad2deg)); hold on;\r\nplot(yawraterpt.t, yawraterpt.yaw_rate); hold on;\r\nplot(wheelspeedrpt.t, [wheelspeedrpt.yaw_from_front_wheelspeeds, wheelspeedrpt.yaw_from_rear_wheelspeeds] * rad2deg); hold on;\r\nplot(tp, yawrate_from_ndt * rad2deg); hold on;\r\nplot(tp, omega_steer_p * rad2deg); hold on;\r\nplot(tp, omega_steer_p_fitting * rad2deg); grid on;\r\nlegend('imu -z', 'yawraterpt','front wheelspeed diff', 'rear wheelspeed diff', 'ndt deriv + LPF(3d-butter, fc=1Hz)', ...\r\n    'kinematics model (theorical: L=2.79)', 'kinematics model (fitting: L=2.69)');\r\nxlabel('t [s]'); ylabel('yawrate estimated [deg/s]')\r\n"
  },
  {
    "path": "path_design/path_design.m",
    "content": "%% design path from points by spline\r\n\r\npoint = [0, 0;\r\n    1, 0;\r\n    2,0\r\n    3,0.5\r\n    4,1.5\r\n    4.8, 1.5\r\n    5,0.8\r\n    6, 0.5\r\n    6.5, 0\r\n    7.5, 0.5\r\n    7,2\r\n    6, 3\r\n    5, 4\r\n    4., 2.5\r\n    3, 3\r\n    2., 3.5;\r\n    1.3, 2.2\r\n    0.5, 2.\r\n    0,3];\r\n\r\ns = 1:1:length(point);\r\n\r\n\r\npx_spline = spline(s, point(:,1), 1:0.01:length(point));\r\npy_spline = spline(s, point(:,2), 1:0.01:length(point));\r\n\r\np_spline = [px_spline', py_spline'];\r\n\r\n\r\n\r\n%% insert yaw\r\n\r\nyaw = zeros(length(px_spline), 1);\r\nfor i = 2:length(px_spline)-1\r\n    x_forward = px_spline(i+1);\r\n    x_backward = px_spline(i-1);\r\n    y_forward = py_spline(i+1);\r\n    y_backward = py_spline(i-1);\r\n    yaw(i) = atan2(y_forward-y_backward, x_forward-x_backward);\r\nend\r\nyaw(1) = yaw(2);\r\nyaw(end) = yaw(end-1);\r\n\r\n\r\n%% plot with attitude\r\n\r\narrow_scale = 0.01;\r\n\r\nfigure(101);\r\nplot(point(:,1), point(:,2), 'bo-'); hold on;\r\nquiver(px_spline', py_spline', cos(yaw)*arrow_scale, sin(yaw)*arrow_scale);\r\nplot(px_spline, py_spline,'r-'); grid on; hold off;\r\n\r\n%% save path\r\npath = [px_spline', py_spline', yaw];\r\n\r\nsave('path', 'path')"
  },
  {
    "path": "simulation/kinematics_model.m",
    "content": "function d_state = kinematics_model(state, input, param)\r\n\r\nv_des = input(1);\r\ndelta_des = input(2);\r\n\r\n% limit\r\ndelta_des = max(min(delta_des, param.steer_lim), -param.steer_lim);\r\nv_des = max(min(v_des, param.vel_max), param.vel_min);\r\n\r\n% x = state(1);\r\n% y = state(2);\r\nyaw = state(3);\r\ndelta = state(4);\r\n\r\nv = v_des;\r\n\r\nd_x = v * cos(yaw);\r\nd_y = v * sin(yaw);\r\nd_yaw = v * tan(delta) / param.wheelbase;\r\nd_delta = - (delta - delta_des) / param.tau;\r\n\r\n% add steady state error caused by friction\r\nif abs(delta - delta_des) < param.steering_steady_state_error_deg / 180 * pi\r\n    d_delta = 0;\r\nend\r\n\r\n\r\nd_state = [d_x, d_y, d_yaw, d_delta];\r\n\r\n\r\n\r\n"
  },
  {
    "path": "simulation/main.m",
    "content": "% \r\n% state = [x, y, yaw, delta]\r\n% input = [v_des, delta_des]\r\n% ref = [x_ref, y_ref, yaw_ref, v_ref]\r\n% \r\n% \r\n% \r\n\r\nclear variables;\r\nclose all;\r\n\r\n\r\nset(0, 'defaultAxesFontSize', 12);\r\nset(0, 'defaultTextFontSize', 20);\r\nset(0, 'DefaultAxesLineWidth', 1.0, 'DefaultLineLineWidth', 1.0);\r\n\r\n\r\naddpath ../path_design\r\n\r\ncontrol_mode_option = [\"pure_pursuit\", \"pid\", \"mpc\", \"mpc_no_constraints\"];\r\ncontrol_mode = control_mode_option(3);\r\n\r\nsave_video = 0; %1:save, 0:no\r\n\r\n%% preliminaries\r\nrad2deg = 180 / pi;\r\ndeg2rad = pi / 180;\r\nkmh2ms = 1000 / 3600;\r\n\r\nsimulation_time = 35;\r\nsimulation_rk4_time_step = 0.002; % simulation time step\r\n\r\nvel_ref = 30 * kmh2ms;\r\n\r\n% for dynamics model\r\nparam.tau = 0.27; % steering dynamics: 1d-approximated time constant\r\nparam.wheelbase = 2.69;\r\nparam.steer_lim = 30 * deg2rad;\r\nparam.vel_max = 10;\r\nparam.vel_min = -5;\r\n\r\nparam.input_delay = 0.24; % [s]\r\nparam.control_dt = 0.03; % [s]\r\nparam.measurement_noise_stddev = [0.1, 0.1, 1.0*deg2rad, 0.5*deg2rad]; % measurement noise\r\n% param.measurement_noise_stddev = [0,0,0,0]; % measurement noise\r\nparam.steering_steady_state_error_deg = 1;\r\n\r\n% for pure pursuit only\r\nparam.pure_pursuit_lookahead = 8.0; % [m]\r\n\r\n% for mpc only\r\nparam.mpc_dt = 0.1;\r\nparam.mpc_n = 30;\r\nparam.mpc_constraint_steering_deg = 30;\r\nparam.mpc_constraint_steer_rate_deg = 280;\r\nparam.mpc_model_dim = 3;\r\nparam.mpc_Q = diag([1,2]);\r\nparam.mpc_R = 0.5;\r\nparam.mpc_delay_comp_step = round(param.input_delay / param.control_dt);\r\n% param.mpc_delay_comp_step = 0.0;\r\n\r\n% use the input ahead of the delay time\r\nparam.mpc_sensor_delay = param.input_delay; \r\n\r\n% for mpc2\r\nparam.mpc2_dt = 0.2;\r\nparam.mpc2_n = 10;\r\nparam.mpc2_steering_lim_deg = 40;\r\nparam.mpc2_model_dim = 4;\r\nparam.mpc2_Q = diag([1,1,0]);\r\nparam.mpc2_R = 0.05;\r\n\r\n%% simulation parameters\r\n\r\n% initial position (x, y, yaw, delta)\r\nx0 = [0, 0.5, 0, 0];\r\n\r\nts = 0;\r\ndt = simulation_rk4_time_step;\r\ntf = simulation_time;\r\nt = ts:dt:tf;\r\n\r\n%% reference trajectory design\r\n\r\npath_design; % using spline\r\nload path; % x, y, yaw\r\n\r\nref = zeros(length(path), 6);\r\nIDX_X = 1;\r\nIDX_Y = 2;\r\nIDX_XY = 1:2;\r\nIDX_XYYAW = 1:3;\r\nIDX_YAW = 3;\r\nIDX_VEL = 4;\r\nIDX_CURVATURE = 5;\r\nIDX_TIME = 6;\r\n\r\nIDX_STEER = 4;\r\n\r\n\r\npath_size_scale = 15;\r\npath(:,IDX_XY) = path(:,IDX_XY) * path_size_scale;\r\nref(:,IDX_XYYAW) = path(:,IDX_XYYAW);\r\n\r\nref(:,IDX_VEL) = ones(length(path),1)*vel_ref;\r\n\r\n% insert curvature into path\r\nfor i = 2:length(ref)-1\r\n    p1_ = ref(i-1,IDX_XY);\r\n    p2_ = ref(i, IDX_XY);\r\n    p3_ = ref(i+1, IDX_XY);\r\n    A_ = ((p2_(1)-p1_(1))*(p3_(2)-p1_(2)) - (p2_(2)-p1_(2))*(p3_(1)-p1_(1))) / 2;\r\n    ref(i, IDX_CURVATURE) = 4 * A_ / (norm(p1_-p2_) * norm(p2_-p3_) * norm(p3_-p1_));\r\nend\r\n\r\n% insert relative time into path\r\nfor i = 2:length(ref)\r\n    v_ = ref(i,IDX_VEL);\r\n    d_ = norm(ref(i,IDX_XY)-ref(i-1,IDX_XY));\r\n    dt_ = d_ / v_;\r\n    ref(i, IDX_TIME) = ref(i-1, IDX_TIME) + dt_;\r\nend\r\n\r\n\r\n%% simulation\r\n\r\nif control_mode == \"pure_pursuit\"\r\n    [X, U, debug] = simulate_rk4(@kinematics_model, @pure_pursuit, x0, ref, ts, dt, tf, param);\r\n    lat_error_vec = debug(:,end);\r\nelseif control_mode == \"pid\"\r\n   [X, U, debug] = simulate_rk4(@kinematics_model, @pid_controller, x0, ref, ts, dt, tf, param);\r\n   lat_error_vec = debug(:,end);\r\nelseif control_mode == \"mpc\"\r\n    param.mpc_solve_without_constraint = false;\r\n    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);\r\n    lat_error_vec = debug(:,end);\r\nelseif control_mode == \"mpc_no_constraints\"\r\n    param.mpc_solve_without_constraint = true;\r\n    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);\r\n    lat_error_vec = debug(:,end);  \r\nelseif control_mode == \"mpc2\"\r\n    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller2, x0, ref, ts, dt, tf, param);\r\n    lat_error_vec = debug(:,end);\r\nend\r\nfprintf(\"lattitude error: mean square = %f, max = %f\", norm(lat_error_vec)/simulation_time, max(lat_error_vec));\r\n\r\n\r\n%% movie plot\r\n\r\nsp_num = 18;\r\nsubpl1 = 'subplot(sp_num,sp_num, sp_num+1:sp_num*12);';\r\nsubpl2 = 'subplot(sp_num,sp_num, sp_num*13+1:sp_num*15);';\r\nsubpl3 = 'subplot(sp_num,sp_num, sp_num*16+1:sp_num*18);';\r\n\r\n% tire2steer = 12.5;\r\n\r\nfig_trajectory_result = figure(1);\r\nset(fig_trajectory_result, 'Position', [716 735 1026 1146]);\r\neval(subpl1);\r\nplot(ref(:,1), ref(:,2),'k-.'); hold on; grid on;\r\n% plot(X(:,1), X(:,2));\r\n% legend('ref','tracked');\r\nxlabel('x [m]'); ylabel('y [m]');\r\n% img_orig = imread('handle.jpg');\r\n% img = imrotate(img_orig, 10);\r\n% handle_plt = image([150, 170],[110, 90], img);\r\n% handle_plt_point = [160, 100, 0];\r\n\r\n\r\neval(subpl2);\r\nplot(t, lat_error_vec, 'b'); grid on; hold on; \r\nxlabel('t [s]'); ylabel('latitude error [m]');\r\nulim = ceil(2*max(lat_error_vec))/2;\r\ndlim = floor(2*min(lat_error_vec))/2;\r\nylim([dlim, ulim]);\r\n\r\neval(subpl3);\r\np1 = plot(t, X(:,IDX_STEER)*rad2deg, 'b'); grid on; hold on; \r\np2 = plot(t, U(:,2)*rad2deg, 'Color', [0.7 0. 1]); hold on; \r\nlegend([p1,p2], {'measured','command'})\r\nxlabel('t [s]'); ylabel('steering angle [deg]');\r\nulim = round(2*max(X(:,IDX_STEER)*rad2deg))/2;\r\ndlim = round(2*min(X(:,IDX_STEER)*rad2deg))/2;\r\nylim([dlim, ulim]);\r\n\r\nz_axis = [0 0 1];\r\nsetpoint = []; rear_tire = []; front_tire = []; body = []; tracked = []; \r\nsetpoint_ideal = []; error_point = []; steer_point = []; time_bar_laterror = []; time_bar_steer = [];\r\nL = param.wheelbase;\r\nrear_length = 1;\r\nfront_length = 1;\r\nside_width = 0.9;\r\nfig_draw_i = 1:round(1/dt/20):length(t);\r\n\r\n% for movie\r\nclear frame_vec;\r\nframe_vec(length(fig_draw_i)) = struct('cdata', [], 'colormap',[]);\r\n\r\nj = 1;\r\nfig_trajectory_result; hold on;\r\nfor i = fig_draw_i\r\n    eval(subpl1);\r\n    disp(t(i))\r\n    rear_x = X(i,1);\r\n    rear_y = X(i,2);\r\n    yaw = X(i,3);\r\n    delta = X(i,IDX_STEER);\r\n    front_x = rear_x + L;\r\n    front_y = rear_y;\r\n    delete([setpoint, rear_tire, front_tire, body, tracked, setpoint_ideal, error_point, steer_point, time_bar_laterror, time_bar_steer]);\r\n    tracked = plot(X(1:i,1), X(1:i,2),'r');\r\n    title_draw = \"t = \"+num2str(t(i),'%5.1f') + \"[s], steer = \" + num2str(delta*rad2deg,'%+3.1f') + \"[deg], v = \" + ...\r\n        num2str(vel_ref*3600/1000,'%3.1f') + \"[km/h], lat error = \"+num2str(lat_error_vec(i),'%+2.2f') + \"[m]\";\r\n    title_draw = [title_draw; \"Simulation: solver = rk4, sensor-delay = \"  + num2str(param.input_delay*1000, '%d') + \"[ms], control freq=\" + ...\r\n        num2str(1/param.control_dt, '%d') + \"[hz]\"];\r\n    title_draw = [title_draw; \"noise-sigma = \" + num2str(param.measurement_noise_stddev(1),'%2.2f')+\"(pos), \"+ ...\r\n        num2str(param.measurement_noise_stddev(3),'%2.2f')+\"(yaw), \"+num2str(param.measurement_noise_stddev(4),'%2.2f')+\"(steer)\"];\r\n    if control_mode == \"mpc\" || control_mode == \"mpc_no_constraints\"\r\n        title_draw = [title_draw; \"MPC: dt = \" + num2str(param.mpc_dt, '%3.3f') + \"[s], horizon step = \" + num2str(param.mpc_n, '%d')];\r\n%         pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));\r\n%         pred_states = reshape(pred_states, param.mpc_n, length(pred_states)/param.mpc_n);\r\n%         setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error\r\n        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(2+4+1));\r\n        pred_error = reshape(pred_error, param.mpc_n, length(pred_error)/param.mpc_n);\r\n        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error\r\n    elseif control_mode == \"mpc2\"\r\n        title_draw = [title_draw; \"MPC2: dt = \" + num2str(param.mpc_dt, '%3.3f') + \"[s], horizon step = \" + num2str(param.mpc_n, '%d')];\r\n        pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));\r\n        pred_states = transpose(reshape(pred_states, length(pred_states)/param.mpc_n, param.mpc_n));\r\n        setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error\r\n        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(4+4+1));\r\n        pred_error = transpose(reshape(pred_error, length(pred_error)/param.mpc_n, param.mpc_n));\r\n        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error\r\n    elseif control_mode == \"pure_pursuit\"\r\n        title_draw = [title_draw; \"pure-pursuit: lookahead dist=\"+num2str(param.pure_pursuit_lookahead, '%1.1f')+\"[m]\"];\r\n        sp = debug(i,:);\r\n        setpoint = plot(sp(1), sp(2), 'ro');\r\n    elseif control_mode == \"pid\"\r\n        title_draw = [title_draw; \"PID: kp = \" + num2str(0.3, '%3.3f') + \", ki = \" + num2str(0, '%3.3f') + \", kd = \" + num2str(1.5, '%3.3f')];\r\n        sp = debug(i,:);\r\n        setpoint = plot(sp(1), sp(2), 'ro');\r\n    end\r\n    rear_tire = plot([rear_x-0.3, rear_x+0.3],[rear_y, rear_y], 'k', 'LineWidth', 2.0);\r\n    front_tire = plot([front_x-0.3, front_x+0.3],[front_y, front_y], 'k', 'LineWidth', 2.0);\r\n    body = plot([rear_x-rear_length, front_x+front_length, front_x+front_length, rear_x-rear_length, rear_x-rear_length], ...\r\n        [rear_y-side_width, front_y-side_width, front_y+side_width, rear_y+side_width, rear_y-side_width],'k');\r\n    rear_origin = [rear_x, rear_y, 0];\r\n    front_origin = [rear_x + L*cos(yaw), rear_y + L*sin(yaw), 0];\r\n    rotate(body, z_axis, yaw * rad2deg, rear_origin);\r\n    rotate(rear_tire, z_axis, yaw * rad2deg, rear_origin);\r\n    rotate(front_tire, z_axis, yaw * rad2deg, rear_origin);\r\n    rotate(front_tire, z_axis, delta * rad2deg, front_origin);\r\n%     rotate(handle_plt, [0,0,1], delta*tire2steer*rad2deg, handle_plt_point)\r\n    title(title_draw);\r\n    xlim([0 120]);\r\n     \r\n    % lat error\r\n    eval(subpl2);\r\n    error_point = plot(t(i), lat_error_vec(i), 'ko');\r\n    time_bar_laterror = plot([t(i), t(i)], [100, -100], 'k');\r\n    \r\n    % steering\r\n    eval(subpl3);\r\n    steer_point = plot(t(i), X(i, IDX_STEER)*rad2deg, 'ko');\r\n    time_bar_steer = plot([t(i), t(i)], [100, -100], 'k');\r\n    legend([p1,p2], {'measured','command'})\r\n    ylim([-40 40]);\r\n  \r\n    \r\n    drawnow;\r\n    frame_vec(j) = getframe(fig_trajectory_result);\r\n    \r\n    j = j + 1;\r\nend\r\n\r\n\r\n\r\n% for video save\r\nif (save_video == 1)\r\n    cd ./movie\r\n    frame_vec(1) = [];\r\n    vidObj = VideoWriter('result.avi');\r\n    vidObj.FrameRate = 25;\r\n    open(vidObj);\r\n    writeVideo(vidObj, frame_vec);\r\n    close(vidObj);\r\n    cd ../\r\nend\r\n\r\n\r\n\r\n% plot_pid;"
  },
  {
    "path": "simulation/model_predictive_controller.m",
    "content": "function [u, debug_info] = model_predictive_controller(state, t, ref, param)\r\n% \r\n% state = [x, y, yaw, delta]\r\n% u = [v_des, delta_des]\r\n% ref = [x_ref; y_ref; yaw_ref; v_ref; k_ref; t_ref];\r\n% \r\n% \r\n% \r\n\r\n% =================== for delay compensation ==============================\r\npersistent deltades_buffer\r\nif isempty(deltades_buffer)\r\n    deltades_buffer = zeros(param.mpc_delay_comp_step, 1);\r\nend\r\n% =========================================================================\r\n    \r\ndeg2rad = pi / 180;\r\ndelay_step = param.mpc_delay_comp_step;\r\n\r\nIDX_X = 1;\r\nIDX_Y = 2;\r\nIDX_XY = 1:2;\r\nIDX_YAW = 3;\r\nIDX_VEL = 4;\r\nIDX_CURVATURE = 5;\r\nIDX_TIME = 6;\r\n\r\nIDX_DELTA = 4;\r\n\r\n%% convert xy-yaw to error dynamics\r\n\r\n% calculate nearest point (use as initial state)\r\ndistance = vecnorm(ref(:, IDX_XY)' - state(IDX_XY)');\r\n[~, min_index] = min(distance);\r\nref_sp = ref(min_index, :);\r\n\r\n% convert x,y to lon,lat model\r\nsp_yaw = ref_sp(IDX_YAW);\r\nT_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);\r\n             -sin(sp_yaw), cos(sp_yaw)];\r\nerror_xy = (state(IDX_XY) - ref_sp(IDX_XY))';\r\nerror_lonlat = T_xy2lonlat * error_xy;\r\nerror_lat = error_lonlat(2);\r\n\r\n\r\n% calculate yaw error\r\nerror_yaw = state(IDX_YAW) - sp_yaw;\r\nwhile (-2*pi <= error_yaw && error_yaw <= 2*pi) == 0 \r\n    if (error_yaw >= 2*pi)\r\n        error_yaw = error_yaw - 2*pi;\r\n    elseif (error_yaw <= -2*pi)\r\n        error_yaw = error_yaw + 2*pi;\r\n    end\r\nend\r\nif (error_yaw > pi)\r\n    error_yaw = error_yaw - 2*pi;\r\nelseif (error_yaw < -pi)\r\n    error_yaw = error_yaw + 2*pi;\r\nend\r\n\r\n% initial state for error dynamics\r\nx0 = [error_lat; error_yaw; state(IDX_DELTA)];\r\n\r\n%% update error dynamics for holizon\r\n\r\n% -- set mpc parameters --\r\nmpc_dt = param.mpc_dt;\r\nmpc_n = param.mpc_n;\r\nQ = param.mpc_Q;\r\nR = param.mpc_R;\r\nmpc_t = ref_sp(IDX_TIME);\r\nDIM_X = 3;\r\nDIM_Y = 2;\r\nDIM_U = 1;\r\nAex = zeros(DIM_X*mpc_n, DIM_X);\r\nBex = zeros(DIM_X*mpc_n, DIM_U*mpc_n);\r\nWex = zeros(DIM_X*mpc_n, 1);\r\nCex = zeros(DIM_Y*mpc_n, DIM_X*mpc_n);\r\nQex = zeros(DIM_Y*mpc_n, DIM_Y*mpc_n);\r\nRex = zeros(DIM_U*mpc_n, DIM_U*mpc_n);\r\nmpc_ref_v = zeros(mpc_n + delay_step, 1);\r\ndebug_ref_mat = zeros(mpc_n + delay_step,5);\r\n\r\n% =================== for delay compensation ==============================\r\n% -- apply delay compensation : update dynamics with increasing mpt_t --\r\nx_curr = x0;\r\nfor i = 1:delay_step\r\n    if mpc_t > ref(end, IDX_TIME)\r\n        mpc_t = ref(end, IDX_TIME);\r\n        disp('[MPC] path is too short to predict dynamics');\r\n    end\r\n    ref_now = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);\r\n    debug_ref_mat(i,:) = ref_now;\r\n    v_ = ref_now(IDX_VEL);\r\n    k_ = ref_now(IDX_CURVATURE);\r\n    \r\n    % get discrete state matrix\r\n    % NOTE : use control_dt as delta time, not mpc_dt. \r\n    [Ad, Bd, wd, ~] = get_error_dynamics_state_matrix(param.control_dt, v_, param.wheelbase, param.tau, k_);\r\n    u_now = deltades_buffer(end - i + 1);\r\n    x_next = Ad * x_curr + Bd * u_now + wd;\r\n    \r\n    mpc_t = mpc_t + param.control_dt; % THIS IS NOT mpc_dt, BUT control_dt\r\n    x_curr = x_next;\r\n    \r\n    mpc_ref_v(i) = v_;\r\nend\r\nx0 = x_curr;\r\n% =========================================================================\r\n\r\n% -- mpc matrix for i = 1 --\r\nref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);\r\ndebug_ref_mat(1 + delay_step,:) = ref_i_; % MODIFIED FOR DELAY\r\nv_ = ref_i_(IDX_VEL);\r\nk_ = ref_i_(IDX_CURVATURE);\r\n[Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(mpc_dt, v_, param.wheelbase, param.tau, k_); \r\nAex(1:DIM_X, :) = Ad;\r\nBex(1:DIM_X, 1:DIM_U) = Bd;\r\nWex(1:DIM_X) = wd;\r\nCex(1:DIM_Y, 1:DIM_X) = Cd;\r\nQex(1:DIM_Y, 1:DIM_Y) = Q;\r\nRex(1:DIM_U, 1:DIM_U) = R;\r\n\r\nmpc_ref_v(1 + delay_step) = v_;\r\n\r\n% -- mpc matrix for i = 2:n --\r\nfor i = 2:mpc_n\r\n    \r\n    % update mpc time\r\n    mpc_t = mpc_t + mpc_dt;\r\n    if mpc_t > ref(end, IDX_TIME)\r\n        mpc_t = ref(end, IDX_TIME);\r\n        disp('[MPC] path is too short to predict dynamics');\r\n    end\r\n\r\n    % get reference information\r\n    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);\r\n    debug_ref_mat(i + delay_step,:) = ref_i_;\r\n    v_ = ref_i_(IDX_VEL);\r\n    k_ = ref_i_(IDX_CURVATURE);\r\n    \r\n    % get discrete state matrix\r\n    [Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(mpc_dt, v_, param.wheelbase, param.tau, k_);\r\n    \r\n    % update mpc matrix\r\n    idx_x_i = (i-1)*DIM_X+1:i*DIM_X;\r\n    idx_x_i_prev = (i-2)*DIM_X+1:(i-1)*DIM_X;\r\n    idx_u_i = (i-1)*DIM_U+1:i*DIM_U;\r\n    idx_y_i = (i-1)*DIM_Y+1:i*DIM_Y;\r\n    Aex(idx_x_i, :) = Ad * Aex(idx_x_i_prev, :);\r\n    for j = 1:i-1\r\n        idx_u_j = (j-1)*DIM_U+1:j*DIM_U;\r\n        Bex(idx_x_i, idx_u_j) = Ad * Bex(idx_x_i_prev, idx_u_j);\r\n    end\r\n    Bex(idx_x_i, idx_u_i) = Bd;\r\n    Wex(idx_x_i) = Ad * Wex(idx_x_i_prev) + wd;\r\n    Cex(idx_y_i, idx_x_i) = Cd;\r\n    Qex(idx_y_i, idx_y_i) = Q;\r\n    Rex(idx_u_i, idx_u_i) = R;\r\n    \r\n    mpc_ref_v(i + delay_step) = v_;\r\n    \r\nend\r\n\r\n\r\n%% convex optimization\r\n\r\n% The problem is to solve following for U.\r\n%   1/2 * U'* mat1 * U + mat2 * U + C = 0\r\nmat1 = Bex' * Cex' * Qex * Cex * Bex + Rex;\r\nmat2 = (x0' * Aex' + Wex') * Cex' * Qex * Cex * Bex;\r\n\r\nsteering_rate_lim = param.mpc_constraint_steer_rate_deg * deg2rad;\r\n\r\nif param.mpc_solve_without_constraint == true\r\n    input_vec = -mat1 \\ mat2';\r\nelse\r\n    % --- convex optimization ---\r\n    %   minimize for x, s.t.\r\n    %   J(x) = 1/2 * x' * H * x + f' * x, \r\n    %   A*x <= b,   lb <= x <= ub\r\n\r\n    H_ = (mat1 + mat1') / 2;\r\n    f_ = mat2;\r\n    \r\n    % add steering rate constraint\r\n    tmp = -eye(mpc_n-1, mpc_n);\r\n    tmp(1:end,2:end) = tmp(1:end,2:end) + eye(mpc_n-1);\r\n    T_ = kron(tmp, [0,0,1]) / mpc_dt;\r\n    dsteer_vec_tmp_ = T_ * (Aex * x0 + Wex);\r\n    A_ = [T_ * Bex; -T_ * Bex];    \r\n    b_ = [steering_rate_lim * ones(mpc_n-1,1) - dsteer_vec_tmp_; steering_rate_lim * ones(mpc_n-1,1) + dsteer_vec_tmp_];\r\n\r\n    % constraint for upper and lower steering boundary\r\n    lb_ = -param.mpc_constraint_steering_deg * deg2rad * ones(mpc_n * DIM_U,1);\r\n    ub_ = param.mpc_constraint_steering_deg * deg2rad * ones(mpc_n * DIM_U,1);\r\n    options_ = optimoptions('quadprog', 'Algorithm','interior-point-convex', 'Display', 'off');\r\n    [x_, fval, exitflag, output, lambda] = quadprog(H_, f_, A_, b_, [], [], lb_, ub_, [], options_);\r\n    input_vec = x_;\r\n\r\n\r\n    % for debug: compare with / without constraint optimization\r\n    % input_vec_LS = -mat1 \\ mat2';\r\n    % figure(101);\r\n    % plot(input_vec_LS); hold on;\r\n    % plot(input_vec); grid on; hold off;\r\nend\r\n\r\ndelta_des = input_vec(1);\r\nv_des = ref_sp(IDX_VEL);\r\nu = [v_des, delta_des];\r\n\r\n% =================== for delay compensation ==============================\r\ndeltades_buffer = [delta_des; deltades_buffer(1:end-1)];\r\n% =========================================================================\r\n\r\n%% (debug) calculate predicted trajectory \r\n\r\nx_ = state;\r\npredictd_states = zeros(length(input_vec), length(state));\r\nfor i = 1:length(input_vec)\r\n    x_next = calc_kinematics_model(x_, input_vec(i), mpc_dt, mpc_ref_v(i), param.wheelbase, param.tau);\r\n    predictd_states(i,:) = x_next';\r\n    x_ = x_next;\r\nend\r\n\r\npredictd_states_vector = reshape(predictd_states, [], 1);\r\n\r\ndebug_ref_mat_no_delay_comp = debug_ref_mat(delay_step + 1:end, :);\r\n\r\npredicted_error = Aex*x0 + Bex*input_vec + Wex;\r\npredicted_error = transpose(reshape(predicted_error, 3, []));\r\npredicted_state_ideal = debug_ref_mat_no_delay_comp(:,IDX_XY) + ...\r\n    [-sin(debug_ref_mat_no_delay_comp(:,IDX_YAW)).*predicted_error(:,1), cos(debug_ref_mat_no_delay_comp(:,IDX_YAW)).*predicted_error(:,1)];\r\n\r\npredicted_state_ideal = (reshape(predicted_state_ideal, [], 1));\r\n\r\ndebug_info = [input_vec', predictd_states_vector', predicted_state_ideal', error_lat];\r\n\r\n\r\n% for debug \r\n% figure(1);plot(predicted_error(:,1),'b*-');\r\n% figure(3);\r\n% plot(input_vec); hold on;\r\n% plot(predicted_error(:,3));hold on;\r\n% plot(predictd_states(:,4)); hold off;\r\n\r\n\r\nend\r\n\r\n%% time varying error dynamics model\r\n% used for error dynamics update\r\nfunction [Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(dt, v, L, tau, curvature)\r\n    \r\n    % linearization around delta = 0\r\n    % A = [0, v, 0;\r\n    %     0, 0, v/L;\r\n    %     0, 0, -1/tau];\r\n    % B = [0; 0; 1/tau];\r\n    % C = [1, 0, 0;\r\n    %      0, 1, 0];\r\n    % w = [0; \r\n    %     -v*curvature;\r\n    %      0];\r\n\r\n    % linearization around delta = delta_ref (better accuracy than delta=0)\r\n    delta_r = atan(L*curvature);\r\n    if (abs(delta_r) >= 40 /180 * pi)\r\n        delta_r = (40 /180 * pi)*sign(delta_r);\r\n    end\r\n    cos_delta_r_squared_inv = 1 / ((cos(delta_r))^2);\r\n\r\n    % difinition for continuous model\r\n    A = [0, v, 0;\r\n         0, 0, v/L*cos_delta_r_squared_inv;\r\n         0, 0, -1/tau];\r\n    B = [0; 0; 1/tau];\r\n    C = [1, 0, 0;\r\n         0, 1, 0];\r\n    w = [0; \r\n        -v*curvature + v/L*(tan(delta_r)-delta_r*cos_delta_r_squared_inv);\r\n         0];\r\n    \r\n    % discretization\r\n    % Ad = eye(3) + A * dt;\r\n    I = eye(3);\r\n    Ad = (I - dt * 0.5 * A) \\ (I + dt * 0.5 * A);\r\n    Bd = B * dt;\r\n    Cd = C;\r\n    wd = w * dt;\r\nend\r\n\r\n%% xy-yaw dynamics model\r\n% used for debug (predicted trajectory calculation)\r\nfunction x_next = calc_kinematics_model(x, u, dt, v, L, tau)\r\n\r\n    % x = [x, y, yaw, delta]\r\n    x_next = zeros(4,1);\r\n    yaw = x(3);\r\n    delta = x(4);\r\n    \r\n    % x\r\n    x_next(1) = x(1) + v*cos(yaw)*dt;\r\n    \r\n    % y\r\n    x_next(2) = x(2) + v*sin(yaw)*dt;\r\n    \r\n    % yaw\r\n    x_next(3) = x(3) + v*tan(delta)/L*dt;\r\n    \r\n    % delta\r\n    x_next(4) = x(4) - (x(4) - u)/tau*dt;\r\n\r\nend"
  },
  {
    "path": "simulation/model_predictive_controller2.m",
    "content": "function [u, debug_info] = model_predictive_controller2(state, t, ref, param)\r\n% \r\n% state = [x, y, yaw, delta]\r\n% u = [v_des, delta_des]\r\n% ref = [x_ref; y_ref; yaw_ref; v_ref; k_ref; t_ref];\r\n% \r\n% \r\n% \r\ndeg2rad = pi / 180;\r\n\r\nIDX_X = 1;\r\nIDX_Y = 2;\r\nIDX_XY = 1:2;\r\nIDX_YAW = 3;\r\nIDX_VEL = 4;\r\nIDX_CURVATURE = 5;\r\nIDX_TIME = 6;\r\n\r\nIDX_DELTA = 4;\r\n\r\n%% calculate nearest point\r\n\r\n% calculate nearest point (use as initial state)\r\ndistance = vecnorm(ref(:, IDX_XY)' - state(IDX_XY)');\r\n[~, min_index] = min(distance);\r\nref_sp = ref(min_index, :);\r\nv_ref = ref_sp(IDX_VEL);\r\n\r\n% (debug) calculate latitude error\r\nsp_yaw = ref_sp(IDX_YAW);\r\nT_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);\r\n             -sin(sp_yaw), cos(sp_yaw)];\r\nerror_xy = (state(IDX_XY) - ref_sp(IDX_XY))';\r\nerror_lonlat = T_xy2lonlat * error_xy;\r\nerror_lat = error_lonlat(2);\r\n\r\n\r\n%% update error dynamics for holizon\r\n\r\n% set mpc parameters\r\nmpc_dt = param.mpc2_dt;\r\nmpc_n = param.mpc2_n;\r\nQ = param.mpc2_Q;\r\nR = param.mpc2_R;\r\nt_ = ref_sp(IDX_TIME);\r\nDIM_STATE = 4;\r\nDIM_OUTPUT = 3;\r\nDIM_INPUT = 1;\r\nA_ext = zeros(DIM_STATE*mpc_n, DIM_STATE);\r\nB_ext = zeros(DIM_STATE*mpc_n, DIM_INPUT*mpc_n);\r\nW_ext = zeros(DIM_STATE*mpc_n, 1);\r\nC_ext = zeros(DIM_OUTPUT*mpc_n, DIM_STATE*mpc_n);\r\nQ_ext = zeros(DIM_OUTPUT*mpc_n, DIM_OUTPUT*mpc_n);\r\nR_ext = zeros(DIM_INPUT*mpc_n, DIM_INPUT*mpc_n);\r\nmpc_ref_v = zeros(length(mpc_n), 1);\r\nref_vec = zeros(mpc_n,5);\r\n\r\n\r\nfor i = 1\r\n    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), t_);\r\n    ref_vec(i,:) = ref_i_;\r\n    v_ = ref_i_(IDX_VEL);\r\n    [Ad, Bd, wd, Cd] = get_linearized_state_matrix(mpc_dt, ref_i_, param.wheelbase, param.tau);\r\n    \r\n    cols_i = (i-1)*DIM_STATE+1:i*DIM_STATE;\r\n    A_ext(cols_i, :) = Ad;\r\n    \r\n    rows_i = (i-1)*DIM_INPUT+1:i*DIM_INPUT;\r\n    B_ext(cols_i, rows_i) = Bd;\r\n    \r\n    W_ext(cols_i) = wd;\r\n    \r\n    cols_i_C = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    rows_i_C = (i-1)*DIM_STATE+1:i*DIM_STATE;\r\n    C_ext(cols_i_C, rows_i_C) = Cd;\r\n    \r\n    cols_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    rows_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    Q_ext(cols_i_Q, rows_i_Q) = Q;\r\n    \r\n    R_ext(rows_i, rows_i) = R;\r\n    \r\n    mpc_ref_v(i) = v_;\r\n    \r\n    t_ = t_ + mpc_dt;\r\n    if t_ > ref(end, IDX_TIME)\r\n        t_ = ref(end, IDX_TIME);\r\n        disp('[MPC] path is too short to predict dynamics');\r\n    end\r\nend\r\n\r\nfor i = 2:mpc_n\r\n    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), t_);\r\n    ref_vec(i,:) = ref_i_;\r\n    v_ = ref_i_(IDX_VEL);\r\n    [Ad, Bd, wd, Cd] = get_linearized_state_matrix(mpc_dt, ref_i_, param.wheelbase, param.tau);\r\n    \r\n    \r\n    cols_i = (i-1)*DIM_STATE+1:i*DIM_STATE;\r\n    cols_i_prev = (i-2)*DIM_STATE+1:(i-1)*DIM_STATE;\r\n    A_ext(cols_i, :) = Ad * A_ext(cols_i_prev, :);\r\n    \r\n    for j = 1:i-1\r\n        rows_j = (j-1)*DIM_INPUT+1:j*DIM_INPUT;\r\n        B_ext(cols_i, rows_j) = Ad * B_ext(cols_i_prev, rows_j);\r\n    end\r\n    rows_i = (i-1)*DIM_INPUT+1:i*DIM_INPUT;\r\n    B_ext(cols_i, rows_i) = Bd;\r\n    \r\n    W_ext(cols_i) = Ad * W_ext(cols_i_prev) + wd;\r\n    \r\n    cols_i_C = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    rows_i_C = (i-1)*DIM_STATE+1:i*DIM_STATE;\r\n    C_ext(cols_i_C, rows_i_C) = Cd;\r\n    \r\n    cols_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    rows_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;\r\n    Q_ext(cols_i_Q, rows_i_Q) = Q;\r\n    \r\n    R_ext(rows_i, rows_i) = R;\r\n    \r\n    mpc_ref_v(i) = v_;\r\n    \r\n    t_ = t_ + mpc_dt;\r\n    if t_ > ref(end, IDX_TIME)\r\n        t_ = ref(end, IDX_TIME);\r\n        disp('[MPC] path is too short to predict dynamics');\r\n    end\r\nend\r\n\r\n\r\n%% convex optimization\r\n\r\nx0 = state';\r\n\r\n% for yaw singularity\r\n% for i=:length(ref_vec(:,1))\r\n%     if ()\r\n    \r\n\r\n\r\nY_ref = reshape(transpose(ref_vec(:,1:3)), [], 1);\r\n\r\n% The problem is to solve following for U.\r\n%   1/2 * U'* mat1 * U + mat2 * U + C = 0\r\nmat1 = B_ext' * C_ext' * Q_ext * C_ext * B_ext + R_ext;\r\nmat2 = (C_ext * A_ext * x0 + C_ext * W_ext - Y_ref)' * Q_ext * C_ext * B_ext;\r\n\r\n\r\n% --- convex optimization ---\r\n%   minimize for x, s.t.\r\n%   J(x) = 1/2 * x' * H * x + f' * x, \r\n%   A*x <= b,   lb <= x <= ub\r\n\r\nH_ = (mat1 + mat1') / 2;\r\nf_ = mat2;\r\nA_ = [];\r\nb_ = [];\r\n\r\n% constraint for upper and lower steering boundary\r\nlb_ = -param.mpc2_steering_lim_deg * deg2rad * ones(mpc_n * DIM_INPUT,1);\r\nub_ = param.mpc2_steering_lim_deg * deg2rad * ones(mpc_n * DIM_INPUT,1);\r\noptions_ = optimoptions('quadprog', 'Algorithm','interior-point-convex', 'Display', 'off');\r\n[x_, fval, exitflag, output, lambda] = quadprog(H_, f_, A_, b_, [], [], lb_, ub_, [], options_);\r\ninput_vec = x_;\r\n\r\n\r\n% for debug: compare with / without constraint optimization\r\n% input_vec_LS = -mat1 \\ mat2';\r\n% figure(101);\r\n% plot(input_vec_LS,'bo-'); hold on;\r\n% plot(input_vec,'r*-'); grid on; hold off;\r\n\r\n\r\nv_des = v_ref;\r\ndelta_des = input_vec(1);\r\nu = [v_des, delta_des];\r\n\r\n%% (debug) calculate predicted trajectory \r\n\r\nx_ = state;\r\npredictd_states = zeros(length(input_vec), length(state));\r\nfor i = 1:length(input_vec)\r\n    x_next = calc_kinematics_model(x_, input_vec(i), mpc_dt, mpc_ref_v(i), param.wheelbase, param.tau);\r\n    predictd_states(i,:) = x_next';\r\n    x_ = x_next;\r\nend\r\n\r\npredictd_states_real = reshape(predictd_states', [], 1);\r\n\r\npredicted_states_linearized = A_ext*x0 + B_ext*input_vec + W_ext;\r\n\r\ndebug_info = [input_vec', predictd_states_real', predicted_states_linearized', error_lat];\r\n\r\n\r\n% for debug \r\n% predictd_states_real_mat = reshape(predictd_states_real, [], 4);\r\n% predicted_states_linearized_mat = transpose(reshape(predicted_states_linearized, 4, []));\r\n% reshape(predictd_states_real, [], 4);\r\n% figure(1);\r\n% plot(predictd_states_real_mat(:,1), predictd_states_real_mat(:,2),'b*-'); hold on;\r\n% plot(predicted_states_linearized_mat(:,1), predicted_states_linearized_mat(:,2), 'ro-'); hold off;\r\n% xlabel('x [m]'); ylabel('y [m]');\r\n% figure(3);\r\n% plot(input_vec); hold on;\r\n% plot(predicted_error(:,3));hold on;\r\n% plot(predictd_states(:,4)); hold off;\r\n\r\n\r\nend\r\n\r\n%% time varying error dynamics model\r\n% used for error dynamics update\r\nfunction [Ad, Bd, wd, Cd] = get_linearized_state_matrix(dt, ref, L, tau)\r\n    %\r\n    % x = [x, y, yaw, delta]';\r\n    % u = [delta_com];\r\n    %\r\n    IDX_VEL = 4;\r\n    IDX_YAW = 3;\r\n    IDX_CURVATURE = 5;\r\n    \r\n    v_r = ref(IDX_VEL);\r\n    yaw_r = ref(IDX_YAW);\r\n    delta_r = atan(L*ref(IDX_CURVATURE));\r\n    cos_delta_r_squared_inv = 1 / ((cos(delta_r))^2);\r\n\r\n    % difinition for continuous model\r\n    A = [0, 0, -v_r * sin(yaw_r), 0;\r\n         0, 0, v_r*cos(yaw_r),    0;\r\n         0, 0, 0,                v_r/L*cos_delta_r_squared_inv;\r\n         0, 0, 0,                -1/tau];\r\n    B = [0; 0; 0; 1/tau];\r\n    C = [1, 0, 0, 0;\r\n         0, 1, 0, 0;\r\n         0, 0, 1, 0];\r\n    w = [v_r*cos(yaw_r) + v_r*sin(yaw_r)*yaw_r;\r\n         v_r*sin(yaw_r) - v_r*cos(yaw_r)*yaw_r;\r\n         v_r/L*(tan(delta_r)-delta_r*cos_delta_r_squared_inv);\r\n         0];\r\n         \r\n    \r\n    % discretization\r\n    Ad = eye(4) + A * dt;\r\n    Bd = B * dt;\r\n    Cd = C;\r\n    wd = w * dt;\r\nend\r\n\r\n%% xy-yaw dynamics model\r\n% used for debug (predicted trajectory calculation)\r\nfunction x_next = calc_kinematics_model(x, u, dt, v, L, tau)\r\n\r\n    % x = [x, y, yaw, delta]\r\n    x_next = zeros(4,1);\r\n    yaw = x(3);\r\n    delta = x(4);\r\n    \r\n    % x\r\n    x_next(1) = x(1) + v*cos(yaw)*dt;\r\n    \r\n    % y\r\n    x_next(2) = x(2) + v*sin(yaw)*dt;\r\n    \r\n    % yaw\r\n    x_next(3) = x(3) + v*tan(delta)/L*dt;\r\n    \r\n    % delta\r\n    x_next(4) = x(4) - (x(4) - u)/tau*dt;\r\n\r\nend"
  },
  {
    "path": "simulation/pid_controller.m",
    "content": "function [u, debug_info] = pid_controller(state, t, ref, param)\r\n% \r\n% state = [x, y, yaw, delta]\r\n% u = [v_des, delta_des]\r\n% ref = [x_ref; y_ref; yaw_ref; v_ref];\r\n% \r\n% \r\n% \r\n\r\nIDX_X = 1;\r\nIDX_Y = 2;\r\nIDX_XY = 1:2;\r\nIDX_YAW = 3;\r\nIDX_VEL = 4;\r\nIDX_CURVATURE = 5;\r\n% IDX_TIME = 6;\r\n\r\n\r\n% LON = 1;\r\nLAT = 2;\r\n\r\nkp = 0.3 * 1;\r\nkd = 0.5 * 3;\r\n\r\nyaw = state(IDX_YAW);\r\n\r\n\r\ndistance = vecnorm(ref(:,1:2)' - state(1:2)');\r\n[~, min_index] = min(distance);\r\n\r\npr = ref(min_index, :);\r\n\r\nv_des = pr(IDX_VEL);\r\n\r\n\r\n% feedforward input calculation\r\nff_curvature = atan(param.wheelbase * pr(IDX_CURVATURE));\r\n\r\n% coordinate transformation to body frame\r\nT = [cos(yaw), sin(yaw);\r\n    -sin(yaw), cos(yaw)];\r\nerror_xy = (state(IDX_XY) - pr(IDX_XY))';\r\nerror_latlon = T * error_xy;\r\nerror_yaw = yaw - pr(IDX_YAW);\r\n\r\n% should use mod?\r\nwhile (-2*pi <= error_yaw && error_yaw <= 2*pi) == 0 \r\n    if (error_yaw >= 2*pi)\r\n        error_yaw = error_yaw - 2*pi;\r\n    elseif (error_yaw <= -2*pi)\r\n        error_yaw = error_yaw + 2*pi;\r\n    end\r\nend\r\nif (error_yaw > pi)\r\n    error_yaw = error_yaw - 2*pi;\r\nelseif (error_yaw < -pi)\r\n    error_yaw = error_yaw + 2*pi;\r\nend\r\n\r\ndelta_des = -kp * (error_latlon(LAT)) - kd * error_yaw + ff_curvature;\r\nfb_lat = -kp * (error_latlon(LAT));\r\nfb_yaw = - kd * error_yaw;\r\n\r\nu = [v_des, delta_des];\r\n\r\ndebug_info = [pr(IDX_X), pr(IDX_Y), pr(IDX_YAW), fb_lat, fb_yaw, ff_curvature, error_latlon(LAT)];"
  },
  {
    "path": "simulation/pure_pursuit.m",
    "content": "function [u, debug_info] = pure_pursuit(state, t, ref, param)\r\n% \r\n% state = [x, y, yaw, delta]\r\n% u = [v_des, delta_des]\r\n% ref = [x_ref; y_ref; yaw_ref; v_ref];\r\n% \r\n% \r\n% \r\n\r\nIDX_X = 1;\r\nIDX_Y = 2;\r\nIDX_XY = 1:2;\r\nIDX_YAW = 3;\r\nIDX_VEL = 4;\r\n% IDX_CURVATURE = 5;\r\n% IDX_TIME = 6;\r\n\r\n\r\nlookahead_dist = param.pure_pursuit_lookahead;\r\n\r\ndistance = vecnorm(ref(:,IDX_XY)' - state(IDX_XY)');\r\n[~, min_index] = min(distance);\r\npr = ref(min_index, :);\r\n\r\nv_des = pr(IDX_VEL);\r\n\r\nfor i = min_index:length(ref)\r\n    dist = norm(ref(i, IDX_XY) - state(:,IDX_XY));\r\n    if dist > lookahead_dist\r\n        break;\r\n    end\r\nend\r\nlookahead_pt = ref(i, :);\r\n\r\nalpha = atan2(lookahead_pt(IDX_Y) - state(IDX_Y), lookahead_pt(IDX_X) - state(IDX_X)) - state(IDX_YAW);\r\n\r\nomega_des = 2 * v_des * sin(alpha) / param.wheelbase;\r\ndelta_des = atan2(omega_des * param.wheelbase, v_des);\r\n\r\n\r\n\r\nu = [v_des, delta_des];\r\n\r\n\r\n% lattitude error calc for debug\r\nsp_yaw = pr(IDX_YAW);\r\nT_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);\r\n             -sin(sp_yaw), cos(sp_yaw)];\r\nerror_xy = (state(IDX_XY) - pr(IDX_XY))';\r\nerror_lonlat = T_xy2lonlat * error_xy;\r\nerror_lat = error_lonlat(2);\r\n\r\ndebug_info = [lookahead_pt(IDX_X), lookahead_pt(IDX_Y), lookahead_pt(IDX_YAW), error_lat];"
  },
  {
    "path": "simulation/simulate_rk4.m",
    "content": "function [state_log, input_log, debug_info] = simulate_rk4(model, controller, x0, ref, ts, dt, tf, param)\r\n\r\nx = x0;\r\ni = 1;\r\n\r\nt_vec = ts:dt:tf;\r\n\r\nstate_log = zeros(length(t_vec), length(x0));\r\n[tmp_u, tmp_u_debug] = controller(x0, ts, ref, param);\r\ninput_log = zeros(length(t_vec), length(tmp_u));\r\ndebug_info = zeros(length(t_vec), length(tmp_u_debug));\r\n\r\ninput_delay = param.input_delay;\r\ndelay_count = round(input_delay / dt);\r\ninput_buf = zeros(delay_count, length(tmp_u));\r\nu = zeros(size(tmp_u)); % initial input\r\nu_debug = zeros(size(tmp_u_debug));\r\n\r\ncontrol_dt = param.control_dt;\r\ncontrol_count = round(control_dt / dt);\r\n\r\nfor t = ts:dt:tf\r\n    % -- control once per control_count time --\r\n    if mod(i, control_count) == 0\r\n        % add noise\r\n        x_noised = x + rand(1, length(x0)) .* param.measurement_noise_stddev;\r\n        [u, u_debug] = controller(x_noised, t, ref, param);\r\n    end\r\n    \r\n    % -- add input delay --\r\n    input_buf = [u; input_buf(1:end-1, :)];\r\n    u_delayed = input_buf(end,:);\r\n    \r\n    % -- runge-kutta --\r\n    k1 = model(x, u_delayed, param);\r\n    k2 = model(x + k1*dt/2, u_delayed, param);\r\n    k3 = model(x + k2*dt/2, u_delayed, param);\r\n    k4 = model(x + dt*k3, u_delayed, param);\r\n    x = x + (k1 + 2*k2 + 2*k3 + k4) * dt / 6;\r\n    \r\n    % -- save data --\r\n    state_log(i,:) = x;\r\n    input_log(i,:) = u;\r\n    debug_info(i,:) = u_debug;\r\n    \r\n    i = i + 1;\r\n    disp(t);\r\nend\r\n    "
  }
]