C/GMRESシミュレーションを3つの言語で作ったので比較してみた

はじめに

以前、MatlabでC/GMRESとその亜種のコードを書いてみる記事をアップしました。将来的にはROSでC/GMRESを実装するという目標あったことから、「PythonとC++で書いてみよう」と思い立ち、MatlabのコードをPythonとC++に翻訳？してみました。いずれも経験が浅く、コードとしては無駄の多い箇所もまだまだ残っていると思いますが、どうかご容赦下さい。

環境

Python 3.9.6 + NumPy 1.17.4 （windows11）
g++ 9.4.0 + Eigen 3.4.0 （ubuntu20.04 LTS）
Matlab R2023a （windows11）

参考文献

自分が参考にした書籍、記事を先に説明します。これらを先に読んでいただけるとわかりやすいかと思います。

C/GMRESについて、以下の書籍、記事を参考にしております。

• 非線形最適制御入門
• 実時間最適化による制御の実応用
• C/GMRES法の例題実装
• 非線形モデル予測制御におけるC/GMRES法をPythonで実装する

特に自分のコードの大部分はC/GMRES法の例題実装で公開されていたコードを基に改造する形で書いております。コードを一つ一つ数式と照らし合わせて示してくださっており、非常に理解しやすい記事です（この記事のお陰でC/GMRESを理解出来たとも言えるくらいであり、自分としては感謝してもしきれないくらいです）。

内容説明

今回のコードでは、以下のような状態方程式を持つ差分駆動型の二輪車モデルを考えています（図を用意しようかと思ったのですが、面倒だったので数式だけでお許しください...）。

\dot{x} = f(x) = 
\begin{pmatrix}
\frac{R}{2} \cos\theta & \frac{R}{2} \cos\theta \\
\frac{R}{2} \sin\theta & \frac{R}{2} \sin\theta \\
\frac{R}{T} & -\frac{R}{T}
\end{pmatrix}
\begin{pmatrix}
u_{1} \\
u_{2}
\end{pmatrix}

$R$は車輪の半径、$T$は車輪の間の幅です。入力$u$は左右の車輪の角速度です。評価関数は以下の様に設定しています。

J = (x-x_{ob})^{T} S (x-x_{ob}) + \int_{0}^{T} (x-x_{ob})^{T} Q (x-x_{ob}) + u^{T} R u \ d \tau

こちらの$T$は予測ホライズン長さであり先ほど出てきたものとは別物です。コードの中ではクラス変数にする事で分けています。$x_{ob}$は目標地点です。この目標地点に到着するように制御する事になります。

速度比較

というわけで３つのコードを実行してみました。シミュレーションループの計算にかかる速度を測定したところ、Matlabがおよそ3.0~3.5秒、Pythonが15~16秒、C++が16~17秒くらいになりました。NumPyがEigenより速いと言うのは他の所で見たことはあるのですが、Matlabが他２つの5倍くらい速いと言うのは信じがたいですね。。。自分のコーディングが余程無駄だらけなのか。。。とはいえ実装している処理自体は数式的には全く同じものなはずなんですよね。。。

コード

最後にコードを置きます。何か改善点等ありましたらコメントで教えて頂けると助かります。
長いのでこちらのgithubにも置いておきます。

Mtalab

close all; clear all; format compact; beep off;%#ok<CLALL> 

Ts = 0.05;  %サンプリング周期[sec]
t_sim_end = 20;   %シミュレーション時間
tsim = 0:Ts:t_sim_end;   %シミュレーションの基準となる時刻

%コントローラーのハイパーパラメーター
cgmres.ht = Ts;    %前進差分近似の時間幅[sec]
cgmres.zeta = 1/Ts; %操作量の安定化ゲインζ
cgmres.tf = 1.0;      %予測時間の最終長さ[sec]
cgmres.alpha = 0.5;  %予測時間の上昇ゲイン
cgmres.N = 20;  %予測区間の分割数

% 評価関数中の重み
cgmres.Q = 100 * [1 0 0; 0 1 0; 0 0 0];
cgmres.R = 1*eye(2);
cgmres.S = 100 * [1 0 0; 0 1 0; 0 0 0];

%車のパラメーター
car.R = 0.05; %車輪半径[m]
car.T= 0.2; %車輪と車輪の間の幅(車体の横幅)[m]
car.r = car.R/2; %よく使う値を先に計算
car.rT = car.R/car.T;


% 初期条件
car.x = [0;0;0];
cgmres.u = [0;0];

car.len_x = length(car.x);
cgmres.len_u = length(cgmres.u);

cgmres.U = zeros(cgmres.len_u*cgmres.N,1);    %コントローラに与える初期操作量
cgmres.dU = 0;

%目標地点
cgmres.x_ob = [3,2,0]';

