diff --git a/commonFunctions/kalman/correctorZVK.m b/commonFunctions/kalman/correctorZVK.m
index a5b1c6e92fc7d9c4f8a640108f64241c5c4740d5..847bcdc46b9221d9a1644056e09a753d0f9cb64b 100644
--- a/commonFunctions/kalman/correctorZVK.m
+++ b/commonFunctions/kalman/correctorZVK.m
@@ -6,10 +6,13 @@ function [x_new, P_new] = correctorZVK(x_pred, P_pred, a_b_m, om_b_m, mag_meas,
bias_a_pred = x_pred(11:13)';
bias_g_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];
+ % 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];
+ A = [-0.0594 0.0637 -0.9962;
+ -0.7314 -0.6820 0.0000;
+ -0.6794 0.7286 0.0872];
% accelerometer correciton
a_b = a_b_m' - bias_a_pred;
@@ -33,7 +36,7 @@ function [x_new, P_new] = correctorZVK(x_pred, P_pred, a_b_m, om_b_m, mag_meas,
% Measurment matrix
H_x = [zeros(3) eye(3) zeros(3) zeros(3) zeros(3);
zeros(3) zeros(3) zeros(3) zeros(3) -eye(3);
- M zeros(3) zeros(3) zeros(3) zeros(3);
+ M zeros(3) zeros(3) eye(3) zeros(3);
z_mat zeros(3) zeros(3) zeros(3) zeros(3)];
diff --git a/commonFunctions/kalman/correctorZVK1.m b/commonFunctions/kalman/correctorZVK1.m
new file mode 100644
index 0000000000000000000000000000000000000000..e11c958fdc93f76612afcf3245a7a9e7ba591924
--- /dev/null
+++ b/commonFunctions/kalman/correctorZVK1.m
@@ -0,0 +1,57 @@
+function [x_new, P_new] = correctorZVK1(x_pred, P_pred, a_b_m, om_b_m, mag_meas, mag_NED, zvk, Q)
+
+ v_pred = x_pred(1:3)';
+ r_pred = x_pred(4:6)';
+ bias_a_pred = x_pred(7:9)';
+ bias_g_pred = x_pred(10:12)';
+
+ A = [Q(1)^2 - Q(2)^2 - Q(3)^2 + Q(4)^2, 2*(Q(1)*Q(2) + Q(3)*Q(4)), 2*(Q(1)*Q(3) - Q(2)*Q(4));
+ 2*(Q(1)*Q(2) - Q(3)*Q(4)), -Q(1)^2 + Q(2)^2 - Q(3)^2 + Q(4)^2, 2*(Q(2)*Q(3) + Q(1)*Q(4)) ;
+ 2*(Q(1)*Q(3) + Q(2)*Q(4)), 2*(Q(2)*Q(3) - Q(1)*Q(4)), -Q(1)^2 - Q(2)^2 + Q(3)^2 + Q(4)^2];
+
+ % accelerometer correciton
+ a_b = a_b_m' - bias_a_pred;
+
+ g_est = (A * [0, 0, -9.81]');
+ g_meas = a_b;
+
+ M = [0 -g_est(3) g_est(2);
+ g_est(3) 0 -g_est(1);
+ -g_est(2) g_est(1) 0];
+
+ % magnetometer correction
+ mag_meas = mag_meas/norm(mag_meas);
+ mag_NED = mag_NED/norm(mag_NED);
+
+ z = A*mag_NED; %Magnetic vector in the body axis (estimated)
+ z_mat = [ 0 -z(3) z(2);
+ z(3) 0 -z(1);
+ -z(2) z(1) 0;]; %Matrix needed to obtain the derivative H
+
+
+ % Measurment matrix
+ H_x = [zeros(3) eye(3) zeros(3) zeros(3) zeros(3);
+ zeros(3) zeros(3) zeros(3) zeros(3) -eye(3);
+ M zeros(3) zeros(3) eye(3) zeros(3);
+ z_mat zeros(3) zeros(3) zeros(3) zeros(3)];
+ H_x = H_x(:,4:15);
+
+ S = H_x * P_pred * H_x' + zvk.R;
+
+ K = (P_pred * H_x') / S;
+
+
+ error = [zeros(6,1); g_meas; mag_meas'] - [v_pred; om_b_m'-bias_g_pred; g_est; z];
+
+ update = K * error;
+
+
+ x_new = zeros(1,12);
+ % x_new(5:end) = x_pred(5:end) + update(4:end)';
+ x_new = x_pred + update';
+
+ P_new = (eye(12) - K * H_x) * P_pred;
+ disp(x_new)
+
+
+end
\ No newline at end of file
diff --git a/commonFunctions/kalman/predictorQuatZVK.m b/commonFunctions/kalman/predictorQuatZVK.m
new file mode 100644
index 0000000000000000000000000000000000000000..d78a19c01dbdfe7b3691a2d3445fc9e524e3efba
--- /dev/null
+++ b/commonFunctions/kalman/predictorQuatZVK.m
@@ -0,0 +1,40 @@
+function [x_pred,P_pred]=predictorQuatZVK(x,P,w,dt,zvk)
+q_prev = x(1:4); %Definition of the previous quaternion
+beta_prev = x(14:16); %Definition of the previous bias
+x_pred = x;
+Q = zvk.Qq;
+omega = w - beta_prev; %Computation of real w (no bias)
+% omega = w;
+%------------------------------------------------------------------------
+%COMPUTATION OF PROPAGATION MATRIX FOR QUATERNION.
+omega_mat = [ 0 -omega(3) omega(2);
+ omega(3) 0 -omega(1);
+ -omega(2) omega(1) 0;];
+
+Omega = [ -omega_mat omega';
+ -omega 0];
+%-------------------------------------------------------------------------
+%PROPAGATION OF QUATERNION AND BIAS (random walk) + ASSIGMENT TO X
+q_pred = (eye(4) + 0.5*Omega*dt)*q_prev';
+
+q_pred = q_pred/norm(q_pred);
+
+x_pred(1:4) = q_pred';
+
+beta_pred = beta_prev + zvk.sigma_beta_g*randn; %Da capire bene che random walk mettere
+%beta_pred = beta_prev;
+x_pred(14:16) = beta_pred;
+%-------------------------------------------------------------------------
+%PROPAGATION OF COVARIANCE ERROR MATRIX P. DISCRETE APPROXIMATION.
+F = sparse(6,6);
+F11 = -omega_mat; % phi_dot / phi
+F15 = -eye(3); % phi_dot / bias_gyro
+F(1:3,1:3) = F11;
+F(1:3,13:15) = F15;
+
+G = sparse(15,12);
+G_phi = -eye(3);
+
+P_pred = F*P([1:3 14:16],[1:3 14:16])*F'+G*Q*G'; %Da capire come modificare nas.Qq per adattarla allo zvk
+end
+
diff --git a/commonFunctions/kalman/predictorZVK.m b/commonFunctions/kalman/predictorZVK.m
index d6b13ab4defaeb1d29f599bf45eda1298fd34e60..0e539bbd51a3d11fa261829fd0682fa9ca2dd261 100644
--- a/commonFunctions/kalman/predictorZVK.m
+++ b/commonFunctions/kalman/predictorZVK.m
@@ -1,6 +1,6 @@
function [x_pred, P_pred] = predictorZVK( x_prev, P_prev, sf_b_measure, om_b_m, dt, zvk)
- quat_prev = x_prev(1:4)';
+ quat_prev = angle2quat(pi,-1.4835,2.3213)';
v_prev = x_prev(5:7)';
r_prev = x_prev(8:10)';
bias_a_prev = x_prev(11:13)';
@@ -13,6 +13,10 @@ function [x_pred, P_pred] = predictorZVK( x_prev, P_prev, sf_b_measure, om_b_m,
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 = [-0.0594 0.0637 -0.9962;
+ -0.7314 -0.6820 0.0000;
+ -0.6794 0.7286 0.0872];
+
sf_b = (sf_b_measure' - bias_a_prev); % measured specific force, corrected bias.
a_i = A'*sf_b + [0;0;9.81]; % acceleration in inertial frame: correct bias, rotate body to NED and add gravitayional acc
diff --git a/commonFunctions/kalman/predictorZVK1.m b/commonFunctions/kalman/predictorZVK1.m
new file mode 100644
index 0000000000000000000000000000000000000000..186d45aff1ec272f90e03c1ee5a9b6ac68a54330
--- /dev/null
+++ b/commonFunctions/kalman/predictorZVK1.m
@@ -0,0 +1,104 @@
+function [x_pred, P_pred] = predictorZVK1( x_prev, P_prev, sf_b_measure, om_b_m, dt, zvk, Q)
+
+ v_prev = x_prev(1:3)';
+ r_prev = x_prev(4:6)';
+ bias_a_prev = x_prev(7:9)';
+ bias_g_prev = x_prev(10:12)';
+
+
+ %%% Position - Velocity prediciton
+ A = [Q(1)^2 - Q(2)^2 - Q(3)^2 + Q(4)^2, 2*(Q(1)*Q(2) + Q(3)*Q(4)), 2*(Q(1)*Q(3) - Q(2)*Q(4));
+ 2*(Q(1)*Q(2) - Q(3)*Q(4)), -Q(1)^2 + Q(2)^2 - Q(3)^2 + Q(4)^2, 2*(Q(2)*Q(3) + Q(1)*Q(4)) ;
+ 2*(Q(1)*Q(3) + Q(2)*Q(4)), 2*(Q(2)*Q(3) - Q(1)*Q(4)), -Q(1)^2 - Q(2)^2 + Q(3)^2 + Q(4)^2];
+
+
+
+ sf_b = (sf_b_measure' - bias_a_prev); % measured specific force, corrected bias.
+
+ a_i = A'*sf_b + [0;0;9.81]; % acceleration in inertial frame: 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) * Q;
+
+ % disp(rad2deg(quat2eul(quat_pred')))
+
+ %%% Biases random walk
+ % bias_a_pred = bias_a_prev + zvk.sigma_beta_acc*randn;
+ % bias_g_pred = bias_g_prev + zvk.sigma_beta_g*randn;
+ bias_a_pred = bias_a_prev;
+ bias_g_pred = bias_g_prev;
+
+ % Assemble predicted global state
+ x_pred = [v_pred; r_pred; bias_a_pred; bias_g_pred]';
+
+
+
+ %%% Covariance prediction
+
+ % F11 = -omega_mat; % phi_dot / phi
+ % F15 = -eye(3); % phi_dot / bias_gyro
+ % F21 = -A * [ 0 -sf_b(3) sf_b(2); % v_dot / phi
+ % sf_b(3) 0 -sf_b(1);
+ % -sf_b(2) sf_b(1) 0;];
+ F12 = zeros(3); % v_dot / r -> GRAVITÀ che dipende da r: OFF
+ F13 = -A; % v_dot / bias_acc
+ F21 = eye(3); % r_dot / v
+
+
+ F = sparse(12,12);
+ F(1:3,4:6) = F12;
+ F(1:3,7:9) = F13;
+ F(4:6,1:3) = F21;
+
+ % F(1:3,1:3) = F11;
+ % F(1:3,13:15) = F15;
+ % F(4:6,7:9) = F23;
+ % F(4:6,10:12) = F24;
+ % F(7:9,4:6) = F32;
+
+ PHI = expm(F * dt);
+ % PHI = eye(12) + F*dt;
+
+
+
+ G_phi = -eye(3);
+ G_v = [-A, eye(3)];
+ G_beta_a = eye(3);
+ G_beta_g = eye(3);
+
+ G = sparse(15,12);
+ G(1:3,1:3) = G_phi;
+ G(4:6,7:12) = G_v;
+ G(10:12,10:12) = G_beta_a;
+ G(13:15,4:6) = G_beta_g;
+ G = G(4:15,:);
+
+ G = sparse(12, 12);
+ G(1:3, 7:12) = G_v;
+ G(7:9, 10:12) = G_beta_a;
+ G(10:12, 4:6) = G_beta_g;
+
+
+ GAMMA = (PHI*G);
+
+
+ P_pred = PHI * P_prev * PHI'+ GAMMA * zvk.Q * dt * GAMMA';
+
+end
\ No newline at end of file
diff --git a/commonFunctions/kalman/run_ZVK1.m b/commonFunctions/kalman/run_ZVK1.m
new file mode 100644
index 0000000000000000000000000000000000000000..085ad4f874e9ad965e4c84136e26126131ee5429
--- /dev/null
+++ b/commonFunctions/kalman/run_ZVK1.m
@@ -0,0 +1,59 @@
+function [sensorData, sensorTot] = run_ZVK1(Tf, sensorData, sensorTot, settings, mag_NED, environment)
+
+
+% recall zvk time
+t_zvk = sensorTot.zvk.time(end):1/settings.frequencies.ZVKFrequency:Tf;
+
+% preallocate update
+x = zeros(length(t_zvk),12); % Pre-allocation of pos, vel, quaternion and biases [3v 3r 3beta_a 3beta_g]
+% note that covariance has one state less because error state has 3 small
+% deviation angles instead of quaternions
+P = zeros(12,12,length(t_zvk));
+
+%Quaternions for rotations
+Q = angle2quat(environment.phi, environment.omega, 0*pi/180, 'ZYX')';
+Q = [Q(2:4);Q(1)]; %scalar-first to scalar-last
+
+% first update
+x(1,:) = sensorData.zvk.states(end,:); % Allocation of the initial value
+
+P(:,:,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(:,:,ii)] = predictorZVK1( x(ii-1,:), P(:,:,ii-1), a_b_m, om_b_m, dt_k, settings.zvk, Q);
+
+ mag_meas = sensorTot.imu.magnetometer_measures(index_imu,:);
+
+ %%% Correction
+ [x(ii,:), P(:,:,ii)] = correctorZVK1( x(ii,:), P(:,:,ii), a_b_m, om_b_m, mag_meas, mag_NED, settings.zvk, Q);
+
+
+
+ end
+
+ sensorData.zvk.states = x;
+ sensorData.zvk.P = P;
+ sensorData.zvk.time = t_zvk;
+
+ sensorTot.zvk.P(1:12, 1:12, sensorTot.zvk.n_old:sensorTot.zvk.n_old + size(sensorData.zvk.P,3)-2 ) = sensorData.zvk.P(:,:,2:end);
+ sensorTot.zvk.states(sensorTot.zvk.n_old:sensorTot.zvk.n_old + size(sensorData.zvk.states,1)-2, 1:12 ) = sensorData.zvk.states(2:end,:); % ZVK output
+ sensorTot.zvk.time( sensorTot.zvk.n_old : sensorTot.zvk.n_old+size(sensorData.zvk.states,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 c0eccf6edc4f7a5161b3f710c8d990e4786c4822..6a69b69b1aea03c777850b76ccfc3473d28674ed 100644
--- a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
@@ -150,7 +150,7 @@ settings.nas.Qq = [(settings.nas.sigma_w^2*settings.nas.dt_k+(1/3
settings.zvk.sigma_gyro = 1e1; % [rad/s] gyroscope variance NAS
settings.zvk.sigma_beta_g = 1e-5; % [rad/s] gyroscope bias variance NAS
settings.zvk.sigma_acc = 5e0; % [m/s^2] accelerometer variance NAS
-settings.zvk.sigma_beta_acc = 5e-4; % [m/s^2] accelerometer bias variance BOH
+settings.zvk.sigma_beta_acc = 5e-6; % [m/s^2] accelerometer bias variance BOH
settings.zvk.sigma_mag = 3; % [mgauss] magnetometer variance NAS = 10????? mentre sito STM di 3mGs RMS noise
% Process noise covariance matrix Q
@@ -163,7 +163,7 @@ settings.zvk.Q = diag([
% Initial state covariance matrix P
settings.zvk.P0 = diag([
- 1e-4 * ones(1,3),... % small-angle errors [rad]
+ %1e-4 * ones(1,3),... % small-angle errors [rad]
1e-5 * ones(1,3),... % velocity uncertainty [m/s]
1e-4 * ones(1,3),... % position uncertainty [m]
0.1 * ones(1,3),... % accelerometer bias [m/s^2]
@@ -171,13 +171,13 @@ settings.zvk.P0 = diag([
]);
% Measurement noise covariance matrix R
-settings.zvk.R = [
- 1e-6 * eye(3), zeros(3), zeros(3), zeros(3); % fake zero velocity [m/s]
- zeros(3), 1e-6 * eye(3), zeros(3), zeros(3); % fake zero angular velocity [rad/s]
- zeros(3), zeros(3), settings.zvk.sigma_acc^2 * eye(3), zeros(3); % accelerometer as gravity observation [m/s^2]
- zeros(3), zeros(3), zeros(3), settings.zvk.sigma_mag^2 * eye(3) % magnetometer correction
-];
-
+% settings.zvk.R = [
+% 1e-6 * eye(3), zeros(3), zeros(3), zeros(3); % fake zero velocity [m/s]
+% zeros(3), 1e-6 * eye(3), zeros(3), zeros(3); % fake zero angular velocity [rad/s]
+% zeros(3), zeros(3), settings.zvk.sigma_acc^2 * eye(3), zeros(3); % accelerometer as gravity observation [m/s^2]
+% zeros(3), zeros(3), zeros(3), settings.zvk.sigma_mag^2 * eye(3) % magnetometer correction
+% ];
+settings.zvk.R = 1e-6*eye(12);
%% ADA TUNING PARAMETER
diff --git a/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m b/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
index c1e9404cb55d544bc7eb48cecdb5e2b5f3254ae5..ab96e9f762f61c0d51aa1e71310f0454f1bf8381 100644
--- a/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
@@ -100,9 +100,9 @@ sensorSettings.accelerometer.bit = 16;
% sensorSettings.accelerometer.offsetX = (-0.05)/9.81*1e3; % +-90 in mg
% sensorSettings.accelerometer.offsetY = ( 0.10)/9.81*1e3; % +-90 in mg
% sensorSettings.accelerometer.offsetZ = (-0.15)/9.81*1e3; % +-90 in mg
-sensorSettings.accelerometer.offsetX = ( 0)/9.81*1e3; % +-90 in mg
-sensorSettings.accelerometer.offsetY = ( 0)/9.81*1e3; % +-90 in mg
-sensorSettings.accelerometer.offsetZ = ( 0)/9.81*1e3; % +-90 in mg
+sensorSettings.accelerometer.offsetX = ( -0.03)/9.81*1e3; % +-90 in mg
+sensorSettings.accelerometer.offsetY = ( 0.019)/9.81*1e3; % +-90 in mg
+sensorSettings.accelerometer.offsetZ = ( -0.015)/9.81*1e3; % +-90 in mg
sensorSettings.accelerometer.walkDiffusionCoef = 0; % guess
sensorSettings.accelerometer.dt = 0.01; % sampling time
@@ -114,12 +114,12 @@ sensorSettings.gyroscope = Sensor3D();
sensorSettings.gyroscope.maxMeasurementRange = 245e3; % 245e3, 500e3, 2000e3 in mdps
sensorSettings.gyroscope.minMeasurementRange = -245e3; % -245e3, -500e3, -2000e3 in mdps
sensorSettings.gyroscope.bit = 16;
-% sensorSettings.gyroscope.offsetX = rad2deg( 0.008)*1e3; % +-30e3 in mdps
-% sensorSettings.gyroscope.offsetY = rad2deg(-0.005)*1e3; % +-30e3 in mdps
-% sensorSettings.gyroscope.offsetZ = rad2deg( 0.002)*1e3; % +-30e3 in mdps
-sensorSettings.gyroscope.offsetX = rad2deg( 0)*1e3; % +-30e3 in mdps
-sensorSettings.gyroscope.offsetY = rad2deg( 0)*1e3; % +-30e3 in mdps
-sensorSettings.gyroscope.offsetZ = rad2deg( 0)*1e3; % +-30e3 in mdps
+sensorSettings.gyroscope.offsetX = rad2deg( -0.004)*1e3; % +-30e3 in mdps
+sensorSettings.gyroscope.offsetY = rad2deg(0.001)*1e3; % +-30e3 in mdps
+sensorSettings.gyroscope.offsetZ = rad2deg( -0.002)*1e3; % +-30e3 in mdps
+% sensorSettings.gyroscope.offsetX = rad2deg( 0)*1e3; % +-30e3 in mdps
+% sensorSettings.gyroscope.offsetY = rad2deg( 0)*1e3; % +-30e3 in mdps
+% sensorSettings.gyroscope.offsetZ = rad2deg( 0)*1e3; % +-30e3 in mdps
sensorSettings.gyroscope.walkDiffusionCoef = 1; % guess
sensorSettings.gyroscope.dt = 0.01; % sampling time
sensorSettings.gyroscope.transMatrix = diag([1 1 1]); % axis transformation
diff --git a/simulator/src/std_run.m b/simulator/src/std_run.m
index 40a5817e90192c7c51ee40ebe0b6c64af4ddb2fb..dd395c1a7304cb3336c871e32792fd1e3a6ee3af 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -490,56 +490,56 @@ while settings.flagStopIntegration && n_old < nmax % St
figure()
subplot(3,1,1)
plot(zvk.time,ones(1,length(zvk.time)).*bias_acc_x, 'k--',...
- zvk.time,zvk.states(:,11), 'r', 'LineWidth',2);
+ zvk.time,zvk.states(:,7), 'r', 'LineWidth',2);
legend('X','bias accelerometro x')
subplot(3,1,2)
plot(zvk.time,ones(1,length(zvk.time)).*bias_acc_y, 'k--',...
- zvk.time,zvk.states(:,12), 'g','LineWidth',2);
+ zvk.time,zvk.states(:,8), 'g','LineWidth',2);
legend('Y','bias accelerometro y')
subplot(3,1,3)
plot(zvk.time,ones(1,length(zvk.time)).*bias_acc_z, 'k--',...
- zvk.time,zvk.states(:,13),'b', 'LineWidth',2);
+ zvk.time,zvk.states(:,9),'b', 'LineWidth',2);
legend('Z','bias acceleromtro z')
figure()
subplot(3,1,1)
plot(zvk.time,ones(1,length(zvk.time)).*bias_gyro_x,'k--',...
- zvk.time,zvk.states(:,14), 'r', 'LineWidth',2);
+ zvk.time,zvk.states(:,10), 'r', 'LineWidth',2);
legend('X','bias gyro x')
subplot(3,1,2)
plot(zvk.time,ones(1,length(zvk.time)).*bias_gyro_y, 'k--',...
- zvk.time,zvk.states(:,15), 'g','LineWidth',2);
+ zvk.time,zvk.states(:,11), 'g','LineWidth',2);
legend('Y','bias gyro y')
subplot(3,1,3)
plot(zvk.time,ones(1,length(zvk.time)).*bias_gyro_z, 'k--',...
- zvk.time,zvk.states(:,16), 'b','LineWidth',2);
+ zvk.time,zvk.states(:,12), 'b','LineWidth',2);
legend('Z','bias gyro z')
- % media_acc = sum(zvk.states(:,11:13))/length(zvk.time);
- % error_acc = media_acc - [bias_acc_x bias_acc_y bias_acc_z]
- %
- % media_gyro = sum(zvk.states(:,14:16))/length(zvk.time);
- % error_gyro = media_gyro - [bias_gyro_x bias_gyro_y bias_gyro_z]
+ media_acc = sum(zvk.states(:,7:9))/length(zvk.time);
+ error_acc = media_acc - [bias_acc_x bias_acc_y bias_acc_z]
+
+ media_gyro = sum(zvk.states(:,10:12))/length(zvk.time);
+ error_gyro = media_gyro - [bias_gyro_x bias_gyro_y bias_gyro_z]
- estimated_bias_acc = zvk.states(end,11:13);
- estimated_bias_acc_sigma = sqrt( diag(zvk.P(10:12, 10:12, end))' );
+ estimated_bias_acc = zvk.states(end,7:9);
+ estimated_bias_acc_sigma = sqrt( diag(zvk.P(7:9, 7:9, end))' );
- estimated_bias_gyro = zvk.states(end,14:16);
- estimated_bias_gyro_sigma = sqrt( diag(zvk.P(13:15, 13:15, end))' );
+ estimated_bias_gyro = zvk.states(end,10:12);
+ estimated_bias_gyro_sigma = sqrt( diag(zvk.P(10:12, 10:12, end))' );
disp("estimated_bias_acc")
disp(estimated_bias_acc)
- disp("estimated_bias_acc_sigma")
- disp(estimated_bias_acc_sigma)
+ % disp("estimated_bias_acc_sigma")
+ % disp(estimated_bias_acc_sigma)
disp("estimated_bias_gyro")
disp(estimated_bias_gyro)
- disp("estimated_bias_gyro_sigma")
- disp(estimated_bias_gyro_sigma)
+ % disp("estimated_bias_gyro_sigma")
+ % disp(estimated_bias_gyro_sigma)
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index 3427f1b495e1f6cc4a4509d7b7c0ba738043725f..9d5fed05e0a6d44fbb2809bc3d86f72ab70cec11 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -196,12 +196,14 @@ settings.nas.mag_freq = settings.frequencies.NASFrequency/settings.nas.mag_decim
% sensorData.zvk.states = [Q0(2:4); Q0(1); V0; X0; [-0.05; 0.1; -0.05]; [-0.005; 0; 0.007]]';
% sensorData.zvk.states = [Q0(2:4); Q0(1); V0; X0; [-0.25; 0.20; 0.10]; 0*ones(3,1)]';
-sensorData.zvk.states = [Q0(2:4); Q0(1); V0; X0; 0*ones(6,1)]';
-
+% sensorData.zvk.states = [Q0(2:4); Q0(1); V0; X0; [0 -0.015 0.023]'; 0*ones(3,1)]';
+sensorData.zvk.states = [V0; X0; 0*ones(3,1); 0*ones(3,1)]';
if settings.scenario ~="descent"
- sensorData.zvk.states(10) = -environment.z0;
+ % sensorData.zvk.states(10) = -environment.z0;
+ sensorData.zvk.states(6) = -environment.z0;
else
- sensorData.zvk.states(10) = -settings.z_final-environment.z0;
+ % sensorData.zvk.states(10) = -settings.z_final-environment.z0;
+ sensorData.zvk.states(6) = -settings.z_final-environment.z0;
end
sensorData.zvk.P = settings.zvk.P0;
diff --git a/simulator/src/std_run_parts/std_subsystems.m b/simulator/src/std_run_parts/std_subsystems.m
index f9aa200f51953b27434e2e3581f7ba810f2eadcf..8a22d9030da91709a289fb7b3d41344c5f73facd 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -153,5 +153,6 @@ end
%% ZVK
if currentState == availableStates.on_ground
- [sensorData, sensorTot] = run_ZVK(t1,sensorData, sensorTot, settings, XYZ0*0.01, environment);
+ % [sensorData, sensorTot] = run_ZVK(t1,sensorData, sensorTot, settings, XYZ0*0.01, environment);
+ [sensorData, sensorTot] = run_ZVK1(t1,sensorData, sensorTot, settings, XYZ0*0.01, environment);
end
\ No newline at end of file
diff --git a/test.txt b/test.txt
new file mode 100644
index 0000000000000000000000000000000000000000..df57a1aac4811c9e0e9d45d52bd124582259401f
--- /dev/null
+++ b/test.txt
@@ -0,0 +1 @@
+prova