clear
format compact
% -----------------------
load ident_arm_data
% -----------------------
kP = 90;                    % 比例ゲイン
rc = pi/3;                  % アーム角度の目標値
% -----------------------
t_data = t;                 % いったん，データを移動
theta1_data = theta1;
v_data = v;
clear t theta1 v
% -----------------------
k = 0;
for i = 1:length(t_data)    % 2 秒までのデータを捨てる
   if t_data(i) >= 1;
       k = k + 1;
       t(k) = t_data(i) - 2;
       theta1(k) = theta1_data(i);
       v(k) = v_data(i);
   end
end

% ++++++++++ Step 1 ++++++++++
[theta1_max imax] = max(theta1);    % ステップ応答の最大値の抽出

imax_last = imax;
for i = imax:length(t)
    if theta1(i) == theta1_max
        imax_last = i;
    end
    if theta1(i) < theta1_max
        break
    end
end

Tp    = (t(imax) + t(imax_last))/2; % 行き過ぎ時間
Amax  = theta1_max - rc;            % オーバーシュート

fprintf('Tp   = %3.2e\n',Tp)
fprintf('Amax = %3.2e\n',Amax)

% ++++++++++ Step 2 ++++++++++
xi   = - (1/Tp)*log(Amax/rc);
wn   = sqrt((pi/Tp)^2 + xi^2);      % 固有角周波数
zeta = xi/wn;                       % 減衰係数

fprintf('wn   = %3.2e\n',wn)
fprintf('zeta = %3.2e\n',zeta)

% ++++++++++ Step 3 ++++++++++
a = 2*zeta*wn;                      % a の同定
b = wn^2/kP;                        % b の同定

fprintf('a     = %3.2e\n',a)
fprintf('b     = %3.2e\n',b)

% -----------------------
t_sim = 0:0.001:1;                  % 同定されたパラメータを用いてシミュレーション
theta1_sim = rc*step(wn^2,[1 2*zeta*wn wn^2],t_sim);

figure(1)
stairs(t,theta1*180/pi,'b','linewidth',2)
hold on
plot(t_sim,theta1_sim*180/pi,'r')

plot(Tp,theta1_max*180/pi,'ko','linewidth',2,'markersize',10)
plot([Tp Tp],[0 theta1_max]*180/pi,'k')
plot([0 Tp],[theta1_max theta1_max]*180/pi,'k')
plot([0 1],[rc rc]*180/pi,'g')
hold off
xlim([0 1]);     ylim([0 90])

set(gca,'fontname','arial','fontsize',20)
xlabel('$$t$$ [s]', 'interpreter', 'latex','fontsize',24)
ylabel('$${\theta}_{1}(t)$$ [deg]', 'interpreter', 'latex','fontsize',24)

legend({'Experiment','Simulation'}, 'interpreter', 'latex', 4)
set(legend,'fontsize',22)

set(gca,'xtick',0:0.2:1)
set(gca,'ytick',0:30:90)