%拘束条件
cgmres.umax = [15,15];
cgmres.umin = [-15,-15];

% シミュレーション(ode45)の設定
opts = odeset('RelTol',1e-6,'AbsTol',1e-8);


%描画範囲
area=[-0.2 3 -0.2 3];

tic;
%シミュレーションループ
for i = 1:length(tsim)

    log.u(:,i) = cgmres.u;
    log.U(:,i) = cgmres.U;
    log.x(:,i) = car.x;
    car.x
    
    %シミュレーション計算（最後だけは計算しない）
    if i ~= length(tsim)
        [t,xi] = ode45( @(t,xi) two_wheel_car(t,xi,cgmres.u,car.R,car.T),...
            [tsim(i) tsim(i+1)], car.x, opts);
        %xi = car.x + func(car.x,cgmres.u,car.R,car.T)*Ts;
    else
        break
    end

    %予測ホライズンを計算
    Tl = cgmres.tf*(1-exp(-cgmres.alpha*Ts*i));

    %平衡状態で必要な入力を計算するがこの条件では影響が無いので計算しない
    
    %IandIを使った適応C/GMRES法コントローラーを関数として実装
    [U, dU] = CGMRES_control(car,cgmres,Tl);

    cgmres.u = U(1:2);
    cgmres.U = U;
    cgmres.dU = dU;

    
    car.x_preb = car.x;
    car.x = xi(end,:)';
    %car.x = xi;

end
toc;


%グラフ化
f=figure();

subplot(5,1,1)
plot(tsim,log.x(1,:),'LineWidth',1); ylabel('x1'); xlabel('Time[s]');
ylim([0,3])
%yticks(0:1:3)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,2);
plot(tsim,log.x(2,:),'LineWidth',1); ylabel('x2'); xlabel('Time[s]');
ylim([0,2])
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,3);
plot(tsim,log.x(3,:),'LineWidth',1); ylabel('Φ'); xlabel('Time[s]');
%yticks(0:0.1:0.7)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,4);
plot(tsim,log.u(1,:),'LineWidth',1); ylabel('u1'); xlabel('Time[s]');
%yticks(0:1:11)
grid on;
set(gca, 'FontSize', 9.5)
subplot(5,1,5);
plot(tsim,log.u(2,:),'LineWidth',1); ylabel('u2'); xlabel('Time[s]');
%yticks(0:1:10)
grid on;
set(gca, 'FontSize', 9.5)



%C/GMRES法コントローラー
function [U, dU] = CGMRES_control(car,cgmres,Tl)
    
    dt = Tl/cgmres.N; %制御ホライズン内での1ステップを計算
    car.dx = func(car.x,cgmres.u,car.R,car.T);

    %Fの各種を計算
    Fux = CalcF(cgmres.U+cgmres.ht*cgmres.dU, car.x+cgmres.ht*car.dx, cgmres, car, dt);
    Fx = CalcF(cgmres.U, car.x+cgmres.ht*car.dx, cgmres, car, dt);
    F = CalcF(cgmres.U, car.x, cgmres, car, dt);

    %クリロフ部分空間の最初の基底を求める
    left = (Fux - Fx)/cgmres.ht;
    right = -cgmres.zeta*F - ((Fx-F)/cgmres.ht);
    r0 = right - left;
    
    % GMRESの繰り返し回数(基底ベクトルの数)
    m = cgmres.len_u * cgmres.N;

    Vm = zeros(cgmres.len_u*cgmres.N,m+1);
    %1つ目の基底ベクトル
    Vm(:,1) = r0 ./ norm(r0);

    % Arnoldi process(Hm_を作成する)
    Hm = zeros(m+1,m);
    
    %Arnoldi法
    for j=1:m
        Fux = CalcF(cgmres.U + ( Vm(:,j) * cgmres.ht ) , ...
                    car.x + (car.dx * cgmres.ht),cgmres,car,dt);        
        Av = ( ( Fux - Fx ) / cgmres.ht );    %Fxtは関数Fの状態微分
        
        %Line 3
        for k=1:j
            Hm(k,j)=Av'*Vm(:,k);
        end    
        
        %Line 4
        temp_vec = 0;
        for k = 1:j
            temp_vec = temp_vec + Hm(k,j).*Vm(:,k);
        end
        
        v_hat = Av - temp_vec;
        
        %Line 5
        Hm(j+1,j) = norm(v_hat);
        
        Vm(:,j+1) = v_hat ./ Hm(j+1,j);
    end

    [UTMat,Omega] = ToUTMat(Hm);
    
    e = zeros( k + 1, 1 );
    e(1) = 1;   
    gm_ = norm(r0)*e;
    for k=1:length(Omega)
        gm_ = Omega{k}*gm_;
    end

    min_y = zeros(length(UTMat)-1,1);   %解の保存先

    for k=length(UTMat)-1 :-1:1
        min_y(k) = (gm_(k) - UTMat(k,:) * min_y)/UTMat(k,k);
    end

    du_new = cgmres.dU + Vm(:,1:m)*min_y;
    
    dU = du_new;
    U = cgmres.U + dU .* cgmres.ht;

