From 7cad2fc4f34ba92c5367e0ad3fa0b85e7712d41a Mon Sep 17 00:00:00 2001
From: "guglielmo.gualdana" <guglielmo.gualdana@skywarder.eu>
Date: Thu, 17 Apr 2025 13:46:49 +0200
Subject: [PATCH] prima simulaizone
---
commonFunctions/kalman/correctorZVK.m | 17 ++++-
commonFunctions/kalman/predictorZVK.m | 74 ++++++++++---------
commonFunctions/kalman/run_ZVK.m | 7 +-
.../config2025_Orion_Portugal_October.m | 12 +--
.../src/std_run_parts/std_setInitialParams.m | 2 +-
simulator/src/std_run_parts/std_subsystems.m | 2 +-
6 files changed, 62 insertions(+), 52 deletions(-)
diff --git a/commonFunctions/kalman/correctorZVK.m b/commonFunctions/kalman/correctorZVK.m
index 835f8d8..1ac6628 100644
--- a/commonFunctions/kalman/correctorZVK.m
+++ b/commonFunctions/kalman/correctorZVK.m
@@ -25,12 +25,14 @@ function [x_new, P_new] = correctrZVK(x_pred, P_pred)
% 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);
+ P_xx_pred = P_pred(1:15,1:15);
+ P_xp_pred = P_pred(1:15,16:33);
+ P_pp_pred = P_pred(16:33,16:33);
+ R = [1e-3*diag(ones(3,1)), zeros(3);
+ zeros(3), 1e-3*diag(ones(3,1))];
- S = H_x * P_xx_pred * H_x';
+ S = H_x * P_xx_pred * H_x' + R;
K = P_xx_pred * H_x' * S;
@@ -49,4 +51,11 @@ function [x_new, P_new] = correctrZVK(x_pred, P_pred)
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)' ];
+
+ P_xx_new = (1 - K * H_x) * P_xx_pred;
+
+ P_new = [P_xx_new, P_xp_pred;
+ P_xp_pred', P_pp_pred];
+
+
end
\ No newline at end of file
diff --git a/commonFunctions/kalman/predictorZVK.m b/commonFunctions/kalman/predictorZVK.m
index 02c1b4c..2e3bfb4 100644
--- a/commonFunctions/kalman/predictorZVK.m
+++ b/commonFunctions/kalman/predictorZVK.m
@@ -39,8 +39,8 @@ function [z_pred, P_pred] = predictorZVK( x_prev, P_prev, a_b_m, om_b_m, dt, zvk
%%% 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;
+ bias_a_pred = eye(3)*bias_a_prev + zvk.acc_bias_noise;
+ bias_g_pred = eye(3)*bias_g_prev + zvk.gyro_bias_noise;
% Assemble predicted global state
@@ -51,13 +51,14 @@ function [z_pred, P_pred] = predictorZVK( x_prev, P_prev, a_b_m, om_b_m, dt, zvk
%%% 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;
+ F11 = - omega_mat; % phi/phi
+ F14 = - eye(3); % phi/beta_gyro
+ F23 = eye(3); % r/v
+ F31 = - A * [ 0 -a_i(3) a_i(2); % v/phi
+ a_i(3) 0 -a_i(1);
+ -a_i(2) a_i(1) 0;];
+ F35 = - A; % v/beta_acc
+
% x = r_prev(1);
% y = r_prev(2);
@@ -71,39 +72,40 @@ function [z_pred, P_pred] = predictorZVK( x_prev, P_prev, a_b_m, om_b_m, dt, zvk
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)];
+ F = [F11, zeros(3), zeros(3), F14, zeros(3);
+ zeros(3), zeros(3), F23, zeros(3), zeros(3);
+ F31, F32, zeros(3), zeros(3) F35;
+ 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)];
+ 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), diag(ones(3,1)), zeros(3), zeros(3);
+ zeros(3), zeros(3), zeros(3), diag(ones(3,1))];
+
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];
+ G34 = - A * diag(a_i);
+ G35 = A * diag(a_i);
+ G36 = - A * [ 0, a_i(3), a_i(2);
+ a_i(3), 0, a_i(2);
+ a_i(2), a_i(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)];
+ 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);
+ PHI = expm(F * dt);
GAMMA = (PHI*G_w);
@@ -111,18 +113,18 @@ function [z_pred, P_pred] = predictorZVK( x_prev, P_prev, a_b_m, om_b_m, dt, zvk
- 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_prev = P_prev(1:15,1:15);
+ P_xp_prev = P_prev(1:15,16:33);
+ P_pp_prev = P_prev(16:33,16:33);
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'
+ 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_xp_pred = PHI * P_xp_prev + PSI * P_pp_prev;
% P_pp does nto change
diff --git a/commonFunctions/kalman/run_ZVK.m b/commonFunctions/kalman/run_ZVK.m
index a73ee9d..69326c5 100644
--- a/commonFunctions/kalman/run_ZVK.m
+++ b/commonFunctions/kalman/run_ZVK.m
@@ -1,4 +1,4 @@
-function [sensorTot] = run_ZVK(Tf, sensorData, sensorTot, settings, environment)
+function [sensorData, sensorTot] = run_ZVK(Tf, sensorData, sensorTot, settings, environment)
% recall zvk time
@@ -46,10 +46,9 @@ if length(t_zvk) > 1
%%% Correction
- correctorZVK( x(ii,:), P_z(:,:,ii) );
+ [x(ii,:), P_z(:,:,ii)] = correctorZVK( x(ii,:), P_z(:,:,ii) );
-
-
+
end
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 5a2b347..4f656d3 100644
--- a/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
+++ b/data/2025_Orion_Portugal_October/config2025_Orion_Portugal_October.m
@@ -145,14 +145,14 @@ settings.nas.Qq = [(settings.nas.sigma_w^2*settings.nas.dt_k+(1/3
% 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.mu = 3.986004418e14;
+% settings.zvk.Re = 6378137;
+% settings.zvk.J2 = 1.082636e-3;
-settings.zvk.P = 0.5*eye(33);
+settings.zvk.P = 0.1*eye(33);
-settings.zvk.gyro_bias_noise = 0.05;
-settings.zvk.acc_bias_noise = 0.05;
+settings.zvk.gyro_bias_noise = deg2rad(0.5)/sqrt(3600);
+settings.zvk.acc_bias_noise = 10000*9.81e-6/sqrt(50);
diff --git a/simulator/src/std_run_parts/std_setInitialParams.m b/simulator/src/std_run_parts/std_setInitialParams.m
index 11560ec..175ab5c 100644
--- a/simulator/src/std_run_parts/std_setInitialParams.m
+++ b/simulator/src/std_run_parts/std_setInitialParams.m
@@ -194,7 +194,7 @@ settings.nas.mag_freq = settings.frequencies.NASFrequency/settings.nas.mag_decim
% if settings.flagNAS || settings.electronics ??????
-sensorData.zvk.states = [Q0(2:4); Q0(1); X0; V0; zeros(6,1); zeros(18,1)]';
+sensorData.zvk.states = [Q0(2:4); Q0(1); X0; V0; zeros(3,1); zeros(3,1); zeros(18,1)]';
if settings.scenario ~="descent"
sensorData.zvk.states(7) = -environment.z0;
diff --git a/simulator/src/std_run_parts/std_subsystems.m b/simulator/src/std_run_parts/std_subsystems.m
index 5b188c2..7f8a323 100644
--- a/simulator/src/std_run_parts/std_subsystems.m
+++ b/simulator/src/std_run_parts/std_subsystems.m
@@ -153,5 +153,5 @@ end
%% ZVK
if currentState == availableStates.on_ground
- [sensorTot] = run_ZVK(t1,sensorData, sensorTot, settings, environment);
+ [sensorData, sensorTot] = run_ZVK(t1,sensorData, sensorTot, settings, environment);
end
\ No newline at end of file
--
GitLab