Repository: TakaHoribe/trajectory_tracking_simulation
Branch: master
Commit: ba86f63e0644
Files: 19
Total size: 39.6 KB

Directory structure:
gitextract_93d0kyut/

├── .gitignore
├── README.md
├── model_fitting/
│   ├── figure/
│   │   ├── steering_model_estimate_1d.fig
│   │   ├── steering_model_estimate_1d_enlarged.fig
│   │   └── yawrate_all.fig
│   ├── func/
│   │   └── quat2elu.m
│   ├── main.m
│   └── matlab.mat
├── path.mat
├── path_design/
│   ├── path.mat
│   └── path_design.m
└── simulation/
    ├── kinematics_model.m
    ├── main.m
    ├── model_predictive_controller.m
    ├── model_predictive_controller2.m
    ├── path.mat
    ├── pid_controller.m
    ├── pure_pursuit.m
    └── simulate_rk4.m

================================================
FILE CONTENTS
================================================

================================================
FILE: .gitignore
================================================
# all movies
*.avi

# rosbag data
/model_fitting/data

================================================
FILE: README.md
================================================
# trajectory_tracking_simulation
For developing trajectory tracking algorithm with MATLAB. 

Run /simulation/main.m to compute trajectory tracking simulation.

Reference path is designed in /path_design/path_design.m using spline from used-defined reference points.

・MPC

![mpc](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/mpc.gif)

・PID

![pid](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/pid.gif)

・pure pursuit

