Merge branch 'test_ex_singletrack'

Author: lucasrm25

GP.m

@@ -38,6 +38,10 @@
         var_w  % measurement noise covariance matrix. w ~ N(0,var_w)
     end
 
+    properties (SetAccess=public)
+        % is_d_active = false
+    end
+    
     methods (Abstract)
         xdot = f (obj, x, u)
         %------------------------------------------------------------------
@@ -196,7 +200,7 @@
 
             % evaluate disturbance
             [mu_d, var_d] = obj.d(z);
-            
+                
             % A) Mean Equivalent Approximation:
             var_x_d_w = blkdiag(var_xk, var_d, obj.var_w);
 

InvertedPendulumModel.m

@@ -44,6 +44,8 @@
 
             % add deffect
             xdot(3) = xdot(3) + (0.1 * x(3) - 0.01*x(4) + deg2rad(3)) *10;
+            
+            % xdot(2) = xdot(2) + ( -0.1 * x(1) + deg2rad(3));
         end
 
 %         function [xkp1, gradx_xkp1] = fd (obj, xk, uk, dt)

MotionModelGP.m

@@ -31,8 +31,8 @@
 %--------------------------------------------------------------------------
 
     properties
-        M    = 500*1.5      % vehicle mass
-        I_z  = 600*1.5      % vehicle moment of inertia (yaw axis)
+        M    = 500 *1.5      % vehicle mass
+        I_z  = 600 *1.5      % vehicle moment of inertia (yaw axis)
         g    = 9.81     % gravitation
         l_f  = 0.9      % distance of the front wheel to the center of mass 
         l_r  = 1.5      % distance of the rear wheel to the center of mass
@@ -45,21 +45,19 @@
 
 
         % Pacejka lateral dynamics parameters
-        c_f = 14000 * 1.3 % = 1*g*M/deltamax  % front coornering stiffness (C*delta=Fy~M*a)
-        c_r = 14000 * 1.3 % = 2*g*M/deltamax  % rear coornering stiffness
+        c_f = 14000 * 2.5 % = 1*g*M/deltamax  % front coornering stiffness (C*delta=Fy~M*a)
+        c_r = 14000 * 2.5 % = 2*g*M/deltamax  % rear coornering stiffness
     end
 
     properties(Constant)
         % keep in mind the dimensions:  xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
         % where z = [Bz_x*x;Bz_u*u] 
-        Bz_x = [0 0 0  1 0 0  0;
-                0 0 0  0 1 0  0;
-                0 0 0  0 0 1  0] 
+        Bz_x = [zeros(3), eye(3), zeros(3,1)] 
         Bz_u = [1 0 0;
                 0 1 0] 
         Bd = [zeros(3);
               eye(3); 
-              0 0 0];               
+              zeros(1,3)]
         n  = 7   % number of outputs x(t)
         m  = 3   % number of inputs u(t)
         nz = 5   % dimension of z(t)
@@ -75,7 +73,7 @@
             % call superclass constructor
             obj = obj@MotionModelGP(d,sigmaw);
             % report single track dynamics analysis
-            obj.analyseSingleTrack();
+            % obj.analyseSingleTrack();
 
             % add folder CODEGEN to path. Here there will be some functions
             % generated with the method generate_grad_functions()

MotionModelGP_InvPendulum_deffect.m

@@ -68,14 +68,12 @@
     properties(Constant)
         % keep in mind the dimensions:  xk+1 = fd(xk,uk) + Bd*(d(z)+w)),
         % where z = [Bz_x*x;Bz_u*u] 
-        Bz_x = [0 0 0  1 0 0  0;
-                0 0 0  0 1 0  0;
-                0 0 0  0 0 1  0] 
+        Bz_x = [zeros(3), eye(3), zeros(3,1)] 
         Bz_u = [1 0 0;
-                0 1 0]
+                0 1 0] 
         Bd = [zeros(3);
               eye(3); 
-              0 0 0];               
+              zeros(1,3)]               
         n  = 7   % number of outputs x(t)
         m  = 3   % number of inputs u(t)
         nz = 5   % dimension of z(t)
@@ -224,7 +222,6 @@
         end
 
         function testTyres(obj)
-            %%
             c_f = 14000; % = 1*g*M/deltamax  % front coornering stiffness (C*delta=Fy~M*a)
             c_r = 14000; % = 2*g*M/deltamax  % rear coornering stiffness
 
@@ -239,6 +236,16 @@ function testTyres(obj)
             D_r = 4000;             % peak value (Pacejka) (rear wheel)
             E_r = -0.5;             % curvature factor (Pacejka) (rear wheel)
 
+            B_f = 10.96;              % stiffnes factor (Pacejka) (front wheel)
+            C_f = 1.3;                % shape factor (Pacejka) (front wheel)
+            D_f = 4560.4;             % peak value (Pacejka) (front wheel)
+            E_f = -0.5;               % curvature factor (Pacejka) (front wheel)
+
+            B_r = 12.67;              % stiffnes factor (Pacejka) (rear wheel)
+            C_r = 1.3;                % shape factor (Pacejka) (rear wheel)
+            D_r = 3947.81;            % peak value (Pacejka) (rear wheel)
+            E_r = -0.5;               % curvature factor (Pacejka) (rear wheel)
+            
             a_r = deg2rad(-25:0.1:25);
             a_f = deg2rad(-25:0.1:25);
             W_Fy_r = D_r*sin(C_r*atan(B_r*a_r-E_r*(B_r*a_r -atan(B_r*a_r)))); % rear lateral force

MotionModelGP_SingleTrack_nominal.m

@@ -120,7 +120,7 @@
         end
 
 
-        function [u_opt, e_opt] = optimize(obj, x0, t0, r)
+        function [u_opt, e_opt] = optimize(obj, x0, t0, r, UseParallel)
         %------------------------------------------------------------------
         % Calculate first uptimal control input
         %------------------------------------------------------------------
@@ -144,7 +144,7 @@
                                    'Display','iter',...
                                    'Algorithm', 'interior-point',... % 'interior-point',... % 'sqp','interior-point'
                                    'SpecifyConstraintGradient',false,...
-                                   'UseParallel',false,... %'ConstraintTolerance',obj.tol,...
+                                   'UseParallel',UseParallel,... %'ConstraintTolerance',obj.tol,...
                                    'MaxIterations',obj.maxiter);
 
             % solve optimization problem