diff --git a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
index 2fe4fa7f0f3392f00039ab40bfa6c67ff6c68ba5..c101dbfd8564efdcb5a4712e5f50bd08bf08287e 100644
--- a/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
+++ b/RoccarasoFlight/postprocessing/RCS/descentPostProcess.m
@@ -52,65 +52,127 @@ para.descent.altitudetime = [para.descent.pressureRCSalt(:,1); para.descent.stat
     para.descent.static2alt(:,1); para.descent.NASalt(:,1)];
 para.descent.altitude = [para.descent.pressureRCSalt(:,2); ...
      para.descent.static1alt(:,2); para.descent.static2alt(:,2); -para.descent.NASalt(:,2) + h0];
+% para.descent.altitude = movmean(para.descent.altitude, 100);
 
-para.descent.NASvd = main.NASData(main.NASData(:,1)>desc(1) & main.NASData(:,1)<desc(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,:);
 
+baseline = @(x) interp1(para.descent.altitudetime, para.descent.altitude, x);
+
+% NAS vertical speed
+para.descent.NASvd = main.NASData(main.NASData(:,1)>desc(1) & main.NASData(:,1)<desc(2),[1,7]);
+
+
 [~, ~, ~, para.descent.rhoVec] = atmosisa(para.descent.altitude);
 
+% Model for speed vs altitude
+para.descent.cfit = fit(para.descent.altitudetime, para.descent.altitude, 'poly1');
+
+
+
+%% Descent Cd evaluation
+para.descent.refMass  = M;
+para.descent.paraS    = S;
+
+N = 100;
+problem.objective = @(x)errFun(x, para, baseline);
+problem.x0 = para.descent.cfit.p1*ones(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.nonlcon = @(x)nonlincon(x, para, baseline);
+problem.solver = 'fmincon';
+problem.options = optimoptions('fmincon', 'Display', 'iter', 'Algorithm','active-set');
 
-% Pressure @ drogue descent
+V = fmincon(problem);
 
+tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
+hvec_int = para.descent.altitude(1) + cumtrapz(tvec, 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);
+
+% Plot Altitude profile
 figure('Name', [name, 'DescentAlt'])
 plot(para.descent.pressureRCSalt(:,1), para.descent.pressureRCSalt(:,2)); hold on
 plot(para.descent.static1alt(:,1), para.descent.static1alt(:,2))
 plot(para.descent.static2alt(:,1), para.descent.static2alt(:,2))
 plot(para.descent.NASalt(:,1), -para.descent.NASalt(:,2)+h0)
 legend('RCS analogic', 'STATIC 1', 'STATIC 2', 'NAS')
-grid on
-
-% Model for speed vs altitude
-
-para.descent.cfit = fit(para.descent.altitudetime, para.descent.altitude, 'poly2');
-para.descent.cfit2 = fit(para.descent.altitudetime, para.descent.altitude, 'poly1');
-
 xlim(desc)
 plot(para.descent.cfit);
+plot(tvec, hvec_int, 's-', 'DisplayName', 'Optimizer Result')
 
-para.descent.v_interp = differentiate(para.descent.cfit, para.descent.altitudetime);
-figure
-plot(para.descent.altitudetime, para.descent.v_interp);
+grid on
+xlabel('time [s]')
+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.static1alt(:,1), abs(para.descent.static1alt(:,2)- baseline(para.descent.static1alt(:,1)))./baseline(para.descent.static1alt(:,1)))
+plot(para.descent.static2alt(:,1), abs(para.descent.static2alt(:,2)- baseline(para.descent.static2alt(:,1)))./baseline(para.descent.static2alt(:,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')
+
+legend('RCS analogic', 'STATIC 1', 'STATIC 2', 'NAS', 'Optimizer Result')
+grid on
+axis tight
+xlabel('time [s]')
+ylabel('Altitude err [m]')
+
+figure('name',[name, ' Vertical speed'])
+yline(para.descent.cfit.p1);
 hold on
 plot(para.descent.NASvd(:,1), -para.descent.NASvd(:,2))
+plot(tvec, V);
 xlabel('Time [s]')
 ylabel('Vz [m/2]')
-%% Descent Cd evaluation
-para.descent.refMass  = M;
-para.descent.paraS    = S;
-para.descent.avgSpeed = -para.descent.cfit.p1;
-para.descent.Cd       = 2 * para.descent.refMass * 9.81 ./ ...
-                       (para.descent.paraS * para.descent.v_interp.^2 .*rhofun(para.descent.cfit(para.descent.altitudetime)));
+grid on; axis tight
+fprintf('%s Cd = %3.2f +/- %7.5f', name, para.descent.Cd, para.descent.Cd_std)
 
-para.descent.Cd2       = 2 * para.descent.refMass * 9.81 ./ ...
-                       (para.descent.paraS * para.descent.cfit2.p1.^2 .*rhofun(para.descent.cfit(para.descent.altitudetime)));
+end
 
-figure
-plot(para.descent.altitudetime, para.descent.Cd )
-xlabel('Time [s]')
-ylabel('Cd [-]')
+function rho = rhofun(h)
 
-hold on
-plot(para.descent.altitudetime, para.descent.Cd2 )
-legend('poly2', 'poly1')
-fprintf('%s Cd = %3.2f~%3.2f\n', name, para.descent.Cd)
-fprintf('%s Vz = %3.2f m/s\n', name, para.descent.avgSpeed)
+[~, ~, ~, rho] = atmosisa(h(:));
 
 end
 
-function rho = rhofun(h)
+function X = errFun(V, para, baseline)
+
+g = 9.81;
+S = para.descent.paraS;
+M = para.descent.refMass;
+
+N = length(V);
+tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
+
+hvec = baseline(tvec);
+
+rvec = rhofun(hvec);
+
+Cd = 2 * M * g ./ (V.^2 .* rvec * S);
+
+X = std(Cd);
+end
+
+function [c, ceq] = nonlincon(V, para, baseline)
+
+% g = 9.81;
+% S = para.descent.paraS;
+% M = para.descent.refMass;
+
+N = length(V);
+tvec = linspace(para.descent.altitudetime(1),para.descent.altitudetime(end), N);
+
+hvec = baseline(tvec);
+hvec_int = hvec(1) + cumtrapz(tvec, V);
 
-[~, ~, ~, rho] = atmosisa(h);
 
+ceq = [];
+c = abs(hvec_int(:) - hvec(:))./hvec(:) - 0.005;
 end
\ No newline at end of file