%% LoadFile
% The easiest way to read in the XTRA or BASE .csv is to read it in as
% a table.
AIM_Data=readtable('E190_9SEP2017_J825R_1_XTRA.csv');
% You have to actually look at the column headings to make sure
% you have the correct column names.
% Because the AIM XTRA uses different sample rates for the different
% sensors, most columns (except for the z-axis accelerometer and associated
% time) will have NaNs for much of the column. They show up once the actual
% column data cease. THe easiest way to remove them is with a conditional
% using isfinite.

% The data in a column heading may be accessed by appending with a dot, ".",
% and either the column name, e.g., .time, or the column number in
% parentheses, e.g., .(1). The example below shows how to plot two columns
% using either method of addressing.

% %% PlotDotName
% plot(AIM_Data.time,AIM_Data.GyroZ)
% title('Test Plot Table Columns .name')
% xlabel('time (s)')
% ylabel('\omega_z (°/s)')
% %% PlotDotNumber
% plot(AIM_Data.(1),AIM_Data.(4))
% title('Test Plot Table Columns .(#)')
% xlabel('time (s)')
% ylabel('\omega_z (°/s)')

%% SearchApogee
% Find the apogee time
accelTimePost=AIM_Data.time_5(AIM_Data.time_5>=0);
accelZPost=AIM_Data.acceleration(AIM_Data.time_5>=0);
plot(accelTimePost,accelZPost)
title('z-axis acceleration')
xlabel('time (s)')
ylabel('a_z (g)')

%% SetApogee
apogeeTime = 20.54; % Determined by looking at the above plot and zooming in.
% A clever person may be able to automate the selection process.
sp = 0.001; % Set the sample period
timeArr = 0:sp:apogeeTime; % The Z accelerometer has an approximate 
% sample rate of 100 SPS, or a period of 0.01 s. Using a precise time
% array makes a lot of things simpler. We'll resample later using timeArr.

% Table data are column vectors.
% You should transpose most table data before using.

%% DisplayTemperatures
% Below is the procedure for calculating the temperatures of the
% thermistors from the voltages in Channels C & D. Note the transposition
% and the isfinite to remove NaNs.
VoltC=transpose(AIM_Data.CVoltage(isfinite(AIM_Data.CVoltage)));
[TCC,TCK]=TfromV(VoltC);
TtimeC=transpose(AIM_Data.time_3(isfinite(AIM_Data.time_3)));
VoltD=transpose(AIM_Data.DVoltage(isfinite(AIM_Data.DVoltage)));
[TDC,TDK]=TfromV(VoltD);
TtimeD=transpose(AIM_Data.time_4(isfinite(AIM_Data.time_4)));
plot(TtimeC,TCC,TtimeD,TDC)
title('Temperature vs. Time Profile')
xlabel('time (s)')
ylabel('Temperature (°C)')
legend('Channel C','Channel D')

%% GetGPSdata
timeGPS=transpose(AIM_Data.time_8(isfinite(AIM_Data.time_8)));
timeGPSDesc = timeGPS(timeGPS>apogeeTime);
lat=transpose(AIM_Data.lat(isfinite(AIM_Data.lat)));
long=transpose(AIM_Data.long(isfinite(AIM_Data.long)));
GPSMSL=transpose(AIM_Data.GPSMSL(isfinite(AIM_Data.GPSMSL)));
GPSMSLDesc = GPSMSL(timeGPS>apogeeTime);
plot(timeGPS,lat, 'g*', timeGPS,long, 'b*', timeGPS,GPSMSL, 'r*')
title('GPS Data')
xlabel('time (s)')
ylabel('Latitude (°), Longitude (°), Altitude (m MSL)')
legend('lat', 'long', 'alt')

%% CalcGPSBaseline
latBase = mean(lat(timeGPS<0)); % Remove ; to get for mag vec & gravity
longBase = mean(long(timeGPS<0)); % Remove ; to get for mag vec
GPSMSLBase = mean(GPSMSL(timeGPS<0)); % Remove ; to get for mag vec & gravity