![purepursuit](https://raw.github.com/wiki/TakaHoribe/trajectory_tracking_simulation/images/purepursuit.gif)

## Currently available controller
* pure-pursuit
* PID
* MPC (use Optimization Toolbox)
* MPC without constraint

## Parameters
parameters are all set in /simulation/main.m

### simulation
* simulation_time : overall simulation time
* simulation_rk4_time_step : time span for integrate dynamics

### path
* vel_ref : reference speed [m/s]

### simulated vehicle model
* param.tau : steering dynamics 1d-approximated time constant [s]
* param.wheelbase : wheelbase length [m]
* param.steer_lim : tire angle limit [rad]
* param.vel_max : max velocity limit [m/s]
* param.vel_min : min velocity limit [m/s]
* param.input_delay : input delay [s]
* param.measurement_noise_stddev :  measurement noise standard deviation for [pos x[m], pos y[m], heading[rad], steering[rad]]
* param.steering_steady_state_error_deg : steady state error for steering model

### controllers
for all
* param.control_dt : control time span (zero order hold) [s]

for pure-pursuit
* param.pure_pursuit_lookahead : lookahead distance for pure-pursuit controller [m]

for mpc
* param.mpc_dt : time span for prediction
* param.mpc_n : step numbers for prediction
* param.mpc_constraint_steering_deg : steering limit constraint for optimization problem
* param.mpc_constraint_steer_rate_deg : steering rate limit constraint for optimization problem
* param.mpc_model_dim : mpc state model dimension
* param.mpc_Q : state weight matrix for optimization
* param.mpc_R : input weight matrix for optimization
* param.mpc_sensor_delay : for sensor delay compensation


================================================
FILE: model_fitting/func/quat2elu.m
================================================
function rpy = quat2elu(qw, qx, qy, qz)


sinr_cosp = 2 * qw .* qx + qy .* qz;
cosr_cosp = 1 - 2 * qx .* qx + qy .* qy;
roll = atan2(sinr_cosp, cosr_cosp);

sinp = 2 * (qw .* qy - qz .* qx);
% remove out of range 
sinp(sinp > 1) = 1;
sinp(sinp < -1) = -1;

pitch = asin(sinp);

siny_cosp = 2 * (qw .* qz + qx .* qy);
cosy_cosp = 1 - 2 * (qy .* qy + qz .* qz);
yaw = atan2(siny_cosp, cosy_cosp);

rpy = [roll, pitch, yaw];

================================================
FILE: model_fitting/main.m
================================================
set(0, 'defaultAxesFontSize', 14);
set(0, 'defaultTextFontSize', 14);
set(0, 'DefaultAxesLineWidth', 1.2, 'DefaultLineLineWidth', 1.5);

rad2deg = 180 / pi;
deg2rad = pi / 180;

lexus_param.tread_rear = 1.63;
lexus_param.tread_front = 1.64;
lexus_param.wheelbase = 2.79;

addpath func


%% Ԃ낦
imuraw.t = imuraw.headerstampsecs + imuraw.headerstampnsecs * 1e-9;
wheelspeedrpt.t = wheelspeedrpt.headerstampsecs + wheelspeedrpt.headerstampnsecs * 1e-9;
yawraterpt.t = yawraterpt.headerstampsecs + yawraterpt.headerstampnsecs * 1e-9;
steerrpt.t = steerrpt.headerstampsecs + steerrpt.headerstampnsecs * 1e-9;
ndtpose.t = ndtpose.headerstampsecs + ndtpose.headerstampnsecs * 1e-9;
shiftrpt.t = shiftrpt.headerstampsecs + shiftrpt.headerstampnsecs * 1e-9;

tmp = max([imuraw.t(1), wheelspeedrpt.t(1), yawraterpt.t(1), steerrpt.t(1), ndtpose.t(1), shiftrpt.t(1)]);
imuraw.t = imuraw.t - tmp;
wheelspeedrpt.t = wheelspeedrpt.t - tmp;
yawraterpt.t = yawraterpt.t - tmp;
steerrpt.t = steerrpt.t - tmp;
ndtpose.t = ndtpose.t - tmp;
shiftrpt.t = shiftrpt.t - tmp;
ts = max([imuraw.t(1), wheelspeedrpt.t(1), yawraterpt.t(1), steerrpt.t(1), ndtpose.t(1), shiftrpt.t(1)]);
tf = min([imuraw.t(end), wheelspeedrpt.t(end), yawraterpt.t(end), steerrpt.t(end), ndtpose.t(end), shiftrpt.t(end)]);


%% ԑf[^yawrate߂

wheelspeed_all = [wheelspeedrpt.front_left_wheel_speed, wheelspeedrpt.front_right_wheel_speed, ...
            wheelspeedrpt.rear_left_wheel_speed, wheelspeedrpt.rear_right_wheel_speed];
        
figure;
plot(wheelspeedrpt.t, wheelspeed_all);grid on
xlabel('t [s]'); ylabel('vel [m/s]'); legend('front-left','front-right','rear-left','rear-right');



wheelspeedrpt.velsign = round(interp1q(shiftrpt.t, shiftrpt.output, wheelspeedrpt.t));
wheelspeedrpt.velsign(isnan(wheelspeedrpt.velsign) == 1) = 0;
wheelspeedrpt.velsign(wheelspeedrpt.velsign == 1) = -1;
wheelspeedrpt.velsign(wheelspeedrpt.velsign == 3) = 1;

wheelspeedrpt.yaw_from_rear_wheelspeeds = (wheelspeedrpt.rear_left_wheel_speed - wheelspeedrpt.rear_right_wheel_speed) /4 / lexus_param.tread_rear .* wheelspeedrpt.velsign;
wheelspeedrpt.yaw_from_front_wheelspeeds = (wheelspeedrpt.front_left_wheel_speed - wheelspeedrpt.front_right_wheel_speed) /4 / lexus_param.tread_front .* wheelspeedrpt.velsign;
wheelspeedrpt.vel_from_reat_wheelspeeds = (wheelspeedrpt.rear_left_wheel_speed + wheelspeedrpt.rear_right_wheel_speed) /2 .* wheelspeedrpt.velsign;


% figure;plot(imuraw.t, [imuraw.angular_velocityx, imuraw.angular_velocityy, imuraw.angular_velocityz]); grid on;

%% XeAOkinematicsfpĊpxvZ

% Ƃ肠vbg
figure;plot(steerrpt.t, [steerrpt.manual_input, steerrpt.command], steerrpt.t, steerrpt.output, '-.');grid on;
legend('manual', 'command','output');
xlabel('t [s]'), ylabel('steering [deg]');


% `⊮  `B֐F萔1/8.7=0.115[s]
dt = 0.03;
tp = (ts:dt:tf)';
steer_p.cmd = interp1q(steerrpt.t, steerrpt.command, tp);
steer_p.out = interp1q(steerrpt.t, steerrpt.output, tp);


% XeAOpx{֎ԑpxvZodompxAimupxƔr
wheelspeed_p.vel_from_rear = interp1q(wheelspeedrpt.t, wheelspeedrpt.vel_from_reat_wheelspeeds, tp);
omega_steer_p = wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad) / lexus_param.wheelbase;



%% NDT̔lpxvZ

figure;plot(ndtpose.posepositionx, ndtpose.posepositiony)

eul = quat2elu(ndtpose.poseorientationw, ndtpose.poseorientationx, ndtpose.poseorientationy, ndtpose.poseorientationz);
for i = 2:length(eul)
    if eul(i,3) - eul(i-1,3) > pi
        eul(i:end,3) = eul(i:end,3) - 2 * pi;
    elseif eul(i,3) - eul(i-1,3) < -pi
        eul(i:end,3) = eul(i:end,3) + 2 * pi;
    end
end

eul_p = interp1q(ndtpose.t, eul, tp);
eul_p_f = filtfilt(SOS, G, eul_p);
yawrate_from_ndt = (eul_p_f(2:end,3) - eul_p_f(1:end-1,3)) / dt;
yawrate_from_ndt = [yawrate_from_ndt; yawrate_from_ndt(end)];

%% ndt狁߂pxɑ΂wheelbasẽp[^tBbeBO

wheelbase_fitting = yawrate_from_ndt \ (wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad));
omega_steer_p_fitting = wheelspeed_p.vel_from_rear .* tan(steer_p.out * deg2rad) / wheelbase_fitting;


%% Spxf[^vbg

figure;
plot(imuraw.t, imuraw.angular_velocityz * (-rad2deg)); hold on;
plot(yawraterpt.t, yawraterpt.yaw_rate); hold on;
plot(wheelspeedrpt.t, [wheelspeedrpt.yaw_from_front_wheelspeeds, wheelspeedrpt.yaw_from_rear_wheelspeeds] * rad2deg); hold on;
plot(tp, yawrate_from_ndt * rad2deg); hold on;
plot(tp, omega_steer_p * rad2deg); hold on;
plot(tp, omega_steer_p_fitting * rad2deg); grid on;
legend('imu -z', 'yawraterpt','front wheelspeed diff', 'rear wheelspeed diff', 'ndt deriv + LPF(3d-butter, fc=1Hz)', ...
    'kinematics model (theorical: L=2.79)', 'kinematics model (fitting: L=2.69)');
xlabel('t [s]'); ylabel('yawrate estimated [deg/s]')


================================================
FILE: path_design/path_design.m
================================================
%% design path from points by spline

point = [0, 0;
    1, 0;
    2,0
    3,0.5
    4,1.5
    4.8, 1.5
    5,0.8
    6, 0.5
    6.5, 0
    7.5, 0.5
    7,2
    6, 3
    5, 4
    4., 2.5
    3, 3
    2., 3.5;
    1.3, 2.2
    0.5, 2.
    0,3];

s = 1:1:length(point);


px_spline = spline(s, point(:,1), 1:0.01:length(point));
py_spline = spline(s, point(:,2), 1:0.01:length(point));

p_spline = [px_spline', py_spline'];



%% insert yaw

yaw = zeros(length(px_spline), 1);
for i = 2:length(px_spline)-1
    x_forward = px_spline(i+1);
    x_backward = px_spline(i-1);
    y_forward = py_spline(i+1);
    y_backward = py_spline(i-1);
    yaw(i) = atan2(y_forward-y_backward, x_forward-x_backward);
end
yaw(1) = yaw(2);
yaw(end) = yaw(end-1);


%% plot with attitude

arrow_scale = 0.01;

figure(101);
plot(point(:,1), point(:,2), 'bo-'); hold on;
quiver(px_spline', py_spline', cos(yaw)*arrow_scale, sin(yaw)*arrow_scale);
plot(px_spline, py_spline,'r-'); grid on; hold off;

%% save path
path = [px_spline', py_spline', yaw];

save('path', 'path')

================================================
FILE: simulation/kinematics_model.m
================================================
function d_state = kinematics_model(state, input, param)

v_des = input(1);
delta_des = input(2);

% limit
delta_des = max(min(delta_des, param.steer_lim), -param.steer_lim);
v_des = max(min(v_des, param.vel_max), param.vel_min);

% x = state(1);
% y = state(2);
yaw = state(3);
delta = state(4);

v = v_des;

d_x = v * cos(yaw);
d_y = v * sin(yaw);
d_yaw = v * tan(delta) / param.wheelbase;
d_delta = - (delta - delta_des) / param.tau;

% add steady state error caused by friction
if abs(delta - delta_des) < param.steering_steady_state_error_deg / 180 * pi
    d_delta = 0;
end


d_state = [d_x, d_y, d_yaw, d_delta];





================================================
FILE: simulation/main.m
================================================
% 
% state = [x, y, yaw, delta]
% input = [v_des, delta_des]
% ref = [x_ref, y_ref, yaw_ref, v_ref]
% 
% 
% 

clear variables;
close all;


set(0, 'defaultAxesFontSize', 12);
set(0, 'defaultTextFontSize', 20);
set(0, 'DefaultAxesLineWidth', 1.0, 'DefaultLineLineWidth', 1.0);


addpath ../path_design

control_mode_option = ["pure_pursuit", "pid", "mpc", "mpc_no_constraints"];
control_mode = control_mode_option(3);

save_video = 0; %1:save, 0:no

%% preliminaries
rad2deg = 180 / pi;
deg2rad = pi / 180;
kmh2ms = 1000 / 3600;

simulation_time = 35;
simulation_rk4_time_step = 0.002; % simulation time step

vel_ref = 30 * kmh2ms;

% for dynamics model
param.tau = 0.27; % steering dynamics: 1d-approximated time constant
param.wheelbase = 2.69;
param.steer_lim = 30 * deg2rad;
param.vel_max = 10;
param.vel_min = -5;

param.input_delay = 0.24; % [s]
param.control_dt = 0.03; % [s]
param.measurement_noise_stddev = [0.1, 0.1, 1.0*deg2rad, 0.5*deg2rad]; % measurement noise
% param.measurement_noise_stddev = [0,0,0,0]; % measurement noise
param.steering_steady_state_error_deg = 1;

% for pure pursuit only
param.pure_pursuit_lookahead = 8.0; % [m]

% for mpc only
param.mpc_dt = 0.1;
param.mpc_n = 30;
param.mpc_constraint_steering_deg = 30;
param.mpc_constraint_steer_rate_deg = 280;
param.mpc_model_dim = 3;
param.mpc_Q = diag([1,2]);
param.mpc_R = 0.5;
param.mpc_delay_comp_step = round(param.input_delay / param.control_dt);
% param.mpc_delay_comp_step = 0.0;

% use the input ahead of the delay time
param.mpc_sensor_delay = param.input_delay; 

% for mpc2
param.mpc2_dt = 0.2;
param.mpc2_n = 10;
param.mpc2_steering_lim_deg = 40;
param.mpc2_model_dim = 4;
param.mpc2_Q = diag([1,1,0]);
param.mpc2_R = 0.05;

%% simulation parameters

% initial position (x, y, yaw, delta)
x0 = [0, 0.5, 0, 0];

ts = 0;
dt = simulation_rk4_time_step;
tf = simulation_time;
t = ts:dt:tf;

%% reference trajectory design

path_design; % using spline
load path; % x, y, yaw

ref = zeros(length(path), 6);
IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_XYYAW = 1:3;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
IDX_TIME = 6;

IDX_STEER = 4;


path_size_scale = 15;
path(:,IDX_XY) = path(:,IDX_XY) * path_size_scale;
ref(:,IDX_XYYAW) = path(:,IDX_XYYAW);

ref(:,IDX_VEL) = ones(length(path),1)*vel_ref;

% insert curvature into path
for i = 2:length(ref)-1
    p1_ = ref(i-1,IDX_XY);
    p2_ = ref(i, IDX_XY);
    p3_ = ref(i+1, IDX_XY);
    A_ = ((p2_(1)-p1_(1))*(p3_(2)-p1_(2)) - (p2_(2)-p1_(2))*(p3_(1)-p1_(1))) / 2;
    ref(i, IDX_CURVATURE) = 4 * A_ / (norm(p1_-p2_) * norm(p2_-p3_) * norm(p3_-p1_));
end

% insert relative time into path
for i = 2:length(ref)
    v_ = ref(i,IDX_VEL);
    d_ = norm(ref(i,IDX_XY)-ref(i-1,IDX_XY));
    dt_ = d_ / v_;
    ref(i, IDX_TIME) = ref(i-1, IDX_TIME) + dt_;
end


%% simulation

if control_mode == "pure_pursuit"
    [X, U, debug] = simulate_rk4(@kinematics_model, @pure_pursuit, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "pid"
   [X, U, debug] = simulate_rk4(@kinematics_model, @pid_controller, x0, ref, ts, dt, tf, param);
   lat_error_vec = debug(:,end);
elseif control_mode == "mpc"
    param.mpc_solve_without_constraint = false;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "mpc_no_constraints"
    param.mpc_solve_without_constraint = true;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);  
elseif control_mode == "mpc2"
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller2, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
end
fprintf("lattitude error: mean square = %f, max = %f", norm(lat_error_vec)/simulation_time, max(lat_error_vec));


%% movie plot

sp_num = 18;
subpl1 = 'subplot(sp_num,sp_num, sp_num+1:sp_num*12);';
subpl2 = 'subplot(sp_num,sp_num, sp_num*13+1:sp_num*15);';
subpl3 = 'subplot(sp_num,sp_num, sp_num*16+1:sp_num*18);';

% tire2steer = 12.5;

fig_trajectory_result = figure(1);
set(fig_trajectory_result, 'Position', [716 735 1026 1146]);
eval(subpl1);
plot(ref(:,1), ref(:,2),'k-.'); hold on; grid on;
% plot(X(:,1), X(:,2));
% legend('ref','tracked');
xlabel('x [m]'); ylabel('y [m]');
% img_orig = imread('handle.jpg');
% img = imrotate(img_orig, 10);
% handle_plt = image([150, 170],[110, 90], img);
% handle_plt_point = [160, 100, 0];


eval(subpl2);
plot(t, lat_error_vec, 'b'); grid on; hold on; 
xlabel('t [s]'); ylabel('latitude error [m]');
ulim = ceil(2*max(lat_error_vec))/2;
dlim = floor(2*min(lat_error_vec))/2;
ylim([dlim, ulim]);

eval(subpl3);
p1 = plot(t, X(:,IDX_STEER)*rad2deg, 'b'); grid on; hold on; 
p2 = plot(t, U(:,2)*rad2deg, 'Color', [0.7 0. 1]); hold on; 
legend([p1,p2], {'measured','command'})
xlabel('t [s]'); ylabel('steering angle [deg]');
ulim = round(2*max(X(:,IDX_STEER)*rad2deg))/2;
dlim = round(2*min(X(:,IDX_STEER)*rad2deg))/2;
ylim([dlim, ulim]);

z_axis = [0 0 1];
setpoint = []; rear_tire = []; front_tire = []; body = []; tracked = []; 
setpoint_ideal = []; error_point = []; steer_point = []; time_bar_laterror = []; time_bar_steer = [];
L = param.wheelbase;
rear_length = 1;
front_length = 1;
side_width = 0.9;
fig_draw_i = 1:round(1/dt/20):length(t);

% for movie
clear frame_vec;
frame_vec(length(fig_draw_i)) = struct('cdata', [], 'colormap',[]);

j = 1;
fig_trajectory_result; hold on;
for i = fig_draw_i
    eval(subpl1);
    disp(t(i))
    rear_x = X(i,1);
    rear_y = X(i,2);
    yaw = X(i,3);
    delta = X(i,IDX_STEER);
    front_x = rear_x + L;
    front_y = rear_y;
    delete([setpoint, rear_tire, front_tire, body, tracked, setpoint_ideal, error_point, steer_point, time_bar_laterror, time_bar_steer]);
    tracked = plot(X(1:i,1), X(1:i,2),'r');
    title_draw = "t = "+num2str(t(i),'%5.1f') + "[s], steer = " + num2str(delta*rad2deg,'%+3.1f') + "[deg], v = " + ...
        num2str(vel_ref*3600/1000,'%3.1f') + "[km/h], lat error = "+num2str(lat_error_vec(i),'%+2.2f') + "[m]";
    title_draw = [title_draw; "Simulation: solver = rk4, sensor-delay = "  + num2str(param.input_delay*1000, '%d') + "[ms], control freq=" + ...
        num2str(1/param.control_dt, '%d') + "[hz]"];
    title_draw = [title_draw; "noise-sigma = " + num2str(param.measurement_noise_stddev(1),'%2.2f')+"(pos), "+ ...
        num2str(param.measurement_noise_stddev(3),'%2.2f')+"(yaw), "+num2str(param.measurement_noise_stddev(4),'%2.2f')+"(steer)"];
    if control_mode == "mpc" || control_mode == "mpc_no_constraints"
        title_draw = [title_draw; "MPC: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
%         pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
%         pred_states = reshape(pred_states, param.mpc_n, length(pred_states)/param.mpc_n);
%         setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(2+4+1));
        pred_error = reshape(pred_error, param.mpc_n, length(pred_error)/param.mpc_n);
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "mpc2"
        title_draw = [title_draw; "MPC2: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
        pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
        pred_states = transpose(reshape(pred_states, length(pred_states)/param.mpc_n, param.mpc_n));
        setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(4+4+1));
        pred_error = transpose(reshape(pred_error, length(pred_error)/param.mpc_n, param.mpc_n));
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "pure_pursuit"
        title_draw = [title_draw; "pure-pursuit: lookahead dist="+num2str(param.pure_pursuit_lookahead, '%1.1f')+"[m]"];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    elseif control_mode == "pid"
        title_draw = [title_draw; "PID: kp = " + num2str(0.3, '%3.3f') + ", ki = " + num2str(0, '%3.3f') + ", kd = " + num2str(1.5, '%3.3f')];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    end
    rear_tire = plot([rear_x-0.3, rear_x+0.3],[rear_y, rear_y], 'k', 'LineWidth', 2.0);
    front_tire = plot([front_x-0.3, front_x+0.3],[front_y, front_y], 'k', 'LineWidth', 2.0);
    body = plot([rear_x-rear_length, front_x+front_length, front_x+front_length, rear_x-rear_length, rear_x-rear_length], ...
        [rear_y-side_width, front_y-side_width, front_y+side_width, rear_y+side_width, rear_y-side_width],'k');
    rear_origin = [rear_x, rear_y, 0];
    front_origin = [rear_x + L*cos(yaw), rear_y + L*sin(yaw), 0];
    rotate(body, z_axis, yaw * rad2deg, rear_origin);
    rotate(rear_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, delta * rad2deg, front_origin);
%     rotate(handle_plt, [0,0,1], delta*tire2steer*rad2deg, handle_plt_point)
    title(title_draw);
    xlim([0 120]);
     
    % lat error
    eval(subpl2);
    error_point = plot(t(i), lat_error_vec(i), 'ko');
    time_bar_laterror = plot([t(i), t(i)], [100, -100], 'k');
    
    % steering
    eval(subpl3);
    steer_point = plot(t(i), X(i, IDX_STEER)*rad2deg, 'ko');
    time_bar_steer = plot([t(i), t(i)], [100, -100], 'k');
    legend([p1,p2], {'measured','command'})
    ylim([-40 40]);
  
    
    drawnow;
    frame_vec(j) = getframe(fig_trajectory_result);
    
    j = j + 1;
end



% for video save
if (save_video == 1)
    cd ./movie
    frame_vec(1) = [];
    vidObj = VideoWriter('result.avi');
    vidObj.FrameRate = 25;
    open(vidObj);
    writeVideo(vidObj, frame_vec);
    close(vidObj);
    cd ../
end



% plot_pid;

================================================
FILE: simulation/model_predictive_controller.m
================================================
function [u, debug_info] = model_predictive_controller(state, t, ref, param)
% 
% state = [x, y, yaw, delta]
% u = [v_des, delta_des]
% ref = [x_ref; y_ref; yaw_ref; v_ref; k_ref; t_ref];
% 
% 
% 

% =================== for delay compensation ==============================
persistent deltades_buffer
if isempty(deltades_buffer)
    deltades_buffer = zeros(param.mpc_delay_comp_step, 1);
end
% =========================================================================
    
deg2rad = pi / 180;
delay_step = param.mpc_delay_comp_step;

IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
IDX_TIME = 6;

IDX_DELTA = 4;

%% convert xy-yaw to error dynamics

% calculate nearest point (use as initial state)
distance = vecnorm(ref(:, IDX_XY)' - state(IDX_XY)');
[~, min_index] = min(distance);
ref_sp = ref(min_index, :);

% convert x,y to lon,lat model
sp_yaw = ref_sp(IDX_YAW);
T_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);
             -sin(sp_yaw), cos(sp_yaw)];
error_xy = (state(IDX_XY) - ref_sp(IDX_XY))';
error_lonlat = T_xy2lonlat * error_xy;
error_lat = error_lonlat(2);


% calculate yaw error
error_yaw = state(IDX_YAW) - sp_yaw;
while (-2*pi <= error_yaw && error_yaw <= 2*pi) == 0 
    if (error_yaw >= 2*pi)
        error_yaw = error_yaw - 2*pi;
    elseif (error_yaw <= -2*pi)
        error_yaw = error_yaw + 2*pi;
    end
end
if (error_yaw > pi)
    error_yaw = error_yaw - 2*pi;
elseif (error_yaw < -pi)
    error_yaw = error_yaw + 2*pi;
end

% initial state for error dynamics
x0 = [error_lat; error_yaw; state(IDX_DELTA)];

%% update error dynamics for holizon

% -- set mpc parameters --
mpc_dt = param.mpc_dt;
mpc_n = param.mpc_n;
Q = param.mpc_Q;
R = param.mpc_R;
mpc_t = ref_sp(IDX_TIME);
DIM_X = 3;
DIM_Y = 2;
DIM_U = 1;
Aex = zeros(DIM_X*mpc_n, DIM_X);
Bex = zeros(DIM_X*mpc_n, DIM_U*mpc_n);
Wex = zeros(DIM_X*mpc_n, 1);
Cex = zeros(DIM_Y*mpc_n, DIM_X*mpc_n);
Qex = zeros(DIM_Y*mpc_n, DIM_Y*mpc_n);
Rex = zeros(DIM_U*mpc_n, DIM_U*mpc_n);
mpc_ref_v = zeros(mpc_n + delay_step, 1);
debug_ref_mat = zeros(mpc_n + delay_step,5);

% =================== for delay compensation ==============================
% -- apply delay compensation : update dynamics with increasing mpt_t --
x_curr = x0;
for i = 1:delay_step
    if mpc_t > ref(end, IDX_TIME)
        mpc_t = ref(end, IDX_TIME);
        disp('[MPC] path is too short to predict dynamics');
    end
    ref_now = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);
    debug_ref_mat(i,:) = ref_now;
    v_ = ref_now(IDX_VEL);
    k_ = ref_now(IDX_CURVATURE);
    
    % get discrete state matrix
    % NOTE : use control_dt as delta time, not mpc_dt. 
    [Ad, Bd, wd, ~] = get_error_dynamics_state_matrix(param.control_dt, v_, param.wheelbase, param.tau, k_);
    u_now = deltades_buffer(end - i + 1);
    x_next = Ad * x_curr + Bd * u_now + wd;
    
    mpc_t = mpc_t + param.control_dt; % THIS IS NOT mpc_dt, BUT control_dt
    x_curr = x_next;
    
    mpc_ref_v(i) = v_;
