├── Digital_Control_Project_Proposal.pdf ├── Digital_Control_Project_Report.pdf ├── Outputs ├── TestTrack.mat ├── discreteMPCcontrolTraj.mat ├── finalTraj.jpg ├── finalTrajZoom.jpg ├── finalTrajZoomDiffInitialState.jpg ├── finalTrajZoomErrorInStates.jpg ├── initialTrack.jpg ├── initialTraj.jpg ├── initialTrajZoom.jpg └── proportionalControlTraj.mat ├── README.md ├── TestTrack.mat ├── discreteMPC.m ├── discreteMPCcontrolTraj.mat ├── initialTrajectoryGenerator.m ├── main.m ├── multiStepDynamics.m ├── proportionalControlTraj.mat └── singleStepDynamics.m /Digital_Control_Project_Proposal.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Digital_Control_Project_Proposal.pdf -------------------------------------------------------------------------------- /Digital_Control_Project_Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Digital_Control_Project_Report.pdf -------------------------------------------------------------------------------- /Outputs/TestTrack.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/TestTrack.mat -------------------------------------------------------------------------------- /Outputs/discreteMPCcontrolTraj.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/discreteMPCcontrolTraj.mat -------------------------------------------------------------------------------- /Outputs/finalTraj.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/finalTraj.jpg -------------------------------------------------------------------------------- /Outputs/finalTrajZoom.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/finalTrajZoom.jpg -------------------------------------------------------------------------------- /Outputs/finalTrajZoomDiffInitialState.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/finalTrajZoomDiffInitialState.jpg -------------------------------------------------------------------------------- /Outputs/finalTrajZoomErrorInStates.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/finalTrajZoomErrorInStates.jpg -------------------------------------------------------------------------------- /Outputs/initialTrack.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/initialTrack.jpg -------------------------------------------------------------------------------- /Outputs/initialTraj.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/initialTraj.jpg -------------------------------------------------------------------------------- /Outputs/initialTrajZoom.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/initialTrajZoom.jpg -------------------------------------------------------------------------------- /Outputs/proportionalControlTraj.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/Outputs/proportionalControlTraj.mat -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EECS561-Digital-Control-Project: Discrete MPC 2 | In this implementation, we attempt to control a car using trajectory optimization techniques. The car is assumed to follow the dynamics of a bicycle model. To achieve stable and safe tracking, we are using discrete-time Model Predictive Control. 3 | 4 | A discrete proportional controller (on the error defined as a function of the deviation from the center-line 5 | of the race-track) is used to generate an approximate trajectory that our car will follow. We will use MATLAB’s ode45 6 | solver to generate this trajectory. We will then use this trajectory to linearize our model and attempt to implement 7 | a discrete MPC controller to follow this trajectory. We will solve the MPC optimization problem using MATLAB’s 8 | quadprog. 9 | 10 | ## Usage instructions: 11 | Clone or download this repository 12 | In MATLAB, run main.m 13 | -------------------------------------------------------------------------------- /TestTrack.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/TestTrack.mat -------------------------------------------------------------------------------- /discreteMPC.m: -------------------------------------------------------------------------------- 1 | function finalTrajectoryFileName = discreteMPC(initialTrajectoryFileName,initialState) 2 | 3 | %% Inputs 4 | load(initialTrajectoryFileName); 5 | Y_ref = InitialTraj.states; 6 | U_ref = InitialTraj.inputs; 7 | 8 | % Adding 5% noise in reference trajectory (This doesn't test our MPC 9 | % controller since MPC tracks reference traj regardless of the accuracy of 10 | % reference trajectory) 11 | % Y_ref = Y_ref.*(1+randn(size(Y_ref))/20); 12 | % U_ref = U_ref.*(1+randn(size(U_ref))/20); 13 | 14 | %% Reduced-order Dynamic Model 15 | 16 | nstates = 5; 17 | ninputs = 1; 18 | 19 | % Vehicle Parameters 20 | a = 1.14; % distance c.g. to front axle (m) 21 | L = 2.54; % wheel base (m) 22 | m = 1500; % mass (kg) 23 | Iz = 2420.0; % yaw moment of inertia (kg-m^2) 24 | b=L-a; % distance of c.g to rear axel (m) 25 | g=9.81; 26 | u_0=8; 27 | 28 | % Tire forces 29 | B_tf=10; 30 | C_tf=1.3; 31 | D_tf=1; 32 | E_tf=0.97; 33 | 34 | % Timespan for all simulations 35 | dt = 0.01; 36 | T=0:0.01:size(U_ref,1)*dt; 37 | U_ref = U_ref'; 38 | Y_ref = Y_ref'; 39 | input_range = [-0.5, 0.5]; 40 | 41 | % The front and rear cornerning stiffness 42 | Ca_r= (a*B_tf*C_tf*D_tf*m*g)/L; 43 | Ca_f= (b*B_tf*C_tf*D_tf*m*g)/L; 44 | 45 | % Linearizing the bicycle model about the trajectory generated using the P controller 46 | 47 | A = @(i)eye(5) + dt* [0, 0, -sin(Y_ref(5,i)), -Y_ref(2,i)*sin(Y_ref(5,i)) - Y_ref(4,i)*cos(Y_ref(5,i)), 0;... 48 | 0, 0, cos(Y_ref(5,i)), -Y_ref(4,i)*sin(Y_ref(5,i))+Y_ref(2,i)*cos(Y_ref(5,i)), 0;... 49 | 0, 0, 0, 0, 1;... 50 | 0, 0,-(Ca_r+a*Ca_f*cos(U_ref(1,i)))/(m*Y_ref(2,i)), 0, -((Ca_r*b + Ca_f*a*cos(U_ref(1,i)))/(m*Y_ref(2,i)))-Y_ref(1,i);... 51 | 0, 0,(Ca_r*b - a*Ca_f*cos(U_ref(1,i)))/(Iz*Y_ref(2,i)), 0, -((b^2*Ca_r)+ (a^2*Ca_f*cos(U_ref(1,i))))/(Iz*Y_ref(2,i))]; 52 | 53 | B = @(i) dt*[0;... 54 | 0;... 55 | Ca_f*(cos(U_ref(1,i))-U_ref(1,i)*sin(U_ref(1,i))+((Y_ref(4,i)+a*Y_ref(6,i))*sin(U_ref(1,i))/Y_ref(2,i)))/m;... 56 | 0;... 57 | a*Ca_f*(cos(U_ref(1,i))-U_ref(1,i)*sin(U_ref(1,i))+((Y_ref(4,i)+a*Y_ref(6,i))*sin(U_ref(1,i)))/Y_ref(2,i))]; 58 | 59 | %% Hyper-parameters 60 | 61 | npred = 10; % Number of time-steps in Prediction horizon 62 | Q = [10,10,0.5,500,1]; % state error 63 | R = 15; % control input 64 | 65 | %% Control Loop 66 | 67 | statesTranspose = initialState'; 68 | for i = 1:length(T)-1 69 | disp('##################################################'); 70 | i 71 | disp('##################################################'); 72 | 73 | npred1 = min([npred,(length(T)-i)]); 74 | Ndec1= (nstates*(npred1+1))+(ninputs*npred1); 75 | % adding noise to the state matrix 76 | % erroneousState = statesTranspose(1:nstates+1, i)+ initialState'.*(0.05*randn(size(initialState'))); 77 | % initcond(:,i) = erroneousState - Y_ref(1:nstates+1, i); 78 | initcond(:,i) = statesTranspose(1:nstates+1, i) - Y_ref(1:nstates+1, i); 79 | 80 | % beq = zeros([nstates*(npred1+1),1]); 81 | % Aeq = zeros([nstates*(npred1+1),(Ndec1)]); 82 | xsize = nstates*(npred1+1); 83 | 84 | stateindx = nstates+1:nstates:xsize; 85 | inputindx = xsize+1:ninputs:Ndec1; 86 | initialindx = i; 87 | 88 | [Aeq, beq] = eq_cons(A,B,stateindx,nstates,initialindx,ninputs,inputindx,npred1,Ndec1,initcond([1,3:6],i)); 89 | 90 | [Lb,Ub]= bound_cons(initialindx,U_ref,input_range,xsize,Ndec1,npred1,nstates); 91 | Lb = Lb'; 92 | Ub = Ub'; 93 | 94 | H= diag([repmat(Q,[1,npred1+1]), repmat(R,[1,npred1])]); 95 | f = zeros([Ndec1,1]); 96 | 97 | [Z, ~] = quadprog(H,f,[],[],Aeq,beq,Lb,Ub); 98 | U_mpc = Z(((nstates*(npred1+1))+1): ((nstates*(npred1+1))+ninputs)); 99 | inputs(:,i) = U_ref(:,i)- [U_mpc;0]; 100 | 101 | Y = singleStepDynamics(inputs(:,i),statesTranspose(1:nstates+1, i)'); 102 | Y = Y'; 103 | statesTranspose(1:nstates+1, i+1) = Y(1:nstates+1,end); 104 | end 105 | 106 | %% Outputs 107 | 108 | inputs = inputs'; 109 | states = multiStepDynamics(inputs, initialState); 110 | finalTrajectoryFileName = 'discreteMPCcontrolTraj.mat'; 111 | FinalTraj = struct('states',states,'inputs',inputs); 112 | save(finalTrajectoryFileName,'FinalTraj'); 113 | 114 | end 115 | 116 | %% Helper Functions 117 | 118 | function [Aeq1,beq1]=eq_cons(A,B,stateindx,nstates,initialindx,ninputs,inputindx,npred2,Ndec2,init_cond1) 119 | % Aeq*z=beq 120 | % initial_idx specifies the time index of initial condition from the reference trajectory 121 | % A and B are function handles above 122 | Aeq1(1:nstates,1:nstates)= eye(nstates); 123 | beq1(1:nstates) = init_cond1(1:nstates); 124 | beq1(nstates+1:(nstates*(npred2+1)))= zeros(((nstates*(npred2+1))-nstates),1); 125 | for i=1:npred2 126 | Aeq1(stateindx(i):stateindx(i)+nstates-1,stateindx(i):stateindx(i)+nstates-1)= -eye(nstates); 127 | Aeq1(stateindx(i):stateindx(i)+nstates-1,stateindx(i)-nstates:stateindx(i)-1)=A(initialindx + i -1); 128 | Aeq1(stateindx(i):stateindx(i)+nstates-1,inputindx(i):inputindx(i)+ninputs-1)=B(initialindx + i -1); 129 | end 130 | beq1 = beq1'; 131 | end 132 | 133 | function [Lb,Ub]=bound_cons(idx,U_ref,input_range,xsize,Ndec2,npred2,nstates) 134 | % initial_idx is the index along uref the initial condition is at 135 | Ub(1:nstates*(npred2+1)) = inf; 136 | Lb(1:nstates*(npred2+1)) = -inf; 137 | Ub(xsize+1:Ndec2)= input_range(1,2) - U_ref(1,idx:idx +npred2 -1); 138 | % Ub(xsize+2:2:Ndec2)= input_range(2,2) - U_ref(2,idx:idx +npred2 -1); 139 | Lb(xsize+1:Ndec2)= input_range(1,1)-U_ref(1,idx:idx +npred2 -1); 140 | % Lb(xsize+2:2:Ndec2)= input_range(2,1)-U_ref(2,idx:idx +npred2 -1); 141 | end 142 | -------------------------------------------------------------------------------- /discreteMPCcontrolTraj.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/discreteMPCcontrolTraj.mat -------------------------------------------------------------------------------- /initialTrajectoryGenerator.m: -------------------------------------------------------------------------------- 1 | function initialTrajectoryFileName = initialTrajectoryGenerator(trackDataFileName, initialState) 2 | 3 | %% Inputs 4 | 5 | load(trackDataFileName); 6 | leftLine = TestTrack.bl; 7 | rightLine = TestTrack.br; 8 | centerLine = TestTrack.cline; 9 | heading = TestTrack.theta; 10 | %n=10; 11 | n = size(TestTrack.theta,2); % number of coordinates provided in the test-track data 12 | 13 | %% Hyper-parameters (Control Parameters) 14 | 15 | kp = 0.85; 16 | kdist = 0.3; 17 | 18 | %% Control loop 19 | 20 | i=1; % control input iterator 21 | tp=1; % track point iterator 22 | currentState = initialState; 23 | 24 | while tp < n 25 | distToNextLine = distanceToLine([currentState(1),currentState(3),0], [leftLine(:,tp+1)',0], [rightLine(:,tp+1)',0] ); 26 | distToCentreLine = distanceToLine([currentState(1),currentState(3),0], [centerLine(:,tp)',0], [centerLine(:,tp+1)',0]); 27 | tempCrossProduct = cross( [centerLine(:,tp+1);0]-[centerLine(:,tp);0], [currentState(1);currentState(3);0]-[centerLine(:,tp);0] ); 28 | steer = sum(tempCrossProduct./norm(tempCrossProduct)); 29 | 30 | if distToNextLine<=1 31 | tp=tp+1; 32 | end 33 | 34 | % Control Inputs: delta_f and F 35 | error(i) = (currentState(5)-heading(tp)) + kdist*steer*distToCentreLine; 36 | delta_f = -kp*error(i); 37 | 38 | if currentState(2) < 8 39 | F = 300; 40 | else 41 | F = 75; 42 | end 43 | 44 | currentInput = [delta_f,F]; 45 | inputs(i,:) = currentInput; 46 | 47 | Y = singleStepDynamics(currentInput, currentState); 48 | currentState = Y(end,:); 49 | i=i+1; 50 | end 51 | 52 | %% Outputs 53 | 54 | states = multiStepDynamics(inputs, initialState); 55 | initialTrajectoryFileName = 'proportionalControlTraj'; 56 | InitialTraj = struct('states',states,'inputs',inputs); 57 | save(initialTrajectoryFileName,'InitialTraj'); 58 | 59 | end 60 | 61 | %% Helper functions 62 | 63 | function dist = distanceToLine (point, linePoint1, linePoint2) 64 | a = linePoint1-linePoint2; 65 | b = point-linePoint2; 66 | dist = norm(cross(a,b))/norm(a); 67 | end -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | clear 2 | close all 3 | clc 4 | 5 | trackDataFileName = 'TestTrack'; 6 | 7 | %% Proportional Control 8 | 9 | initialState = [287,5,-176,0,2,0]; 10 | initialTrajectoryFileName = initialTrajectoryGenerator(trackDataFileName,initialState); 11 | disp('Initial Trajectory Generated'); 12 | 13 | %% MPC 14 | 15 | % differentInitialState = initialState; % to keep the same initial state 16 | differentInitialState = [284,5,-180,0,2,0]; % to provide a different initial state 17 | finalTrajectoryFileName = discreteMPC(initialTrajectoryFileName,differentInitialState); 18 | disp('MPC Trajectory Generated'); 19 | 20 | %% Plots 21 | 22 | load(trackDataFileName); 23 | leftLine = TestTrack.bl; 24 | rightLine = TestTrack.br; 25 | 26 | load(initialTrajectoryFileName); 27 | initialTraj_X = InitialTraj.states(:,1); 28 | initialTraj_Y = InitialTraj.states(:,3); 29 | 30 | load(finalTrajectoryFileName); 31 | finalTraj_X = FinalTraj.states(:,1); 32 | finalTraj_Y = FinalTraj.states(:,3); 33 | 34 | figure(1) 35 | title('Initial Trajectory - Using Proportional Controller') 36 | hold on 37 | plot(initialTraj_X, initialTraj_Y, 'r','LineWidth',2); 38 | plot(leftLine(1,:), leftLine(2,:), 'k','LineWidth',1); 39 | plot(rightLine(1,:),rightLine(2,:),'k','LineWidth',1); 40 | legend('Trajectory','Left Border','Right Border','Location','NorthWest') 41 | hold off 42 | 43 | figure(2) 44 | title('Final Trajectory - Using MPC Controller') 45 | hold on 46 | plot(finalTraj_X, finalTraj_Y, 'r','LineWidth',2); 47 | plot(leftLine(1,:), leftLine(2,:), 'k','LineWidth',1); 48 | plot(rightLine(1,:),rightLine(2,:),'k','LineWidth',1); 49 | legend('Trajectory','Left Border','Right Border','Location','NorthWest') 50 | hold off 51 | 52 | figure(3) 53 | title('Comparing initial part of track') 54 | subplot(2,1,1); 55 | hold on 56 | plot(initialTraj_X, initialTraj_Y, 'r','LineWidth',2); 57 | plot(leftLine(1,:), leftLine(2,:), 'k','LineWidth',1); 58 | plot(rightLine(1,:),rightLine(2,:),'k','LineWidth',1); 59 | legend('Trajectory','Left Border','Right Border','Location','NorthWest') 60 | hold off 61 | subplot(2,1,2); 62 | hold on 63 | plot(finalTraj_X, finalTraj_Y, 'r','LineWidth',2); 64 | plot(leftLine(1,:), leftLine(2,:), 'k','LineWidth',1); 65 | plot(rightLine(1,:),rightLine(2,:),'k','LineWidth',1); 66 | legend('Trajectory','Left Border','Right Border','Location','NorthWest') 67 | hold off 68 | -------------------------------------------------------------------------------- /multiStepDynamics.m: -------------------------------------------------------------------------------- 1 | function Y = multiStepDynamics(U,x0) 2 | 3 | T=0:0.01:(size(U,1)-1)*0.01; 4 | [~,Y]=ode45(@(t,x)bikeMultiStep(t,x,T,U),T,x0); 5 | end 6 | 7 | function dzdt=bikeMultiStep(t,x,T,U) 8 | %constants 9 | Nw=2; 10 | f=0.01; 11 | Iz=2667; 12 | a=1.35; 13 | b=1.45; 14 | By=0.27; 15 | Cy=1.2; 16 | Dy=0.7; 17 | Ey=-1.6; 18 | Shy=0; 19 | Svy=0; 20 | m=1400; 21 | g=9.806; 22 | 23 | 24 | %generate input functions 25 | delta_f=interp1(T,U(:,1),t,'previous','extrap'); 26 | F_x=interp1(T,U(:,2),t,'previous','extrap'); 27 | 28 | %slip angle functions in degrees 29 | a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2))); 30 | a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2))); 31 | 32 | %Nonlinear Tire Dynamics 33 | phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy)); 34 | phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy)); 35 | 36 | F_zf=b/(a+b)*m*g; 37 | F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy; 38 | 39 | F_zr=a/(a+b)*m*g; 40 | F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy; 41 | 42 | F_total=sqrt((Nw*F_x)^2+(F_yr^2)); 43 | F_max=0.7*m*g; 44 | 45 | if F_total>F_max 46 | 47 | F_x=F_max/F_total*F_x; 48 | 49 | F_yr=F_max/F_total*F_yr; 50 | end 51 | 52 | %vehicle dynamics 53 | dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));... 54 | (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);... 55 | x(2)*sin(x(5))+x(4)*cos(x(5));... 56 | (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);... 57 | x(6);... 58 | (F_yf*a*cos(delta_f)-F_yr*b)/Iz]; 59 | end 60 | -------------------------------------------------------------------------------- /proportionalControlTraj.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VarunSatyadevShetty/EECS561_DigitalControls_Project/9d9bff540bcfdcc99c16dd876c501b1709e4a332/proportionalControlTraj.mat -------------------------------------------------------------------------------- /singleStepDynamics.m: -------------------------------------------------------------------------------- 1 | function Y = singleStepDynamics(U,x0) 2 | T=[0,0.01]; 3 | [~,Y]=ode45(@(t,x)bikeSingleStep(x,U),T,x0); 4 | end 5 | 6 | function dzdt=bikeSingleStep(x,U) 7 | %constants 8 | Nw=2; 9 | f=0.01; 10 | Iz=2667; 11 | a=1.35; 12 | b=1.45; 13 | By=0.27; 14 | Cy=1.2; 15 | Dy=0.7; 16 | Ey=-1.6; 17 | Shy=0; 18 | Svy=0; 19 | m=1400; 20 | g=9.806; 21 | 22 | delta_f = U(1); 23 | F_x = U(2); 24 | 25 | %slip angle functions in degrees 26 | a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2))); 27 | a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2))); 28 | 29 | %Nonlinear Tire Dynamics 30 | phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy)); 31 | phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy)); 32 | 33 | F_zf=b/(a+b)*m*g; 34 | F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy; 35 | 36 | F_zr=a/(a+b)*m*g; 37 | F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy; 38 | 39 | F_total=sqrt((Nw*F_x)^2+(F_yr^2)); 40 | F_max=0.7*m*g; 41 | 42 | if F_total>F_max 43 | 44 | F_x=F_max/F_total*F_x; 45 | 46 | F_yr=F_max/F_total*F_yr; 47 | end 48 | 49 | %vehicle dynamics 50 | dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));... 51 | (-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);... 52 | x(2)*sin(x(5))+x(4)*cos(x(5));... 53 | (F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);... 54 | x(6);... 55 | (F_yf*a*cos(delta_f)-F_yr*b)/Iz]; 56 | end 57 | --------------------------------------------------------------------------------