end


%Fの計算
function F = CalcF(U,x,cgmres,car,dt)

    %Uを成形
    U_temp = reshape(U,[cgmres.len_u,cgmres.N]);
    
    %xの予測計算＆Bの値の再利用
    [X, B_all] = Forward(x, U_temp, dt, car, cgmres);
   
    %随伴変数の計算
    Lambda = Backward(X, U_temp, dt, car, cgmres);

    %Fのサイズを定める
    F = zeros(cgmres.len_u*cgmres.N,1);

    %Fの算出
    for i=1:cgmres.N
        F(cgmres.len_u*(i-1)+1:cgmres.len_u*i) = CalcHu(U_temp(:,i), Lambda(:,i), cgmres.R, B_all(:,cgmres.len_u*(i-1)+1:cgmres.len_u*i),cgmres.umax,cgmres.umin);
    end
end


%xの予測計算
function [X, B_all] = Forward(x,U,dt,car,cgmres)
    
    X = zeros(car.len_x,cgmres.N+1); %Xのサイズを定義
    B_all = zeros(car.len_x,cgmres.len_u*(cgmres.N)); %B_allのサイズを定義

    X(:,1) = x; 

    for i = 2:cgmres.N+1
        [dx, B] = funcB(X(:,i-1),U(:,i-1),car.r,car.rT);
        X(:,i) = X(:,i-1) + dt*dx;
        B_all(:,cgmres.len_u*(i-2)+1:cgmres.len_u*(i-1)) = B;
    end
end


%随伴変数の計算
function Lambda = Backward(X, U, dt, car, cgmres)

    Lambda = zeros(car.len_x, cgmres.N);
    Lambda(:,cgmres.N) = cgmres.S*(X(:,cgmres.N+1)-cgmres.x_ob);

    for i = cgmres.N-1:-1:1
        Lambda(:,i) = Lambda(:,i+1) + CalcHx(X(:,i+1),cgmres.x_ob,U(:,i+1),Lambda(:,i+1),cgmres.Q,car.R)*dt;
    end
end


%dH/du(の転置)
function dHdu = CalcHu(u,lambda,R,B,umax,umin)
    if u(1) < umax(1) && u(1) > umin(1) && u(2) < umax(2) && u(2) > umin(2)
        dHdu = R*u + B'*lambda...
            + 0.15*[(2*u(1)-umax(1)-umin(1))*((u(1)-umin(1))*(umax(1)-u(1)))^(-1)
            (2*u(2)-umax(2)-umin(2))*((u(2)-umin(2))*(umax(2)-u(2)))^(-1)];
    else
        dHdu = R*u + B'*lambda...
            + 1*[sign(u(1))
            sign(u(2))];
            %+ 0.5*[2*u(1)-umax(1)-umin(1)
            %2*u(2)-umax(2)-umin(2)];
    end
end


%dH/dx(の転置)
function dHdx = CalcHx(x,x_ob,u,lambda,Q,car_R)
    dfdx = Calcfx(x,u,car_R);
    dHdx = Q*(x-x_ob) + dfdx'*lambda;
end


%df/dx(状態方程式の状態変数微分)
function dfdx = Calcfx(x,u,car_R)
    dfdx = [0 0 -(car_R/2)*sin(x(3))*(u(1)+u(2))
        0 0 (car_R/2)*cos(x(3))*(u(1)+u(2)); 0 0 0];
end


function [UTMat,Omega] = ToUTMat(H)
%UNTITLED HをGivens回転でもって上三角行列に変換する
    m = length(H)-1;  %Givens回転する回数
    
    for i=1:m
        nu = sqrt(H(i,i)^2 + H(i+1,i)^2);
        
        c_i = H(i,i)/nu;
        s_i = H(i+1,i)/nu;
        
        Omega{i} = diag(ones(m+1,1));
        Omega{i}(i:i+1,i:i+1) = [c_i,s_i
                                -s_i,c_i];
                            
        H = Omega{i} * H;        
    end
    
    UTMat = H;
end


%差分駆動型二輪車のモデル(ode用)
function dxi = two_wheel_car(t,xi,u,car_R,T)
    dxi = zeros(3,1); %dxiの型を定義
    r = car_R/2;
    rT = car_R/T;
    cos_ = cos(xi(3));
    sin_ = sin(xi(3));
    dxi(1) = r*cos_*u(1) + r*cos_*u(2);
    dxi(2) = r*sin_*u(1) + r*sin_*u(2);
    dxi(3) = rT*u(1) - rT*u(2);
