-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathquad_dynamics_nonlinear.m
75 lines (49 loc) · 2.58 KB
/
quad_dynamics_nonlinear.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
function quad_dynamics_nonlinear
global Quad;
Quad.X_ddot = (-[cos(Quad.phi)*sin(Quad.theta)*cos(Quad.psi)+sin(Quad.phi)*sin(Quad.psi)]*Quad.U1-Quad.Kdx*Quad.X_dot)/Quad.m;
Quad.Y_ddot = (-[cos(Quad.phi)*sin(Quad.psi)*sin(Quad.theta)-cos(Quad.psi)*sin(Quad.phi)]*Quad.U1-Quad.Kdy*Quad.Y_dot)/Quad.m;
Quad.Z_ddot = (-[cos(Quad.phi)*cos(Quad.theta)]*Quad.U1-Quad.Kdz*Quad.Z_dot)/Quad.m+Quad.g;
Quad.p_dot = (Quad.q*Quad.r*(Quad.Jy - Quad.Jz) - Quad.Jp*Quad.p*Quad.Obar + Quad.l*Quad.U2)/Quad.Jx;
Quad.q_dot = (Quad.p*Quad.r*(Quad.Jz - Quad.Jx) + Quad.Jp*Quad.q*Quad.Obar + Quad.l*Quad.U3)/Quad.Jy;
Quad.r_dot = (Quad.p*Quad.q*(Quad.Jx - Quad.Jy) + Quad.U4)/Quad.Jz;
Quad.phi_dot = Quad.p + sin(Quad.phi)*tan(Quad.theta)*Quad.q + cos(Quad.phi)*tan(Quad.theta)*Quad.r;
Quad.theta_dot = cos(Quad.phi)*Quad.q - sin(Quad.phi)*Quad.r;
Quad.psi_dot = sin(Quad.phi)/cos(Quad.theta)*Quad.q + cos(Quad.phi)/cos(Quad.theta)*Quad.r;
Quad.X_ddot = Quad.X_ddot + Quad.X_dis/Quad.m;
Quad.Y_ddot = Quad.Y_ddot + Quad.Y_dis/Quad.m;
Quad.Z_ddot = Quad.Z_ddot + Quad.Z_dis/Quad.m;
Quad.phi_dot = Quad.phi_dot + Quad.phi_dis/Quad.Jx*Quad.Ts;
Quad.theta_dot = Quad.theta_dot + Quad.theta_dis/Quad.Jy*Quad.Ts;
Quad.psi_dot = Quad.psi_dot + Quad.psi_dis/Quad.Jz*Quad.Ts;
% Calculating the Z velocity & position
Quad.Z_dot = Quad.Z_ddot*Quad.Ts + Quad.Z_dot;
Quad.Z = Quad.Z_dot*Quad.Ts + Quad.Z;
% Calculating the X velocity & position
Quad.X_dot = Quad.X_ddot*Quad.Ts + Quad.X_dot;
Quad.X = Quad.X_dot*Quad.Ts + Quad.X;
% Calculating the Y velocity & position
Quad.Y_dot = Quad.Y_ddot*Quad.Ts + Quad.Y_dot;
Quad.Y = Quad.Y_dot*Quad.Ts + Quad.Y;
% Calculating p,q,r
Quad.p = Quad.p_dot*Quad.Ts+Quad.p;
Quad.q = Quad.q_dot*Quad.Ts+Quad.q;
Quad.r = Quad.r_dot*Quad.Ts+Quad.r;
% Calculating angular velocity and position
Quad.phi = Quad.phi_dot*Quad.Ts + Quad.phi;
Quad.theta = Quad.theta_dot*Quad.Ts+Quad.theta;
Quad.psi = Quad.psi_dot*Quad.Ts+Quad.psi;
% Flip positive Z axis up for intuitive plotting
Quad.Z_plot(Quad.counter) = -Quad.Z;
Quad.Z_ref_plot(Quad.counter) = -Quad.Z_des;
Quad.X_plot(Quad.counter) = Quad.X;
Quad.X_ref_plot(Quad.counter) = Quad.X_des;
Quad.Y_plot(Quad.counter) = Quad.Y;
Quad.Y_ref_plot(Quad.counter) = Quad.Y_des;
Quad.phi_plot(Quad.counter) = Quad.phi;
Quad.phi_ref_plot(Quad.counter) = Quad.phi_des;
Quad.theta_plot(Quad.counter) = Quad.theta;
Quad.theta_ref_plot(Quad.counter) = Quad.theta_des;
Quad.psi_plot(Quad.counter) = Quad.psi;
Quad.psi_ref_plot(Quad.counter) = Quad.psi_des;
Quad.counter = Quad.counter + 1;
end