diff --git a/commonFunctions/kalman/ZVK.m b/commonFunctions/kalman/ZVK.m
deleted file mode 100644
index 8685d08287a8bda55f4c952c6246292fb8d619e7..0000000000000000000000000000000000000000
--- a/commonFunctions/kalman/ZVK.m
+++ /dev/null
@@ -1,61 +0,0 @@
-function [] = ZVK(Tf, mag_NED, sensorData, sensorTot, settings, environment)
-
-% settings
-settings.frequencies.ZVKFrequency = 50;
-
-
-
-% recall zvk time
-t_zvk = sensorTot.zvk.time(end):1/settings.frequencies.ZVKFrequency:Tf;
-
-
-% preallocate update
-x = zeros(length(t_zvk),6); % Pre-allocation of corrected estimation
-p = zeros(length(t_zvk),7); % Pre-allocation of quaternions and biases
-z = zeros(length(t_zvk),13);
-
-P_xx = zeros(12,12,length(t_zvk));
-P_pp = zeros(6,6,length(t_zvk)); % Pre-allocation of the covariance matrix
-P_z = zeros(6,6,length(t_zvk));
-
-
-% first update
-x(1,:) = sensorData.zvk.states(end,1:6); % Allocation of the initial value
-p(1,:) = sensorData.zvk.states(end,7:13);
-z(1,:) = [x(1,:), p(1,:)];
-
-P_xx(:,:,1) = sensorData.zvk.P(1:6,1:6,end);
-P_pp(:,:,1) = sensorData.zvk.P(7:12,7:12,end);
-P_z(:,:,1) = sensorData.zvk.P(:,:,end);
-
-
-
-if length(t_zvk) > 1
- dt_k = t_zvk(2)-t_zvk(1); % Kalman time step
-
- % Time vectors agumentation
- t_gpstemp = [sensorTot.gps.time];
- t_barotemp = [sensorTot.barometer.time];
- t_imutemp = [sensorTot.imu.time];
- t_pittemp = [sensorTot.pitot.time];
-
- for ii = 2:length(t_zvk)
-
- %%% Prediction
- index_imu = sum(t_zvk(ii) >= t_imutemp);
-
- %%%% PREDICTOR %%%% PREDICTOR %%%% PREDICTOR %%%%
- % [x_lin(ii,:),~,P_lin(:,:,ii)] = predictorLinear2(x_lin(ii-1,:),P_lin(:,:,ii-1), dt_k,sensorTot.imu.accelerometer_measures(index_imu,:),xq(ii-1,1:4),nas.QLinear);
- % [xq(ii,:),P_q(:,:,ii)] = predictorQuat(xq(ii-1,:),P_q(:,:,ii-1), sensorTot.imu.gyro_measures(index_imu,:),dt_k,nas.Qq);
- %%%% PREDICTOR %%%% PREDICTOR %%%% PREDICTOR %%%%
-
-
-
- %%% Correction
-
- % PROVA
- end
-
-end
-
-end
\ No newline at end of file
diff --git a/commonFunctions/kalman/correctorZVK.m b/commonFunctions/kalman/correctorZVK.m
new file mode 100644
index 0000000000000000000000000000000000000000..835f8d8711751c9d6a8d96483d0e47c11dfa4f9d
--- /dev/null
+++ b/commonFunctions/kalman/correctorZVK.m
@@ -0,0 +1,52 @@
+function [x_new, P_new] = correctrZVK(x_pred, P_pred)
+
+ quat_pred = x_pred(1:4)';
+ r_pred = x_pred(5:7)';
+ v_pred = x_pred(8:10)';
+ bias_g_pred = x_pred(11:13)';
+ bias_a_pred = x_pred(14:16)';
+
+ A = [quat_pred(1)^2 - quat_pred(2)^2 - quat_pred(3)^2 + quat_pred(4)^2, 2*(quat_pred(1)*quat_pred(2) + quat_pred(3)*quat_pred(4)), 2*(quat_pred(1)*quat_pred(3) - quat_pred(2)*quat_pred(4));
+ 2*(quat_pred(1)*quat_pred(2) - quat_pred(3)*quat_pred(4)), -quat_pred(1)^2 + quat_pred(2)^2 - quat_pred(3)^2 + quat_pred(4)^2, 2*(quat_pred(2)*quat_pred(3) + quat_pred(1)*quat_pred(4)) ;
+ 2*(quat_pred(1)*quat_pred(3) + quat_pred(2)*quat_pred(4)), 2*(quat_pred(2)*quat_pred(3) - quat_pred(1)*quat_pred(4)), -quat_pred(1)^2 - quat_pred(2)^2 + quat_pred(3)^2 + quat_pred(4)^2];
+
+
+ H_x = [zeros(3) eye(3) zeros(3) zeros(3) zeros(3);
+ zeros(3) zeros(3) eye(3) zeros(3) zeros(3)];
+
+ % % Velocità angolare della Terra [rad/s]
+ % omega_e = 7.2921150e-5;
+ %
+ % skewOmega = [0 -omega_e 0;
+ % omega_e 0 0;
+ % 0 0 0];
+ %
+ % H_p = [zeros(3) zeros(3) A zeros(3) zeros(3) zeros(3);
+ % zeros(3) zeros(3) skewOmega*A zeros(3) zeros(3) zeros(3)];
+
+
+ P_xx_pred = P_pred(1:15,1:15,end);
+ P_xp_pred = P_pred(16:33,1:15,end);
+ P_pp_pred = P_pred(16:33,16:33,end);
+
+
+ S = H_x * P_xx_pred * H_x';
+
+ K = P_xx_pred * H_x' * S;
+
+ error = [0 0 -160, 0, 0, 0]' - [r_pred' v_pred']';
+
+ update = K * error;
+
+ x_new = zeros(1,16);
+ x_new(5:end) = x_pred(5:end) + update(4:end)';
+
+ quat_error = [update(1:3)',1];
+ quat_pred = x_pred(1:4);
+
+ % quat_error(4)*quat_pred(4) - quat_error(1:3)*quat_pred(1:3)'
+
+ x_new(1:4) = [ (quat_pred(4)*quat_error(1:3) + quat_error(4)*quat_pred(1:3) - cross(quat_error(1:3),quat_pred(1:3)) )';
+ quat_error(4)*quat_pred(4) - quat_error(1:3)*quat_pred(1:3)' ];
+
+end
\ No newline at end of file
diff --git a/commonFunctions/kalman/predictorQuat.m b/commonFunctions/kalman/predictorQuat.m
index 9d875e15c50b09c4aa479bab2f1ab298495ac5a1..1d7eaf2aa86bf44e173f79fac72fee7797e5000f 100644
--- a/commonFunctions/kalman/predictorQuat.m
+++ b/commonFunctions/kalman/predictorQuat.m
@@ -38,6 +38,7 @@ omega = w - beta_prev; %Computation of real w (
omega_mat = [ 0 -omega(3) omega(2);
omega(3) 0 -omega(1);
-omega(2) omega(1) 0;];
+
Omega = [ -omega_mat omega';
-omega 0];
%-------------------------------------------------------------------------
diff --git a/commonFunctions/kalman/predictorZVK.m b/commonFunctions/kalman/predictorZVK.m
new file mode 100644
index 0000000000000000000000000000000000000000..02c1b4c8ee874319a98e5a15e17b89e6731046b9
--- /dev/null
+++ b/commonFunctions/kalman/predictorZVK.m
@@ -0,0 +1,132 @@
+function [z_pred, P_pred] = predictorZVK( x_prev, P_prev, a_b_m, om_b_m, dt, zvk)
+
+ quat_prev = x_prev(1:4)';
+ r_prev = x_prev(5:7)';
+ v_prev = x_prev(8:10)';
+ bias_g_prev = x_prev(11:13)';
+ bias_a_prev = x_prev(14:16)';
+
+
+ %%% Position - Velocity prediciton
+ A = [quat_prev(1)^2 - quat_prev(2)^2 - quat_prev(3)^2 + quat_prev(4)^2, 2*(quat_prev(1)*quat_prev(2) + quat_prev(3)*quat_prev(4)), 2*(quat_prev(1)*quat_prev(3) - quat_prev(2)*quat_prev(4));
+ 2*(quat_prev(1)*quat_prev(2) - quat_prev(3)*quat_prev(4)), -quat_prev(1)^2 + quat_prev(2)^2 - quat_prev(3)^2 + quat_prev(4)^2, 2*(quat_prev(2)*quat_prev(3) + quat_prev(1)*quat_prev(4)) ;
+ 2*(quat_prev(1)*quat_prev(3) + quat_prev(2)*quat_prev(4)), 2*(quat_prev(2)*quat_prev(3) - quat_prev(1)*quat_prev(4)), -quat_prev(1)^2 - quat_prev(2)^2 + quat_prev(3)^2 + quat_prev(4)^2];
+
+
+ a_b = (a_b_m' - bias_a_prev);
+
+ a_i = A'*a_b + [0;0;9.81]; % correct bias, rotate body to NED and add gravitayional acc
+
+ % Pos pred ned
+ r_pred = r_prev + dt*v_prev;
+ % Vel pred ned
+ v_pred = v_prev + dt*a_i;
+ % % Vel pred body
+ % v_pred_body = ( A*v_pred)')'; % NED to body
+
+
+ %%% Attitude prediction
+ om_b = om_b_m' - bias_g_prev; % in body
+
+ omega_mat = [ 0 -om_b(3) om_b(2);
+ om_b(3) 0 -om_b(1);
+ -om_b(2) om_b(1) 0;];
+
+ Omega = [ -omega_mat om_b;
+ -om_b' 0];
+
+ quat_pred = (eye(4) + 0.5*Omega*dt) * quat_prev;
+
+
+ %%% Biases random walk
+ bias_a_pred = (eye(3) + zvk.acc_bias_noise) * bias_a_prev;
+ bias_g_pred = (eye(3) + zvk.gyro_bias_noise) * bias_g_prev;
+
+
+ % Assemble predicted global state
+ z_pred = [quat_pred; r_pred; v_pred; bias_g_pred; bias_a_pred]';
+
+
+
+
+ %%% Covariance prediction
+
+ F11 = - omega_mat;
+ F15 = - eye(3);
+ F23 = eye(3);
+ F31 = - A * [ 0 -a_b(3) a_b(2);
+ a_b(3) 0 -a_b(1);
+ -a_b(2) a_b(1) 0;];
+ F34 = - A;
+
+ % x = r_prev(1);
+ % y = r_prev(2);
+ % z = r_prev(3);
+ % r = norm(r_prev);
+ % Gamma2 = [ 5*(x^2 + z^2) - (35*x^2*z^2)/r^2 - r^2, 5*x*y - (35*x*y*z^2)/r^2, 15*x*z - (35*x*z^3)/r^2;
+ % 5*x*y - (35*x*y*z^2)/r^2, 5*(y^2 + z^2) - (35*y^2*z^2)/r^2 - r^2, 15*y*z - (35*y*z^3)/r^2;
+ % 15*x*z - (35*x*z^3)/r^2, 15*y*z - (35*y*z^3)/r^2, 30*z^2 - (35*z^4)/r^2 - 3*r^2];
+ % F32 = (zvk.mu / r^5) * (3 * (r_prev * r_prev') - eye(3) * r^2 + (3/2) * zvk.J2 * (zvk.Re / r)^2 * Gamma2);
+
+ F32 = zeros(3);
+ % Ho tolto la dipendenza dell'acceleraizone dalla posizone (i.e. la perturbazione J2)
+
+ F = [F11, zeros(3), zeros(3), zeros(3), F15;
+ zeros(3), zeros(3), F23, zeros(3), zeros(3);
+ F31, F32, zeros(3), F34, zeros(3);
+ zeros(3), zeros(3), zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), zeros(3), zeros(3)];
+
+
+ G_w = [-diag(ones(3,1)), zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), -A, diag(ones(3,1));
+ zeros(3), zeros(3), zeros(3), diag(ones(3,1));
+ zeros(3), diag(ones(3,1)), zeros(3), zeros(3)];
+
+ G11 = - diag(om_b);
+ G12 = - omega_mat;
+ G13 = - [ 0, om_b(3), om_b(2);
+ om_b(3), 0, om_b(2);
+ om_b(2), om_b(1), 0];
+ G34 = - A * diag(a_b);
+ G35 = A * diag(a_b);
+ G36 = - A * [ 0, a_b(3), a_b(2);
+ a_b(3), 0, a_b(2);
+ a_b(2), a_b(1), 0];
+
+
+ G_p = [G11, G12, G13, zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), G34, G35, G36;
+ zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), zeros(3), zeros(3), zeros(3)];
+
+
+ PHI = exp(F * dt);
+
+ GAMMA = (PHI*G_w);
+
+ PSI = (PHI*G_p);
+
+
+
+ P_xx_prev = P_prev(1:15,1:15,end);
+ P_xp_prev = P_prev(16:33,1:15,end);
+ P_pp_prev = P_prev(16:33,16:33,end);
+
+
+ P_xx_pred = PHI * P_xx_prev * PHI'+ ...
+ GAMMA * zvk.Q * dt * GAMMA' + ...
+ PHI * P_xp_prev' * PSI' + ...
+ PSI * P_xp_prev * PHI' + ... % note: P_PX_prev = P_XP_prev'
+ PSI * P_pp_prev * PSI';
+
+ P_xp_pred = PHI * P_xp_prev' + PSI * P_pp_prev;
+
+ % P_pp does nto change
+
+ P_pred = [ P_xx_pred, P_xp_pred;
+ P_xp_pred', P_pp_prev];
+
+end
\ No newline at end of file
diff --git a/commonFunctions/kalman/run_ZVK.m b/commonFunctions/kalman/run_ZVK.m
new file mode 100644
index 0000000000000000000000000000000000000000..a73ee9dd2be50fe14073f8520f37df5880ad2cd3
--- /dev/null
+++ b/commonFunctions/kalman/run_ZVK.m
@@ -0,0 +1,65 @@
+function [sensorTot] = run_ZVK(Tf, sensorData, sensorTot, settings, environment)
+
+
+% recall zvk time
+t_zvk = sensorTot.zvk.time(end):1/settings.frequencies.ZVKFrequency:Tf;
+
+% preallocate update
+x = zeros(length(t_zvk),16); % Pre-allocation of pos, vel, quaternion and biases [4quat 3r 3v 3beta_g 3beta_a]
+p = zeros(length(t_zvk),18); % Pre-allocation of cal parameters [9W_g 9W_a] oppure [3lam_g 3mu_g 3nu_g + idem per _a]
+z = zeros(length(t_zvk),34);
+
+% note that covariance has one state less because error state has 3 small
+% deviation angles instead of quaternions
+% P_xx = zeros(15,15,length(t_zvk));
+% P_xp = zeros(18,18,length(t_zvk)); % Pre-allocation of the covariance matrix
+P_z = zeros(33,33,length(t_zvk));
+
+
+% first update
+x(1,:) = sensorData.zvk.states(end,1:16); % Allocation of the initial value
+p(1,:) = sensorData.zvk.states(end,17:end);
+z(1,:) = [x(1,:), p(1,:)];
+
+% P_xx(:,:,1) = sensorData.zvk.P(1:15,1:15,end);
+% P_xp(:,:,1) = sensorData.zvk.P(16:33,1:15,end);
+P_z(:,:,1) = sensorData.zvk.P(:,:,end);
+
+
+if length(t_zvk) > 1
+ dt_k = t_zvk(2)-t_zvk(1); % Kalman time step
+
+ % Time vectors agumentation
+ t_imutemp = [sensorTot.imu.time];
+
+ for ii = 2:length(t_zvk)
+
+ %%% Prediction
+ index_imu = sum(t_zvk(ii) >= t_imutemp);
+
+ a_b_m = sensorTot.imu.accelerometer_measures(index_imu,:);
+ om_b_m = sensorTot.imu.gyro_measures(index_imu,:);
+
+ [x(ii,:), P_z(:,:,ii)] = predictorZVK( x(ii-1,:), P_z(:,:,ii-1), a_b_m, om_b_m, dt_k, settings.zvk);
+
+
+
+
+ %%% Correction
+ correctorZVK( x(ii,:), P_z(:,:,ii) );
+
+
+
+
+ end
+
+ sensorData.zvk.states = [x,p];
+ sensorData.zvk.time = t_zvk;
+
+ sensorTot.zvk.states(sensorTot.zvk.n_old:sensorTot.zvk.n_old + size(sensorData.zvk.states(:,1),1)-2, 1:16 ) = sensorData.zvk.states(2:end,1:16); % ZVK output
+ sensorTot.zvk.time( sensorTot.zvk.n_old : sensorTot.zvk.n_old+size(sensorData.zvk.states(:,1),1)-2 ) = sensorData.zvk.time(2:end); % ZVK time output
+ % sensorTot.zvk.time(:)
+ sensorTot.zvk.n_old = sensorTot.zvk.n_old + size(sensorData.zvk.states,1)-1;
+end
+
+end
\ No newline at end of file
diff --git a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
index 34ef98821241e2f6ebd149be6d0bf69dacf3e9ae..5a2b3478783dceafc67b80d9a7076fc081be461e 100644
--- a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
@@ -47,6 +47,9 @@ settings.frequencies.NASFrequency = 50; % [hz
settings.frequencies.ADAFrequency = 50; % [hz] sensor frequency
settings.frequencies.MEAFrequency = 50; % [hz] sensor frequency
+settings.frequencies.ZVKFrequency = 50; % [hz] sensor frequency
+
+
% Servo (MARK STAR - HBL 3850)
settings.servo.tau = 0.0374588; % Servo motor time constant
settings.servo.delay = 0.070548; % Servo motor delay
@@ -136,6 +139,24 @@ settings.nas.QLinear = 0.005*...
% Process noise covariance matrix for the quaternion dynamics
settings.nas.Qq = [(settings.nas.sigma_w^2*settings.nas.dt_k+(1/3)*settings.nas.sigma_beta^2*settings.nas.dt_k^3)*eye(3) 0.5*settings.nas.sigma_beta^2*settings.nas.dt_k^2*eye(3);
0.5*settings.nas.sigma_beta^2*settings.nas.dt_k^2*eye(3) settings.nas.sigma_beta^2*settings.nas.dt_k*eye(3)];
+
+%% ZVK TUNING PARAMETER
+
+% settings.zvk
+settings.zvk.Q = diag( 0.1^2*ones(12,1) );
+
+settings.zvk.mu = 3.986004418e14;
+settings.zvk.Re = 6378137;
+settings.zvk.J2 = 1.082636e-3;
+
+settings.zvk.P = 0.5*eye(33);
+
+settings.zvk.gyro_bias_noise = 0.05;
+settings.zvk.acc_bias_noise = 0.05;
+
+
+
+
%% ADA TUNING PARAMETER
settings.ada.Q = [30 0 0; % Process noise covariance matrix
diff --git a/simulator/src/std_plots.m b/simulator/src/std_plots.m
index 627ce6b7bd50e82089d8369dd834bcb7c0584ae2..f0fdfe601e7a88a2b0bbced7387192ec4791559e 100644
--- a/simulator/src/std_plots.m
+++ b/simulator/src/std_plots.m
@@ -486,5 +486,15 @@ xlabel('Altitude [m]')
ylabel('Angle [deg]')
drawnow
+
+
+
+%% ZVK
+% figure.zvk
+
+
+
+
+
end
diff --git a/simulator/src/std_run.m b/simulator/src/std_run.m
index c18e077cf8a34bec7ecd513c3cc699e5749d9e18..4e82425fb4abf8742ddfb7a159af54f240d1c665 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -706,6 +706,10 @@ if settings.montecarlo
struct_out.sensors.nas.states = interp1(struct_out.sensors.nas.time',struct_out.sensors.nas.states,t_vec);
struct_out.sensors.nas.time = t_vec;
+ % sensors - ZVK
+ struct_out.sensors.zvk.states = interp1(struct_out.sensors.zvk.time',struct_out.sensors.zvk.states,t_vec);
+ struct_out.sensors.zvk.time = t_vec;
+
% sensors - MEA already good
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index 1b505d8a106416903188f4ec092c32ef6fdad90e..11560ec9f967d495bdb45dd80bfd1b4d3ebc0b4e 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -189,6 +189,21 @@ sensorData.nas.time = 0;
settings.nas.flagStopPitotCorrection = false;
settings.nas.mag_freq = settings.frequencies.NASFrequency/settings.nas.mag_decimate;
+
+%% ZVK initial
+
+% if settings.flagNAS || settings.electronics ??????
+
+sensorData.zvk.states = [Q0(2:4); Q0(1); X0; V0; zeros(6,1); zeros(18,1)]';
+
+if settings.scenario ~="descent"
+ sensorData.zvk.states(7) = -environment.z0;
+else
+ sensorData.zvk.states(7) = -settings.z_final-environment.z0;
+end
+sensorData.zvk.P = settings.zvk.P;
+
+
%% MEA PARAMETERS (mass estimation algorithm)
sensorData.mea.x = [0,0,rocket.mass(1)]; % initial state estimate
sensorData.mea.estimated_mass(1) = rocket.mass(1);
@@ -234,6 +249,11 @@ sensorTot.mea.mass = rocket.motor.mass(1);
sensorTot.mea.prediction = 0;
sensorTot.mea.time = 0;
+
+sensorTot.zvk.time = 0;
+sensorTot.zvk.states = sensorData.zvk.states;
+
+
% inizializzare i tempi dei sensori a 0 e poi mettere tutti i n_old = 2
sensorTot.barometer_sens{1}.time = 0;
sensorTot.barometer_sens{2}.time = 0;
@@ -250,6 +270,9 @@ sensorTot.ada.time = 0;
sensorTot.nas.time = 0;
sensorTot.mea.time = 0;
+
+
+
% initialization of the indexes
sensorTot.barometer_sens{1}.n_old = 2;
sensorTot.barometer_sens{2}.n_old = 2;
@@ -266,6 +289,9 @@ sensorTot.sfd.n_old = 2;
sensorTot.gps.lastindex = 0;
sensorTot.pitot.lastindex = 0;
+
+sensorTot.zvk.n_old = 2;
+
% initialization params
flagFlight = false;
flagBurning = false;
diff --git a/simulator/src/std_run_parts/std_subsystems.m b/simulator/src/std_run_parts/std_subsystems.m
index b6d232ff519732d147145a9b560f166521cc9368..5b188c202af7a5945d2ec72be1d5acefa05b624a 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -152,6 +152,6 @@ end
%% ZVK
-% if currentState == availableStates.on_ground
-% ZVK(t1, XYZ0*0.01, sensorData, sensorTot, settings, environment)
-% end
\ No newline at end of file
+if currentState == availableStates.on_ground
+ [sensorTot] = run_ZVK(t1,sensorData, sensorTot, settings, environment);
+end
\ No newline at end of file