end


%二輪車の状態方程式
function dx = func(xi,u,car_R,T)
    dx = zeros(3,1); %dxiの型を定義
    r = car_R/2;
    rT = car_R/T;
    cos_ = cos(xi(3));
    sin_ = sin(xi(3));
    dx(1) = r * cos_ * (u(1) + u(2));
    dx(2) = r * sin_ * (u(1) + u(2));
    dx(3) = rT * (u(1) - u(2));
end


%二輪車の状態方程式(CalcFで使う用)
function [dx,B] = funcB(xi,u,car_r,car_rT)
    cos_ = cos(xi(3));
    sin_ = sin(xi(3));
    B = [car_r*cos_ car_r*cos_; car_r*sin_ car_r*sin_; car_rT -car_rT];
    dx = B*u;
end

Python

import math
import numpy as np
import time

#移動ロボットクラス
class car:
    def __init__(self):
        self.R = 0.05
        self.T = 0.2
        self.r = self.R/2
        self.rT = self.R/self.T

    def func(self,u,x):
        cos_ = math.cos(x[2][0])
        sin_ = math.sin(x[2][0])
        dx = np.array([[self.r*cos_*(u[0][0]+u[1][0])],
                       [self.r*sin_*(u[0][0]+u[1][0])],
                       [self.rT*(u[0][0]-u[1][0])]])
        return dx
    

