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
Branches
Tags
No related merge requests found
......@@ -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);
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))];
% 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);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment