Add nonlinear system

2025-09-02 21:53:13 +00:00 · 2020-04-20 21:02:53 -05:00
parent 80c686c517
commit bf612c0dfc
6 changed files with 125 additions and 69 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,4 @@
-*.slxc
-*.slx.autosave
-*.asv
-slprj
+*.slxc
+*.slx.autosave
+*.asv
+slprj
--- a/42
+++ b/42
@@ -1,21 +1,21 @@
-MIT License
-
-Copyright (c) 2020 ME-561-W20-Quadcopter-Project
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+MIT License
+
+Copyright (c) 2020 ME-561-W20-Quadcopter-Project
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
--- a/docs/home.md
+++ b/docs/home.md
@@ -1,25 +1,25 @@
-# Full State Feedback and Control of a Quadcopter Drone <!-- omit in toc -->
-
-## Table of Contents <!-- omit in toc -->
- [Contributors](#contributors)
- [Documents](#documents)
-
-## Contributors
-
-### Project Team <!-- omit in toc -->
-
- Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu))
- Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu))
- Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu))
- Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu))
- Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu))
-
-### EECS / MECHENG 561 W20 Course Staff <!-- omit in toc -->
-
- Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu))
- Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu))
-
-## Documents
-
-1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf)
-2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg)
+# Full State Feedback and Control of a Quadcopter Drone <!-- omit in toc -->
+
+## Table of Contents <!-- omit in toc -->
+- [Contributors](#contributors)
+- [Documents](#documents)
+
+## Contributors
+
+### Project Team <!-- omit in toc -->
+
+- Sravan Balaji ([balajsra@umich.edu](mailto:balajsra@umich.edu))
+- Aditya Iyer ([adiyer@umich.edu](mailto:adiyer@umich.edu))
+- Lakshmanan Periakaruppan ([lperiaka@umich.edu](mailto:lperiaka@umich.edu))
+- Naman Shah ([namanvs@umich.edu](mailto:namanvs@umich.edu))
+- Sumedh Vaishampayan ([sumi@umich.edu](mailto:sumi@umich.edu))
+
+### EECS / MECHENG 561 W20 Course Staff <!-- omit in toc -->
+
+- Ram Vasudevan ([ramv@umich.edu](mailto:ramv@umich.edu))
+- Sid Dey ([siddey@umich.edu](mailto:siddey@umich.edu))
+
+## Documents
+
+1. [Project Proposal](1.%20ME%20561%20Project%20Proposal.pdf)
+2. [Final Report - Overleaf (Read-Only)](https://www.overleaf.com/read/kyjvdsxkfnmg)
--- a/src/Naman_LQR_Working.m
+++ b/src/Naman_LQR_Working.m
@@ -83,18 +83,18 @@ x_0_up = [0, 0, -1, ...
       0, 0, 0]'; %Redefine origin! 

 %Goal 2: Stabilize from a 10-degree roll and pitch with <3deg overshoot
-x_0_pitch = [0, 0, 0, ...
+x_0_pitchroll = [0, 0, 0, ...
       0, 0, 0, ...
-       10, 0, 0, ...
-       0, 0, 0]'; %Pitch of 10 degrees
+       10*(pi/180), 10*(pi/180), 0, ...
+       0, 0, 0]'; %Pitch and roll of 10 degrees

 x_0_roll = [0, 0, 0, ...
       0, 0, 0, ...
-       0, 10, 0, ...
+       0, 10*(pi/180), 0, ...
       0, 0, 0]'; %Roll of 10 degrees
   
 %Goal 3: Move from position (0,0,0) to within 5 cm of (1,1,1) within 5 seconds.
-x_0_trans = [-1, -1, -1, ...
+x_0_trans = [-1, -1, 0, ...
       0, 0, 0, ...
       0, 0, 0, ...
       0, 0, 0]'; %Redefine origin! 
@@ -114,6 +114,8 @@ nSteps = length(tSpan);

 %Determine gains
 [K, P] = LQR_LTI(discrete.A, discrete.B, Q, R, nSteps);
+FiniteLQR_Goal_1_K = K;
+save('FiniteLQRGoal_1_K.mat', 'K');

 %Propagate
 [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_up, K, discrete.A, discrete.B);
@@ -122,12 +124,31 @@ nSteps = length(tSpan);
 xlqr(3,:) = xlqr(3,:) + 1;
 %Plot
 plot_states(xlqr, tSpan);
-zd = diff(xlqr(6,:))./T_s
+zd = diff(xlqr(6,:))./T_s;
+
+%Simulate nonlinear model
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_up); 
+set_param('LQRNonlinearSim', 'StopTime', '2')
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
+
+time = simout.allVals.Time(:,1);
+figure();
+plot(time, simout.z+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on;
+plot(time, xlqr(3,:), '-b','LineWidth', 1);
+xlabel('Time (s)');
+ylabel('Z (m)');
+legend({'Nonlinear Model', 'Linear Model'});
+
+

 %% Finite-Time Horizon LQR for Goal 2
+Q = diag([0, 0, 0, ... % x, y, z
+          0, 0, 0, ... % x', y', z'
+          200, 200, 1, ... % roll, pitch, yaw
+          10, 10, 1]);   % roll', pitch', yaw'

 %Calculate number of timesteps.
-tSpan = 0:T_s:2;
+tSpan = 0:T_s:4;
 nSteps = length(tSpan);

 %Determine gains
@@ -136,22 +157,40 @@ nSteps = length(tSpan);

 %Pitch Goal
 %Propagate
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitch, K, discrete.A, discrete.B);
+[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_pitchroll, K, discrete.A, discrete.B);

 %Plot
 plot_states(xlqr, tSpan);
 yd = diff(xlqr(5,:))./T_s
 pd = diff(xlqr(7,:))./T_s
+
 %Propagate
-[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B);
+%[ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_roll, K, discrete.A, discrete.B);

 %Plot
-plot_states(xlqr, tSpan);
-xd = diff(xlqr(4,:))./T_s
-rd = diff(xlqr(8,:))./T_s
+%plot_states(xlqr, tSpan);
+%xd = diff(xlqr(4,:))./T_s
+%rd = diff(xlqr(8,:))./T_s
+
+%Simulate nonlinear model
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_pitchroll); 
+set_param('LQRNonlinearSim', 'StopTime', '4')
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
+
+time = simout.allVals.Time(:,1);
+figure();
+plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on;
+plot(time, xlqr(8,:), '-b','LineWidth', 1);
+%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on;
+xlabel('Time (s)');
+ylabel('Pitch/Roll Angle (rad)');
+legend({'Nonlinear Model', 'Linear Model'});

 %% Finite-Time Horizon For Goal 3
-
+Q = diag([1000, 1000, 0, ... % x, y, z
+          0, 0, 0, ... % x', y', z'
+          200, 200, 1, ... % roll, pitch, yaw
+          0, 0, 1]);   % roll', pitch', yaw'
 %Calculate number of timesteps.
 tSpan = 0:T_s:5;
 nSteps = length(tSpan);
@@ -162,11 +201,25 @@ nSteps = length(tSpan);
 %Pitch Goal
 %Propagate
 [ulqr, xlqr] = propagate(nInputs, nStates, nSteps, x_0_trans, K, discrete.A, discrete.B);
-xlqr(1:3,:) = xlqr(1:3,:) + 1;
+xlqr(1:2,:) = xlqr(1:2,:) + 1;

 %Plot
 plot_states(xlqr, tSpan);

+%Simulate nonlinear model
+[x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(x_0_trans); 
+set_param('LQRNonlinearSim', 'StopTime', '5')
+simout = sim('LQRNonlinearSim', 'FixedStep', '.01');
+
+time = simout.allVals.Time(:,1);
+figure();
+plot(time, simout.x+1, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on;
+plot(time, xlqr(1,:), '-b','LineWidth', 1);
+%plot(time, simout.roll, 'LineStyle', '--', 'color',[0 0.5 0], 'LineWidth', 2); hold on;
+xlabel('Time (s)');
+ylabel('x (m)');
+legend({'Nonlinear Model', 'Linear Model'});
+
 %% Helper Functions

 function [K, P] = LQR_LTI(A, B, Q, R, nSteps)
@@ -223,5 +276,9 @@ function plot_states(xlqr, tSpan)
    legend('Pitch (about x)', 'Roll (about y)', 'Yaw (about z)', 'Pitch Rate', 'Roll Rate', 'Yaw Rate');
    title("Angular Displacements(-) and Velocities(--)");
    xlabel("Time(s)");
-    ylabel("Displacement (deg)");
-end
+    ylabel("Displacement (rad)");
+end
+
+function [x0, y0, z0, xdot0, ydot0, zdot0, phi0, theta0, psi0, phidot0, thetadot0, psidot0] = unpack(X0)
+x0=X0(1); y0=X0(2); z0=X0(3); xdot0=X0(4); ydot0=X0(5); zdot0=X0(6); phi0=X0(7); theta0=X0(8); psi0=X0(9); phidot0=X0(10); thetadot0=X0(11); psidot0=X0(12);
+end
--- a/src/PlantModel.m
+++ b/src/PlantModel.m
@@ -79,7 +79,6 @@ kdps = 0.0486;
 kpz = 4.0670;
 kdz = 2.9031;

-Q = zeros(12); Q(7:9, 7:9) = eye(3);
-R = 1;
-%K = lqr(A,B,Q,R);
+%% Load LQR gains
+LQRGoal1 = matfile('FiniteLQRGoal_1_K.mat');

--- a/src/PlantModelSim.slx
+++ b/src/PlantModelSim.slx