#コントローラークラス
class controller:
    def __init__(self, car, x_ob):
        #コントローラーのパラメータ
        self.Ts = 0.05 #制御周期
        self.ht = self.Ts 
        self.zeta = 1.0/self.Ts #安定化係数
        self.tf = 1.0 #予測ホライズンの最終値
        self.alpha = 0.5 #予測ホライズンの変化パラメータ
        self.N = 20 #予測ホライズンの分割数
        self.Time = 0.0 #時刻を入れる変数
        self.dt = 0.0 #予測ホライズンの分割幅

        #入力と状態の次元
        self.len_u = 2 #入力の次元
        self.len_x = 3 #状態変数の次元

        #評価関数の重み
        self.Q = np.array([[100, 0, 0],
                           [0, 100, 0],
                           [0, 0, 0]])
        self.R = np.array([[1, 0],
                           [0, 1]])
        self.S = np.array([[100, 0, 0],
                           [0, 100, 0],
                           [0, 0, 0]])
        
        #コントローラーの変数
        self.u = np.zeros((self.len_u, 1))
        self.U = np.zeros((self.len_u * self.N, 1))
        self.x = np.zeros((self.len_x, 1))
        self.dU = np.zeros((self.len_u * self.N, 1))

        #入力の制限
        self.umax = np.array([[15],
                              [15]]) #各入力の最大値
        self.umin = np.array([[-15],
                              [-15]]) #各入力の最小値
        
        #目標地点
        self.x_ob = x_ob

        #操縦する車
        self.car = car

    def CGMRES_control(self):
        self.dt = (1-math.exp(-self.alpha*self.Time))*self.tf/self.N
        dx = self.func(self.x, self.u)

        Fux = self.CalcF(self.x + dx*self.ht, self.U + self.dU*self.ht)
        Fx = self.CalcF(self.x + dx*self.ht, self.U)
        F = self.CalcF(self.x, self.U)

        left = (Fux - Fx)/self.ht
        right = -self.zeta*F - (Fx - F)/self.ht
        r0 = right - left

        m = self.len_u*self.N

        Vm = np.zeros((self.len_u*self.N, m+1))
        Vm[:,0:1] = r0/np.linalg.norm(r0)

        Hm = np.zeros((m+1,m))

        for i in range(m):
            Fux = self.CalcF(self.x + dx*self.ht, self.U + Vm[:,i:i+1]*self.ht)
            Av = (Fux - Fx)/self.ht

            for k in range(i+1):
                Hm[k][i] = np.matmul(Av.T,Vm[:,k:k+1])

            temp_vec = np.zeros((self.len_u*self.N, 1))
            for k in range(i+1):
                temp_vec = temp_vec + Hm[k][i]*Vm[:,k:k+1]

            v_hat = Av - temp_vec

            Hm[i+1][i] = np.linalg.norm(v_hat)
            Vm[:,i+1:i+2] = v_hat/Hm[i+1][i]

        e = np.zeros((m+1, 1))
        e[0][0] = 1.0
        gm_ = np.linalg.norm(r0)*e

        UTMat, gm_ = self.ToUTMat(Hm, gm_, m)

        min_y = np.zeros((m, 1))

        for i in range(m):
            min_y[i][0] = (gm_[i][0] - np.matmul(UTMat[i:i+1,:],min_y))/UTMat[i][i]

        dU_new = self.dU + np.matmul(Vm[:,0:m], min_y)

        self.dU = dU_new
        self.U = self.U + self.dU*self.ht
        self.u = self.U[0:2,0:1]


    #コントローラー側の運動方程式
    def func(self, x, u):
        cos_ = math.cos(x[2][0])
        sin_ = math.sin(x[2][0])
        dx = np.array([[self.car.r*cos_*(u[0][0]+u[1][0])],
                       [self.car.r*sin_*(u[0][0]+u[1][0])],
                       [self.car.rT*(u[0][0]-u[1][0])]])
        return dx
    
    #F
    def CalcF(self, x, U):
        F = np.zeros((self.len_u*self.N, 1))
        U = U.reshape(self.len_u, self.N, order='F')
        X, B_all = self.Forward(x, U)

        Lambda = self.Backward(X, U)

        for i in range(self.N):
            F[self.len_u*i:self.len_u*(i+1), 0:1] = self.CalcHu(U[:,i:i+1], Lambda[:,i:i+1], B_all[:,self.len_u*i:self.len_u*(i+1)])

        return F
    
    #xの予測計算
    def Forward(self, x, U):
        X = np.zeros((self.len_x, self.N+1))
        B_all = np.zeros((self.len_x, self.len_u*self.N))

        X[:,0:1] = x

        for i in range(1,self.N+1):
            dx, B = self.funcB(X[:,i-1:i], U[:,i-1:i])
            X[:,i:i+1] = X[:,i-1:i] + dx*self.dt
            B_all[:,self.len_u*(i-1):self.len_u*i] = B

        return X, B_all
    
    #随伴変数の計算
    def Backward(self, X, U):
        Lambda = np.zeros((self.len_x, self.N))
        Lambda[:,self.N-1:self.N] = np.matmul(self.S, X[:,self.N:self.N+1]-self.x_ob)

        for i in reversed(range(self.N-1)):
            Lambda[:,i:i+1] = Lambda[:,i+1:i+2] + self.CalcHx(X[:,i+1:i+2], U[:,i+1:i+2], Lambda[:,i+1:i+2])*self.dt
        
        return Lambda
    
    #dH/du
    def CalcHu(self, u, lambd, B):
        dHdu = np.zeros((self.len_u, 1))
        dHdu = np.matmul(self.R, u) + np.matmul(B.T, lambd)
        dHdu[0][0] += 0.15*(2*u[0][0] - self.umax[0][0] - self.umin[0][0])/((u[0][0] - self.umin[0][0])*(self.umax[0][0] - u[0][0]))
        dHdu[1][0] += 0.15*(2*u[1][0] - self.umax[1][0] - self.umin[1][0])/((u[1][0] - self.umin[1][0])*(self.umax[1][0] - u[1][0]))

        return dHdu
    
    #dHdx
    def CalcHx(self, x, u, lambd):
        dHdx = np.zeros((self.len_x, 1))
        dfdx = self.Calcfx(x,u)
        dHdx = np.matmul(self.Q, x-self.x_ob) + np.matmul(dfdx.T, lambd)
        return dHdx
    
    #dfdx
    def Calcfx(self, x, u):
        dfdx = np.array([[0, 0, -self.car.r*math.sin(x[2][0])*(u[0][0]+u[1][0])],
                         [0, 0, self.car.r*math.cos(x[2][0])*(u[0][0]+u[1][0])],
                         [0, 0, 0]])
        return dfdx
    
    #Givens回転
    def ToUTMat(self, Hm, gm, m):
        for i in range(m):
            nu = math.sqrt(Hm[i][i]**2 + Hm[i+1][i]**2)
            c_i = Hm[i][i]/nu
            s_i = Hm[i+1][i]/nu
            Omega = np.eye(m+1)
            Omega[i][i] = c_i
            Omega[i][i+1] = s_i
            Omega[i+1][i] = -s_i
            Omega[i+1][i+1] = c_i

            Hm = np.matmul(Omega, Hm)
            gm = np.matmul(Omega, gm)

        return Hm, gm
    
    #Forward用func
    def funcB(self, x, u):
        cos_ = math.cos(x[2][0])
        sin_ = math.sin(x[2][0])

        B = np.array([[self.car.r*cos_, self.car.r*cos_],
                      [self.car.r*sin_, self.car.r*sin_],
                      [self.car.rT, -self.car.rT]])
        
        dx = np.matmul(B, u)

        return dx, B
    