end
x0 = x_curr;
% =========================================================================

% -- mpc matrix for i = 1 --
ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);
debug_ref_mat(1 + delay_step,:) = ref_i_; % MODIFIED FOR DELAY
v_ = ref_i_(IDX_VEL);
k_ = ref_i_(IDX_CURVATURE);
[Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(mpc_dt, v_, param.wheelbase, param.tau, k_); 
Aex(1:DIM_X, :) = Ad;
Bex(1:DIM_X, 1:DIM_U) = Bd;
Wex(1:DIM_X) = wd;
Cex(1:DIM_Y, 1:DIM_X) = Cd;
Qex(1:DIM_Y, 1:DIM_Y) = Q;
Rex(1:DIM_U, 1:DIM_U) = R;

mpc_ref_v(1 + delay_step) = v_;

% -- mpc matrix for i = 2:n --
for i = 2:mpc_n
    
    % update mpc time
    mpc_t = mpc_t + mpc_dt;
    if mpc_t > ref(end, IDX_TIME)
        mpc_t = ref(end, IDX_TIME);
        disp('[MPC] path is too short to predict dynamics');
    end

    % get reference information
    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), mpc_t);
    debug_ref_mat(i + delay_step,:) = ref_i_;
    v_ = ref_i_(IDX_VEL);
    k_ = ref_i_(IDX_CURVATURE);
    
    % get discrete state matrix
    [Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(mpc_dt, v_, param.wheelbase, param.tau, k_);
    
    % update mpc matrix
    idx_x_i = (i-1)*DIM_X+1:i*DIM_X;
    idx_x_i_prev = (i-2)*DIM_X+1:(i-1)*DIM_X;
    idx_u_i = (i-1)*DIM_U+1:i*DIM_U;
    idx_y_i = (i-1)*DIM_Y+1:i*DIM_Y;
    Aex(idx_x_i, :) = Ad * Aex(idx_x_i_prev, :);
    for j = 1:i-1
        idx_u_j = (j-1)*DIM_U+1:j*DIM_U;
        Bex(idx_x_i, idx_u_j) = Ad * Bex(idx_x_i_prev, idx_u_j);
    end
    Bex(idx_x_i, idx_u_i) = Bd;
    Wex(idx_x_i) = Ad * Wex(idx_x_i_prev) + wd;
    Cex(idx_y_i, idx_x_i) = Cd;
    Qex(idx_y_i, idx_y_i) = Q;
    Rex(idx_u_i, idx_u_i) = R;
    
    mpc_ref_v(i + delay_step) = v_;
    
end


%% convex optimization

% The problem is to solve following for U.
%   1/2 * U'* mat1 * U + mat2 * U + C = 0
mat1 = Bex' * Cex' * Qex * Cex * Bex + Rex;
mat2 = (x0' * Aex' + Wex') * Cex' * Qex * Cex * Bex;

steering_rate_lim = param.mpc_constraint_steer_rate_deg * deg2rad;

if param.mpc_solve_without_constraint == true
    input_vec = -mat1 \ mat2';
else
    % --- convex optimization ---
    %   minimize for x, s.t.
    %   J(x) = 1/2 * x' * H * x + f' * x, 
    %   A*x <= b,   lb <= x <= ub

    H_ = (mat1 + mat1') / 2;
    f_ = mat2;
    
    % add steering rate constraint
    tmp = -eye(mpc_n-1, mpc_n);
    tmp(1:end,2:end) = tmp(1:end,2:end) + eye(mpc_n-1);
    T_ = kron(tmp, [0,0,1]) / mpc_dt;
    dsteer_vec_tmp_ = T_ * (Aex * x0 + Wex);
    A_ = [T_ * Bex; -T_ * Bex];    
    b_ = [steering_rate_lim * ones(mpc_n-1,1) - dsteer_vec_tmp_; steering_rate_lim * ones(mpc_n-1,1) + dsteer_vec_tmp_];

    % constraint for upper and lower steering boundary
    lb_ = -param.mpc_constraint_steering_deg * deg2rad * ones(mpc_n * DIM_U,1);
    ub_ = param.mpc_constraint_steering_deg * deg2rad * ones(mpc_n * DIM_U,1);
    options_ = optimoptions('quadprog', 'Algorithm','interior-point-convex', 'Display', 'off');
    [x_, fval, exitflag, output, lambda] = quadprog(H_, f_, A_, b_, [], [], lb_, ub_, [], options_);
    input_vec = x_;


    % for debug: compare with / without constraint optimization
    % input_vec_LS = -mat1 \ mat2';
    % figure(101);
    % plot(input_vec_LS); hold on;
    % plot(input_vec); grid on; hold off;
end

delta_des = input_vec(1);
v_des = ref_sp(IDX_VEL);
u = [v_des, delta_des];

% =================== for delay compensation ==============================
deltades_buffer = [delta_des; deltades_buffer(1:end-1)];
% =========================================================================

%% (debug) calculate predicted trajectory 

x_ = state;
predictd_states = zeros(length(input_vec), length(state));
for i = 1:length(input_vec)
    x_next = calc_kinematics_model(x_, input_vec(i), mpc_dt, mpc_ref_v(i), param.wheelbase, param.tau);
    predictd_states(i,:) = x_next';
    x_ = x_next;
end

predictd_states_vector = reshape(predictd_states, [], 1);

debug_ref_mat_no_delay_comp = debug_ref_mat(delay_step + 1:end, :);

predicted_error = Aex*x0 + Bex*input_vec + Wex;
predicted_error = transpose(reshape(predicted_error, 3, []));
predicted_state_ideal = debug_ref_mat_no_delay_comp(:,IDX_XY) + ...
    [-sin(debug_ref_mat_no_delay_comp(:,IDX_YAW)).*predicted_error(:,1), cos(debug_ref_mat_no_delay_comp(:,IDX_YAW)).*predicted_error(:,1)];

predicted_state_ideal = (reshape(predicted_state_ideal, [], 1));

debug_info = [input_vec', predictd_states_vector', predicted_state_ideal', error_lat];


% for debug 
% figure(1);plot(predicted_error(:,1),'b*-');
% figure(3);
% plot(input_vec); hold on;
% plot(predicted_error(:,3));hold on;
% plot(predictd_states(:,4)); hold off;


end

%% time varying error dynamics model
% used for error dynamics update
function [Ad, Bd, wd, Cd] = get_error_dynamics_state_matrix(dt, v, L, tau, curvature)
    
    % linearization around delta = 0
    % A = [0, v, 0;
    %     0, 0, v/L;
    %     0, 0, -1/tau];
    % B = [0; 0; 1/tau];
    % C = [1, 0, 0;
    %      0, 1, 0];
    % w = [0; 
    %     -v*curvature;
    %      0];

    % linearization around delta = delta_ref (better accuracy than delta=0)
    delta_r = atan(L*curvature);
    if (abs(delta_r) >= 40 /180 * pi)
        delta_r = (40 /180 * pi)*sign(delta_r);
    end
    cos_delta_r_squared_inv = 1 / ((cos(delta_r))^2);

    % difinition for continuous model
    A = [0, v, 0;
         0, 0, v/L*cos_delta_r_squared_inv;
         0, 0, -1/tau];
    B = [0; 0; 1/tau];
    C = [1, 0, 0;
         0, 1, 0];
    w = [0; 
        -v*curvature + v/L*(tan(delta_r)-delta_r*cos_delta_r_squared_inv);
         0];
    
    % discretization
    % Ad = eye(3) + A * dt;
    I = eye(3);
    Ad = (I - dt * 0.5 * A) \ (I + dt * 0.5 * A);
    Bd = B * dt;
    Cd = C;
    wd = w * dt;
end

%% xy-yaw dynamics model
% used for debug (predicted trajectory calculation)
function x_next = calc_kinematics_model(x, u, dt, v, L, tau)

    % x = [x, y, yaw, delta]
    x_next = zeros(4,1);
    yaw = x(3);
    delta = x(4);
    
    % x
    x_next(1) = x(1) + v*cos(yaw)*dt;
    
    % y
    x_next(2) = x(2) + v*sin(yaw)*dt;
    
    % yaw
    x_next(3) = x(3) + v*tan(delta)/L*dt;
    
    % delta
    x_next(4) = x(4) - (x(4) - u)/tau*dt;

end

================================================
FILE: simulation/model_predictive_controller2.m
================================================
function [u, debug_info] = model_predictive_controller2(state, t, ref, param)
% 
% state = [x, y, yaw, delta]
% u = [v_des, delta_des]
% ref = [x_ref; y_ref; yaw_ref; v_ref; k_ref; t_ref];
% 
% 
% 
deg2rad = pi / 180;

IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
IDX_TIME = 6;

IDX_DELTA = 4;

%% calculate nearest point

% calculate nearest point (use as initial state)
distance = vecnorm(ref(:, IDX_XY)' - state(IDX_XY)');
[~, min_index] = min(distance);
ref_sp = ref(min_index, :);
v_ref = ref_sp(IDX_VEL);

% (debug) calculate latitude error
sp_yaw = ref_sp(IDX_YAW);
T_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);
             -sin(sp_yaw), cos(sp_yaw)];
error_xy = (state(IDX_XY) - ref_sp(IDX_XY))';
error_lonlat = T_xy2lonlat * error_xy;
error_lat = error_lonlat(2);


%% update error dynamics for holizon

% set mpc parameters
mpc_dt = param.mpc2_dt;
mpc_n = param.mpc2_n;
Q = param.mpc2_Q;
R = param.mpc2_R;
t_ = ref_sp(IDX_TIME);
DIM_STATE = 4;
DIM_OUTPUT = 3;
DIM_INPUT = 1;
A_ext = zeros(DIM_STATE*mpc_n, DIM_STATE);
B_ext = zeros(DIM_STATE*mpc_n, DIM_INPUT*mpc_n);
W_ext = zeros(DIM_STATE*mpc_n, 1);
C_ext = zeros(DIM_OUTPUT*mpc_n, DIM_STATE*mpc_n);
Q_ext = zeros(DIM_OUTPUT*mpc_n, DIM_OUTPUT*mpc_n);
R_ext = zeros(DIM_INPUT*mpc_n, DIM_INPUT*mpc_n);
mpc_ref_v = zeros(length(mpc_n), 1);
ref_vec = zeros(mpc_n,5);


for i = 1
    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), t_);
    ref_vec(i,:) = ref_i_;
    v_ = ref_i_(IDX_VEL);
    [Ad, Bd, wd, Cd] = get_linearized_state_matrix(mpc_dt, ref_i_, param.wheelbase, param.tau);
    
    cols_i = (i-1)*DIM_STATE+1:i*DIM_STATE;
    A_ext(cols_i, :) = Ad;
    
    rows_i = (i-1)*DIM_INPUT+1:i*DIM_INPUT;
    B_ext(cols_i, rows_i) = Bd;
    
    W_ext(cols_i) = wd;
    
    cols_i_C = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    rows_i_C = (i-1)*DIM_STATE+1:i*DIM_STATE;
    C_ext(cols_i_C, rows_i_C) = Cd;
    
    cols_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    rows_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    Q_ext(cols_i_Q, rows_i_Q) = Q;
    
    R_ext(rows_i, rows_i) = R;
    
    mpc_ref_v(i) = v_;
    
    t_ = t_ + mpc_dt;
    if t_ > ref(end, IDX_TIME)
        t_ = ref(end, IDX_TIME);
        disp('[MPC] path is too short to predict dynamics');
    end
