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