if __name__ == "__main__":
    x_ob = np.array([[3, 2, 0]]).T

    nonholo_car = car()
    CGMRES_cont = controller(nonholo_car, x_ob)

    Time = 0

    start = time.time()
    while Time <= 20:
        print("-------------Position-------------")
        print(CGMRES_cont.x)

        x = CGMRES_cont.x + nonholo_car.func(CGMRES_cont.u, CGMRES_cont.x)*CGMRES_cont.Ts

        CGMRES_cont.Time = Time + CGMRES_cont.Ts

        CGMRES_cont.CGMRES_control()

        Time += CGMRES_cont.Ts

        CGMRES_cont.x = x

    end = time.time()

    loop_time = end - start

    print("計算時間：{}[s]".format(loop_time))

C++

#include <iostream>
#include <math.h>
#include <time.h>
#include "Eigen/Dense"


/*非ホロノミック移動ロボットクラス*/
class car{
    public:
    /*車のサイズに関するパラメータ*/
    double R = 0.05;
    double T = 0.2;
    double r = R/2.0;
    double rT = R/T;

    /*車側の運動方程式（同じもの）*/
    Eigen::VectorXd func(const Eigen::Vector2d &u, const Eigen::Vector3d &x){
        Eigen::VectorXd dx(3);
        double cos_ = cos(x(2));
        double sin_ = sin(x(2));
        dx(0) = r*cos_*(u(0)+u(1));
        dx(1) = r*sin_*(u(0)+u(1));
        dx(2) = rT*(u(0)-u(1));
        return dx;
    }
};


/*コントローラークラス*/
class controller{
    public:
    /*コントローラーのパラメータ*/
    double Ts = 0.05; /*制御周期*/
    double ht = Ts;
    double zeta = 1.0/Ts; /*安定化係数*/
    double tf = 1.0; /*予測ホライズンの最終値*/
    double alpha = 0.5; /*予測ホライズンの変化のパラメータ*/
    int N = 20; /*予測ホライズンの分割数*/
    double Time = 0.0; /*時刻を入れる変数*/
    double dt = 0.0; /*予測ホライズンの分割幅*/

    /*入力と状態変数のサイズ*/
    int len_u = 2;
    int len_x = 3;

    /*評価関数中の重み*/
    Eigen::MatrixXd Q; /*途中の状態の重み*/
    Eigen::MatrixXd R; /*入力の重み*/
    Eigen::MatrixXd S; /*終端状態の重み*/
    
    /*コントローラーの変数及び関数値*/
    Eigen::VectorXd u; /*入力*/
    Eigen::VectorXd U; /*予測ホライズン内の全ての入力*/
    Eigen::VectorXd x; /*状態変数*/
    Eigen::VectorXd dU; /*予測ホライズン内の全ての入力の微分*/

    /*入力の制限*/
    Eigen::VectorXd umax = Eigen::VectorXd::Zero(len_u); /*入力最大値*/
    Eigen::VectorXd umin = Eigen::VectorXd::Zero(len_u); /*入力最小値*/

    /*目標地点*/
    Eigen::VectorXd x_ob;

    /*操縦する車*/
    car car_ob;

    controller(car car_ob_, Eigen::VectorXd x_ob_){
        Q = 100*Eigen::MatrixXd::Identity(len_x,len_x);
        Q(2,2) = 0.0;
        R = Eigen::MatrixXd::Identity(len_u,len_u);
        S = 100*Eigen::MatrixXd::Identity(len_x,len_x);
        S(2,2) = 0.0;
        u = Eigen::VectorXd::Zero(len_u);
        U = Eigen::VectorXd::Zero(len_u*N);
        x = Eigen::VectorXd::Zero(len_x);
        dU = Eigen::VectorXd::Zero(len_u*N);
        umax(0) = 15.0;
        umax(1) = 15.0;
        umin(0) = -15.0;
        umin(1) = -15.0;
        car_ob = car_ob_;
        x_ob = x_ob_;
    }

