mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-28 20:33:14 +00:00
Re-Organize Files Again
- Deliverables folder holds final files for submission - Experimentation holds scripts and things related to our development of a final solution - Simulation holds the provided files - Added template for part 2 submission with comments
This commit is contained in:
Sravan Balaji
220
Experimentation/Main_Peter.m
Normal file
220
Experimentation/Main_Peter.m
Normal file
@@ -0,0 +1,220 @@
|
||||
%% ======================================================================
|
||||
% ROB 535 Control Project: Peerayos Pongsachai
|
||||
% clear all;clc; close all;
|
||||
load("TestTrack.mat");
|
||||
|
||||
% Objectives:
|
||||
% - Come up with an inequality constraint function that keeps the
|
||||
% center of mass inside the track limits for all timesteps [hard]
|
||||
% - Compute gradient of road inequality constraints [medium]
|
||||
X0= [287;5;-176;0;2;0];
|
||||
|
||||
% rng(0);
|
||||
n = 25;
|
||||
Pose = [randi([900,960],1,n);randi([440,520],1,n)];
|
||||
|
||||
xhat = 370; yhat = 140; Phat = [xhat;yhat];
|
||||
Pinit = [X0(1);X0(3)];
|
||||
Pend = [TestTrack.cline(1,end);TestTrack.cline(2,end)];
|
||||
[m,nPts] = size(TestTrack.cline);
|
||||
|
||||
% Plot Track
|
||||
figure;hold;
|
||||
plot(TestTrack.bl(1,:),TestTrack.bl(2,:), 'k')
|
||||
plot(TestTrack.cline(1,:),TestTrack.cline(2,:), '--k')
|
||||
plot(TestTrack.br(1,:),TestTrack.br(2,:), 'k')
|
||||
plot([TestTrack.bl(1,end),TestTrack.br(1,end)], ...
|
||||
[TestTrack.bl(2,end),TestTrack.br(2,end)], 'r')
|
||||
hold;
|
||||
|
||||
|
||||
d = vecnorm(TestTrack.cline - Pinit);
|
||||
eps = 1;
|
||||
Idx = find(d <= eps);
|
||||
|
||||
while(isempty(Idx))
|
||||
eps = eps + 1;
|
||||
Idx = find(d <= eps);
|
||||
end
|
||||
|
||||
idx = Idx(1);
|
||||
prev_idx = max(1-1, 1);
|
||||
next_idx = idx+1;
|
||||
|
||||
bl_vec = [TestTrack.bl(1,[prev_idx,next_idx]);TestTrack.bl(2,[prev_idx,next_idx])];
|
||||
br_vec = [TestTrack.br(1,[prev_idx,next_idx]);TestTrack.br(2,[prev_idx,next_idx])];
|
||||
|
||||
cp = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2));
|
||||
|
||||
% fprintf('Pose (%.2f, %.2f) - nearest point (%.2f, %.2f): %i \n', ...
|
||||
% Pose(1,i), Pose(2,i),TestTrack.cline(1,idx),TestTrack.cline(2,idx),cp)
|
||||
|
||||
hold;
|
||||
plot(Pinit(1),Pinit(2),'og')
|
||||
plot(Pend(1),Pend(2),'or')
|
||||
plot(TestTrack.cline(1,idx),TestTrack.cline(2,idx), 'o')
|
||||
plot(bl_vec(1,:),bl_vec(2,:), 'ob')
|
||||
plot(br_vec(1,:),br_vec(2,:), 'ob')
|
||||
hold;
|
||||
|
||||
|
||||
[LB, UB] = bounds(TestTrack, 10);
|
||||
size(LB)
|
||||
|
||||
[g,h,dg,dh]=nonlcon(z, TestTrack);
|
||||
|
||||
function cp = ontrack(pose, bl_prev, bl_next, br_prev, br_next)
|
||||
|
||||
l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1
|
||||
r_vec = br_next - br_prev; % r = r_i+1 - r_i-1
|
||||
|
||||
pl = pose - bl_prev; % p_l = p - l_i-1
|
||||
pr = pose - br_prev; % p_r = p - r_i-1
|
||||
|
||||
c1 = l_vec(1)*pl(2) - l_vec(2)*pl(1);
|
||||
c2 = r_vec(1)*pr(2) - r_vec(2)*pr(1);
|
||||
|
||||
cp = c1*c2;
|
||||
end
|
||||
|
||||
function dg = diff_g(pose, bl_prev, bl_next, br_prev, br_next)
|
||||
% syms Lx Ly Rx Ry ly lx rx ry X Y;
|
||||
% g = (Lx*(Y - ly)-Ly*(X-lx))*(Rx*(Y - ry)-Ry*(X-rx));
|
||||
% diff(g,X);
|
||||
% diff(g,Y);
|
||||
|
||||
dg = [0,0];
|
||||
l_vec = bl_next - bl_prev; % l = l_i+1 - l_i-1
|
||||
r_vec = br_next - br_prev; % r = r_i+1 - r_i-1
|
||||
|
||||
pl = pose - bl_prev; % p_l = p - l_i-1
|
||||
pr = pose - br_prev; % p_r = p - r_i-1
|
||||
|
||||
dg(1) = r_vec(2)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) + ...
|
||||
l_vec(2)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
|
||||
dg(2) = - r_vec(1)*(l_vec(2)*pl(1) - l_vec(1)*pl(2)) - ...
|
||||
l_vec(1)*(r_vec(2)*pr(1) - r_vec(1)*pr(2));
|
||||
end
|
||||
|
||||
function [lb, ub] = bounds(TestTrack, StepsPerPoint)
|
||||
|
||||
[m,nPts] = size(TestTrack.cline);
|
||||
% numState = 6;
|
||||
% numInput = 2;
|
||||
nsteps = StepsPerPoint * nPts;
|
||||
|
||||
lb_u = [-0.5;-5000];
|
||||
ub_u = [0.5;5000];
|
||||
|
||||
bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ...
|
||||
TestTrack.br(1,1), TestTrack.br(1,2)];
|
||||
bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ...
|
||||
TestTrack.br(2,1), TestTrack.br(2,2)];
|
||||
phi_init = TestTrack.theta(1);
|
||||
|
||||
lb = [min(bound_X); -Inf; min(bound_Y); -Inf; phi_init-(pi/2); -Inf];
|
||||
ub = [max(bound_X); Inf; max(bound_Y); Inf; phi_init+(pi/2); Inf];
|
||||
|
||||
|
||||
for i=1:nPts
|
||||
prev_idx = max(i-1, 1);
|
||||
next_idx = min(i+1, 246);
|
||||
|
||||
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
|
||||
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
|
||||
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
|
||||
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
|
||||
phi_init = TestTrack.theta(i);
|
||||
|
||||
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; phi_init-(pi/2); -Inf];
|
||||
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; phi_init+(pi/2); Inf];
|
||||
|
||||
for num = 1:StepsPerPoint
|
||||
lb=[lb;lb_x];
|
||||
ub=[ub;ub_x];
|
||||
end
|
||||
end
|
||||
|
||||
for i=1:nsteps
|
||||
ub=[ub;ub_u];
|
||||
lb=[lb;lb_u];
|
||||
end
|
||||
end
|
||||
|
||||
function [g,h,dg,dh]=nonlcon(z, TestTrack)
|
||||
if size(z,2)>size(z,1)
|
||||
z = z' ;
|
||||
end
|
||||
numState = 6;
|
||||
numInput = 2;
|
||||
numXandU = 8;
|
||||
nsteps = (size(z,1)+2)/numXandU;
|
||||
|
||||
dt = 0.01;
|
||||
|
||||
zx=z(1:nsteps*numState);
|
||||
zu=z(nsteps*numState + 1:end);
|
||||
|
||||
g = zeros(nsteps,1) ;
|
||||
dg = zeros(nsteps,numXandU*nsteps-2) ;
|
||||
|
||||
h = zeros(numState*nsteps,1) ;
|
||||
dh = zeros(numState*nsteps,numXandU*nsteps-2);
|
||||
|
||||
for i=1:nsteps
|
||||
% At given position (Xi, Yi) at Zi, find nearest centerline
|
||||
% Use the index of nearest centerline to calculate g function
|
||||
|
||||
% pos = [Xi Yi];
|
||||
car_pos = [z(numState*i-numState+1); z(numState*i-numState+3)];
|
||||
d = vecnorm(TestTrack.cline - car_pos);
|
||||
eps = 1;
|
||||
Idx = find(d <= eps);
|
||||
while(isempty(Idx))
|
||||
eps = eps + 1;
|
||||
Idx = find(d <= eps);
|
||||
end
|
||||
|
||||
idx = Idx(1);
|
||||
prev_idx = max(idx-1, 1);
|
||||
next_idx = min(idx+1, size(TestTrack.cline, 2));
|
||||
|
||||
bl_vec = [TestTrack.bl(1,[prev_idx,next_idx]);TestTrack.bl(2,[prev_idx,next_idx])];
|
||||
br_vec = [TestTrack.br(1,[prev_idx,next_idx]);TestTrack.br(2,[prev_idx,next_idx])];
|
||||
|
||||
g = ontrack(Pinit, bl_vec(:,1), bl_vec(:,2), br_vec(:,1), br_vec(:,2));
|
||||
|
||||
dgi_dzj = diff_g(pose, bl_prev, bl_next, br_prev, br_next);
|
||||
dg(i,numState*i-numState+1) = dgi_dzj(1);
|
||||
dg(i,numState*i-numState+2) = dgi_dzj(2);
|
||||
|
||||
% if i==1
|
||||
% h(1:3) = z(1:3,:) ;
|
||||
% dh(1:3,1:3)=eye(3);
|
||||
% else
|
||||
% h(3*i-2:3*i) = zx(3*i-2:3*i)-zx(3*i-5:3*i-3)-...
|
||||
% dt*odefun(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ;
|
||||
% dh(3*i-2:3*i,3*i-5:3*i) = [-eye(3)-dt*statepart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)),eye(3)] ;
|
||||
% dh(3*i-2:3*i,3*nsteps+2*i-3:3*nsteps+2*i-2) = -dt*inputpart(zx(3*i-5:3*i-3),zu(2*i-3:2*i-2)) ;
|
||||
% end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
221
Experimentation/part1_generate_trajectory.m
Normal file
221
Experimentation/part1_generate_trajectory.m
Normal file
@@ -0,0 +1,221 @@
|
||||
%% ROB 535 Team 3 Control Project
|
||||
close all;
|
||||
clear;
|
||||
clc;
|
||||
|
||||
%% Model Parameters
|
||||
% Vehicle Parameters
|
||||
delta_lim = [-0.5, 0.5];
|
||||
F_x_lim = [-5000, 5000];
|
||||
m = 1400;
|
||||
N_w = 2;
|
||||
f = 0.01;
|
||||
I_z = 2667;
|
||||
a = 1.35;
|
||||
b = 1.45;
|
||||
B_y = 0.27;
|
||||
C_y = 1.2;
|
||||
D_y = 0.7;
|
||||
E_y = -1.6;
|
||||
S_hy = 0;
|
||||
S_vy = 0;
|
||||
g = 9.806;
|
||||
% Note: The Pacejka parameters are for the slip angle in degrees
|
||||
|
||||
% Initial Conditions
|
||||
x_0 = 287; % meters
|
||||
u_0 = 5; % meters/second
|
||||
y_0 = -176; % meters
|
||||
v_0 = 0; % meters/second
|
||||
psi_0 = 2; % radians
|
||||
r_0 = 0; % radians/second
|
||||
state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0];
|
||||
|
||||
load('TestTrack.mat')
|
||||
|
||||
%% Trajectory Synthesis
|
||||
% Segment 1
|
||||
segment_num = 1;
|
||||
num_pts(segment_num) = 6e2;
|
||||
delta_vals(segment_num) = -0.004;
|
||||
F_x_vals(segment_num) = 3900;
|
||||
|
||||
% Segment 2
|
||||
segment_num = 2;
|
||||
num_pts(segment_num) = 4e2;
|
||||
delta_vals(segment_num) = -0.3;
|
||||
F_x_vals(segment_num) = -2000;
|
||||
|
||||
% Segment 3
|
||||
segment_num = 3;
|
||||
num_pts(segment_num) = 1e2;
|
||||
delta_vals(segment_num) = -0.05;
|
||||
F_x_vals(segment_num) = 0;
|
||||
|
||||
% Segment 4
|
||||
segment_num = 4;
|
||||
num_pts(segment_num) = 7.5e2;
|
||||
delta_vals(segment_num) = 0.0;
|
||||
F_x_vals(segment_num) = 1000;
|
||||
|
||||
% Segment 5
|
||||
segment_num = 5;
|
||||
num_pts(segment_num) = 3e2;
|
||||
delta_vals(segment_num) = 0.3;
|
||||
F_x_vals(segment_num) = -500;
|
||||
|
||||
% Segment 6
|
||||
segment_num = 6;
|
||||
num_pts(segment_num) = 3.5e2;
|
||||
delta_vals(segment_num) = -0.03;
|
||||
F_x_vals(segment_num) = 1000;
|
||||
|
||||
% Segment 7
|
||||
segment_num = 7;
|
||||
num_pts(segment_num) = 1e2;
|
||||
delta_vals(segment_num) = -0.005;
|
||||
F_x_vals(segment_num) = -1000;
|
||||
|
||||
% Segment 8
|
||||
segment_num = 8;
|
||||
num_pts(segment_num) = 2e2;
|
||||
delta_vals(segment_num) = 0.0275;
|
||||
F_x_vals(segment_num) = -750;
|
||||
|
||||
% Segment 9
|
||||
segment_num = 9;
|
||||
num_pts(segment_num) = 2.4e2;
|
||||
delta_vals(segment_num) = 0.5;
|
||||
F_x_vals(segment_num) = -500;
|
||||
|
||||
% Segment 10
|
||||
segment_num = 10;
|
||||
num_pts(segment_num) = 5e2;
|
||||
delta_vals(segment_num) = -0.02;
|
||||
F_x_vals(segment_num) = 0;
|
||||
|
||||
% Segment 11
|
||||
segment_num = 11;
|
||||
num_pts(segment_num) = 2.5e2;
|
||||
delta_vals(segment_num) = -0.05;
|
||||
F_x_vals(segment_num) = 500;
|
||||
|
||||
% Segment 12
|
||||
segment_num = 12;
|
||||
num_pts(segment_num) = 2e2;
|
||||
delta_vals(segment_num) = -0.01;
|
||||
F_x_vals(segment_num) = 5000;
|
||||
|
||||
% Segment 13
|
||||
segment_num = 13;
|
||||
num_pts(segment_num) = 2e2;
|
||||
delta_vals(segment_num) = -0.1;
|
||||
F_x_vals(segment_num) = -2000;
|
||||
|
||||
% Segment 14
|
||||
segment_num = 14;
|
||||
num_pts(segment_num) = 3e2;
|
||||
delta_vals(segment_num) = 0.175;
|
||||
F_x_vals(segment_num) = -2000;
|
||||
|
||||
% Segment 15
|
||||
segment_num = 15;
|
||||
num_pts(segment_num) = 4.75e2;
|
||||
delta_vals(segment_num) = 0.0025;
|
||||
F_x_vals(segment_num) = 1000;
|
||||
|
||||
% Segment 16
|
||||
segment_num = 16;
|
||||
num_pts(segment_num) = 4.5e2;
|
||||
delta_vals(segment_num) = 0.05;
|
||||
F_x_vals(segment_num) = 0;
|
||||
|
||||
% Segment 17
|
||||
segment_num = 17;
|
||||
num_pts(segment_num) = 5e2;
|
||||
delta_vals(segment_num) = 0.0;
|
||||
F_x_vals(segment_num) = 500;
|
||||
|
||||
% Segment 18
|
||||
segment_num = 18;
|
||||
num_pts(segment_num) = 8e2;
|
||||
delta_vals(segment_num) = -0.05;
|
||||
F_x_vals(segment_num) = -500;
|
||||
|
||||
% Segment 19
|
||||
segment_num = 19;
|
||||
num_pts(segment_num) = 5.8e2;
|
||||
delta_vals(segment_num) = 0.065;
|
||||
F_x_vals(segment_num) = 0;
|
||||
|
||||
% Segment 20
|
||||
segment_num = 20;
|
||||
num_pts(segment_num) = 7.5e2;
|
||||
delta_vals(segment_num) = 0;
|
||||
F_x_vals(segment_num) = 2000;
|
||||
|
||||
% Segment 21
|
||||
segment_num = 21;
|
||||
num_pts(segment_num) = 2.25e2;
|
||||
delta_vals(segment_num) = 0.5;
|
||||
F_x_vals(segment_num) = -4400;
|
||||
|
||||
% Segment 22
|
||||
segment_num = 22;
|
||||
num_pts(segment_num) = 9e2;
|
||||
delta_vals(segment_num) = 0.0;
|
||||
F_x_vals(segment_num) = 5000;
|
||||
|
||||
%% Load Inputs from File
|
||||
load('ROB535_ControlProject_part1_Team3.mat');
|
||||
|
||||
%% Simulate Trajectory
|
||||
for i = 1:length(num_pts)
|
||||
[start_idx, end_idx] = get_indices(i, num_pts);
|
||||
|
||||
delta = delta_vals(i);
|
||||
F_x = F_x_vals(i);
|
||||
U(start_idx:end_idx,:) = [delta * ones(num_pts(i),1), F_x * ones(num_pts(i),1)];
|
||||
end
|
||||
|
||||
[Y, T] = forwardIntegrateControlInput(U, state_0);
|
||||
info = getTrajectoryInfo(Y,U)
|
||||
|
||||
[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
|
||||
info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)
|
||||
|
||||
%% Figures
|
||||
% Plot segmented trajectory for debugging purposes
|
||||
figure(1)
|
||||
hold on;
|
||||
grid on;
|
||||
|
||||
for i = 1:length(num_pts)
|
||||
[start_idx, end_idx] = get_indices(i, num_pts);
|
||||
plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-');
|
||||
end
|
||||
|
||||
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '--r');
|
||||
plot(TestTrack.br(1,:), TestTrack.br(2,:), '--r');
|
||||
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '-.g');
|
||||
|
||||
% Plot final trajectory from submission inputs
|
||||
figure(2)
|
||||
hold on;
|
||||
grid on;
|
||||
|
||||
plot(Y_submission(:,1), Y_submission(:,3), '-b');
|
||||
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), '-r');
|
||||
plot(TestTrack.br(1,:), TestTrack.br(2,:), '-r');
|
||||
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), '--g');
|
||||
|
||||
%% Functions
|
||||
function [start_idx, end_idx] = get_indices(segment_num, num_pts)
|
||||
if segment_num == 1
|
||||
start_idx = 1;
|
||||
end_idx = num_pts(segment_num);
|
||||
else
|
||||
start_idx = sum(num_pts(1:segment_num-1)) + 1;
|
||||
end_idx = sum(num_pts(1:segment_num));
|
||||
end
|
||||
end
|
||||
BIN
Experimentation/reftrack_info.mat
Normal file
BIN
Experimentation/reftrack_info.mat
Normal file
Binary file not shown.
BIN
Experimentation/segments_info.mat
Normal file
BIN
Experimentation/segments_info.mat
Normal file
Binary file not shown.
206
Experimentation/xenia_final_project.m
Normal file
206
Experimentation/xenia_final_project.m
Normal file
@@ -0,0 +1,206 @@
|
||||
%Vehicle Parameterrs
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
Iz=2667;
|
||||
a=1.35;
|
||||
b=1.45;
|
||||
By=0.27;
|
||||
Cy=1.2;
|
||||
Dy=0.7;
|
||||
Ey=-1.6;
|
||||
Shy=0;
|
||||
Svy=0;
|
||||
m=1400;
|
||||
g=9.806;
|
||||
|
||||
|
||||
%note constraints on input
|
||||
%delta [-0.5, 0.5]
|
||||
%Fx [-5000, 5000]
|
||||
|
||||
init = [287, 5, -176, 0, 2, 0];
|
||||
curr_xy = [287, -176];
|
||||
U_try = [0.4, 5000];
|
||||
|
||||
for i = 1:12
|
||||
U_try = [U_try; U_try];
|
||||
end
|
||||
U_final = [];
|
||||
U_try = [0.4, 5000; 0.4, 5000];
|
||||
[Y, T] = forwardIntegrateControlInput(U_try);
|
||||
traj_inf = getTrajectoryInfo(Y, U_try);
|
||||
%traj_inf.t_finished
|
||||
|
||||
init = [287, 5, -176, 0, 2, 0];
|
||||
U_try = [0.25, 2500; 0.25, 2500; 0.25, 2500];
|
||||
noise = [randn*0.01, randn*10];
|
||||
U_try = U_try + noise;
|
||||
[Y, T] = forwardIntegrateControlInput(U_try);
|
||||
traj_inf = getTrajectoryInfo(Y, U_try);
|
||||
U_final = U_try;
|
||||
|
||||
max_percent = 0;
|
||||
num_iter = 0;
|
||||
subt = 0;
|
||||
randomize = 0;
|
||||
percent_comp = 0;
|
||||
avgcount = [];
|
||||
while (size(traj_inf.t_finished) == 0)
|
||||
%for i = 1:100
|
||||
noise = [randn*0.01, randn*10];
|
||||
U_try = U_try + noise;
|
||||
[Y_temp, T] = forwardIntegrateControlInput(U_try, Y(end,:));
|
||||
traj_inf = getTrajectoryInfo([Y;Y_temp], [U_final; U_try]);
|
||||
|
||||
|
||||
if mod(num_iter+1, 11) == 0 | traj_inf.percent_of_track_completed < (percent_comp - 0.0001)
|
||||
randomize = 1;
|
||||
end
|
||||
%if crashes, redo, if not add on and keep going
|
||||
%also will allow it to change inputs once in a while for randomization
|
||||
|
||||
%next thing to onsider trying is the intersectLineSegment thing and
|
||||
%following centerline
|
||||
count = 1;
|
||||
factor = 3;
|
||||
while size(traj_inf.left_track_position) ~= 0 | randomize == 1
|
||||
if subt == 0
|
||||
U_og = U_try;
|
||||
U_try = U_try + [0.011, 250];
|
||||
if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
|
||||
subt = 1;
|
||||
end
|
||||
if U_try(1,1) > 0.45
|
||||
U_try = U_try - [0.05, 0];
|
||||
end
|
||||
if U_try(1,2) > 4500
|
||||
U_try = U_try - [0, 660];
|
||||
end
|
||||
noise = [randn*0.01, randn*10];
|
||||
U_try = U_try + noise;
|
||||
end
|
||||
|
||||
if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
|
||||
subt = 1;
|
||||
[Y_comp1, T] = forwardIntegrateControlInput(U_try, Y(end,:));
|
||||
U_try = U_og;
|
||||
end
|
||||
|
||||
|
||||
if subt == 1
|
||||
if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
|
||||
subt = 0;
|
||||
end
|
||||
U_try = U_try - [0.011, 250];
|
||||
if U_try(1,1) < -0.45
|
||||
U_try = U_try + [0.05, 0];
|
||||
end
|
||||
if U_try(1,2) < -4500
|
||||
U_try = U_try + [0, 660];
|
||||
end
|
||||
noise = [randn*0.01, randn*10];
|
||||
U_try = U_try + noise;
|
||||
end
|
||||
|
||||
if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
|
||||
[Y_comp2, T] = forwardIntegrateControlInput(U_try, Y(end,:));
|
||||
|
||||
|
||||
for i = 1:(246/3)
|
||||
dist1 = norm([Y_comp1(:,1)'; Y_comp1(:,3)'] - TestTrack.cline(:,i:i+2));
|
||||
dist2 = norm([Y_comp2(:,1)'; Y_comp2(:,3)'] - TestTrack.cline(:,i:i+2));
|
||||
|
||||
diff1 = min(dist1);
|
||||
diff2 = min(dist2);
|
||||
|
||||
if diff1 < 20 | diff2 < 20
|
||||
break
|
||||
end
|
||||
end
|
||||
|
||||
if diff1 > diff2
|
||||
subt = 0;
|
||||
end
|
||||
if diff2 > diff1
|
||||
subt = 1;
|
||||
end
|
||||
|
||||
U_try = U_og;
|
||||
if subt == 0
|
||||
U_try = U_try + [0.011, 250];
|
||||
% if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
|
||||
% subt = 1;
|
||||
% end
|
||||
if U_try(1,1) > 0.5
|
||||
U_try = U_try - [0.05, 0];
|
||||
end
|
||||
if U_try(1,2) > 4000
|
||||
U_try = U_try - [0, 760];
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
if subt == 1
|
||||
% if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
|
||||
% subt = 0;
|
||||
% end
|
||||
U_try = U_try - [0.011, 250];
|
||||
if U_try(1,1) < -0.5
|
||||
U_try = U_try + [0.05, 0];
|
||||
end
|
||||
if U_try(1,2) < -4000
|
||||
U_try = U_try + [0, 760];
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
||||
%instead we'll go back one step and so we'll send 6 commands and
|
||||
%check
|
||||
|
||||
if mod(count,14) == 0
|
||||
factor = factor + 3;
|
||||
end
|
||||
|
||||
if factor > 5
|
||||
enteredifstatementon166 = 1
|
||||
break
|
||||
break
|
||||
end
|
||||
|
||||
U_try(:,1) = min(max(U_try(:,1), -0.5), 0.5);
|
||||
U_try(:,2) = min(max(U_try(:,2), -5000), 5000);
|
||||
U_try2 = U_try;
|
||||
|
||||
for j = 1:(factor/3)
|
||||
U_try2 = [U_try2; U_try];
|
||||
end
|
||||
|
||||
[Y_temp, T] = forwardIntegrateControlInput(U_try2, Y(end-factor,:));
|
||||
traj_inf = getTrajectoryInfo([Y(1:end-factor,:);Y_temp], [U_final(1:end-factor,:); U_try2]);
|
||||
|
||||
if size(traj_inf.left_track_position) == 0
|
||||
U_final = U_final(1:end-factor,:);
|
||||
Y = Y(1:end-factor,:);
|
||||
end
|
||||
count = count + 1;
|
||||
randomize = 0;
|
||||
end
|
||||
|
||||
if count ~= 1
|
||||
avgcount = [avgcount ; count];
|
||||
end
|
||||
|
||||
if (traj_inf.percent_of_track_completed > percent_comp)
|
||||
percent_comp = traj_inf.percent_of_track_completed;
|
||||
end
|
||||
|
||||
U_final = [U_final; U_try];
|
||||
Y = [Y; Y_temp(1:3,:)];
|
||||
num_iter = num_iter + count
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
482
Experimentation/xenia_nonlinearopt.m
Normal file
482
Experimentation/xenia_nonlinearopt.m
Normal file
@@ -0,0 +1,482 @@
|
||||
%Vehicle Parameterrs
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
Iz=2667;
|
||||
a=1.35;
|
||||
b=1.45;
|
||||
By=0.27;
|
||||
Cy=1.2;
|
||||
Dy=0.7;
|
||||
Ey=-1.6;
|
||||
Shy=0;
|
||||
Svy=0;
|
||||
m=1400;
|
||||
g=9.806;
|
||||
|
||||
%note constraints on input
|
||||
%delta [-0.5, 0.5]
|
||||
%Fx [-5000, 5000]
|
||||
|
||||
load("TestTrack.mat");
|
||||
|
||||
init = [287, 5, -176, 0, 2, 0];
|
||||
curr_pos = [init(1);init(3)];
|
||||
|
||||
|
||||
%state_size = tbd;
|
||||
% z = [init, init, -0.004, 3900]; %for testing purposes
|
||||
% nsteps = 2;
|
||||
% [LB, UB] = bounds(nsteps, 1);
|
||||
% [g,h,dg,dh]=nonlcon(z, Xobs, nsteps);
|
||||
% [J, dJ] = costfun(z, TestTrack.cline(:,1), TestTrack.theta(1), nsteps);
|
||||
|
||||
%%okay, idea is to create an iterative process that uses the centerline as a
|
||||
%checkpoint
|
||||
%keep trajecting until closer to the next point on centerline, then repeat
|
||||
%bounds will be based on index of current&previous cp index
|
||||
%cost function uses next checkpoint in centerline
|
||||
|
||||
%generate z and make it bigger if didn't reach close enough to the goal
|
||||
%(modify nsteps)
|
||||
Nobs = randi([10 25], 1,1);
|
||||
global Xobs
|
||||
Xobs = generateRandomObstacles(Nobs);
|
||||
|
||||
% global index_cl_cp %index of current checkpoint in centerline
|
||||
% index_cl_cp = 1;
|
||||
% global index_cl_nextcp %index of next checkpoint in centerline
|
||||
% index_cl_nextcp = 2;
|
||||
%
|
||||
% global nsteps
|
||||
% nsteps = 100;
|
||||
|
||||
% x0 = init;
|
||||
% x0vec = [];
|
||||
% %initialize x0vec to be initial condition for n steps and Sravan's ref inputs
|
||||
% %note: function 'initvec' changes inputs, but i'm not sure how
|
||||
% %initialization should look with fmincon
|
||||
% for i = 1:nsteps
|
||||
% x0vec = [x0vec, init];
|
||||
% end
|
||||
% for i = 1:nsteps-1
|
||||
% x0vec = [x0vec, -0.004, 3900];
|
||||
% end
|
||||
|
||||
% dist12atcp = [];
|
||||
% iter = 0;
|
||||
%
|
||||
% factor = 0.1; %next checkpoint if significantly closer to next checkpoint
|
||||
U_final = [];
|
||||
Y_final = [];
|
||||
options = optimoptions('fmincon','SpecifyConstraintGradient',true,...
|
||||
'SpecifyObjectiveGradient',true) ;
|
||||
|
||||
load('ROB535_ControlProject_part1_Team3.mat');
|
||||
%[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, init);
|
||||
load('reftrack_info.mat');
|
||||
load('segments_info.mat');
|
||||
|
||||
for i = 1:length(num_pts)
|
||||
[start_idx, end_idx] = get_indices(i, num_pts);
|
||||
|
||||
delta = delta_vals(i);
|
||||
F_x = F_x_vals(i);
|
||||
|
||||
if (end_idx >= size(Y_submission,1))
|
||||
start_idx = start_idx - 1;
|
||||
end_idx = end_idx - 1;
|
||||
end
|
||||
|
||||
x0 = [];
|
||||
for j = start_idx:end_idx+1 %+1 end idx to keep z size consistent to hw
|
||||
x0 = [x0, Y_submission(j,:)];
|
||||
end
|
||||
|
||||
for j = start_idx:end_idx
|
||||
x0 = [x0, delta, F_x];
|
||||
end
|
||||
|
||||
%start and end index used to maintain size of vectors
|
||||
[lb, ub] = bounds(start_idx, end_idx);
|
||||
|
||||
%define for cost function, goal is to reach end of segment
|
||||
global target_vec
|
||||
target_vec = [Y_submission(end_idx,1), Y_submission(end_idx,3), Y_submission(end_idx,5)];
|
||||
|
||||
global nsteps
|
||||
nsteps = num_pts(i)+1;
|
||||
|
||||
cf=@costfun
|
||||
nc=@nonlcon
|
||||
z=fmincon(cf,x0,[],[],[],[],lb',ub',nc,options);
|
||||
Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
info = getTrajectoryInfo(Y0,U)
|
||||
U_final = [U_final; U];
|
||||
end
|
||||
% while (index_cl_cp < size(TestTrack.cline,2))
|
||||
% %because of the way cf/nc need to be)
|
||||
% index_cl_cp
|
||||
% iter = iter + 1;
|
||||
% [lb, ub] = bounds(nsteps, index_cl_cp);
|
||||
% cf=@costfun
|
||||
% nc=@nonlcon
|
||||
% z=fmincon(cf,x0vec,[],[],[],[],lb',ub',nc,options);
|
||||
% Y0=reshape(z(1:6*nsteps),6,nsteps)';
|
||||
% U=reshape(z(6*nsteps+1:end),2,nsteps-1)';
|
||||
%
|
||||
% [Y, T] = forwardIntegrateControlInput(U, x0vec(1:6));
|
||||
%
|
||||
% curr_xy = [Y(end,1); Y(end,3)];
|
||||
% dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
|
||||
% dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
|
||||
%
|
||||
% if (dist2cp2 < (dist2cp1 - dist2cp1*factor))
|
||||
% dist12atcp = [dist12atcp; dist2cp1, dist2cp2];
|
||||
% %add to final solution
|
||||
% U_final = [U_final; U];
|
||||
% Y_final = [Y_final; Y];
|
||||
%
|
||||
% %reinstantiate
|
||||
% %nsteps = 100;
|
||||
% x0vec = initvec(Y_final(end,:), U_final(end,:));
|
||||
%
|
||||
% %update checkpoint
|
||||
% index_cl_cp = index_cl_cp + 1
|
||||
% index_cl_nextcp = index_cl_nextcp + 1;
|
||||
% if (index_cl_nextcp > size(TestTrack.cline, 2))
|
||||
% index_cl_nextcp = size(TestTrack.cline, 2);
|
||||
% end
|
||||
% else
|
||||
% %resize and try again
|
||||
% %nsteps = nsteps + 20;
|
||||
% x0vec = initvec(x0vec(1:6), U(1,:));
|
||||
%
|
||||
% end
|
||||
%
|
||||
% end
|
||||
|
||||
function [start_idx, end_idx] = get_indices(segment_num, num_pts)
|
||||
if segment_num == 1
|
||||
start_idx = 1;
|
||||
end_idx = num_pts(segment_num);
|
||||
else
|
||||
start_idx = sum(num_pts(1:segment_num-1)) + 1;
|
||||
end_idx = sum(num_pts(1:segment_num));
|
||||
end
|
||||
end
|
||||
|
||||
function x0vec = initvec(x0, u0)
|
||||
%function used because fmincon needs initial condition to be size of
|
||||
%state vector
|
||||
%x0 - last row of Y at checkpoint
|
||||
%u0 - last row of U at checkpoint
|
||||
global nsteps
|
||||
x0vec = [];
|
||||
for i = 1:nsteps
|
||||
x0vec = [x0vec, x0];
|
||||
end
|
||||
|
||||
%not sure if inputs should be instantiated or not
|
||||
%will instantiate them to previous u
|
||||
for i = 1:nsteps-1
|
||||
x0vec = [x0vec, u0];
|
||||
end
|
||||
end
|
||||
|
||||
function [lb, ub] = bounds(start_idx, end_idx)
|
||||
load('TestTrack.mat');
|
||||
load('reftrack_info.mat');
|
||||
|
||||
%[m,nPts] = size(TestTrack.cline);
|
||||
% numState = 6;
|
||||
% numInput = 2;
|
||||
% nsteps = StepsPerPoint * nPts;
|
||||
|
||||
lb_u = [-0.5;-5000];
|
||||
ub_u = [0.5;5000];
|
||||
|
||||
bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ...
|
||||
TestTrack.br(1,1), TestTrack.br(1,2)];
|
||||
bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ...
|
||||
TestTrack.br(2,1), TestTrack.br(2,2)];
|
||||
phi_init = TestTrack.theta(1);
|
||||
|
||||
%phi restricted to just [-pi pi]
|
||||
%lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
|
||||
%ub = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
|
||||
lb = []; ub = [];
|
||||
|
||||
%hijacking this for loop to only consider one index for bounds
|
||||
for i=start_idx:end_idx+1
|
||||
Y_ref_curxy = [Y_submission(i,1); Y_submission(i,3)];
|
||||
sqdist_to_cl = (TestTrack.cline - Y_ref_curxy).^2;
|
||||
dist_to_cl = (sqdist_to_cl(1,:) + sqdist_to_cl(2,:)).^0.5;
|
||||
[minDist, indMin] = min(dist_to_cl);
|
||||
|
||||
prev_idx = max(indMin-1, 1);
|
||||
next_idx = min(indMin, 246);
|
||||
|
||||
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
|
||||
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
|
||||
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
|
||||
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
|
||||
%phi_init = TestTrack.theta(i);
|
||||
|
||||
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -pi; -Inf];
|
||||
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +pi; Inf];
|
||||
|
||||
|
||||
lb=[lb;lb_x];
|
||||
ub=[ub;ub_x];
|
||||
|
||||
end
|
||||
|
||||
for i=start_idx:end_idx
|
||||
ub=[ub;ub_u];
|
||||
lb=[lb;lb_u];
|
||||
end
|
||||
end
|
||||
|
||||
function [J, dJ] = costfun(z)
|
||||
|
||||
global nsteps
|
||||
global target_vec
|
||||
load('TestTrack.mat');
|
||||
|
||||
targetPos = target_vec(1:2);
|
||||
targetTheta = target_vec(3);
|
||||
sumPos = [];
|
||||
sumInput = [];
|
||||
for i = 1:nsteps
|
||||
zIndx = 6*(i-1) + 1;
|
||||
sumPos(i) = (z(zIndx) - targetPos(1))^2 + (z(zIndx+2) - targetPos(2))^2 + (z(zIndx+4) - targetTheta)^2;
|
||||
|
||||
if (i <= nsteps-1)
|
||||
uInd = 2*(i-1) + nsteps*6 - 1;
|
||||
sumInput(i) = z(uInd)^2 + z(uInd+1)^2;
|
||||
end
|
||||
end
|
||||
|
||||
J = sum(sumPos) + sum(sumInput);
|
||||
dJ = transpose(z.*2);
|
||||
|
||||
uStart = nsteps*6 - 1;
|
||||
for i = 1:6:nsteps*6
|
||||
zIndx = i;
|
||||
dJ(i) = (z(zIndx) - targetPos(1))*2;
|
||||
dJ(i+2) = (z(zIndx+2) - targetPos(2))*2;
|
||||
dJ(i+4) = (z(zIndx+4) - targetTheta)*2;
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
function [g, h, dg, dh] = nonlcon(z)
|
||||
|
||||
%nsteps = (size(z,2)/8);
|
||||
global nsteps
|
||||
global Xobs
|
||||
curr_pos = [z(1); z(3)];
|
||||
|
||||
Xobs_seen = senseObstacles(curr_pos, Xobs);
|
||||
centroids = [];
|
||||
for i = 1:size(Xobs_seen,2)
|
||||
centroids = [centroids; mean(Xobs_seen{1, i})];
|
||||
end
|
||||
|
||||
dt = 0.01;
|
||||
g = []; dg = [];
|
||||
|
||||
%radius size
|
||||
r = 3;
|
||||
|
||||
for i = 1:nsteps
|
||||
zInd_x = 6*(i-1) + 1;
|
||||
zInd_y = 6*(i-1) + 3;
|
||||
curr_xy = [z(zInd_x), z(zInd_y)];
|
||||
|
||||
%g based on if there's obstacles around
|
||||
%initialize to zero
|
||||
g_curr = 0;
|
||||
zeroRow = zeros(1,size(z,2));
|
||||
dg(i,:) = zeroRow;
|
||||
if (~isempty(centroids))
|
||||
dist2Obst = [];
|
||||
for j = 1:size(Xobs_seen,2)
|
||||
dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2));
|
||||
dist2Obst = [dist2Obst; dist];
|
||||
end
|
||||
|
||||
%closest obstacle used for constraint
|
||||
[minDist, indMin] = min(dist2Obst);
|
||||
|
||||
%g
|
||||
g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2;
|
||||
|
||||
%dg
|
||||
dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2;
|
||||
dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2;
|
||||
end
|
||||
g = [g; g_curr];
|
||||
end
|
||||
dg = transpose(dg);
|
||||
|
||||
h = z(1:6);
|
||||
for i = 2:nsteps
|
||||
zInd_x = 6*(i-1) + 1;
|
||||
zInd_x_prev = 6*(i-2) + 1;
|
||||
|
||||
stateCurr = z(zInd_x:zInd_x+5);
|
||||
statePrev = z(zInd_x_prev:zInd_x_prev+5);
|
||||
|
||||
%get previous input
|
||||
uInd_prev = 2*(i-1) + nsteps*6 - 1;
|
||||
uPrev = [z(uInd_prev), z(uInd_prev + 1)];
|
||||
|
||||
derPrev= dt*bike(statePrev, uPrev);
|
||||
|
||||
currH = stateCurr - statePrev - derPrev;
|
||||
h(6*(i-1)+1) = currH(1);
|
||||
h(6*(i-1)+2) = currH(2);
|
||||
h(6*(i-1)+3) = currH(3);
|
||||
h(6*(i-1)+4) = currH(4);
|
||||
h(6*(i-1)+5) = currH(5);
|
||||
h(6*(i-1)+6) = currH(6);
|
||||
end
|
||||
|
||||
dh = zeros(size(z,2), nsteps*6);
|
||||
dh(1:6, 1:6) = eye(6);
|
||||
for i = 1:nsteps-1
|
||||
uInd = 2*(i-1) + nsteps*6 + 1;
|
||||
uCurr = [z(uInd), z(uInd + 1)];
|
||||
zInd_x = 6*(i-1) + 1;
|
||||
stateCurr = z(zInd_x:zInd_x+5);
|
||||
[A, B] = bikeLinearize(stateCurr, uCurr);
|
||||
|
||||
dh(6*i+1:6*i+6, 6*i+1:6*i+6) = eye(6);
|
||||
dh(6*i-5:6*i, 6*i+1:6*i+6) = -eye(6) - dt*A;
|
||||
dh(nsteps*6+2*i-1:nsteps*6+2*i, 6*i+1:6*i+6) = -dt*transpose(B);
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
function dzdt=bike(x,U)
|
||||
%constants
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
Iz=2667;
|
||||
a=1.35;
|
||||
b=1.45;
|
||||
By=0.27;
|
||||
Cy=1.2;
|
||||
Dy=0.7;
|
||||
Ey=-1.6;
|
||||
Shy=0;
|
||||
Svy=0;
|
||||
m=1400;
|
||||
g=9.806;
|
||||
|
||||
|
||||
%generate input functions
|
||||
delta_f= U(1);
|
||||
F_x= U(2);
|
||||
|
||||
%slip angle functions in degrees
|
||||
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
||||
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
||||
|
||||
%Nonlinear Tire Dynamics
|
||||
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
||||
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
||||
|
||||
F_zf=b/(a+b)*m*g;
|
||||
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
||||
|
||||
F_zr=a/(a+b)*m*g;
|
||||
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
||||
|
||||
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
||||
F_max=0.7*m*g;
|
||||
|
||||
if F_total>F_max
|
||||
|
||||
F_x=F_max/F_total*F_x;
|
||||
|
||||
F_yr=F_max/F_total*F_yr;
|
||||
end
|
||||
|
||||
%vehicle dynamics
|
||||
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
|
||||
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
|
||||
x(2)*sin(x(5))+x(4)*cos(x(5));...
|
||||
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
|
||||
x(6);...
|
||||
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
||||
end
|
||||
|
||||
function [A, B] =bikeLinearize(x,U)
|
||||
%constants
|
||||
Nw=2;
|
||||
f=0.01;
|
||||
Iz=2667;
|
||||
a=1.35;
|
||||
b=1.45;
|
||||
By=0.27;
|
||||
Cy=1.2;
|
||||
Dy=0.7;
|
||||
Ey=-1.6;
|
||||
Shy=0;
|
||||
Svy=0;
|
||||
m=1400;
|
||||
g=9.806;
|
||||
|
||||
|
||||
%generate input functions
|
||||
delta_f= U(1);
|
||||
F_x= U(2);
|
||||
|
||||
%slip angle functions in degrees
|
||||
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
||||
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
||||
|
||||
%Nonlinear Tire Dynamics
|
||||
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
||||
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
||||
|
||||
F_zf=b/(a+b)*m*g;
|
||||
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
||||
|
||||
F_zr=a/(a+b)*m*g;
|
||||
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
||||
|
||||
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
||||
F_max=0.7*m*g;
|
||||
|
||||
if F_total>F_max
|
||||
|
||||
F_x=F_max/F_total*F_x;
|
||||
|
||||
F_yr=F_max/F_total*F_yr;
|
||||
end
|
||||
|
||||
|
||||
|
||||
%we are just going to use cornering stiffness to make linear so this derivative
|
||||
%easier, the vehicle parameter's are close enough to problem 1 hw 2
|
||||
B=10;
|
||||
C=1.3;
|
||||
D=1;
|
||||
Ca_r= F_zr*B*C*D;
|
||||
Ca_f= F_zf*B*C*D;
|
||||
|
||||
A = [0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0;
|
||||
0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
|
||||
0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0;
|
||||
0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - x(2);
|
||||
0, 0, 0, 0, 0, 1
|
||||
0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/x(2)) + a*Ca_f/Iz*(-a/x(2))];
|
||||
|
||||
B = [0, -Ca_f/(x(2)*m), 0, Ca_f/m, 0, a*Ca_f/Iz;
|
||||
0, Nw/m, 0, 0, 0, 0]';
|
||||
end
|
||||
Reference in New Issue
Block a user