fixes #3

Author: ChoiJangho

lib/@CtrlAffineSys/ctrlCbfClfQp.m

@@ -53,7 +53,7 @@
             LfB + obj.params.cbf.rate * B];                
         % Add input constraints if u_max or u_min exists.
         if isfield(obj.params, 'u_max')
-            A = [A; ones(obj.udim), zeros(obj.udim, 1);];
+            A = [A; eye(obj.udim), zeros(obj.udim, 1);];
             if size(obj.params.u_max, 1) == 1
                 b = [b; obj.params.u_max * ones(obj.udim, 1)];
             elseif size(obj.params.u_max, 1) == obj.udim
@@ -63,7 +63,7 @@
             end
         end
         if isfield(obj.params, 'u_min')
-            A = [A; -ones(obj.udim), zeros(obj.udim, 1);];
+            A = [A; -eye(obj.udim), zeros(obj.udim, 1);];
             if size(obj.params.u_min, 1) == 1
                 b = [b; -obj.params.u_min * ones(obj.udim, 1)];
             elseif size(obj.params.u_min, 1) == obj.udim
@@ -79,7 +79,7 @@
             LfB + obj.params.cbf.rate * B];                
         % Add input constraints if u_max or u_min exists.
         if isfield(obj.params, 'u_max')
-            A = [A; ones(obj.udim)];
+            A = [A; eye(obj.udim)];
             if size(obj.params.u_max, 1) == 1
                 b = [b; obj.params.u_max * ones(obj.udim, 1)];
             elseif size(obj.params.u_max, 1) == obj.udim
@@ -89,7 +89,7 @@
             end
         end
         if isfield(obj.params, 'u_min')
-            A = [A; -ones(obj.udim)];
+            A = [A; -eye(obj.udim)];
             if size(obj.params.u_min, 1) == 1
                 b = [b; -obj.params.u_min * ones(obj.udim, 1)];
             elseif size(obj.params.u_min, 1) == obj.udim

lib/@CtrlAffineSys/ctrlCbfQp.m

@@ -36,7 +36,7 @@
     b = [LfB + obj.params.cbf.rate * B];                
     % Add input constraints if u_max or u_min exists.
     if isfield(obj.params, 'u_max')
-        A = [A; ones(obj.udim)];
+        A = [A; eye(obj.udim)];
         if size(obj.params.u_max, 1) == 1
             b = [b; obj.params.u_max * ones(obj.udim, 1)];
         elseif size(obj.params.u_max, 1) == obj.udim
@@ -46,7 +46,7 @@
         end
     end
     if isfield(obj.params, 'u_min')
-        A = [A; -ones(obj.udim)];
+        A = [A; -eye(obj.udim)];
         if size(obj.params.u_min, 1) == 1
             b = [b; -obj.params.u_min * ones(obj.udim, 1)];
         elseif size(obj.params.u_min, 1) == obj.udim

lib/@CtrlAffineSys/ctrlClfQp.m

@@ -47,7 +47,7 @@
         b = -LfV-obj.params.clf.rate*V;                
         % Add input constraints if u_max or u_min exists.
         if isfield(obj.params, 'u_max')
-            A = [A; ones(obj.udim), zeros(obj.udim, 1);];
+            A = [A; eye(obj.udim), zeros(obj.udim, 1);];
             if size(obj.params.u_max, 1) == 1
                 b = [b; obj.params.u_max * ones(obj.udim, 1)];
             elseif size(obj.params.u_max, 1) == obj.udim
@@ -57,7 +57,7 @@
             end
         end
         if isfield(obj.params, 'u_min')
-            A = [A; -ones(obj.udim), zeros(obj.udim, 1);];
+            A = [A; -eye(obj.udim), zeros(obj.udim, 1);];
             if size(obj.params.u_min, 1) == 1
                 b = [b; -obj.params.u_min * ones(obj.udim, 1)];
             elseif size(obj.params.u_min, 1) == obj.udim
@@ -72,7 +72,7 @@
         b = -LfV-obj.params.clf.rate*V;                
         % Add input constraints if u_max or u_min exists.
         if isfield(obj.params, 'u_max')
-            A = [A; ones(obj.udim)];
+            A = [A; eye(obj.udim)];
             if size(obj.params.u_max, 1) == 1
                 b = [b; obj.params.u_max * ones(obj.udim, 1)];
             elseif size(obj.params.u_max, 1) == obj.udim
@@ -82,7 +82,7 @@
             end
         end
         if isfield(obj.params, 'u_min')
-            A = [A; -ones(obj.udim)];
+            A = [A; -eye(obj.udim)];
             if size(obj.params.u_min, 1) == 1
                 b = [b; -obj.params.u_min * ones(obj.udim, 1)];
             elseif size(obj.params.u_min, 1) == obj.udim