From 0600bdb99d3f3d14b79924419f49ac0c1ba267ae Mon Sep 17 00:00:00 2001
From: "guglielmo.gualdana" <guglielmo.gualdana@skywarder.eu>
Date: Sat, 17 May 2025 11:46:00 +0200
Subject: [PATCH] unstable

---
 commonFunctions/kalman/correctorZVK.m         | 12 ++--
 commonFunctions/kalman/predictorZVK.m         |  8 +--
 .../config2025_Orion_Portugal_October.m       | 20 +++---
 .../initSensors2025_Orion_Portugal_October.m  | 24 ++++----
 simulator/src/std_run.m                       | 61 +++++++------------
 .../src/std_run_parts/std_setInitialParams.m  |  8 +++
 6 files changed, 63 insertions(+), 70 deletions(-)

diff --git a/commonFunctions/kalman/correctorZVK.m b/commonFunctions/kalman/correctorZVK.m
index a5b1c6e..7ee4698 100644
--- a/commonFunctions/kalman/correctorZVK.m
+++ b/commonFunctions/kalman/correctorZVK.m
@@ -33,9 +33,10 @@ 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)];
 
+    % H_x = H_x(1:6,:);
 
     S = H_x * P_pred * H_x' + zvk.R;
 
@@ -44,6 +45,8 @@ function [x_new, P_new] = correctorZVK(x_pred, P_pred, a_b_m, om_b_m, mag_meas,
 
     error = [zeros(6,1); g_meas; mag_meas'] - [v_pred; om_b_m'-bias_g_pred; g_est; z];
 
+    % error = [zeros(6,1); g_meas] - [v_pred; om_b_m'-bias_g_pred; g_est];
+
     update = K * error;
 
     
@@ -52,12 +55,7 @@ function [x_new, P_new] = correctorZVK(x_pred, P_pred, a_b_m, om_b_m, mag_meas,
 
     quat_pred = x_pred(1:4);
 
-    % quat_error = [update(1:3)'/2, 1];
-    % 
-    % 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)' ];
 
 
     OM = [ quat_pred(4), -quat_pred(3),  quat_pred(2);
@@ -70,7 +68,7 @@ function [x_new, P_new] = correctorZVK(x_pred, P_pred, a_b_m, om_b_m, mag_meas,
     x_new(1:4) = quat_star / norm(quat_star);
 
 
-    disp(rad2deg(quat2eul(x_new(1:4))))
+    disp(rad2deg(quat2eul( [x_new(4), x_new(1:3)] )))
 
     
     P_new = (eye(15) - K * H_x) * P_pred;
diff --git a/commonFunctions/kalman/predictorZVK.m b/commonFunctions/kalman/predictorZVK.m
index d6b13ab..79c5e66 100644
--- a/commonFunctions/kalman/predictorZVK.m
+++ b/commonFunctions/kalman/predictorZVK.m
@@ -36,12 +36,12 @@ function [x_pred, P_pred] = predictorZVK( x_prev, P_prev, sf_b_measure, om_b_m,
                     -om_b'      0];
     
     quat_pred  = (eye(4) + 0.5*Omega*dt) * quat_prev;
+    quat_pred = quat_pred / norm(quat_pred);
 
-    % disp(rad2deg(quat2eul(quat_pred')))
+
+    disp(rad2deg(quat2eul( [ quat_pred(4), quat_pred(1:3)' ] )))
 
     %%% 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;
     
@@ -90,6 +90,6 @@ function [x_pred, P_pred] = predictorZVK( x_prev, P_prev, sf_b_measure, om_b_m,
     GAMMA = (PHI*G);
 
   
-    P_pred = PHI * P_prev * PHI'+ GAMMA * zvk.Q * dt * GAMMA';
+    P_pred = PHI * P_prev * PHI'+ GAMMA * zvk.Q * GAMMA';
     
 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 c0eccf6..26b40e4 100644
--- a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
@@ -147,10 +147,10 @@ settings.nas.Qq              =   [(settings.nas.sigma_w^2*settings.nas.dt_k+(1/3
 % settings.zvk.J2 = 1.082636e-3;
 
 % Estimated noise standard deviations
-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_gyro         = 5e-4;     % [rad/s]   gyroscope variance
+settings.zvk.sigma_beta_g       = 1e-8;     % [rad/s]   gyroscope bias variance
+settings.zvk.sigma_acc          = 5e-2;     % [m/s^2]   accelerometer variance
+settings.zvk.sigma_beta_acc     = 1e-6;     % [m/s^2]   accelerometer bias variance
 settings.zvk.sigma_mag          = 3;       % [mgauss]  magnetometer variance            NAS = 10?????   mentre sito STM di 3mGs RMS noise
 
 % Process noise covariance matrix Q
@@ -163,11 +163,11 @@ settings.zvk.Q = diag([
 
 % Initial state covariance matrix P
 settings.zvk.P0 = diag([
-    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]
-    0.01 * ones(1,3)...    % gyroscope bias [rad/s]
+    deg2rad(0.05)^2 * ones(1,3),...   % small-angle errors [rad]          2sigma = 0.5deg
+    1e-6 * ones(1,3),...   % velocity uncertainty [m/s]
+    1e-6 * ones(1,3),...   % position uncertainty [m]
+    (0.025)^2  * ones(1,3),...   % accelerometer bias [m/s^2]             2sigma = 0.05  -> (0.025)^2
+    (0.0025)^2 * ones(1,3)...    % gyroscope bias [rad/s]                 2sigma = 0.005 -> (0.0025)^2
 ]);
 
 % Measurement noise covariance matrix R
@@ -178,6 +178,8 @@ settings.zvk.R = [
     zeros(3),           zeros(3),           zeros(3),                               settings.zvk.sigma_mag^2 * eye(3)       % magnetometer correction
 ];
 
+% settings.zvk.R = settings.zvk.R(1:6,1:6);
+
 
 %% 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 c1e9404..d4bf1f8 100644
--- a/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/initSensors2025_Orion_Portugal_October.m
@@ -97,12 +97,12 @@ sensorSettings.accelerometer = Sensor3D();
 sensorSettings.accelerometer.maxMeasurementRange   =   16000;                       % 2000, 4000, 8000, 16000 in mg
 sensorSettings.accelerometer.minMeasurementRange   =   -16000;                      % -2000, -4000, -8000, -16000 in mg
 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)/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.05)/9.81*1e3;            % +-90 in mg
+% sensorSettings.accelerometer.offsetY               =   ( -0.05)/9.81*1e3;            % +-90 in mg
+% sensorSettings.accelerometer.offsetZ               =   ( 0.05)/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)*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.002)*1e3;             % +-30e3 in mdps
+% sensorSettings.gyroscope.offsetY               =   rad2deg( -0.001)*1e3;             % +-30e3 in mdps
+% sensorSettings.gyroscope.offsetZ               =   rad2deg( 0.003)*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 40a5817..123efb8 100644
--- a/simulator/src/std_run.m
+++ b/simulator/src/std_run.m
@@ -150,7 +150,7 @@ else
 end
 
 if ~settings.montecarlo
-    time_on_ground = 50; % [s] - How much time the rockets stays on ramp before launch
+    time_on_ground = 100; % [s] - How much time the rockets stays on ramp before launch
 else
     time_on_ground = 0; % [s] - If montecarlo the rocket starts immediately
 end
@@ -515,12 +515,6 @@ while settings.flagStopIntegration && n_old < nmax                          % St
              zvk.time,zvk.states(:,16), '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]
-
 
         estimated_bias_acc = zvk.states(end,11:13);
         estimated_bias_acc_sigma = sqrt( diag(zvk.P(10:12, 10:12, end))' ); 
@@ -542,13 +536,7 @@ while settings.flagStopIntegration && n_old < nmax                          % St
         disp(estimated_bias_gyro_sigma)
 
 
-
-
-        
-        
-        
-        
-        [a,b,c] = quat2angle(zvk.states(:,1:4));
+        [a,b,c] = quat2angle( [ zvk.states(:,4) zvk.states(:,1:3) ] );
         a = wrapTo2Pi(a);
         
         a = rad2deg(a);
@@ -559,40 +547,37 @@ while settings.flagStopIntegration && n_old < nmax                          % St
         subplot(3,1,1)
         hold on
         plot(zvk.time,a)
-        yline(a(1))
+        yline(133)
         subplot(3,1,2)
         hold on
         plot(zvk.time,b)
-        yline(b(1))
+        yline(85)
         subplot(3,1,3)
         hold on
         plot(zvk.time,c)
-        yline(c(1))
-        
-        
+        yline(0)
         
-        quat_init = zvk.states(1,1:4);
-        quat_end = zvk.states(end,1:4);
         
-        disp("quaternions")
-        disp(quat_init)
-        disp(quat_end )
-        
-        disp("euler angles")
-        disp(rad2deg(quat2eul(quat_init)))
-        disp(rad2deg(quat2eul(quat_end)))
-        
-        
-        % A_init   = [quat_init(1)^2 - quat_init(2)^2 - quat_init(3)^2 + quat_init(4)^2,               2*(quat_init(1)*quat_init(2) + quat_init(3)*quat_init(4)),                 2*(quat_init(1)*quat_init(3) - quat_init(2)*quat_init(4));
-        %      2*(quat_init(1)*quat_init(2) - quat_init(3)*quat_init(4)),      -quat_init(1)^2 + quat_init(2)^2 - quat_init(3)^2 + quat_init(4)^2,                2*(quat_init(2)*quat_init(3) + quat_init(1)*quat_init(4)) ;
-        %      2*(quat_init(1)*quat_init(3) + quat_init(2)*quat_init(4)),               2*(quat_init(2)*quat_init(3) - quat_init(1)*quat_init(4)),       -quat_init(1)^2 - quat_init(2)^2 + quat_init(3)^2 + quat_init(4)^2];
-        % 
-        % A_end   = [quat_end(1)^2 - quat_end(2)^2 - quat_end(3)^2 + quat_end(4)^2,               2*(quat_end(1)*quat_end(2) + quat_end(3)*quat_end(4)),                 2*(quat_end(1)*quat_end(3) - quat_end(2)*quat_end(4));
-        %      2*(quat_end(1)*quat_end(2) - quat_end(3)*quat_end(4)),      -quat_end(1)^2 + quat_end(2)^2 - quat_end(3)^2 + quat_end(4)^2,                2*(quat_end(2)*quat_end(3) + quat_end(1)*quat_end(4)) ;
-        %      2*(quat_end(1)*quat_end(3) + quat_end(2)*quat_end(4)),               2*(quat_end(2)*quat_end(3) - quat_end(1)*quat_end(4)),       -quat_end(1)^2 - quat_end(2)^2 + quat_end(3)^2 + quat_end(4)^2];
-        % disp(A_end-A_init)
         
+                
         
+        acc_m = sensorTot.imu.accelerometer_measures;
+        % gyro_m = sensorTot.imu.gyro_measures;
+        % mag_m = sensorTot.imu.magnetometer_measures;
+
+        % cov(acc_m)
+        % cov(gyro_m)
+        % cov(mag_m)
+
+
+        figure()
+        hold on
+        title('Accelerometro misure')
+        plot(acc_m(:,1))
+        plot(acc_m(:,2))
+        plot(acc_m(:,3))
+        legend('x','y','z')
+
         pause(1e6)
 
     end
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index 3427f1b..11f21a2 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -194,6 +194,14 @@ settings.nas.mag_freq = settings.frequencies.NASFrequency/settings.nas.mag_decim
 
 % if settings.flagNAS || settings.electronics  ??????
 
+% small = [1 deg2rad([0.005 -0.005 0.005]) ];
+% 
+% Q0 = quatmultiply(Q0', small);
+% 
+% rad2deg(quat2eul(Q0))
+% 
+% Q0 = Q0';
+
 % 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)]';
-- 
GitLab