function DY=double_augm_ode_ver5(t,Y);
% function DY=double_augm_ode_ver5(t,Y);
%
%  ■　(g) 微分方程式の定義
%
% 数値積分ルーチン ode45 を用いて、数値積分を行う微分方程式を定義する 
% 式(4.14)(4.30)(4.47)(4.49) 参照
%
% Copyright (C) 2007 清水 信行, 曽我部 潔, 竹原 昭一郎 All Rights Reserved.

% =================================================================================
% プログラムの使用条件
% 1. 本プログラムおよびデータを商用で，頒布してはならない．
% 2. 本プログラムおよびデータを著作者の許可なく営利目的で利用してはならない．
% 3. 本プログラムおよびデータの改変は，営利目的でない限り自由である．
% 4. 本プログラムおよびデータを使用することにより生じた損害などについては，著者，
%    プロクラム著作者，(社)日本機械学会，および(株)コロナ社は一切の責任を負わない．
% 5. 本プログラムの著作権は，本書編著者ならびにプログラム著作者に帰す．
% =================================================================================

global fa1 fa2 c1 c2 M invM Jd1 Jd2 LGC1 LGC2...
                            rGCd1 rGCd2 s1tilde s12tilde s2tilde Alpha Beta
						                   % オイラー角は3-1-3（Z'X'Z'）変換
                    % オイラー角は3-1-3（Z'X'Z'）変換
thOG1=Y(13:15);     % 状態変数から オイラー角(φ,θ,ψ)を抽出
thOG2=Y(16:18);     % 状態変数から オイラー角(φ,θ,ψ)を抽出
thOG=[thOG1;thOG2];
omOGd1=Y(19:21);  % 状態変数から 剛体１に関する角速度(ωx',ωy',ωz')を抽出
omOGd2=Y(22:24);  % 状態変数から 剛体２に関する角速度(ωx',ωy',ωz')を抽出
C=cos(thOG);      % cos(φ),cos(θ),cos(ψ)
S=sin(thOG);      % sin(φ),sin(θ),sin(ψ)
nd1=-c1*omOGd1;   % 剛体１に関する回転に対する外トルク（この場合は減衰による外トルク）
nd2=-c2*omOGd1;   % 剛体２に関する回転に対する外トルク（この場合は減衰による外トルク）
%
AOG1=[ C(1)*C(3)-S(1)*C(2)*S(3)  -C(1)*S(3)-S(1)*C(2)*C(3)   S(1)*S(2);
       S(1)*C(3)+C(1)*C(2)*S(3)  -S(1)*S(3)+C(1)*C(2)*C(3)  -C(1)*S(2);
       S(2)*S(3)                  S(2)*C(3)                  C(2)];
AOG2=[ C(4)*C(6)-S(4)*C(5)*S(6)  -C(4)*S(6)-S(4)*C(5)*C(6)   S(4)*S(5);
       S(4)*C(6)+C(4)*C(5)*S(6)  -S(4)*S(6)+C(4)*C(5)*C(6)  -C(4)*S(5);
       S(5)*S(6)                  S(5)*C(6)                  C(5)];% 式(4.35), 基式(4.50)
%
%  オイラー角の時間微分を求めるための係数マトリックス    式(4.14)
%  
invGed1=1/S(2)*[S(3)        C(3)          0;
                S(2)*C(3)  -S(2)*S(3)     0;
               -C(2)*S(3)  -C(2)*C(3)     S(2)];
invGed2=1/S(5)*[S(6)        C(6)          0;%オイラー角の時間微分を
                S(5)*C(6)  -S(5)*S(6)     0;%求めるための角速度係数行列
               -C(5)*S(6)  -C(5)*C(6)     S(5)] ;%基式(5.54)
%
omOGdtilde1=tilde(omOGd1);
omOGdtilde2=tilde(omOGd2);  %角速度omOGdから作った歪み対称マトリックス
%
Q=[fa1;fa2;nd1-omOGdtilde1*Jd1*omOGd1;nd2-omOGdtilde2*Jd2*omOGd2];
                            %外力に関するマトリックス
Cr11=eye(3);
Cr12=zeros(3,3);
Cr21=-eye(3);
Cr22=eye(3);
Ctheta11=-AOG1*s1tilde;
Ctheta12=zeros(3,3);
Ctheta21=AOG1*s12tilde;
Ctheta22=-AOG2*s2tilde;
Cr=[Cr11 Cr12; Cr21 Cr22];
Ctheta=[Ctheta11 Ctheta12; Ctheta21 Ctheta22];
Cq=[Cr Ctheta];
                            %拘束条件式から求めたヤコビアンマトリックス
gamma1=[AOG1*omOGdtilde1*s1tilde*omOGd1-2*Alpha*(Y(7:9)-AOG1*s1tilde...
                                       *omOGd1)-Beta^2*(Y(1:3)+AOG1*rGCd1)];
gamma2=[-AOG1*omOGdtilde1*s12tilde*omOGd1+2*Alpha*(Y(7:9)-AOG1*s12tilde*...
       omOGd1)+Beta^2*(Y(1:3)+AOG1*-rGCd1)+AOG2*omOGdtilde2*s2tilde*omOGd2-...
         2*Alpha*(Y(10:12)-AOG2*s2tilde*omOGd2)-Beta^2*(Y(4:6)+AOG2*rGCd2)];
gamma=[gamma1; gamma2];                      % 式(4.47)
Lambda=inv(Cq*invM*Cq.')*(Cq*invM*Q - gamma);% 式(4.48)
%
Ydot=invM*(Q - Cq.'*Lambda);
DvG1=Ydot(1:3);
DvG2=Ydot(4:6);             %並進速度の微分DvG（基式(13.30)より）
DomOGd1=Ydot(7:9);
DomOGd2=Ydot(10:12);        %角速度の時間微分について（基式(13.31)より）
DthOG1=invGed1*omOGd1;
DthOG2=invGed2*omOGd2;      %オイラー角の微分と角速度の関係（基式(5.52b),式(5.54)より)  
%
% 状態量の時間微分
DY=[Y(7:9); Y(10:12); DvG1; DvG2; DthOG1; DthOG2; DomOGd1; DomOGd2];% ode45で数値積分する微分方程式（拡大系）
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
