diff --git a/RoccarasoFlight/postprocessing/GNC/diagMagCalibration.m b/RoccarasoFlight/postprocessing/GNC/diagMagCalibration.m
new file mode 100644
index 0000000000000000000000000000000000000000..16818c6dde85923b76202b0b527e1108ee53f668
--- /dev/null
+++ b/RoccarasoFlight/postprocessing/GNC/diagMagCalibration.m
@@ -0,0 +1,65 @@
+function [A,b,B] = diagMagCalibration(data)
+% Function diagMag from https://git.skywarder.eu/avn/gnc/control-systems/-/blob/main/sensor-calibration/Magnetometers/02_scripts/diagMag.m
+% Summary: The diagMag function compute the Hard and Soft Iron correction from the
+% data, assuming the Soft Iron effects parallel to the x, y, z directions 
+%
+% Input: Data matrix (Nx3)
+%
+% Output:
+% A: Soft Iron correction, diagonal matrix (3x3) 
+% b: Hard Iron correction, vector (3x1)
+% B: Magnetic field 
+%
+% Description:
+% - The ellipsoid has equation: (X-b)'S(X-b) = B^2, X'S X -2*b'S X +b'S b -B^2 = 0
+% - The residual are  R = D*solx
+%                     ri = Sx^2*xi^2 + Sy^2*yi^2 + Sz^2*zi^2 - 2bx*Sx^2*xi - 2by*Sy^2*yi - 2bz*Sz^2*zi + Sx^2*bx^2 + Sy^2*by^2 + Sz^2*bz^2 - B^2 
+%                     solx = [ Sx^2; Sy^2; Sz^2; -2bx*Sx^2;  -2by*Sy^2;  -2bz*Sz^2; bx*Sx^2 + by*Sy^2 + bz*Sz^2 - B^2]
+
+x = data(:,1);
+y = data(:,2);
+z = data(:,3);
+
+Y = [...
+        x.*x, ...
+        y.*y, ...
+        z.*z, ...
+        x, ...
+        y, ...
+        z, ...
+        ones(size(x))];
+    
+D = Y'*Y;
+
+[eigvect, eigval] = eig(D);
+
+eigvals = diag(eigval);
+
+[~, index] = min(eigvals); %least square solved with Lagrange multiplier
+
+solx = eigvect(:,index); 
+
+R = diag(solx(1:3));
+
+dR = det(R);
+
+if dR < 0
+        R = -R; 
+        solx = -solx;
+        dR = -dR;
+end
+    b = -0.5*(solx(4:6)./solx(1:3)); %hard iron offset
+
+    B = sqrt(abs(sum([...
+        R(1,1)*b(1)*b(1), ...
+        R(2,2)*b(2)*b(2), ...
+        R(3,3)*b(3)*b(3), ...
+        -solx(end)] ...
+    )));
+
+ Rnew = R./nthroot(dR,3);% Normalization of R -> det(Rnew) = 1 
+ 
+ A = sqrtm(Rnew); %soft iron offset 
+ 
+ B = B./sqrt(nthroot(dR,3));% Normalization of B
+end
\ No newline at end of file
diff --git a/RoccarasoFlight/postprocessing/GNC/postProcess.m b/RoccarasoFlight/postprocessing/GNC/postProcess.m
index 88380364bab5f66f6e57178edb06c9d3e0bf95c5..6299801d40b0f49a0d7e25518669614e2cc6c0d3 100644
--- a/RoccarasoFlight/postprocessing/GNC/postProcess.m
+++ b/RoccarasoFlight/postprocessing/GNC/postProcess.m
@@ -139,7 +139,7 @@ localAmbientCond = [1414, 25+273.15, main.STATIC_PRESSURE1(1,2)];
 sensorTot.nas.time = main.NASData(:,1);
 q_init = eul2quat([settings.launchRail.azimuth,settings.launchRail.elevation,0]);
 bias_init = zeros(1,3);
-sensorTot.nas.states = [main.NASData(1,2:7), q_init ,bias_init];% pos vel quat bias
+sensorTot.nas.states = [main.NASData(1,2:7), q_init([2:4 1]) ,bias_init];% pos vel quat bias
 sensorTot.nas.n_old = 2;
 sensorTot.nas.P   = eye(12);
 sensorTot.nas.P(3,3) = 10;
@@ -190,12 +190,16 @@ grid on;
 
 
 %%% magnetometer calibration:
-center_offset = [+0.3, -0.3, 0.46];
-sensorTot.imu.magnetometerMeasuresCalib = sensorTot.imu.magnetometerMeasures - center_offset;
-R_b2r = quat2dcm(eul2quat(settings.sensorBoard.board2rocket));
-% R_calib = findRotationMatrix(sensorTot.imu.magnetometerMeasures(1,:),[1 0 0]);
-% 
-sensorTot.imu.magnetometerMeasuresCalib = (ROT*(R_b2r*sensorTot.imu.magnetometerMeasuresCalib'))';
+% center_offset = [+0.3, -0.3, 0.46];
+% sensorTot.imu.magnetometerMeasuresCalib = sensorTot.imu.magnetometerMeasures - center_offset;
+% R_b2r = quat2dcm(eul2quat(settings.sensorBoard.board2rocket));
+% % R_calib = findRotationMatrix(sensorTot.imu.magnetometerMeasures(1,:),[1 0 0]);
+% % 
+% sensorTot.imu.magnetometerMeasuresCalib = (R_b2r*sensorTot.imu.magnetometerMeasuresCalib')';
+
+[A_mag, b_mag, B_mag] = diagMagCalibration(sensorTot.imu.magnetometerMeasures([1:500 1800:end],:));
+sensorTot.imu.magnetometerMeasuresCalib = ((sensorTot.imu.magnetometerMeasures-b_mag')*A_mag);
+
 % set configuration parameters ( equal to OBSW )
 nas.dt_k          =   0.02;                                    % [s]        nas time step
 nas.sigma_baro    =   4.3;      % [m/2]   estimated barometer variance