Anson/Lander-Simulink
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-sim.git synced 2025-08-03 11:51:27 +00:00
Code Releases Activity

LQR implementation and general improvements

Browse Source
This commit is contained in:
Brendan McGeeney
2021-08-12 00:31:00 +00:00
parent f3428e00e3
commit d8cb278a4a
1 changed files with 84 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

116
simPrototypeSTART.m
View File

@@ -3,77 +3,130 @@ close all; clear all; clc;
%% User Defined Values %% User Defined Values
% Initial Conditions % Initial Conditions
v_0 = 0; % Initial Velocity [m/s] v0 = 0; % Initial Velocity (z) [m/s]
M0 = 1.2; % Initial Mass [kg]
yaw0 = 0; % Initial Yaw [deg]
pitch0 = 5; % Initial Pitch [deg]
roll0 = 0; % Initial Roll [deg]
p0 = 0; % Initial Angular Velocity (x) [deg/s]
q0 = 0; % Initial Angular Velocity (y) [deg/s]
r0 = 0; % Initial Angular Velocity (z) [deg/s]
% Thrust Curve Data from Text File [t, N] Tcurve = readmatrix('F15_thrustCurve.txt'); % Thrust Curve from .txt [t, N]
Tcurve = readmatrix('F15_thrustCurve.txt');
% Constants % Constants
g = -9.81; % Gravitational Acceleration [m/s2] g = -9.81; % Gravitational Acceleration [m/s2]
M0 = 0.8; % Initial Mass [kg] Mp = 0.06; % Propellant Mass [kg]
Mp = 0.06; % Propellant Mass [kg] Mb = M0 - Mp; % Burnout Mass [kg]
Mb = M0 - Mp; % Burnout Mass [kg] tb = Tcurve(end, 1) - Tcurve(1, 1); % Burn Time [s]
tb = Tcurve(end, 1); % Burn Time [s] mdot = Mp / tb; % Mass Flow Rate [kg/s]
mdot = Mp / tb; % Mass Flow Rate [kg/s] D = 0; % Drag [N]
D = 0; % Drag [N] stepSize = 0.001; % Simulation Step Size [s]
stepSize = 0.001; % Simulation Step Size [s] maxServo = 15; % Max Servo Rotation [deg]
[h_0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb) % Moment of Inertia / Mass
I11 = (1/12) * 0.5318^2 + 0.25 * 0.05105^2; % (1/12) * h^2 + 0.25 * r^2
I22 = (1/12) * 0.5318^2 + 0.25 * 0.05105^2; % (1/12) * h^2 + 0.25 * r^2
I33 = 0.5 * 0.05105^2; % 0.5 * r^2
I = [I11 0 0; 0 I22 0; 0 0 I33]; % I divided by Mass... this is taken care of in Simulink since our mass isn't constant
%% Pre-Sim Calcs
K = calcLQR(I*M0); % LQR Gain Calcs
[h0, vb, burnStartTime] = burnStartTimeCalc(Tcurve, tb, M0, mdot, Mb, v0); % Burn Start Time Calc
simTime = burnStartTime + tb; % Simulation Time [s]
yaw0 = yaw0 * pi / 180; % Initial Yaw [rad]
pitch0 = pitch0 * pi / 180; % Initial Pitch [rad]
roll0 = roll0 * pi / 180; % Initial Roll [rad]
p0 = p0 * pi / 180; % Initial Angular Velocity (x) [rad/s]
q0 = q0 * pi / 180; % Initial Angular Velocity (y) [rad/s]
r0 = r0 * pi / 180; % Initial Angular Velocity (z) [rad/s]
%% Simulink %% Simulink
tic tic
simTime = burnStartTime + tb; % Simulation Time [s]
model = 'simProtoype'; model = 'simProtoype';
load_system(model); load_system(model);
simOut = sim(model); simOut = sim(model);
toc toc
%% Output %% Outputs
% Acceleration
figure(1) figure(1)
plot(simOut.a)
% Acceleration
subplot(3, 1, 1)
plot(simOut.a.Data(:, 3))
title('Acceleration vs Time') title('Acceleration vs Time')
xlabel('Time (s)') xlabel('Time (s)')
ylabel('Acceleration (g''s)') ylabel('Acceleration (g''s)')
legend('X','Y')
saveas(gcf,'outputs/Acceleration vs Time.png')
% Velocity % Velocity
figure(2) subplot(3, 1, 2)
plot(simOut.v) plot(simOut.v.Data(:, 3))
title('Velocity vs Time') title('Velocity vs Time')
xlabel('Time (s)') xlabel('Time (s)')
ylabel('Velocity (m/s)') ylabel('Velocity (m/s)')
legend('X','Y')
saveas(gcf,'outputs/Velocity vs Time.png')
% Altitude % Altitude
figure(3)%k) subplot(3, 1, 3)
plot(sqrt(simOut.h.Data(:,2).^2 + simOut.h.Data(:,1).^2), simOut.h.Time) plot(simOut.h.Time, simOut.h.Data(:,3))
title('Altitude vs Time') title('Altitude vs Time')
% title(['burnStart = ',num2str(burnStart),' s'])
xlabel('Time (s)') xlabel('Time (s)')
ylabel('Altitude (m)') ylabel('Altitude (m)')
saveas(gcf,'outputs/Altitude vs Time.png') ylim([0 h0+5])
saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
figure(2)
% Euler Angles
subplot(2, 1, 1)
plot(simOut.YPR.Data)
title('Euler Angles vs Time')
xlabel('Time (ms)')
ylabel('Euler Angles (deg)')
legend('Yaw', 'Pitch', 'Roll')
% Angular Velocity
subplot(2, 1, 2)
plot(simOut.YPRdot.Data)
title('Angular Velocity vs Time')
xlabel('Time (ms)')
ylabel('Angular Velocity (deg/s)')
legend('X', 'Y', 'Z')
saveas(gcf,'outputs/Euler Angles vs Time.png')
figure(3)
% Servo 1 Position
subplot(2, 1, 1)
plot(simOut.servo1.Data)
title('Servo 1 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 1 Position (rad)')
% Servo 2 Position
subplot(2, 1, 2)
plot(simOut.servo2.Data)
title('Servo 2 Position vs Time')
xlabel('Time (ms)')
ylabel('Servo 2 Position (rad)')
saveas(gcf,'outputs/Servo Position vs Time.png')
% Animation % Animation
h = figure(4); h = figure(4);
K = animatedline('Marker', 'o'); K = animatedline('Marker', 'o');
axis([0, 2, 0, h_0]) axis([-10, 10, 0, h0])
xlabel('X-Position (m)') xlabel('X-Position (m)')
ylabel('Altitude (m)') ylabel('Altitude (m)')
title('Altitude') title('Altitude')
grid on grid on
for i = 1 : length(simOut.h.Data) for i = 1 : length(simOut.h.Data)
clearpoints(K); clearpoints(K);
addpoints(K, 1, simOut.h.Data(i, 2)); addpoints(K, simOut.h.Data(i, 1), simOut.h.Data(i, 3));
title(sprintf('Altitude at T = %f', simOut.h.Time(i))) title(sprintf('Altitude at T = %f', simOut.h.Time(i)))
drawnow limitrate drawnow limitrate
% Write Animation to gif, set to zero when testing since its slow to render. % Write Animation to gif, set to zero when testing since its slow to render.
outframes = 50; % 50 is a nice default outframes = 50; % 50 is a nice default
if outframes if outframes
@@ -98,5 +151,4 @@ for i = 1 : length(simOut.h.Data)
imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2); imwrite(imind,cm,'outputs/Altitude.gif','gif','WriteMode','append', 'DelayTime', .2);
end end
end end
end end