end

for i = 2:mpc_n
    ref_i_ = interp1q(ref(:, IDX_TIME), ref(:,1:5), t_);
    ref_vec(i,:) = ref_i_;
    v_ = ref_i_(IDX_VEL);
    [Ad, Bd, wd, Cd] = get_linearized_state_matrix(mpc_dt, ref_i_, param.wheelbase, param.tau);
    
    
    cols_i = (i-1)*DIM_STATE+1:i*DIM_STATE;
    cols_i_prev = (i-2)*DIM_STATE+1:(i-1)*DIM_STATE;
    A_ext(cols_i, :) = Ad * A_ext(cols_i_prev, :);
    
    for j = 1:i-1
        rows_j = (j-1)*DIM_INPUT+1:j*DIM_INPUT;
        B_ext(cols_i, rows_j) = Ad * B_ext(cols_i_prev, rows_j);
    end
    rows_i = (i-1)*DIM_INPUT+1:i*DIM_INPUT;
    B_ext(cols_i, rows_i) = Bd;
    
    W_ext(cols_i) = Ad * W_ext(cols_i_prev) + wd;
    
    cols_i_C = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    rows_i_C = (i-1)*DIM_STATE+1:i*DIM_STATE;
    C_ext(cols_i_C, rows_i_C) = Cd;
    
    cols_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    rows_i_Q = (i-1)*DIM_OUTPUT+1:i*DIM_OUTPUT;
    Q_ext(cols_i_Q, rows_i_Q) = Q;
    
    R_ext(rows_i, rows_i) = R;
    
    mpc_ref_v(i) = v_;
    
    t_ = t_ + mpc_dt;
    if t_ > ref(end, IDX_TIME)
        t_ = ref(end, IDX_TIME);
        disp('[MPC] path is too short to predict dynamics');
    end
