diff --git a/RoccarasoFlight/postprocessing/RCS/cadrega.m b/RoccarasoFlight/postprocessing/RCS/cadrega.m index 5fc28273fff02c08c7b2863fcd2850ff2d6e5528..a0175f63d956ecef87d0fe9a4eb8b07f43ed2555 100644 --- a/RoccarasoFlight/postprocessing/RCS/cadrega.m +++ b/RoccarasoFlight/postprocessing/RCS/cadrega.m @@ -3,7 +3,7 @@ clc; close all; clear; Meuroc_vera = 25.11; v_tgt = 23; [~, ~, ~, rho_tgt] = atmosisa(350 + 160); % Considerando l'elevazione di costancia -S = (0.58/0.93)^2*pi; +S = 1; % S = 1.0; Cd = 2*Meuroc_vera * 9.81 / (v_tgt^2 * rho_tgt*S); diff --git a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m index 8583211dddea2bf25c239142ce950eb33009531b..e47d7676dedcdec5e4ed7489b3fe75e870db288c 100644 --- a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m +++ b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m @@ -155,22 +155,36 @@ para.cfit = fit(para.altitudetime, para.altitude, 'poly1'); para.refMass = M; para.paraS = S; -N = 100; -problem.objective = @(x)errFun(x, para, baseline); -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.cfit.p1*ones(N,1)*1.2; -problem.ub = para.cfit.p1*ones(N,1)*0.8; -problem.nonlcon = @(x)nonlincon(x, para, baseline); +N = 200; +tvecCd = linspace(para.altitudetime(1), para.altitudetime(end), N); +problem.objective = @(x)errFun(x, para, tvecCd); +% problem.x0 = para.cfit.p1*ones(N,1); +problem.Aineq = eye(N) - diag(ones(N-1, 1), 1); +problem.Aineq = -problem.Aineq(1:end-1,:); +problem.Bineq = zeros(N-1, 1); + +% problem.lb = para.cfit.p1*ones(N,1)*1.2; +% problem.ub = para.cfit.p1*ones(N,1)*0.8; +problem.x0 = baseline(tvecCd); +problem.lb = baseline(tvecCd) - 10; +problem.ub = baseline(tvecCd) + 10; + +% problem.nonlcon = @(x)nonlincon(x, para, baseline); problem.solver = 'fmincon'; -problem.options = optimoptions('fmincon', 'Display', 'none', 'Algorithm','active-set'); +problem.options = optimoptions('fmincon', 'Display', 'iter-detailed', 'Algorithm','active-set', 'MaxFunctionEvaluations',1e6, 'MaxIterations', 1000); -V = fmincon(problem); +hvec_int = fmincon(problem); tvecCd = linspace(para.altitudetime(1),para.altitudetime(end), N); -hvec_int = para.altitude(1) + cumtrapz(tvecCd, V); +% hvec_int = para.altitude(1) + cumtrapz(tvecCd, V); +dt = tvecCd(2) - tvecCd(1); +V = zeros(N, 1); +V(1) = (hvec_int(2) - hvec_int(1))/dt; +V(end) = (hvec_int(end) - hvec_int(end-1))/dt; + +V(2:end-1) = hvec_int(3:end) - hvec_int(1:end-2); + + Cd_vec = 2 * M * 9.81 ./ (V.^2 .*rhofun(hvec_int) * S); para.Cd = mean(Cd_vec); para.Cd_std = std(Cd_vec); @@ -179,11 +193,13 @@ para.Cd_std = std(Cd_vec); figure('Name', [name, 'DescentAlt']) 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') +legend('RCS analogic', 'STATIC') xlim(tvec) plot(para.cfit); plot(tvecCd, hvec_int, 's-', 'DisplayName', 'Optimizer Result') +plot(tvecCd, problem.ub, 's-', 'DisplayName', 'Optimizer ub') +plot(tvecCd, problem.lb, 'x-', 'DisplayName', 'Optimizer lb') +plot(tvecCd, problem.x0, 'x-', 'DisplayName', 'Optimizer x0') grid on xlabel('time [s]') @@ -194,7 +210,7 @@ figure('Name', [name, 'DescentAlt from baseline']) 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') +plot(tvecCd, abs(hvec_int - baseline(tvecCd))./ baseline(tvecCd), 's-', 'DisplayName', 'Optimizer Result') legend('RCS analogic', 'STATIC', 'NAS', 'Optimizer Result') grid on @@ -222,37 +238,47 @@ function rho = rhofun(h) end -function X = errFun(V, para, baseline) +% function X = errFun(V, para, baseline) +% +% g = 9.81; +% S = para.paraS; +% M = para.refMass; +% +% N = length(V); +% tvec = linspace(para.altitudetime(1),para.altitudetime(end), N); +% +% hvec = baseline(tvec); +% +% rvec = rhofun(hvec); +% +% Cd = 2 * M * g ./ (V.^2 .* rvec * S); +% +% X = std(Cd); +% end + +function X = errFun(hvec, para, tvec) g = 9.81; S = para.paraS; M = para.refMass; -N = length(V); -tvec = linspace(para.altitudetime(1),para.altitudetime(end), N); +N = length(hvec); +% tvec = linspace(para.altitudetime(1),para.altitudetime(end), N); -hvec = baseline(tvec); +V_vec = zeros(N, 1); -rvec = rhofun(hvec); +dt = tvec(2) - tvec(1); -Cd = 2 * M * g ./ (V.^2 .* rvec * S); +V_vec(1) = (hvec(2) - hvec(1))/dt; +V_vec(end) = (hvec(end) - hvec(end-1))/dt; -X = std(Cd); -end +V_vec(2:end-1) = hvec(3:end) - hvec(1:end-2); -function [c, ceq] = nonlincon(V, para, baseline) - -% g = 9.81; -% S = para.paraS; -% M = para.refMass; +rvec = rhofun(hvec); -N = length(V); -tvec = linspace(para.altitudetime(1),para.altitudetime(end), N); +Cd = 2 * M * g ./ (V_vec.^2 .* rvec * S); -hvec = baseline(tvec); -hvec_int = hvec(1) + cumtrapz(tvec, V); +X = std(Cd); +end -ceq = []; -c = abs(hvec_int(:) - hvec(:)) - 10; -end diff --git a/RoccarasoFlight/postprocessing/RCS/lol.avi b/RoccarasoFlight/postprocessing/RCS/lol.avi deleted file mode 100644 index 62496eece41c0f8914eda08fe87a9ccf7c791526..0000000000000000000000000000000000000000 Binary files a/RoccarasoFlight/postprocessing/RCS/lol.avi and /dev/null differ diff --git a/RoccarasoFlight/postprocessing/RCS/rotationTest.m b/RoccarasoFlight/postprocessing/RCS/rotationTest.m new file mode 100644 index 0000000000000000000000000000000000000000..2ffcf368f55dad044489883454a9536e9538c5ab --- /dev/null +++ b/RoccarasoFlight/postprocessing/RCS/rotationTest.m @@ -0,0 +1,52 @@ +clc; close all; clear; +addpath('Functions&Files'); + +v0 = 200; +M = 20; +g = 9.81; +v0dir = [1; 2; 5]; +v0 = v0dir / norm(v0dir) * v0; + +x0 = [0;0;0]; + +y0 = [x0;v0]; + +dyn = @(t, y) [y(4:6);0;0;-g]; + +% ef = @(t, y) deal(y(3),0,0); + +[t, y] = ode45(dyn, [0, 100], y0, odeset('Events', @(t, y)ef(t,y))); + +plot3(y(:,1), y(:,2), y(:,3)) +axis equal +grid on + +vec0 = [1,0,0]'; + +N = length(t); + + +dcm = zeros(3, 3, N); + +for ii = 1:N + + vec = y(ii,4:6)'; + vec = vec / norm(vec); + v = cross(vec0, vec); + s = norm(v); + c = dot(vec0, vec); + + vx = [0, -v(3), v(2); v(3), 0, -v(1); -v(2), v(1), 0]; + + dcm(:,:,ii) = (eye(3) + vx + vx * vx / (1 + c))'; + + +end + +animateOrientation(dcm, t) +function [val, isterm, dir] = ef(~, y) + +val = y(3); +isterm = 1; +dir = 0; +end \ No newline at end of file