From 8eb9c26b18cdbd8c7bfa3ef96169ec794ff7d0e4 Mon Sep 17 00:00:00 2001
From: Sravan Balaji <balajsra@umich.edu>
Date: Sat, 11 Apr 2020 15:20:25 -0400
Subject: [PATCH] System Simulation Attempt

- Add comments for axes used in roll, pitch, yaw
- Attempt to simulate system over time
---
 src/PlantModel.m | 58 +++++++++++++++++++++++++++++++++++++++++++-----
 1 file changed, 52 insertions(+), 6 deletions(-)

diff --git a/src/PlantModel.m b/src/PlantModel.m
index da84ed2..6ed1a03 100644
--- a/src/PlantModel.m
+++ b/src/PlantModel.m
@@ -17,12 +17,12 @@ Iz = 8.801*10^-3;
 % X4: Velocity along x axis - x'
 % X5: Velocity along y axis - y'
 % X6: Velocity along z axis - z'
-% X7: Roll angle - phi
-% X8: Pitch angle - theta
-% X9: Yaw angle - psi
-% X10: Roll rate - phi'
-% X11: Pitch rate - theta'
-% X12: Yaw rate - psi'
+% X7: Roll angle - phi (y-axis)
+% X8: Pitch angle - theta (x-axis)
+% X9: Yaw angle - psi (z-axis)
+% X10: Roll rate - phi' (y-axis)
+% X11: Pitch rate - theta' (x-axis)
+% X12: Yaw rate - psi' (z-axis)
 
 % Inputs:
 % U1: Total Upward Force on the quad rotor along z-axis
@@ -77,3 +77,49 @@ C = [1 0 0 0 0 0 0 0 0 0 0 0;...
      0 0 0 0 0 0 0 0 1 0 0 0];
 
 D = zeros(6,4);
+
+continuous_system = ss(A, B, C, D);
+T_s = 0.05;
+discrete_system = c2d(continuous_system, T_s);
+
+Q = diag([1, 1, 1, ... % x, y, z
+          1, 1, 1, ... % x', y', z'
+          1, 1, 1, ... % roll, pitch, yaw
+          1, 1, 1]);   % roll', pitch', yaw'
+
+R = diag([1e3, 1e3, 1e3, 1e3]); % upward force, pitch torque, roll torque, yaw torque
+
+% u = -Kx
+K = dlqr(discrete_system.A, discrete_system.B, Q, R, 0);
+
+num_steps = 10;
+X = zeros(12, num_steps);
+% Initial Condition
+X_t = zeros(12,1);
+X(:,1) = [0, 0, 0, ... % x, y, z
+          0, 0, 0, ... % x', y', z'
+          deg2rad(5), deg2rad(0), deg2rad(0), ... % roll, pitch, yaw
+          deg2rad(0), deg2rad(0), deg2rad(0)].';  % roll', pitch', yaw'
+
+for i = 1:num_steps
+    X(:,i+1) = (A - B*K) * X(:,i);
+end
+
+Y = C * X;
+
+figure(1);
+t = 1:num_steps;
+
+subplot(1,2,1);
+hold on;
+plot(t, Y(1, t), 'r-');
+plot(t, Y(2, t), 'b-');
+plot(t, Y(3, t), 'g-');
+legend('x', 'y', 'z')
+
+subplot(1,2,2);
+hold on;
+plot(t, Y(4, t), 'r-');
+plot(t, Y(5, t), 'b-');
+plot(t, Y(6, t), 'g-');
+legend('roll (y-axis)', 'pitch (x-axis)', 'yaw (z-axis)')