Updated balancing mode and hybrid SF model

load_params.m

 clear prms
 clear rpi
 
+setenv('TMPDIR','/home/simon/project/');
+
 %% PARAMETERS
 
 prms.g = 9.807;         % gravitational acceleration

m/eqn/balancing_mode/bal_ctrl.m → m/eqn/balancing_mode/bal_lin.m

@@ -39,7 +39,10 @@ B = diff(f, tau);
 B = double(subs(B,{'theta','Dtheta','d','Dd','tau'},{xp(1),xp(2),xp(3),xp(4),taup}));
 
 % matrix C
-C = diag(ones(4,1));
+C = [1 0 0 0;
+     0 0 0 0; 
+     0 0 1 0; 
+     0 0 0 0];
 
 % matrix D
 D = 0;

m/eqn/balancing_mode/bal_matrices.m → m/eqn/balancing_mode/bal_mat.m

-function [A, B] = bm_matrices(X, coefs)
+function [A, B] = bal_mat(X, coefs)
     
     % X = [theta; Dtheta; d; Dd]
     theta = X(1);

m/eqn/revolving_mode/rev_ctrl.m → m/eqn/revolving_mode/rev_lin.m

@@ -40,7 +40,10 @@ B = diff(f, tau);
 B = double(subs(B,{'theta','Dtheta','alpha','Dalpha','tau'},{xp(1),xp(2),xp(3),xp(4),taup}));
 
 % matrix C
-C = diag(ones(4,1));
+C = [1 0 0 0;
+     0 0 0 0; 
+     0 0 1 0; 
+     0 0 0 0];
 
 % matrix D
 D = 0;

m/eqn/revolving_mode/rev_matrices.m → m/eqn/revolving_mode/rev_mat.m

-function [A, B] = rm_matrices(X, coefs)
+function [A, B] = rev_mat(X, coefs)
     
     % x = [theta; Dtheta; alpha; Dalpha];
     theta  = X(1);