diff --git a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
index ecd296187b7d008af88b79d2c3787740fc6f50d1..8583211dddea2bf25c239142ce950eb33009531b 100644
--- a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
+++ b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
@@ -2,15 +2,13 @@ clear; clc; close all;
%% adding path and recall data
addpath('Functions&Files')
-addpath("..\commonFunctions\");
+addpath(genpath("..\commonFunctions\"));
main = importData('main');
h0 = atmospalt(main.STATIC_PRESSURE1(1,2));
-
-
%% Time of flight phases
-drogueopening = main.EVENTData.timestamp(9) + [0,3];
+drogueopening = main.EVENTData.timestamp(9) + [0,10];
droguedescent = [main.EVENTData.timestamp(9) + 3, main.EVENTData.timestamp(12)];
mainopening = main.EVENTData.timestamp(12) + [0, 5];
maindescent = [main.EVENTData.timestamp(12) + 5, 128];
@@ -20,6 +18,7 @@ maindescent = [main.EVENTData.timestamp(12) + 5, 128];
t_acc = main.IMU(:,1);
acc = main.IMU(:,2:4);
+figure
plot(t_acc, acc/9.81)
grid on
axis tight
@@ -32,7 +31,7 @@ legend('x', 'y', 'z')
t_baro = main.STATIC_PRESSURE1(:,1);
baro = main.STATIC_PRESSURE1(:,2);
-
+figure
plot(t_baro, atmospalt(baro))
grid on
axis tight
@@ -41,8 +40,8 @@ xlabel('Time elapsed [s]')
ylabel('Pressure altitude [m]')
%% Drogue Descent Post
-% droguePara = descentPost(main, droguedescent, h0, 'drogue', (0.58/0.93)^2*pi, 21.95);
-% mainPara = descentPost(main, maindescent, h0, 'main', 7.34, 21.90);
+droguePara.descent = descentPost(main, droguedescent, h0, 'drogue', (0.58/0.93)^2*pi, 21.95);
+mainPara.descent = descentPost(main, maindescent, h0, 'main', 7.34, 21.90);
%% Attitude visualization
sensor = load('NAS quaternion.mat');
@@ -66,85 +65,125 @@ A_dcm = (qs.^2 - pagemtimes(qq, 'transpose', qq, 'none')) .* eye(3) + ...
% animateOrientation(A_dcm, time_qq);
%% Opening force estimation
+droguePara.opening = openingparameters(main, drogueopening, h0, 'drogue', (0.58/0.93)^2*pi, 21.95, droguePara.descent.Cd, 1.4);
+mainPara.opening = openingparameters(main, mainopening , h0, 'main' , 7.34 , 21.90, 1.7, 1.6);
+
+
+
+function para = openingparameters(main, tvec, h0, name, S, M, Cd, Cx)
+
+static1 = main.STATIC_PRESSURE1(main.STATIC_PRESSURE1(:,1)>tvec(1) & main.STATIC_PRESSURE1(:,1)<tvec(2),:);
+static2 = main.STATIC_PRESSURE2(main.STATIC_PRESSURE2(:,1)>tvec(1) & main.STATIC_PRESSURE2(:,1)<tvec(2),:);
+gps = main.GPS(main.GPS(:,1)>tvec(1) & main.GPS(:,1)<tvec(2),:);
+
+fun_palt = @(x) [x(:,1), atmospalt(x(:,2))];
+
+para.altitude = fun_palt(0.5*(static1 + static2));
+para.xyzspeed = gps(:,[1, 5,6,7]);
+para.xyzspeed(:,5) = vecnorm( para.xyzspeed(:,[2,3,4]), 2, 2);
+para.v0ref = mean(para.xyzspeed(:,5));
+para.rhoref = mean(rhofun(para.altitude(:,2)));
+figure
+plot(para.xyzspeed(:,1), para.xyzspeed(:,[2,3,4,5]))
+hold on
+grid on
+axis tight
+legend('n', 'e', 'd', 'norm')
+xlabel('time [s]'); ylabel('GPS speed [m/s]')
+
+Spec_can_load = (2.2 * M) / (Cd * S * 10.76);
+tf = 8*sqrt(S/pi)*2/para.v0ref;
+
+A = (2 * M )./(Cd * S * para.rhoref * para.v0ref .* tf);
+FRF_BP = computeForceReductionFactorFromBallisticParameter(A, 1);
+FRF_SL = computeForceReductionFactorFromCanopyLoading(Spec_can_load);
-function para = descentPost(main, desc, h0, name, S, M)
+F_max_expected_1 = 0.5 * para.rhoref * para.v0ref^2 * S * Cd * Cx * FRF_BP
+F_max_expected_2 = 0.5 * para.rhoref * para.v0ref^2 * S * Cd * Cx * FRF_SL
+
+acc_1 = F_max_expected_1 / M
+acc_2 = F_max_expected_2 / M
+
+end
+
+function para = descentPost(main, tvec, h0, name, S, M)
fun_palt = @(x) [x(:,1), atmospalt(x(:,2))];
% Collecting sensor data
-para.descent.pressureRCS = main.RCS_PRESSURE(main.RCS_PRESSURE(:,1)>desc(1) & main.RCS_PRESSURE(:,1)<desc(2),:);
-static1 = main.STATIC_PRESSURE1(main.STATIC_PRESSURE1(:,1)>desc(1) & main.STATIC_PRESSURE1(:,1)<desc(2),:);
-static2 = main.STATIC_PRESSURE2(main.STATIC_PRESSURE2(:,1)>desc(1) & main.STATIC_PRESSURE2(:,1)<desc(2),:);
+para.pressureRCS = main.RCS_PRESSURE(main.RCS_PRESSURE(:,1)>tvec(1) & main.RCS_PRESSURE(:,1)<tvec(2),:);
+static1 = main.STATIC_PRESSURE1(main.STATIC_PRESSURE1(:,1)>tvec(1) & main.STATIC_PRESSURE1(:,1)<tvec(2),:);
+static2 = main.STATIC_PRESSURE2(main.STATIC_PRESSURE2(:,1)>tvec(1) & main.STATIC_PRESSURE2(:,1)<tvec(2),:);
-para.descent.static = [static1(:,1), 0.5*(static1(:,2) + static2(:,2))];
+para.static = [static1(:,1), 0.5*(static1(:,2) + static2(:,2))];
-para.descent.pressureRCSalt = fun_palt(para.descent.pressureRCS);
-para.descent.staticalt = fun_palt(para.descent.static);
+para.pressureRCSalt = fun_palt(para.pressureRCS);
+para.staticalt = fun_palt(para.static);
-para.descent.NASalt = main.NASData(main.NASData(:,1)>desc(1) & main.NASData(:,1)<desc(2),[1,4]);
+para.NASalt = main.NASData(main.NASData(:,1)>tvec(1) & main.NASData(:,1)<tvec(2),[1,4]);
-para.descent.altitudetime = [para.descent.pressureRCSalt(:,1); para.descent.static(:,1)];
-para.descent.altitude = [para.descent.pressureRCSalt(:,2); para.descent.static(:,2)];
-% para.descent.altitude = movmean(para.descent.altitude, 100);
+para.altitudetime = [para.pressureRCSalt(:,1); para.static(:,1)];
+para.altitude = [para.pressureRCSalt(:,2); para.static(:,2)];
+% para.altitude = movmean(para.altitude, 100);
-para.descent.GPS = main.GPS(main.GPS(:,1)>desc(1) & main.GPS(:,1)<desc(2),[1,7]);
+para.GPS = main.GPS(main.GPS(:,1)>tvec(1) & main.GPS(:,1)<tvec(2),[1,7]);
% Post procesing of altitude values
-[para.descent.altitudetime, IC] = unique(para.descent.altitudetime);
-para.descent.altitude = para.descent.altitude(IC);
-para.descent.altitude = movmean(para.descent.altitude, 10);
-[para.descent.altitudetime, idx_sort] = sort(para.descent.altitudetime);
-para.descent.altitude = para.descent.altitude(idx_sort,:);
+[para.altitudetime, IC] = unique(para.altitudetime);
+para.altitude = para.altitude(IC);
+para.altitude = movmean(para.altitude, 10);
+[para.altitudetime, idx_sort] = sort(para.altitudetime);
+para.altitude = para.altitude(idx_sort,:);
-baseline = @(x) interp1(para.descent.altitudetime, para.descent.altitude, x);
+baseline = @(x) interp1(para.altitudetime, para.altitude, x);
% NAS vertical speed
-para.descent.NASvd = main.NASData(main.NASData(:,1)>desc(1) & main.NASData(:,1)<desc(2),[1,7]);
+para.NASvd = main.NASData(main.NASData(:,1)>tvec(1) & main.NASData(:,1)<tvec(2),[1,7]);
-[~, ~, ~, para.descent.rhoVec] = atmosisa(para.descent.altitude);
+[~, ~, ~, para.rhoVec] = atmosisa(para.altitude);
% Model for speed vs altitude
-para.descent.cfit = fit(para.descent.altitudetime, para.descent.altitude, 'poly1');
+para.cfit = fit(para.altitudetime, para.altitude, 'poly1');
%% Descent Cd evaluation
-para.descent.refMass = M;
-para.descent.paraS = S;
+para.refMass = M;
+para.paraS = S;
N = 100;
problem.objective = @(x)errFun(x, para, baseline);
-problem.x0 = para.descent.cfit.p1*ones(N,1);
+problem.x0 = para.cfit.p1*ones(N,1);
% problem.A = eye(N) - diag(ones(N-1, 1), 1);
% problem.A = problem.A(1:end-1,:);
% problem.B = zeros(N, 1);
-problem.lb = para.descent.cfit.p1*ones(N,1)*1.2;
-problem.ub = para.descent.cfit.p1*ones(N,1)*0.8;
+problem.lb = para.cfit.p1*ones(N,1)*1.2;
+problem.ub = para.cfit.p1*ones(N,1)*0.8;
problem.nonlcon = @(x)nonlincon(x, para, baseline);
problem.solver = 'fmincon';
-problem.options = optimoptions('fmincon', 'Display', 'iter', 'Algorithm','active-set');
+problem.options = optimoptions('fmincon', 'Display', 'none', 'Algorithm','active-set');
V = fmincon(problem);
-tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
-hvec_int = para.descent.altitude(1) + cumtrapz(tvec, V);
+tvecCd = linspace(para.altitudetime(1),para.altitudetime(end), N);
+hvec_int = para.altitude(1) + cumtrapz(tvecCd, V);
Cd_vec = 2 * M * 9.81 ./ (V.^2 .*rhofun(hvec_int) * S);
-para.descent.Cd = mean(Cd_vec);
-para.descent.Cd_std = std(Cd_vec);
+para.Cd = mean(Cd_vec);
+para.Cd_std = std(Cd_vec);
% Plot Altitude profile
figure('Name', [name, 'DescentAlt'])
-plot(para.descent.pressureRCSalt(:,1), para.descent.pressureRCSalt(:,2)); hold on
-plot(para.descent.staticalt(:,1), para.descent.staticalt(:,2))
-plot(para.descent.NASalt(:,1), -para.descent.NASalt(:,2)+h0)
+plot(para.pressureRCSalt(:,1), para.pressureRCSalt(:,2)); hold on
+plot(para.staticalt(:,1), para.staticalt(:,2))
+plot(para.NASalt(:,1), -para.NASalt(:,2)+h0)
legend('RCS analogic', 'STATIC', 'NAS')
-xlim(desc)
-plot(para.descent.cfit);
-plot(tvec, hvec_int, 's-', 'DisplayName', 'Optimizer Result')
+xlim(tvec)
+plot(para.cfit);
+plot(tvecCd, hvec_int, 's-', 'DisplayName', 'Optimizer Result')
grid on
xlabel('time [s]')
@@ -152,10 +191,10 @@ ylabel('Altitude [m]')
% Plot distance from baseline
figure('Name', [name, 'DescentAlt from baseline'])
-plot(para.descent.pressureRCSalt(:,1), abs(para.descent.pressureRCSalt(:,2) - baseline(para.descent.pressureRCSalt(:,1)))./baseline(para.descent.pressureRCSalt(:,1))); hold on
-plot(para.descent.staticalt(:,1), abs(para.descent.staticalt(:,2)- baseline(para.descent.staticalt(:,1)))./baseline(para.descent.staticalt(:,1)))
-plot(para.descent.NASalt(:,1), abs(-para.descent.NASalt(:,2)+h0- baseline(para.descent.NASalt(:,1)))./baseline(para.descent.NASalt(:,1)))
-plot(tvec, abs(hvec_int - baseline(tvec)')./ baseline(tvec)', 's-', 'DisplayName', 'Optimizer Result')
+plot(para.pressureRCSalt(:,1), abs(para.pressureRCSalt(:,2) - baseline(para.pressureRCSalt(:,1)))./baseline(para.pressureRCSalt(:,1))); hold on
+plot(para.staticalt(:,1), abs(para.staticalt(:,2)- baseline(para.staticalt(:,1)))./baseline(para.staticalt(:,1)))
+plot(para.NASalt(:,1), abs(-para.NASalt(:,2)+h0- baseline(para.NASalt(:,1)))./baseline(para.NASalt(:,1)))
+plot(tvecCd, abs(hvec_int - baseline(tvecCd)')./ baseline(tvecCd)', 's-', 'DisplayName', 'Optimizer Result')
legend('RCS analogic', 'STATIC', 'NAS', 'Optimizer Result')
grid on
@@ -164,16 +203,16 @@ xlabel('time [s]')
ylabel('Altitude err [m]')
figure('name',[name, ' Vertical speed'])
-yline(para.descent.cfit.p1);
+yline(para.cfit.p1);
hold on
-plot(para.descent.NASvd(:,1), -para.descent.NASvd(:,2))
-plot(para.descent.GPS(:,1), -para.descent.GPS(:,2))
-plot(tvec, V);
+plot(para.NASvd(:,1), -para.NASvd(:,2))
+plot(para.GPS(:,1), -para.GPS(:,2))
+plot(tvecCd, V);
% fplot(baseline, desc)
xlabel('Time [s]')
ylabel('Vz [m/2]')
grid on; axis tight
-fprintf('%s Cd = %3.2f +/- %7.5f\n', name, para.descent.Cd, 3*para.descent.Cd_std)
+fprintf('%s Cd = %3.2f +/- %7.5f\n', name, para.Cd, 3*para.Cd_std)
end
@@ -186,11 +225,11 @@ end
function X = errFun(V, para, baseline)
g = 9.81;
-S = para.descent.paraS;
-M = para.descent.refMass;
+S = para.paraS;
+M = para.refMass;
N = length(V);
-tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
+tvec = linspace(para.altitudetime(1),para.altitudetime(end), N);
hvec = baseline(tvec);
@@ -204,11 +243,11 @@ end
function [c, ceq] = nonlincon(V, para, baseline)
% g = 9.81;
-% S = para.descent.paraS;
-% M = para.descent.refMass;
+% S = para.paraS;
+% M = para.refMass;
N = length(V);
-tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
+tvec = linspace(para.altitudetime(1),para.altitudetime(end), N);
hvec = baseline(tvec);
hvec_int = hvec(1) + cumtrapz(tvec, V);
@@ -216,4 +255,4 @@ hvec_int = hvec(1) + cumtrapz(tvec, V);
ceq = [];
c = abs(hvec_int(:) - hvec(:)) - 10;
-end
\ No newline at end of file
+end