Test commit visibilità

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)];
 
 

commonFunctions/kalman/correctorZVK1.m

+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

commonFunctions/kalman/predictorQuatZVK.m

+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
+

commonFunctions/kalman/predictorZVK.m

 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

commonFunctions/kalman/predictorZVK1.m

+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

commonFunctions/kalman/run_ZVK1.m

+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

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
 

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

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)
 
 
 

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;
 

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

test.txt

+prova