    void CGMRES_control(){
        dt = (1.0-exp(-alpha*Time))*tf/N; /*予測ホライズンの分割幅を更新*/
        Eigen::VectorXd dx = func(x,u);

        Eigen::VectorXd Fux = CalcF(x+dx*ht,U+ht*dU);
        Eigen::VectorXd Fx = CalcF(x+dx*ht,U);
        Eigen::VectorXd F = CalcF(x,U);

        Eigen::VectorXd left = (Fux - Fx)/ht;
        Eigen::VectorXd right = -zeta*F - (Fx - F)/ht;
        Eigen::VectorXd r0 = right - left;

        int m = len_u*N;

        Eigen::MatrixXd Vm(len_u*N,m+1);
        Vm.col(0) = r0/r0.norm();

        Eigen::MatrixXd Hm = Eigen::MatrixXd::Zero(m+1,m);

        for(int i=0; i<m; i++){
            Fux = CalcF(x+dx*ht,U+Vm.col(i)*ht);
            Eigen::VectorXd Av = (Fux - Fx)/ht;

            for(int k=0; k<=i; k++){
                Hm(k,i) = Av.dot(Vm.col(k));
            }

            Eigen::VectorXd temp_vec = Eigen::VectorXd::Zero(len_u*N);
            for(int k=0; k<=i; k++){
                temp_vec = temp_vec + Hm(k,i)*Vm.col(k);
            }

            Eigen::VectorXd v_hat = Av - temp_vec;

            Hm(i+1,i) = v_hat.norm();
            Vm.col(i+1) = v_hat/Hm(i+1,i);
        }

        Eigen::VectorXd e = Eigen::VectorXd::Zero(m+1);
        e(0) = 1.0;
        Eigen::VectorXd gm_ = r0.norm()*e;

        /*std::pair<Eigen::MatrixXd, Eigen::MatrixXd> UTgm =*/
        ToUTMat(Hm,gm_,m);
        Eigen::MatrixXd UTMat = Hm; /*UTgm.first;*/
        /*gm_ = UTgm.second;*/

        Eigen::VectorXd min_y = Eigen::VectorXd::Zero(m);

        for(int i=m-1; i>=0; i--){
            min_y(i) = (gm_(i) - UTMat.row(i)*min_y)/UTMat(i,i);
        }

        Eigen::VectorXd dU_new = dU + Vm.block(0,0,len_u*N,m)*min_y;

        dU = dU_new;
        U = U + dU*ht;
        u = U.segment(0,2);
    }

    /*コントローラーに与えられた運動方程式（モデル誤差無しと仮定しているので車側と同じ）*/
    Eigen::VectorXd func(const Eigen::Vector3d &x, const Eigen::Vector2d &u){
        Eigen::VectorXd dx(3);
        double cos_ = cos(x(2));
        double sin_ = sin(x(2));
        dx(0) = car_ob.r*cos_*(u(0)+u(1));
        dx(1) = car_ob.r*sin_*(u(0)+u(1));
        dx(2) = car_ob.rT*(u(0)-u(1));
        /*std::cout << dx << std::endl;*/
        return dx;
    }

    /*Fを計算する関数*/
    Eigen::VectorXd CalcF(const Eigen::VectorXd &x, const Eigen::VectorXd &U){
        Eigen::VectorXd F = Eigen::VectorXd::Zero(len_u*N);
        std::pair<Eigen::MatrixXd, Eigen::MatrixXd> XB = Forward(x,U);
        Eigen::MatrixXd X = XB.first;
        Eigen::MatrixXd B_all = XB.second;

        Eigen::MatrixXd Lambda = Backward(X, U);

        for(int i=0; i<N; i++){
            F.segment(len_u*i,len_u) = CalcHu(U.segment(len_u*i,len_u),Lambda.col(i),B_all.block(0,len_u*i,len_x,len_u));
        }
        /*std::cout << "CalcF OK" << std::endl;*/
        return F;
    }

    /*xの予測計算*/
    std::pair<Eigen::MatrixXd, Eigen::MatrixXd> Forward(const Eigen::VectorXd &x, const Eigen::VectorXd &U){
        Eigen::MatrixXd X(len_x, N+1);
        Eigen::MatrixXd B_all(len_x, len_u*N);
        X.col(0) = x;
        for(int i=1; i<=N; i++){
            std::pair<Eigen::VectorXd, Eigen::MatrixXd> dxB = funcB(X.col(i-1),U.segment(len_u*(i-1),len_u));
            Eigen::VectorXd dx = dxB.first;
            X.col(i) = X.col(i-1) + dt*dx;
            B_all.block(0,len_u*(i-1),len_x,len_u) = dxB.second;
        }
        std::pair<Eigen::MatrixXd, Eigen::MatrixXd> XB;
        XB.first = X;
        XB.second = B_all;
        /*std::cout << "Forward OK" << std::endl;*/
        return XB;
    }

    /*随伴変数の計算*/
    Eigen::MatrixXd Backward(const Eigen::MatrixXd &X, const Eigen::VectorXd &U){
        Eigen::MatrixXd Lambda(len_x,N);
        Lambda.col(N-1) = S*(X.col(N)-x_ob);
        for(int i=N-2; i>=0; i--){
            Lambda.col(i) = Lambda.col(i+1) + CalcHx(X.col(i+1),U.segment(len_u*(i+1),len_u),Lambda.col(i+1))*dt;
        }
        /*std::cout << "Backward OK" << std::endl;*/
        return Lambda;
    }