%% CalcGPSPositionChanges
latDel = 110574*(lat-latBase); % Distance in m
longDel = 111320*(long-longBase).*cos(pi/180*(lat+latBase)/2); % Distance in m
GPSAGL = GPSMSL - GPSMSLBase;
plot(timeGPS,longDel, 'g*', timeGPS,latDel, 'b*', timeGPS,GPSAGL, 'r*') % 
title('GPS Data converted to Position Change')
xlabel('time (s)')
ylabel('Distance (m)')
legend('x', 'y', 'AGL') % 

%% GetMagnetometerData
% The magnetometer chip is a left-handed coordinate system. To
% align X, Y, and Z with ENU, we need to flip the sign on X and Y.
timeMag=transpose(AIM_Data.time_9(isfinite(AIM_Data.time_9)));
magX = -transpose(AIM_Data.MagX(isfinite(AIM_Data.MagX))); % Flip X
magY = -transpose(AIM_Data.MagY(isfinite(AIM_Data.MagY))); % Flip Y
magZ =  transpose(AIM_Data.MagZ(isfinite(AIM_Data.MagZ))); % No flip
plot(timeMag,magX, timeMag,magY, timeMag,magZ)
title('Raw Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% ScaleMagData
magXMax = max(magX);
magXMin = min(magX);
magYMax = max(magY);
magYMin = min(magY);
magZMax = max(magZ);
magZMin = min(magZ);
magXsc = (2*magX-(magXMax+magXMin))/(magXMax-magXMin);
magYsc = (2*magY-(magYMax+magYMin))/(magYMax-magYMin);
magZsc = (2*magZ-(magZMax+magZMin))/(magZMax-magZMin);
magMagsc = sqrt(magXsc.^2 + magYsc.^2 + magZsc.^2);
plot(timeMag,magXsc, timeMag,magYsc, timeMag,magZsc, timeMag,magMagsc)
title('Scaled Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z','mag')

%% RenormalizeMagData
% The previous graph shows that the normalized magnitude isn't always 1.00
% so we renormalize so that all of the vectors have magnitude 1.
magXsc = magXsc./magMagsc;
magYsc = magYsc./magMagsc;
magZsc = magZsc./magMagsc;
plot(timeMag,magXsc, timeMag,magYsc, timeMag,magZsc)
title('Scaled Renormalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% Get Magnetometer Initial Orientation
% Get the magnetometer pre-launch data and calculate the baselines.
% We've skipped getting the pre-launch data as separate variables.
magX0 = mean(magXsc(timeMag<0));
magY0 = mean(magYsc(timeMag<0));
magZ0 = mean(magZsc(timeMag<0));
mag0 = norm([magX0 magY0 magZ0]);
magX0 = magX0/mag0;
magY0 = magY0/mag0;
magZ0 = magZ0/mag0;
magVec0 = [magX0; magY0; magZ0];

% Include the earth magnetic vector from https://www.ngdc.noaa.gov/geomag-web/#igrfwmm
% Use the pre-launch GPS Baselines from above to look up the vectors.
% N 34.4962, W 116.9593, MSL 861.9322 m, 9 July 2017
magN = 23491.6;
magE = 4926.6;
magD = 40795.3;
magUp = - magD;
magEarthVec = [magE; magN; magUp];
magEarthNorm = norm(magEarthVec);
magEarthVecNorm = magEarthVec/magEarthNorm;

% Calculate the variance in roll angle measurements
rollVar = var(atan2(magYsc(timeMag<0),magXsc(timeMag<0)));

%% ResampleMag
% Get the magnetometer data between launch and apogee by resamling at the timeArr
% values.
magXrs = interp1(timeMag,magXsc,timeArr,'pchip');
magYrs = interp1(timeMag,magYsc,timeArr,'pchip');
magZrs = interp1(timeMag,magZsc,timeArr,'pchip');

% Create the set of local measured magnetometer vectors. They are column
% vectors of the form [mx; my; mz] and the column index corresponds to the
% index of a given timeArr value
magVec = [magXrs; magYrs; magZrs]; % Set of magnetometer vectors
%renormalize again
n = length(timeArr);
for i = 1:n
    magVec(1:3,i) = magVec(1:3,i)/norm(magVec(1:3,i));
    roll(i) = atan2(magVec(2,i),magVec(1,i)); % May want to move this
end
plot(timeArr,magVec(1,1:n),timeArr,magVec(2,1:n),timeArr,magVec(3,1:n))
title('Scaled and Normalized Magnetometer Data')
xlabel('time (s)')
ylabel('Magnetic Field Strength')
legend('m_x', 'm_y', 'm_z')

%% RollVariance
% We're trying to determine typical jump sizes from one time step to the
% next.
droll = (unwrap(roll(2:n))-unwrap(roll(1:n-1)));
plot(timeArr(2:n),droll);
title('numerical derivative of roll')
xlabel('time (s)')
ylabel('droll/dt')
actRollVar = var(droll);

%% GetGyroData
% Getting IMU data. Note the transposition and NaN removal.
timeGyro=transpose(AIM_Data.time(isfinite(AIM_Data.time)));
gyroX = transpose(AIM_Data.GyroX(isfinite(AIM_Data.GyroX)));
gyroY = transpose(AIM_Data.GyroY(isfinite(AIM_Data.GyroY)));
gyroZ = transpose(AIM_Data.GyroZ(isfinite(AIM_Data.GyroZ)));
plot(timeGyro,gyroZ, timeGyro,gyroX, timeGyro,gyroY)
title('Gyro Data')
xlabel('time (s)')
ylabel('Rotation Rate (°/s)')
legend('\omega_z', '\omega_x', '\omega_y')

%% GetAccelData
timeAccel=transpose(AIM_Data.time_5(isfinite(AIM_Data.time_5)));
accelZ=transpose(AIM_Data.acceleration(isfinite(AIM_Data.acceleration)));
timeAccelLat=transpose(AIM_Data.time_6(isfinite(AIM_Data.time_6)));
accelX=transpose(AIM_Data.lat_XAccel_(isfinite(AIM_Data.lat_XAccel_)));
accelY=transpose(AIM_Data.lat_YAccel_(isfinite(AIM_Data.lat_YAccel_)));
plot(timeAccel,accelZ, timeAccelLat,accelX, timeAccelLat,accelY)
title('Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (g''s)')
legend('a_z', 'a_x', 'a_y')

% Get local gravitational field from https://www.sensorsone.com/local-gravity-calculator/
%  N 34.4962, GPS alt 861.9322 m
localg = 9.79425;

%% ConvertUnits
% Convert IMU data to radians per second and meters per second.
gyroX=gyroX*pi/180;
gyroY=gyroY*pi/180;
gyroZ=gyroZ*pi/180;
accelX=accelX*9.80665;
accelY=accelY*9.80665;
accelZ=accelZ*9.80665;

%% AccelPreLaunch
% Get the accel pre-launch data. Unlike the gyro data, it's not easy to use
% these data to calculate baselines or offsets unless you're certain that
% the rocket is perfectly vertical on the pad. However, if you've
% determined the baselines separately, these are really useful for the
% TRIAD method gravity vector.
accelZTimePre=timeAccel(timeAccel<0);
accelZPre=accelZ(timeAccel<0);
accelXYTimePre=timeAccelLat(timeAccelLat<0);
accelXPre=accelX(timeAccelLat<0);
accelYPre=accelY(timeAccelLat<0);
plot(accelZTimePre,accelZPre,accelXYTimePre,accelXPre,accelXYTimePre,accelYPre)
title('Pre-Launch Accelerometer Data')
xlabel('time (s)')
ylabel('Acceleration (m/s)')
legend('a_z', 'a_x', 'a_y')

% AccelPadVector
accelPreVec = [mean(accelXPre); mean(accelXPre); mean(accelZPre)];

%% Resample
% Get the accel data between launch and apogee by resamling at the timeArr
% values.
accelZrs = interp1(timeAccel,accelZ,timeArr,'pchip');
accelXrs = interp1(timeAccelLat,accelX,timeArr,'pchip');
accelYrs = interp1(timeAccelLat,accelY,timeArr,'pchip');

% Create the set of local measured acceleration vectors. They are column
% vectors of the form [ax; ay; az] and the column index corresponds to the
% index of a given timeArr value
accelVec = [accelXrs; accelYrs; accelZrs]; % Set of acceleration vectors

% Get the gyro pre-launch data and calculate the baselines or offsets.
% We've skipped getting the pre-launch data as separate variables.
gyroXbase = mean(gyroX(timeGyro<0));
gyroYbase = mean(gyroY(timeGyro<0));
gyroZbase = mean(gyroZ(timeGyro<0));

% Get the variance of the roll gyro
rgVar = var(gyroZ(timeGyro<0));

% Get the gyro data between launch and apogee by resamling at the timeArr
% values.
gyroXrs = interp1(timeGyro,gyroX,timeArr,'pchip');
gyroYrs = interp1(timeGyro,gyroY,timeArr,'pchip');
gyroZrs = interp1(timeGyro,gyroZ,timeArr,'pchip');

% Get z-gyro variance
% We're trying to determine typical jump sizes from one time step to the
% next.
dGyroZ = (gyroZrs(2:n) - gyroZrs(1:n-1));
plot(timeArr(2:n),dGyroZ)
title('numerical derivative of gyro z')
xlabel('time (s)')
ylabel('dwz/dt')
actGyroZVar = var(dGyroZ);

%% RollAngle
% This section can be removed, but remove references in further plotting
rGyro(1) = roll(1);
rGyrocd(1) = roll(1);
for i = 1:n-1
    rGyro(i+1) = rGyro(i) + sp*(gyroZrs(i)-gyroZbase);
    rGyrocd(i+1) = rGyrocd(i) + 0.5*sp * ...
        ((gyroZrs(i+1)-gyroZbase)+(gyroZrs(i)-gyroZbase));
end

%% RollSmoothing
roll = unwrap(roll);
gyTry = (gyroZrs - gyroZbase);
% Be sure to change the sample rate and the weight in smooth if you change
% the sample rate in the resampling
[res, theta] = smooth(roll, rollVar ,gyTry, rgVar, sp, roll(1), 10*sp);

%% CheckRollSmothing
% Replace GyroZrs with smoothed rates
gyroZrsSmooth(1:n-1) = theta(2:n)-theta(1:n-1);
gyroZrsSmooth(n) = gyroZrsSmooth(n-1);
gyroZrsSmooth = gyroZrsSmooth/sp;
% Create the set of local measured rotation-rate vectors. They are column
% vectors of the form [wx; wy; wz] and the column index corresponds to the
% index of a given timeArr value
rotRateVec = [gyroXrs-gyroXbase; gyroYrs-gyroYbase; gyroZrsSmooth];

%% RollKalmanFilter
roll = unwrap(roll);
Fk = [1 0; sp 1];
FkT = transpose(Fk);
Hk = [1 0; 0 1];
HkT = transpose(Hk);
Pk1k1(1:2,1:2,1) = [0 0; 0 0];
xk1k1(1:2,1) = [gyTry(1); roll(1)];
Rk = [rgVar 0; 0 rollVar];
Qk = [actGyroZVar 0; 0 actRollVar]; % Have to guess these values for now
II = [1 0; 0 1];
yk(1) = 0;
ykk(1) = 0;
for i = 1:n-1
    % Predict
    xkk1(1:2,i) = Fk*xk1k1(1:2,i);
    Pkk1(1:2,1:2,i) = Fk*Pk1k1(i)*FkT + Qk;
    % Update
    zk(1:2,i+1) = [gyTry(i+1) roll(i+1)];
    yk(1:2,i+1) = zk(1:2,i+1) - Hk*xkk1(1:2,i);
    Sk(1:2,1:2,i+1) = Rk + Hk*Pkk1(1:2,1:2,i)*HkT;
    Kk(1:2,1:2,i+1) = Pkk1(1:2,1:2,i)*HkT*inv(Sk(1:2,1:2,i+1)); % Kalman gain
    xk1k1(1:2,i+1) = xkk1(1:2,i) + Kk(1:2,1:2,i+1)*yk(1:2,i+1);
    Pk1k1(1:2,1:2,i+1) = (II - Kk(1:2,1:2,i+1)*Hk)*Pkk1(1:2,1:2,i)* ...
        transpose(II - Kk(1:2,1:2,i+1)*Hk)...
        + Kk(1:2,1:2,i+1)*Rk*transpose(Kk(1:2,1:2,i+1));
    ykk(1:2,i+1) = zk(1:2,i+1) - Hk*xk1k1(1:2,i+1); % Measurement post-fit residual
end
rgKalman = xk1k1(2,1:n); % The roll angle put out by the Kalman Filter
rdotKalman = xk1k1(1,1:n); % The roll rotation rate from the Kalman Filter.
rotRateVecK = [gyroXrs-gyroXbase; gyroYrs-gyroYbase; rdotKalman];

%% CheckRollRates
plot(timeArr, 180/pi*gyTry, timeArr, 180/pi*gyroZrsSmooth, timeArr, 180/pi*rdotKalman,'.')
title('New vs Old Rates')
xlabel('time (s)')
ylabel('Roll Rate (°/s)')
legend('rGyro', 'smooth', 'Kalman')

%% Check Rolls
plot(timeArr, 180/pi*roll, timeArr, 180/pi*rGyrocd, timeArr, 180/pi*theta ...
    ,timeArr, 180/pi*rgKalman)
title('Roll Method Comparison')
xlabel('time (s)')
ylabel('Roll Angle (°)')
legend('roll', 'rGyro', 'smooth', 'Kalman')


%% GetAltimeterdata
timeAlt=transpose(AIM_Data.time_7(isfinite(AIM_Data.time_7)));
Pdata=transpose(AIM_Data.pressure(isfinite(AIM_Data.pressure)));
altMSL=transpose(AIM_Data.PressureMSL(isfinite(AIM_Data.PressureMSL)));
altAGL=transpose(AIM_Data.PressureAGL(isfinite(AIM_Data.PressureAGL)));
plot(timeAlt,altMSL, timeAlt,altAGL)
title('Pressure Altimeter Data')
xlabel('time (s)')
ylabel('Altitude MSL (m), Altitude AGL (m)')
legend('MSL', 'AGL')

%% Get Altimeter Parts & Sync w GPS
PBase = Pdata(timeAlt<0);
altMSLBase = altMSL(timeAlt<0);
altAGLBase = altAGL(timeAlt<0);
timeAltBoost = timeAlt(timeAlt>=0 & timeAlt<=apogeeTime);
timeAltDesc = timeAlt(timeAlt>apogeeTime);
altMSLSyncGPS = interp1(timeAlt,altMSL,timeGPSDesc,'pchip');
plot(timeGPSDesc,GPSMSLDesc, timeGPSDesc,altMSLSyncGPS)
title('GPS and Altimeter Data')
xlabel('time (s)')
ylabel('GPS Altitude MSL (m), Altimeter Altitude MSL (m)')
legend('GPS', 'ALT')

%% Get GPS vs Alt Slope Intercept
coeffs = polyfit(altMSLSyncGPS, GPSMSLDesc, 1)
altMSLCorr = coeffs(1)*altMSL +coeffs(2);
altAGLCorr = altMSLCorr - mean (altMSLCorr(timeAlt<0));
plot(timeAlt,altMSLCorr,'.',timeAlt,altMSL,'.',timeGPS,GPSMSL,'+')
title('GPS and Corrected Altimeter Data')
xlabel('time (s)')
ylabel('Altitude MSL (m)')
legend('corr MSL','raw MSL', 'GPS')

%% Resample Corrected Altitude MSL
altAGLCorrrs = interp1(timeAlt,altAGLCorr,timeArr,'pchip');
plot(timeArr,altAGLCorrrs)
title('Corrected Altimeter Data')
xlabel('time (s)')
ylabel('Altitude AGL (m)')

%% Get Temperature vs Altitude
TCCrs = interp1(TtimeC,TCC,timeAltDesc,'pchip');
TDCrs = interp1(TtimeD,TDC,timeAltDesc,'pchip');
altAGLDesc = altAGLCorr(timeAlt>apogeeTime);
plot(altAGLDesc,TCCrs,'.',altAGLDesc,TDCrs,'.')
title('Temperature vs. Altitude')
xlabel('Altitude AGL (m)')
ylabel('Temperature (°C)')
legend('Channel C','Channel D')

%% Correct accel for g, and integrate for velocity and altitude
accelZrsNog = accelZrs - localg;
velZ = cumtrapz(timeArr,accelZrsNog);
altAccleZ = cumtrapz(timeArr,velZ);
plot(timeArr,accelZrsNog)
title('Accel Data to Choose Burnout')
xlabel('time (s)')
ylabel('Acceleration m^s/s')

%% Split for CD and Thrust
burnoutTime = 1.278; % Set from graph
accelTC = accelZrsNog(timeArr<burnoutTime);
velTC = velZ(timeArr<burnoutTime);
timeTC = timeArr(timeArr<burnoutTime);
accelCD = accelZrsNog(timeArr>=burnoutTime);
velCD = velZ(timeArr>=burnoutTime);
timeCD = timeArr(timeArr>=burnoutTime);
plot(timeTC,accelTC, timeTC,velTC)
title('Check Burn Portion of Ascent')
xlabel('time (s)')
ylabel('Accel & Vel')
legend('Accel', 'Vel')

%% Check CD part
plot(timeCD,accelCD, timeCD,velCD)
title('Check Coast Portion of Ascent')
xlabel('time (s)')
ylabel('Accel & Vel')
legend('Accel', 'Vel')

%% Input Rocket Data
% Data are in kg, m, K, Pa, etc.
rocketMass = 1.0815;
casingMass = 0.375;
propellantMass = 0.500;
diameter = 0.042;
Tabs = 298.71; % 78F COuld also get from thermistors before liftoff.
Pabs = mean(PBase);
MAM = 29; % Can correct for humidity if desired
Rgas = 8.314462618;
rho = MAM*Pabs*0.001/Rgas/Tabs; % Could make dynamic to follow altitude
Ap = 0.25*pi*diameter^2;

%% Calculate average CD
yCD = -2*(rocketMass + casingMass)*(accelCD+localg)./Ap./rho;
vsq = velCD.*velCD;
[si, St] = polyfit(vsq,yCD,1);
% Calculate the fit curve and the upper and lower bounds. Set 'predopt'
% to 'observation' for observational bounds or 'curve' for functional
% bounds. You usually can't see the difference between the fit and the
% functional bounds.
[CDfit,StErr] = polyconf(si,vsq,St,'predopt','curve');
CD = si(1)
plot(vsq,yCD,'.',vsq,CDfit, vsq, CDfit+StErr,'m--', vsq, CDfit-StErr, 'm--')
title('Check Average CD')
xlabel('v^2 (m^2/s^2)')
ylabel('-2m(a+g)/Ap \rho')
legend('Data', 'Fit', 'Upp Bound',  'Low Bound')

%% Calculate CD vs v
CDarr = yCD./vsq;
plot(velCD,CDarr,'.')
axis([ 0 inf 0 2])
title('Plot CD vs v')
xlabel('v (m/s)')
ylabel('CD')

%% Determine Thrust Curve
m0 = rocketMass+casingMass+propellantMass;
dmdt = propellantMass/burnoutTime;
mass = m0-dmdt*timeTC; %Begin with linear guess
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC;
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt); %Correct mass with results
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %2nd Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %3rd Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %4th Iteration
impulse = trapz(timeTC,Tx)
dmassdt = Tx/impulse*propellantMass;
mass = m0 - cumtrapz(timeTC,dmassdt);
Tx = mass.*(accelTC+localg)+0.5*CD*Ap*rho*velTC.*velTC; %5th Iteration
impulse = trapz(timeTC,Tx)

yyaxis left
plot(timeTC,Tx)
title('Thrust Curve and Mass Change')
xlabel('time (s)')
ylabel('Thrust (N)')
yyaxis right
plot(timeTC,mass)
ylabel('Rocket Mass (kg)')