end


%% convex optimization

x0 = state';

% for yaw singularity
% for i=:length(ref_vec(:,1))
%     if ()
    


Y_ref = reshape(transpose(ref_vec(:,1:3)), [], 1);

% The problem is to solve following for U.
%   1/2 * U'* mat1 * U + mat2 * U + C = 0
mat1 = B_ext' * C_ext' * Q_ext * C_ext * B_ext + R_ext;
mat2 = (C_ext * A_ext * x0 + C_ext * W_ext - Y_ref)' * Q_ext * C_ext * B_ext;


% --- convex optimization ---
%   minimize for x, s.t.
%   J(x) = 1/2 * x' * H * x + f' * x, 
%   A*x <= b,   lb <= x <= ub

H_ = (mat1 + mat1') / 2;
f_ = mat2;
A_ = [];
b_ = [];

% constraint for upper and lower steering boundary
lb_ = -param.mpc2_steering_lim_deg * deg2rad * ones(mpc_n * DIM_INPUT,1);
ub_ = param.mpc2_steering_lim_deg * deg2rad * ones(mpc_n * DIM_INPUT,1);
options_ = optimoptions('quadprog', 'Algorithm','interior-point-convex', 'Display', 'off');
[x_, fval, exitflag, output, lambda] = quadprog(H_, f_, A_, b_, [], [], lb_, ub_, [], options_);
input_vec = x_;


% for debug: compare with / without constraint optimization
% input_vec_LS = -mat1 \ mat2';
% figure(101);
% plot(input_vec_LS,'bo-'); hold on;
% plot(input_vec,'r*-'); grid on; hold off;


v_des = v_ref;
delta_des = input_vec(1);
u = [v_des, delta_des];

%% (debug) calculate predicted trajectory 

x_ = state;
predictd_states = zeros(length(input_vec), length(state));
for i = 1:length(input_vec)
    x_next = calc_kinematics_model(x_, input_vec(i), mpc_dt, mpc_ref_v(i), param.wheelbase, param.tau);
    predictd_states(i,:) = x_next';
    x_ = x_next;
end

predictd_states_real = reshape(predictd_states', [], 1);

predicted_states_linearized = A_ext*x0 + B_ext*input_vec + W_ext;

debug_info = [input_vec', predictd_states_real', predicted_states_linearized', error_lat];


% for debug 
% predictd_states_real_mat = reshape(predictd_states_real, [], 4);
% predicted_states_linearized_mat = transpose(reshape(predicted_states_linearized, 4, []));
% reshape(predictd_states_real, [], 4);
% figure(1);
% plot(predictd_states_real_mat(:,1), predictd_states_real_mat(:,2),'b*-'); hold on;
% plot(predicted_states_linearized_mat(:,1), predicted_states_linearized_mat(:,2), 'ro-'); hold off;
% xlabel('x [m]'); ylabel('y [m]');
% figure(3);
% plot(input_vec); hold on;
% plot(predicted_error(:,3));hold on;
% plot(predictd_states(:,4)); hold off;


end

%% time varying error dynamics model
% used for error dynamics update
function [Ad, Bd, wd, Cd] = get_linearized_state_matrix(dt, ref, L, tau)
    %
    % x = [x, y, yaw, delta]';
    % u = [delta_com];
    %
    IDX_VEL = 4;
    IDX_YAW = 3;
    IDX_CURVATURE = 5;
    
    v_r = ref(IDX_VEL);
    yaw_r = ref(IDX_YAW);
    delta_r = atan(L*ref(IDX_CURVATURE));
    cos_delta_r_squared_inv = 1 / ((cos(delta_r))^2);

    % difinition for continuous model
    A = [0, 0, -v_r * sin(yaw_r), 0;
         0, 0, v_r*cos(yaw_r),    0;
         0, 0, 0,                v_r/L*cos_delta_r_squared_inv;
         0, 0, 0,                -1/tau];
    B = [0; 0; 0; 1/tau];
    C = [1, 0, 0, 0;
         0, 1, 0, 0;
         0, 0, 1, 0];
    w = [v_r*cos(yaw_r) + v_r*sin(yaw_r)*yaw_r;
         v_r*sin(yaw_r) - v_r*cos(yaw_r)*yaw_r;
         v_r/L*(tan(delta_r)-delta_r*cos_delta_r_squared_inv);
         0];
         
    
    % discretization
    Ad = eye(4) + A * dt;
    Bd = B * dt;
    Cd = C;
    wd = w * dt;
end

%% xy-yaw dynamics model
% used for debug (predicted trajectory calculation)
function x_next = calc_kinematics_model(x, u, dt, v, L, tau)

    % x = [x, y, yaw, delta]
    x_next = zeros(4,1);
    yaw = x(3);
    delta = x(4);
    
    % x
    x_next(1) = x(1) + v*cos(yaw)*dt;
    
    % y
    x_next(2) = x(2) + v*sin(yaw)*dt;
    
    % yaw
    x_next(3) = x(3) + v*tan(delta)/L*dt;
    
    % delta
    x_next(4) = x(4) - (x(4) - u)/tau*dt;

end

================================================
FILE: simulation/pid_controller.m
================================================
function [u, debug_info] = pid_controller(state, t, ref, param)
% 
% state = [x, y, yaw, delta]
% u = [v_des, delta_des]
% ref = [x_ref; y_ref; yaw_ref; v_ref];
% 
% 
% 

IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
% IDX_TIME = 6;


% LON = 1;
LAT = 2;

kp = 0.3 * 1;
kd = 0.5 * 3;

yaw = state(IDX_YAW);


distance = vecnorm(ref(:,1:2)' - state(1:2)');
[~, min_index] = min(distance);

pr = ref(min_index, :);

v_des = pr(IDX_VEL);


% feedforward input calculation
ff_curvature = atan(param.wheelbase * pr(IDX_CURVATURE));

% coordinate transformation to body frame
T = [cos(yaw), sin(yaw);
    -sin(yaw), cos(yaw)];
error_xy = (state(IDX_XY) - pr(IDX_XY))';
error_latlon = T * error_xy;
error_yaw = yaw - pr(IDX_YAW);

% should use mod?
while (-2*pi <= error_yaw && error_yaw <= 2*pi) == 0 
    if (error_yaw >= 2*pi)
        error_yaw = error_yaw - 2*pi;
    elseif (error_yaw <= -2*pi)
        error_yaw = error_yaw + 2*pi;
    end
end
if (error_yaw > pi)
    error_yaw = error_yaw - 2*pi;
elseif (error_yaw < -pi)
    error_yaw = error_yaw + 2*pi;
end

delta_des = -kp * (error_latlon(LAT)) - kd * error_yaw + ff_curvature;
fb_lat = -kp * (error_latlon(LAT));
fb_yaw = - kd * error_yaw;

u = [v_des, delta_des];

debug_info = [pr(IDX_X), pr(IDX_Y), pr(IDX_YAW), fb_lat, fb_yaw, ff_curvature, error_latlon(LAT)];

================================================
FILE: simulation/pure_pursuit.m
================================================
function [u, debug_info] = pure_pursuit(state, t, ref, param)
% 
% state = [x, y, yaw, delta]
% u = [v_des, delta_des]
% ref = [x_ref; y_ref; yaw_ref; v_ref];
% 
% 
% 

IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_YAW = 3;
IDX_VEL = 4;
% IDX_CURVATURE = 5;
% IDX_TIME = 6;


lookahead_dist = param.pure_pursuit_lookahead;

distance = vecnorm(ref(:,IDX_XY)' - state(IDX_XY)');
[~, min_index] = min(distance);
pr = ref(min_index, :);

v_des = pr(IDX_VEL);

for i = min_index:length(ref)
    dist = norm(ref(i, IDX_XY) - state(:,IDX_XY));
    if dist > lookahead_dist
        break;
    end
end
lookahead_pt = ref(i, :);

alpha = atan2(lookahead_pt(IDX_Y) - state(IDX_Y), lookahead_pt(IDX_X) - state(IDX_X)) - state(IDX_YAW);

omega_des = 2 * v_des * sin(alpha) / param.wheelbase;
delta_des = atan2(omega_des * param.wheelbase, v_des);



u = [v_des, delta_des];


% lattitude error calc for debug
sp_yaw = pr(IDX_YAW);
T_xy2lonlat = [cos(sp_yaw), sin(sp_yaw);
             -sin(sp_yaw), cos(sp_yaw)];
error_xy = (state(IDX_XY) - pr(IDX_XY))';
error_lonlat = T_xy2lonlat * error_xy;
error_lat = error_lonlat(2);

debug_info = [lookahead_pt(IDX_X), lookahead_pt(IDX_Y), lookahead_pt(IDX_YAW), error_lat];

================================================
FILE: simulation/simulate_rk4.m
================================================
function [state_log, input_log, debug_info] = simulate_rk4(model, controller, x0, ref, ts, dt, tf, param)

x = x0;
i = 1;

t_vec = ts:dt:tf;

state_log = zeros(length(t_vec), length(x0));
[tmp_u, tmp_u_debug] = controller(x0, ts, ref, param);
input_log = zeros(length(t_vec), length(tmp_u));
debug_info = zeros(length(t_vec), length(tmp_u_debug));

input_delay = param.input_delay;
delay_count = round(input_delay / dt);
input_buf = zeros(delay_count, length(tmp_u));
u = zeros(size(tmp_u)); % initial input
u_debug = zeros(size(tmp_u_debug));

control_dt = param.control_dt;
control_count = round(control_dt / dt);

for t = ts:dt:tf
    % -- control once per control_count time --
    if mod(i, control_count) == 0
        % add noise
        x_noised = x + rand(1, length(x0)) .* param.measurement_noise_stddev;
        [u, u_debug] = controller(x_noised, t, ref, param);
    end
    
    % -- add input delay --
    input_buf = [u; input_buf(1:end-1, :)];
    u_delayed = input_buf(end,:);
    
    % -- runge-kutta --
    k1 = model(x, u_delayed, param);
    k2 = model(x + k1*dt/2, u_delayed, param);
    k3 = model(x + k2*dt/2, u_delayed, param);
    k4 = model(x + dt*k3, u_delayed, param);
    x = x + (k1 + 2*k2 + 2*k3 + k4) * dt / 6;
    
    % -- save data --
    state_log(i,:) = x;
    input_log(i,:) = u;
    debug_info(i,:) = u_debug;
    
    i = i + 1;
    disp(t);
end