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