Skip to content
Snippets Groups Projects
Commit e5115727 authored by davideperani's avatar davideperani
Browse files

Update descentPostProcess.m 

Added first attempt to estimate opening force, spoiler, real forces are way lower than expected
parent c964fbaa
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
+ 99
− 60
View file @ e5115727
...@@ -2,15 +2,13 @@ clear; clc; close all; ...@@ -2,15 +2,13 @@ clear; clc; close all;
%% adding path and recall data %% adding path and recall data
addpath('Functions&Files') addpath('Functions&Files')
addpath("..\commonFunctions\"); addpath(genpath("..\commonFunctions\"));
main = importData('main'); main = importData('main');
h0 = atmospalt(main.STATIC_PRESSURE1(1,2)); h0 = atmospalt(main.STATIC_PRESSURE1(1,2));
%% Time of flight phases %% 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)]; droguedescent = [main.EVENTData.timestamp(9) + 3, main.EVENTData.timestamp(12)];
mainopening = main.EVENTData.timestamp(12) + [0, 5]; mainopening = main.EVENTData.timestamp(12) + [0, 5];
maindescent = [main.EVENTData.timestamp(12) + 5, 128]; maindescent = [main.EVENTData.timestamp(12) + 5, 128];
...@@ -20,6 +18,7 @@ maindescent = [main.EVENTData.timestamp(12) + 5, 128]; ...@@ -20,6 +18,7 @@ maindescent = [main.EVENTData.timestamp(12) + 5, 128];
t_acc = main.IMU(:,1); t_acc = main.IMU(:,1);
acc = main.IMU(:,2:4); acc = main.IMU(:,2:4);
figure
plot(t_acc, acc/9.81) plot(t_acc, acc/9.81)
grid on grid on
axis tight axis tight
...@@ -32,7 +31,7 @@ legend('x', 'y', 'z') ...@@ -32,7 +31,7 @@ legend('x', 'y', 'z')
t_baro = main.STATIC_PRESSURE1(:,1); t_baro = main.STATIC_PRESSURE1(:,1);
baro = main.STATIC_PRESSURE1(:,2); baro = main.STATIC_PRESSURE1(:,2);
figure
plot(t_baro, atmospalt(baro)) plot(t_baro, atmospalt(baro))
grid on grid on
axis tight axis tight
...@@ -41,8 +40,8 @@ xlabel('Time elapsed [s]') ...@@ -41,8 +40,8 @@ xlabel('Time elapsed [s]')
ylabel('Pressure altitude [m]') ylabel('Pressure altitude [m]')
%% Drogue Descent Post %% Drogue Descent Post
% droguePara = descentPost(main, droguedescent, h0, 'drogue', (0.58/0.93)^2*pi, 21.95); droguePara.descent = descentPost(main, droguedescent, h0, 'drogue', (0.58/0.93)^2*pi, 21.95);
% mainPara = descentPost(main, maindescent, h0, 'main', 7.34, 21.90); mainPara.descent = descentPost(main, maindescent, h0, 'main', 7.34, 21.90);
%% Attitude visualization %% Attitude visualization
sensor = load('NAS quaternion.mat'); sensor = load('NAS quaternion.mat');
...@@ -66,85 +65,125 @@ A_dcm = (qs.^2 - pagemtimes(qq, 'transpose', qq, 'none')) .* eye(3) + ... ...@@ -66,85 +65,125 @@ A_dcm = (qs.^2 - pagemtimes(qq, 'transpose', qq, 'none')) .* eye(3) + ...
% animateOrientation(A_dcm, time_qq); % animateOrientation(A_dcm, time_qq);
%% Opening force estimation %% 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);
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, desc, h0, name, S, M) function para = descentPost(main, tvec, h0, name, S, M)
fun_palt = @(x) [x(:,1), atmospalt(x(:,2))]; fun_palt = @(x) [x(:,1), atmospalt(x(:,2))];
% Collecting sensor data % Collecting sensor data
para.descent.pressureRCS = main.RCS_PRESSURE(main.RCS_PRESSURE(:,1)>desc(1) & main.RCS_PRESSURE(:,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)>desc(1) & main.STATIC_PRESSURE1(:,1)<desc(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)>desc(1) & main.STATIC_PRESSURE2(:,1)<desc(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.pressureRCSalt = fun_palt(para.pressureRCS);
para.descent.staticalt = fun_palt(para.descent.static); 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.altitudetime = [para.pressureRCSalt(:,1); para.static(:,1)];
para.descent.altitude = [para.descent.pressureRCSalt(:,2); para.descent.static(:,2)]; para.altitude = [para.pressureRCSalt(:,2); para.static(:,2)];
% para.descent.altitude = movmean(para.descent.altitude, 100); % 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 % Post procesing of altitude values
[para.descent.altitudetime, IC] = unique(para.descent.altitudetime); [para.altitudetime, IC] = unique(para.altitudetime);
para.descent.altitude = para.descent.altitude(IC); para.altitude = para.altitude(IC);
para.descent.altitude = movmean(para.descent.altitude, 10); para.altitude = movmean(para.altitude, 10);
[para.descent.altitudetime, idx_sort] = sort(para.descent.altitudetime); [para.altitudetime, idx_sort] = sort(para.altitudetime);
para.descent.altitude = para.descent.altitude(idx_sort,:); 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 % 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 % 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 %% Descent Cd evaluation
para.descent.refMass = M; para.refMass = M;
para.descent.paraS = S; para.paraS = S;
N = 100; N = 100;
problem.objective = @(x)errFun(x, para, baseline); 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 = eye(N) - diag(ones(N-1, 1), 1);
% problem.A = problem.A(1:end-1,:); % problem.A = problem.A(1:end-1,:);
% problem.B = zeros(N, 1); % problem.B = zeros(N, 1);
problem.lb = para.descent.cfit.p1*ones(N,1)*1.2; problem.lb = para.cfit.p1*ones(N,1)*1.2;
problem.ub = para.descent.cfit.p1*ones(N,1)*0.8; problem.ub = para.cfit.p1*ones(N,1)*0.8;
problem.nonlcon = @(x)nonlincon(x, para, baseline); problem.nonlcon = @(x)nonlincon(x, para, baseline);
problem.solver = 'fmincon'; problem.solver = 'fmincon';
problem.options = optimoptions('fmincon', 'Display', 'iter', 'Algorithm','active-set'); problem.options = optimoptions('fmincon', 'Display', 'none', 'Algorithm','active-set');
V = fmincon(problem); V = fmincon(problem);
tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N); tvecCd = linspace(para.altitudetime(1),para.altitudetime(end), N);
hvec_int = para.descent.altitude(1) + cumtrapz(tvec, V); hvec_int = para.altitude(1) + cumtrapz(tvecCd, V);
Cd_vec = 2 * M * 9.81 ./ (V.^2 .*rhofun(hvec_int) * S); Cd_vec = 2 * M * 9.81 ./ (V.^2 .*rhofun(hvec_int) * S);
para.descent.Cd = mean(Cd_vec); para.Cd = mean(Cd_vec);
para.descent.Cd_std = std(Cd_vec); para.Cd_std = std(Cd_vec);
% Plot Altitude profile % Plot Altitude profile
figure('Name', [name, 'DescentAlt']) figure('Name', [name, 'DescentAlt'])
plot(para.descent.pressureRCSalt(:,1), para.descent.pressureRCSalt(:,2)); hold on plot(para.pressureRCSalt(:,1), para.pressureRCSalt(:,2)); hold on
plot(para.descent.staticalt(:,1), para.descent.staticalt(:,2)) plot(para.staticalt(:,1), para.staticalt(:,2))
plot(para.descent.NASalt(:,1), -para.descent.NASalt(:,2)+h0) plot(para.NASalt(:,1), -para.NASalt(:,2)+h0)
legend('RCS analogic', 'STATIC', 'NAS') legend('RCS analogic', 'STATIC', 'NAS')
xlim(desc) xlim(tvec)
plot(para.descent.cfit); plot(para.cfit);
plot(tvec, hvec_int, 's-', 'DisplayName', 'Optimizer Result') plot(tvecCd, hvec_int, 's-', 'DisplayName', 'Optimizer Result')
grid on grid on
xlabel('time [s]') xlabel('time [s]')
...@@ -152,10 +191,10 @@ ylabel('Altitude [m]') ...@@ -152,10 +191,10 @@ ylabel('Altitude [m]')
% Plot distance from baseline % Plot distance from baseline
figure('Name', [name, 'DescentAlt 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.pressureRCSalt(:,1), abs(para.pressureRCSalt(:,2) - baseline(para.pressureRCSalt(:,1)))./baseline(para.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.staticalt(:,1), abs(para.staticalt(:,2)- baseline(para.staticalt(:,1)))./baseline(para.staticalt(:,1)))
plot(para.descent.NASalt(:,1), abs(-para.descent.NASalt(:,2)+h0- baseline(para.descent.NASalt(:,1)))./baseline(para.descent.NASalt(:,1))) plot(para.NASalt(:,1), abs(-para.NASalt(:,2)+h0- baseline(para.NASalt(:,1)))./baseline(para.NASalt(:,1)))
plot(tvec, abs(hvec_int - baseline(tvec)')./ baseline(tvec)', 's-', 'DisplayName', 'Optimizer Result') plot(tvecCd, abs(hvec_int - baseline(tvecCd)')./ baseline(tvecCd)', 's-', 'DisplayName', 'Optimizer Result')
legend('RCS analogic', 'STATIC', 'NAS', 'Optimizer Result') legend('RCS analogic', 'STATIC', 'NAS', 'Optimizer Result')
grid on grid on
...@@ -164,16 +203,16 @@ xlabel('time [s]') ...@@ -164,16 +203,16 @@ xlabel('time [s]')
ylabel('Altitude err [m]') ylabel('Altitude err [m]')
figure('name',[name, ' Vertical speed']) figure('name',[name, ' Vertical speed'])
yline(para.descent.cfit.p1); yline(para.cfit.p1);
hold on hold on
plot(para.descent.NASvd(:,1), -para.descent.NASvd(:,2)) plot(para.NASvd(:,1), -para.NASvd(:,2))
plot(para.descent.GPS(:,1), -para.descent.GPS(:,2)) plot(para.GPS(:,1), -para.GPS(:,2))
plot(tvec, V); plot(tvecCd, V);
% fplot(baseline, desc) % fplot(baseline, desc)
xlabel('Time [s]') xlabel('Time [s]')
ylabel('Vz [m/2]') ylabel('Vz [m/2]')
grid on; axis tight 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 end
...@@ -186,11 +225,11 @@ end ...@@ -186,11 +225,11 @@ end
function X = errFun(V, para, baseline) function X = errFun(V, para, baseline)
g = 9.81; g = 9.81;
S = para.descent.paraS; S = para.paraS;
M = para.descent.refMass; M = para.refMass;
N = length(V); 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 = baseline(tvec);
...@@ -204,11 +243,11 @@ end ...@@ -204,11 +243,11 @@ end
function [c, ceq] = nonlincon(V, para, baseline) function [c, ceq] = nonlincon(V, para, baseline)
% g = 9.81; % g = 9.81;
% S = para.descent.paraS; % S = para.paraS;
% M = para.descent.refMass; % M = para.refMass;
N = length(V); 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 = baseline(tvec);
hvec_int = hvec(1) + cumtrapz(tvec, V); hvec_int = hvec(1) + cumtrapz(tvec, V);
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment