-
Notifications
You must be signed in to change notification settings - Fork 0
/
model_disturbance.m
95 lines (85 loc) · 1.94 KB
/
model_disturbance.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
% BIKE PROJECT - EGR 556
% SLIDING MODE CONTROL FOR STABILIZATION OF AN AUTONOMOUS BIKE
close all;
clc;
% Bike parameters
% mass (kg)
m = 92; % total mass 92kg
% height of center of mass (m)
h= 1.028;
% Moment of inertia w.r.t X axis
J = 3.28; % rear frame
% Inertia product w.r.t XZ axes
D = 0.603; % rear frame
% Acceleration due to gravity
g= 9.8;
% State space model of Bike
% initial conditions [ psi, psi_dot ]
x0 = [ 0.011; 0.0001]; % 0.63 degrees - 0.011 radian
% state space model
global A2
A2 = ((m*g*h)/J);
% Reference
sim_time = [ 0 80];
global psie_dot
psie_dot = 0;
global i
i =0;
global U
U = zeros(293,1);
[t,x] = ode45(@ODEfunction,sim_time, x0,psie_dot);
% PLOTTING GRAPHS
%plot 1 - states vs time
figure(1)
hold on
set(gca,'Fontsize',10);
grid on
% plot the states
plot(t,x(:,1),'k','Linewidth',2)
plot(t,x(:,2),'b','Linewidth',2)
% add initial values of states to the plot
plot(t(1),x0(1,1),'k-o','Markersize',10,'Linewidth',2)
plot(t(1),x0(2,1),'b-o','Markersize',10,'Linewidth',2)
legend('Roll angle (psi) ','Roll angle derevative (psi_dot)');
xlabel('Time')
ylabel('State')
title('SMC - Disturbance - PlStates vs Time ')
st = size(t);
%plot 2 - control input time
figure(2)
hold on
set(gca,'Fontsize',10);
grid on
% plot the states
plot(t,U(1:st),'r','Linewidth',2)
legend('lamda = 1 and K = 0.05');
xlabel('Time')
ylabel('Control input (U) ')
title('SMC - Disturbance - Control Input vs Time ')
% ODE FUNCTION
function [dx,U] = ODEfunction(t, x, psie_dot)
global i
global U
global A2
global psie_dot
i = i+1;
% system dynamics
A = [ 0 1; A2 0 ];
B = [ 0; 1];
% Reference / set point
Xref = [0; 0];
psie = x(1) - Xref(1);
psiref_dot = 0;
A2X = A(2)*x(1) + A(4)*x(2);
lamda = 0.1;
k = 0.05;
s = psie_dot + (lamda*psie);
delta_temp = -inv(B(2))*( A2X - psiref_dot + (lamda*psie_dot) );
U(i) = delta_temp - (k*sign(s));
% disturbance
d = 0.0001*A*exp(-B*t);
% system output
X_dot = A*x + B*U(i) + d;
psie_dot = X_dot(1);
dx = X_dot;
end