    /*dH/du*/
    Eigen::VectorXd CalcHu(const Eigen::VectorXd &u, const Eigen::VectorXd &lambda, const Eigen::MatrixXd &B){
        Eigen::VectorXd dHdu(len_u);
        dHdu = R*u + B.transpose()*lambda;
        dHdu(0) += 0.15*(2*u(0)-umax(0)-umin(0))/((u(0)-umin(0))*(umax(0)-u(0)));
        dHdu(1) += 0.15*(2*u(1)-umax(1)-umin(1))/((u(1)-umin(1))*(umax(1)-u(1)));
        /*std::cout << "CalcHu OK" << std::endl;*/
        return dHdu;
    }

    /*dH/dx*/
    Eigen::VectorXd CalcHx(const Eigen::VectorXd &x, const Eigen::VectorXd &u, const Eigen::VectorXd &lambda){
        Eigen::MatrixXd dfdx(len_x,len_x);
        Eigen::VectorXd dHdx(len_x);
        dfdx = Calcfx(x,u);
        dHdx = Q*(x-x_ob) + dfdx.transpose()*lambda;
        /*std::cout << "CalcHx OK" << std::endl;*/
        return dHdx;
    }

    /*df/dx*/
    Eigen::MatrixXd Calcfx(const Eigen::VectorXd &x, const Eigen::VectorXd &u){
        Eigen::MatrixXd dfdx(len_x,len_x);

        dfdx << 0.0, 0.0, -car_ob.r*sin(x(2))*(u(0)+u(1)),
        0.0, 0.0, car_ob.r*cos(x(2))*(u(0)+u(1)),
        0.0, 0.0, 0.0;
        
        /*std::cout << "Calcfx OK" << std::endl;*/
        return dfdx;
    }

    /*Givens回転*/
    /*std::pair<Eigen::MatrixXd, Eigen::VectorXd>*/
    void ToUTMat(Eigen::MatrixXd &H, Eigen::VectorXd &gm, const int &m){
        for(int i=0; i<m; i++){
            double nu = sqrt(std::pow(H(i,i),2.0)+std::pow(H(i+1,i),2.0));
            double c_i = H(i,i)/nu;
            double s_i = H(i+1,i)/nu;
            Eigen::MatrixXd Omega = Eigen::MatrixXd::Identity(m+1,m+1);
            Omega(i,i) = c_i;
            Omega(i,i+1) = s_i;
            Omega(i+1,i) = -s_i;
            Omega(i+1,i+1) = c_i;

            H =  Omega*H;
            gm = Omega*gm;
        }

        /*std::pair<Eigen::MatrixXd, Eigen::VectorXd> UTgm;
        UTgm.first = H;
        UTgm.second = gm;*/
        /*std::cout << "ToUTMat OK" << std::endl;*/
        /*return UTgm;*/
    }

    std::pair<Eigen::VectorXd, Eigen::MatrixXd> funcB(const Eigen::VectorXd &x, const Eigen::VectorXd &u){
        Eigen::VectorXd dx(len_x);
        Eigen::MatrixXd B(len_x,len_u);
        double cos_ = cos(x(2));
        double sin_ = sin(x(2));

        B << car_ob.r*cos_, car_ob.r*cos_,
        car_ob.r*sin_, car_ob.r*sin_,
        car_ob.rT, -car_ob.rT;
        
        dx = B*u;
        
        std::pair<Eigen::VectorXd, Eigen::MatrixXd> dxB;
        dxB.first = dx;
        dxB.second = B;
        /*std::cout << "funcB OK" << std::endl;*/
        return dxB;
    }
};



int main(){
    Eigen::VectorXd x(3);

    Eigen::VectorXd x_ob(3);
    x_ob << 3.0, 2.0, 0.0;

    car nonholo_car;
    controller CGMRES_cont(nonholo_car, x_ob);

    double time = 0;

    clock_t start = clock();
    while(time<=20){
        /*画面に表示*/
        /*std::cout << "-------------Inputs-------------\n" << CGMRES_cont.u << std::endl;*/
        std::cout << "-------------Position-------------\n" << CGMRES_cont.x << std::endl;

        /*次の位置を計算する（まだ代入しない）*/
        x = CGMRES_cont.x + nonholo_car.func(CGMRES_cont.u,CGMRES_cont.x)*CGMRES_cont.Ts;
        
        /*時計合わせ*/
        CGMRES_cont.Time = time + CGMRES_cont.Ts;
        
        /*制御入力を更新*/
        CGMRES_cont.CGMRES_control();
        
        /*時刻を進める*/
        time += CGMRES_cont.Ts;
        
        /*座標を更新*/
        CGMRES_cont.x = x;
    }
    clock_t end = clock();
    double loop_time = (double)(end - start) / CLOCKS_PER_SEC;
    printf("計算時間：%.2f [s]\n",loop_time);
    
    return 0;
}