diff --git a/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk.m b/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk.m
index d5e72b07a6ba878e0dc8d0f2d7ae80d4d2c27a14..419fb772c78f22d35e02ef8e8dba78e1cca36718 100644
--- a/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk.m
+++ b/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk.m
@@ -3,30 +3,98 @@ clear variables;
close all;
clc;
-IMU = table2array(readtable('IMU.csv')); % [tempo*e-6 acc_x acc_y acc_z w_x,w_y,w_z]
-dt_IMU = 1000;
-lastImuIdx = 0;
-t_end = 3870000;
+addpath(genpath("logs"))
+
+
+IMU_main = table2array(readtable("main_Boardcore_IMUDataEUROC")); % [tempo*e-6 acc_x acc_y acc_z w_x,w_y,w_z]
+
+dt = mean(diff(IMU_main(:,1)));
+
+idxend_main = find(IMU_main(:,2)>20,1,'first');
+idxend_main = round(idxend_main - 5e6/dt);
+
+time_acc_main = IMU_main(1:idxend_main,1)*1e-6;
+acc_main = IMU_main(1:idxend_main,2:4);
+time_gyr_main = IMU_main(1:idxend_main,5)*1e-6;
+gyr_main = IMU_main(1:idxend_main,6:8);
+
+figure()
+subplot(2,1,1)
+title('acc main')
+hold on
+grid on
+plot(time_acc_main, acc_main(:,1), 'DisplayName','x');
+plot(time_acc_main, acc_main(:,2), 'DisplayName','y');
+plot(time_acc_main, acc_main(:,3), 'DisplayName','z');
+legend
+
+subplot(2,1,2)
+title('gyro main')
+hold on
+grid on
+plot(time_gyr_main, gyr_main(:,1), 'DisplayName','x');
+plot(time_gyr_main, gyr_main(:,2), 'DisplayName','y');
+plot(time_gyr_main, gyr_main(:,3), 'DisplayName','z');
+legend
+
+
+
+
+IMU_payload = table2array(readtable("payload_Boardcore_IMUDataEUROC")); % [tempo*e-6 acc_x acc_y acc_z w_x,w_y,w_z]
+
+
+dt = mean(diff(IMU_payload(:,1)));
+
+idxend_payload = find(IMU_payload(:,2)>20,1,'first');
+idxend_payload = round(idxend_payload - 5e6/dt);
+
+time_acc_payload = IMU_payload(1:idxend_payload,1)*1e-6;
+acc_payload = IMU_payload(1:idxend_payload,2:4);
+time_gyr_payload = IMU_payload(1:idxend_payload,5)*1e-6;
+gyr_payload = IMU_payload(1:idxend_payload,6:8);
+
+figure()
+subplot(2,1,1)
+title('acc payload')
+hold on
+grid on
+plot(time_acc_payload, acc_payload(:,1), 'DisplayName','x');
+plot(time_acc_payload, acc_payload(:,2), 'DisplayName','y');
+plot(time_acc_payload, acc_payload(:,3), 'DisplayName','z');
+legend
+
+subplot(2,1,2)
+title('gyro payload')
+hold on
+grid on
+plot(time_gyr_payload, gyr_payload(:,1), 'DisplayName','x');
+plot(time_gyr_payload, gyr_payload(:,2), 'DisplayName','y');
+plot(time_gyr_payload, gyr_payload(:,3), 'DisplayName','z');
+legend
+
+
+
%% Initial params
-x0 = zeros(1,18);
+x0 = zeros(1,24);
+% vel, acc, bias_acc_main, bias_acc_payload, theta, omega, bias_gyro_main, bias_gyro_payoad
% Estimated noise standard deviations
-sigma_gyro = 5e-5; % [rad/s] gyroscope variance NAS
-sigma_beta_g = 1e-6; % [rad/s] gyroscope bias variance NAS
-sigma_acc = 5e-2; % [m/s^2] accelerometer variance NAS
-sigma_beta_acc = 1e-5; % [m/s^2] accelerometer bias variance BOH
-sigma_mag = 3; % [mgauss] magnetometer variance NAS = 10????? mentre sito STM di 3mGs RMS noise
-
+sigma_gyro = 5e-3; % [rad/s] gyroscope std
+sigma_beta_g = 1e-5; % [rad/s] gyroscope bias std
+sigma_acc = 4e-2; % [m/s^2] accelerometer std
+sigma_beta_acc = 1e-5; % [m/s^2] accelerometer bias std
Q = diag([
(1e-6)^2 * ones(3,1); % vel
(1e-6)^2 * ones(3,1); % acceleration
sigma_beta_acc^2 * ones(3,1); % bias accelerometer
+ sigma_beta_acc^2 * ones(3,1); % bias accelerometer
(1e-6)^2 * ones(3,1); % theta
(1e-6)^2 * ones(3,1); % angular velocity
sigma_beta_g^2 * ones(3,1); % bias gyroscope
+ sigma_beta_g^2 * ones(3,1); % bias gyroscope
]);
@@ -34,96 +102,297 @@ Q = diag([
P0 = diag([
(1e-5)^2 * ones(3,1); % velocity uncertainty [m/s]
(1e-5)^2 * ones(3,1); % acceleration [m/s^2]
- (1e-1)^2 * ones(3,1); % accelerometer bias [m/s^2]
+ (1e-1)^2 * ones(3,1); % accelerometer bias main [m/s^2]
+ (1e-1)^2 * ones(3,1); % accelerometer bias payload [m/s^2]
(1e-5)^2 * ones(3,1); % ang velocity [rad/s]
(1e-5)^2 * ones(3,1); % ang acceleration [rad/s^2]
- (1e-3)^2 * ones(3,1) % gyro bias [rad/s]
+ (1e-3)^2 * ones(3,1) % gyro bias main [rad/s]
+ (1e-3)^2 * ones(3,1) % gyro bias payload [rad/s]
]);
% Measurement noise covariance matrix R
-R = diag([
- (1e-6)^2 * ones(3,1); % FAKE zero velocity [m/s]
- sigma_acc^2 * ones(3,1); % accelerometer [m/s^2]
- (1e-6)^2 * ones(3,1); % FAKE angle [rad]
- sigma_gyro^2 * ones(3,1) % angular velocity [rad/s]
- ]);
+R_fake = diag([
+ (1e-6)^2 * ones(3,1); % FAKE zero velocity [m/s]
+ (1e-6)^2 * ones(3,1); % FAKE angle [rad]
+ ]);
+R_acc = diag( sigma_acc^2 * ones(3,1)); % accelerometer [m/s^2]
+R_gyro = diag(sigma_gyro^2 * ones(3,1)); % angular velocity [rad/s]
+
Q0 = angle2quat(133*pi/180, 85*pi/180, 0*pi/180, 'ZYX')';
-error = [1 0.5*deg2rad([0 1 0]) ];
-Q0 = quatmultiply(Q0', error);
-Q0 = Q0';
+% error = [1 0.5*deg2rad([0 0 0]) ];
+% Q0 = quatmultiply(Q0', error);
+%Q0 = Q0';
quat = [Q0(2:4); Q0(1)];
+% load("Ksave");
+
%% Initializzation
x = x0;
P = P0;
+%% SIMU_mainlation
-%% Simulation
+t_IMU_main = IMU_main(1:idxend_main,1);
+t_IMU_payload = IMU_payload(1:idxend_payload,1);
ii = 2;
-for t = 0:2000:t_end
-
- dt = t*1e-6;
- % Prende la misura dei sensori
-
- imu_idx = sum(IMU(:,1) <= t); %conta quanti campioni sono stati presi
- if imu_idx > lastImuIdx % se c'è una nuova misura
- IMU_measurment = IMU(imu_idx, 2:7); %aggiorna la misura
- lastImuIdx = imu_idx;
- else
- IMU_measurment = IMU(lastImuIdx,2:7); %altrimenti lascia il vecchio indice
- end
- acc_meas = IMU_measurment(1:3);
- omeg_meas = IMU_measurment(4:6);
-
+
+ZVK_freq = 50; %[Hz]
+ZVK_sampling_time = 1/ZVK_freq*1e6;
+
+last_t = t_IMU_main(1);
+
+t_start = t_IMU_main(1);
+t_end = t_IMU_main(end);
+
+ZVK_time = [];
+
+for t = t_start:ZVK_sampling_time:t_end
+
%%% Prediction
+ dt = (t-last_t)*1e-6;
[x(ii,:), P(:,:,ii)] = predictor_zvk( x(ii-1,:), P(:,:,ii-1), dt, Q);
-
+
+
%%% Correction
- [x(ii,:), P(:,:,ii)] = corrector_zvk( x(ii,:), P(:,:,ii), quat, acc_meas, omeg_meas, R);
- x(ii,:)
+ % FAKE
+ [x(ii,[1:3,13:15]), P([1:3,13:15],[1:3,13:15],ii)] = corrector_zvk( x(ii,[1:3,13:15]), P([1:3,13:15],[1:3,13:15],ii), quat, R_fake);
+
+
+ % MAIN
+ index_imu = sum(t >= t_IMU_main);
+ IMU_main_measurment = IMU_main(index_imu,[2:4 6:8]);
+ acc_meas = IMU_main_measurment(1:3);
+ omeg_meas = IMU_main_measurment(4:6);
+
+ [x(ii,[4:6,7:9]), P([4:6,7:9],[4:6,7:9],ii)] = corrector_zvk_acc( x(ii,[4:6,7:9]), P([4:6,7:9],[4:6,7:9],ii), quat, acc_meas, R_acc);
+ [x(ii,[16:18,19:21]), P([16:18,19:21],[16:18,19:21],ii)] = corrector_zvk_gyro( x(ii,[16:18,19:21]), P([16:18,19:21],[16:18,19:21],ii), quat, omeg_meas, R_gyro);
+
+ % PAYLOAD
+ index_imu = sum(t >= t_IMU_payload);
+ IMU_payload_measurment = IMU_payload(index_imu,[2:4 6:8]);
+ acc_meas = IMU_payload_measurment(1:3);
+ omeg_meas = IMU_payload_measurment(4:6);
+
+ [x(ii,[4:6,10:12]), P([4:6,10:12],[4:6,10:12],ii)] = corrector_zvk_acc( x(ii,[4:6,10:12]), P([4:6,10:12],[4:6,10:12],ii), quat, acc_meas, R_acc);
+ [x(ii,[16:18,22:24]), P([16:18,22:24],[16:18,22:24],ii)] = corrector_zvk_gyro( x(ii,[16:18,22:24]), P([16:18,22:24],[16:18,22:24],ii), quat, omeg_meas, R_gyro);
+
ii = ii+1;
+ last_t = t;
+
+ ZVK_time = [ZVK_time, t];
end
x = x(2:end,:);
-%% Plotting
-t_plot = (0:2000:t_end)*1e-6;
+
+%%
+% clearvars
+close all
+% clc
+
+% load("bckp_doppia_imu.mat")
+
+est_bias_a_main = x(end,7:9);
+est_bias_a_payload = x(end,10:12);
+est_bias_g_main = x(end,19:21);
+est_bias_g_payload = x(end,22:24);
+
+est_bias_a_main_std = P(7:9,7:9,end);
+est_bias_a_payload_std = P(10:12,10:12,end);
+est_bias_g_main_std = P(19:21,19:21,end);
+est_bias_g_payload_std = P(22:24,22:24,end);
+
+
+% corrected_acc_main = (acc_main - est_bias_a_main);
+% figure()
+% subplot(2,1,1)
+% plot(movmean(acc_main(end-100:end,:),1,100))
+% subplot(2,1,2)
+% plot(movmean(corrected_acc_main(end-100:end,:),1,100))
+%
+% corrected_acc_payload = (acc_payload - est_bias_a_payload);
+% figure()
+% subplot(2,1,1)
+% plot(movmean(acc_payload(end-100:end,:),1,100))
+% subplot(2,1,2)
+% plot(movmean(corrected_acc_payload(end-100:end,:),1,100))
+
+
+
+corrected_gyr_main = (gyr_main - est_bias_g_main);
+figure()
+subplot(2,1,1)
+plot(movmean(gyr_main(1:end,:),1,100))
+subplot(2,1,2)
+plot(movmean(corrected_gyr_main(1:end,:),1,100))
+
+corrected_gyr_payload = (gyr_payload - est_bias_g_payload);
figure()
+subplot(2,1,1)
+plot(movmean(gyr_payload(1:end,:),1,100))
+subplot(2,1,2)
+plot(movmean(corrected_gyr_payload(1:end,:),1,100))
+
+
+
+
+
+%% Plotting
+close all
+
+ZVK_time = ZVK_time*1e-6;
+
+figure(); title('bias accelerometer main')
subplot(3,1,1)
- plot(t_plot,x(:,7), 'r', 'LineWidth',2);
+ plot(ZVK_time,x(:,7), 'r', 'LineWidth',2);
legend('bias accelerometro x')
xlabel('time [s]'); ylabel('bias');grid on
subplot(3,1,2)
- plot(t_plot,x(:,8), 'g','LineWidth',2);
+ plot(ZVK_time,x(:,8), 'g','LineWidth',2);
legend('bias accelerometro y')
xlabel('time [s]'); ylabel('bias');grid on
subplot(3,1,3)
- plot(t_plot,x(:,9),'b', 'LineWidth',2);
- legend('bias acceleromtro z')
+ plot(ZVK_time,x(:,9),'b', 'LineWidth',2);
+ legend('bias accelerometro z')
xlabel('time [s]'); ylabel('bias');grid on
grid on
-
-figure()
+
+figure(); title('bias accelerometer payload')
+ subplot(3,1,1)
+ plot(ZVK_time,x(:,10), 'r', 'LineWidth',2);
+ legend('bias accelerometro x')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,2)
+ plot(ZVK_time,x(:,11), 'g','LineWidth',2);
+ legend('bias accelerometro y')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,3)
+ plot(ZVK_time,x(:,12),'b', 'LineWidth',2);
+ legend('bias accelerometro z')
+ xlabel('time [s]'); ylabel('bias');grid on
+ grid on
+
+figure(); title('Bias Gyro main')
subplot(3,1,1)
- plot(t_plot,x(:,16), 'r', 'LineWidth',2);
+ plot(ZVK_time,x(:,19), 'r', 'LineWidth',2);
legend('bias gyro x')
xlabel('time [s]'); ylabel('bias');grid on
subplot(3,1,2)
+ plot(ZVK_time,x(:,20), 'g','LineWidth',2);
+ legend('bias gyro y')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,3)
+ plot(ZVK_time,x(:,21), 'b','LineWidth',2);
+ legend('bias gyro z')
+ xlabel('time [s]'); ylabel('bias');grid on
+
+figure(); title('Bias Gyro payload')
+ subplot(3,1,1)
+ plot(ZVK_time,x(:,22), 'r', 'LineWidth',2);
+ legend('bias gyro x')
xlabel('time [s]'); ylabel('bias');grid on
- plot(t_plot,x(:,17), 'g','LineWidth',2);
+ subplot(3,1,2)
+ plot(ZVK_time,x(:,23), 'g','LineWidth',2);
legend('bias gyro y')
+ xlabel('time [s]'); ylabel('bias');grid on
subplot(3,1,3)
- plot(t_plot,x(:,18), 'b','LineWidth',2);
+ plot(ZVK_time,x(:,24), 'b','LineWidth',2);
legend('bias gyro z')
xlabel('time [s]'); ylabel('bias');grid on
-
- estimated_bias_acc = x(end,7:9);
- estimated_bias_acc_sigma = sqrt( diag(P(7:9, 7:9, end))' );
-
- estimated_bias_gyro = x(end,16:18);
- estimated_bias_gyro_sigma = sqrt( diag(P(16:18, 16:18, end))' );
\ No newline at end of file
+% figure(); title('velocità')
+% subplot(3,1,1)
+% plot(ZVK_time,x(:,1), 'r', 'LineWidth',2);
+% legend('velocità x')
+% xlabel('time [s]'); ylabel('m/s');grid on
+% subplot(3,1,2)
+% plot(ZVK_time,x(:,2), 'g','LineWidth',2);
+% legend('velocità y')
+% xlabel('time [s]'); ylabel('m/s');grid on
+% subplot(3,1,3)
+% plot(ZVK_time,x(:,3),'b', 'LineWidth',2);
+% legend('velocità z')
+% xlabel('time [s]'); ylabel('m/s');grid on
+%
+% figure(); title('accelerazione')
+% subplot(3,1,1)
+% plot(ZVK_time,x(:,4), 'r', 'LineWidth',2);
+% legend('accelerazione x')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+% subplot(3,1,2)
+% plot(ZVK_time,x(:,5), 'g','LineWidth',2);
+% legend('accelerazione y')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+% subplot(3,1,3)
+% plot(ZVK_time,x(:,6),'b', 'LineWidth',2);
+% legend('accelerazione z')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+
+% figure(); title('accelerazione vera');
+% subplot(3,1,1)
+% plot(IMU_main(2:length(t_plot)+1,2), 'r', 'LineWidth',2);
+% legend('accelerazione x')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+% subplot(3,1,2)
+% plot(IMU_main(2:length(t_plot)+1,3), 'g','LineWidth',2);
+% legend('accelerazione y')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+% subplot(3,1,3)
+% plot(IMU_main(2:length(t_plot)+1,4),'b', 'LineWidth',2);
+% legend('accelerazione z')
+% xlabel('time [s]'); ylabel('m/s2');grid on
+%
+% figure(); title('angoli')
+% subplot(3,1,1)
+% plot(x(:,10), 'r', 'LineWidth',2);
+% legend('angoli x')
+% xlabel('time [s]'); ylabel('rad');grid on
+% subplot(3,1,2)
+% plot(x(:,11), 'g','LineWidth',2);
+% legend('angoli y')
+% xlabel('time [s]'); ylabel('rad');grid on
+% subplot(3,1,3)
+% plot(x(:,12),'b', 'LineWidth',2);
+% legend('angoli z')
+% xlabel('time [s]'); ylabel('rad');grid on
+
+% figure(); title('omega')
+% subplot(3,1,1)
+% plot(ZVK_time,x(:,13), 'r', 'LineWidth',2);
+% legend('omega x')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+% subplot(3,1,2)
+% plot(ZVK_time,x(:,14), 'g','LineWidth',2);
+% legend('omega y')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+% subplot(3,1,3)
+% plot(ZVK_time,x(:,15),'b', 'LineWidth',2);
+% legend('omega z')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+
+% figure(); title('omega vera')
+% subplot(3,1,1)
+% plot(IMU_main(2:length(t_plot)+1,8), 'r', 'LineWidth',2);
+% legend('omega x')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+% subplot(3,1,2)
+% plot(IMU_main(2:length(t_plot)+1,7), 'g','LineWidth',2);
+% legend('omega y')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+% subplot(3,1,3)
+% plot(IMU_main(2:length(t_plot)+1,8),'b', 'LineWidth',2);
+% legend('omega z')
+% xlabel('time [s]'); ylabel('rad/s');grid on
+
+
+
+
+
+
+% %%
+% Ksave = K(:,:,end);
+% save("Ksave")
diff --git a/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk_IMU.m b/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk_IMU.m
new file mode 100644
index 0000000000000000000000000000000000000000..238db4920588beef25f52ab5f8698c33529f0f5a
--- /dev/null
+++ b/commonFunctions/kalman/ZVK_Staccato/Simulatore_zvk_IMU.m
@@ -0,0 +1,164 @@
+%% Load variables
+clear variables;
+close all;
+clc;
+
+addpath(genpath('logs'))
+
+% IMU = table2array(readtable('log48_Boardcore_LSM6DSRXData.csv')); % [timestamp_acc, acc_x, acc_y, acc_z, timestamp_gyro, w_x, w_y, w_z]
+
+IMU = table2array(readtable('IMU.csv')); % [timestamp, acc_x, acc_y, acc_z, w_x, w_y, w_z]
+
+lastImuIdx = 0;
+
+idxend = 7000;
+
+time_acc = IMU(1:idxend,1)*1e-6;
+acc = IMU(1:idxend,2:4);
+time_gyr = IMU(1:idxend,1)*1e-6;
+gyr = IMU(1:idxend,5:7);
+
+t_end = time_acc(end)*1e6;
+
+figure()
+subplot(2,1,1)
+hold on
+grid on
+plot(time_acc, acc(:,1), 'DisplayName','x');
+plot(time_acc, acc(:,2), 'DisplayName','y');
+plot(time_acc, acc(:,3), 'DisplayName','z');
+legend
+
+subplot(2,1,2)
+hold on
+grid on
+plot(time_gyr, gyr(:,1), 'DisplayName','x');
+plot(time_gyr, gyr(:,2), 'DisplayName','y');
+plot(time_gyr, gyr(:,3), 'DisplayName','z');
+legend
+
+
+
+
+
+
+%% Initial params
+x0 = zeros(1,18);
+
+% Estimated noise standard deviations
+sigma_gyro = 5e-3; % [rad/s] gyroscope variance NAS
+sigma_beta_g = 1e-6; % [rad/s] gyroscope bias variance NAS
+sigma_acc = 5e-2; % [m/s^2] accelerometer variance NAS
+sigma_beta_acc = 1e-5; % [m/s^2] accelerometer bias variance BOH
+sigma_mag = 3; % [mgauss] magnetometer variance NAS = 10????? mentre sito STM di 3mGs RMS noise
+
+
+Q = diag([
+ (1e-6)^2 * ones(3,1); % vel
+ (1e-6)^2 * ones(3,1); % acceleration
+ sigma_beta_acc^2 * ones(3,1); % bias accelerometer
+ (1e-6)^2 * ones(3,1); % theta
+ (1e-6)^2 * ones(3,1); % angular velocity
+ sigma_beta_g^2 * ones(3,1); % bias gyroscope
+ ]);
+
+
+% Initial state covariance matrix P
+P0 = diag([
+ (1e-5)^2 * ones(3,1); % velocity uncertainty [m/s]
+ (1e-5)^2 * ones(3,1); % acceleration [m/s^2]
+ (1e-1)^2 * ones(3,1); % accelerometer bias [m/s^2]
+ (1e-5)^2 * ones(3,1); % ang velocity [rad/s]
+ (1e-5)^2 * ones(3,1); % ang acceleration [rad/s^2]
+ (1e-3)^2 * ones(3,1) % gyro bias [rad/s]
+ ]);
+
+
+% Measurement noise covariance matrix R
+R = diag([
+ (1e-6)^2 * ones(3,1); % FAKE zero velocity [m/s]
+ sigma_acc^2 * ones(3,1); % accelerometer [m/s^2]
+ (1e-6)^2 * ones(3,1); % FAKE angle [rad]
+ sigma_gyro^2 * ones(3,1) % angular velocity [rad/s]
+ ]);
+
+Q0 = angle2quat(133*pi/180, 85*pi/180, 0*pi/180, 'ZYX')';
+error = [1 0.5*deg2rad([0 1 0]) ];
+Q0 = quatmultiply(Q0', error);
+Q0 = Q0';
+quat = [Q0(2:4); Q0(1)];
+
+%% Initializzation
+x = x0;
+P = P0;
+
+
+%% Simulation
+ii = 2;
+for t = 0:2000:t_end
+
+ dt = 2000*1e-6;
+ % Prende la misura dei sensori
+
+ imu_idx = sum(IMU(:,1) <= t); %conta quanti campioni sono stati presi
+ if imu_idx > lastImuIdx % se c'è una nuova misura
+ IMU_measurment = IMU(imu_idx, 2:7); %aggiorna la misura
+ lastImuIdx = imu_idx;
+ else
+ IMU_measurment = IMU(lastImuIdx,2:7); %altrimenti lascia il vecchio indice
+ end
+ acc_meas = IMU_measurment(1:3);
+ omeg_meas = IMU_measurment(4:6);
+
+ %%% Prediction
+ [x(ii,:), P(:,:,ii)] = predictor_zvk( x(ii-1,:), P(:,:,ii-1), dt, Q);
+
+ %%% Correction
+ [x(ii,:), P(:,:,ii)] = corrector_zvk( x(ii,:), P(:,:,ii), quat, acc_meas, omeg_meas, R);
+
+ disp(x(ii,:))
+
+ ii = ii+1;
+end
+x = x(2:end,:);
+
+%% Plotting
+
+t_plot = (0:2000:t_end)*1e-6;
+
+figure()
+ subplot(3,1,1)
+ plot(t_plot,x(:,7), 'r', 'LineWidth',2);
+ legend('bias accelerometro x')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,2)
+ plot(t_plot,x(:,8), 'g','LineWidth',2);
+ legend('bias accelerometro y')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,3)
+ plot(t_plot,x(:,9),'b', 'LineWidth',2);
+ legend('bias acceleromtro z')
+ xlabel('time [s]'); ylabel('bias');grid on
+ grid on
+
+figure()
+ subplot(3,1,1)
+ plot(t_plot,x(:,16), 'r', 'LineWidth',2);
+ legend('bias gyro x')
+ xlabel('time [s]'); ylabel('bias');grid on
+ subplot(3,1,2)
+ xlabel('time [s]'); ylabel('bias');grid on
+ plot(t_plot,x(:,17), 'g','LineWidth',2);
+ legend('bias gyro y')
+ subplot(3,1,3)
+ plot(t_plot,x(:,18), 'b','LineWidth',2);
+ legend('bias gyro z')
+ xlabel('time [s]'); ylabel('bias');grid on
+
+
+
+ estimated_bias_acc = x(end,7:9);
+ estimated_bias_acc_sigma = sqrt( diag(P(7:9, 7:9, end))' );
+
+ estimated_bias_gyro = x(end,16:18);
+ estimated_bias_gyro_sigma = sqrt( diag(P(16:18, 16:18, end))' );
\ No newline at end of file
diff --git a/commonFunctions/kalman/ZVK_Staccato/corrector_zvk.m b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk.m
index e3018cb433dac405e4fa8367840ae6bec293630a..b90397614b803248ccfc89f9f21d47f19fabc28d 100644
--- a/commonFunctions/kalman/ZVK_Staccato/corrector_zvk.m
+++ b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk.m
@@ -1,54 +1,31 @@
-function [x,P] = corrector_zvk(x_pred,P_pred,quat,acc_meas,om_meas,R)
-v_pred = x_pred(1:3)';
-a_pred = x_pred(4:6)';
-bias_a_pred = x_pred(7:9)';
-
-theta_pred = x_pred(10:12)';
-om_pred = x_pred(13:15)';
-bias_g_pred = x_pred(16:18)';
-
-A = [quat(1)^2 - quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(1)*quat(2) + quat(3)*quat(4)), 2*(quat(1)*quat(3) - quat(2)*quat(4));
- 2*(quat(1)*quat(2) - quat(3)*quat(4)), -quat(1)^2 + quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(2)*quat(3) + quat(1)*quat(4)) ;
- 2*(quat(1)*quat(3) + quat(2)*quat(4)), 2*(quat(2)*quat(3) - quat(1)*quat(4)), -quat(1)^2 - quat(2)^2 + quat(3)^2 + quat(4)^2];
-
-
-% Measurment matrix
-H = [eye(3) zeros(3) zeros(3) zeros(3) zeros(3) zeros(3);
- zeros(3) eye(3) eye(3) zeros(3) zeros(3) zeros(3);
- zeros(3) zeros(3) zeros(3) eye(3) zeros(3) zeros(3);
- zeros(3) zeros(3) zeros(3) zeros(3) eye(3) eye(3)];
-
-% S = H * P_pred * H' + R;
-%
-% K = P_pred * H' * inv(S)
-K = [0.6255*eye(3) zeros(3) zeros(3) zeros(3);
- 0.6119*eye(3) zeros(3) zeros(3) zeros(3);
- -0.0083*eye(3) 0.003*eye(3) zeros(3) zeros(3);
- zeros(3) zeros(3) 0.6247*eye(3) 0.0001*eye(3);
- zeros(3) zeros(3) 0.5429*eye(3) 0.09*eye(3);
- zeros(3) zeros(3) -0.284*eye(3) 0.0176*eye(3)];
-
-% fake velocity
-v_fake = zeros(3,1);
-
-% fake attitude ramp
-theta_fake = zeros(3,1);
-
-% accelerometer correciton
-g_meas = acc_meas' - (A * [0, 0, -9.81]');
-g_est = a_pred + bias_a_pred;
-
-% gyro correction
-om_meas = om_meas';
-om_est = om_pred + bias_g_pred;
-
-% compute error and update
-error = [v_fake; g_meas; theta_fake; om_meas] - [v_pred; g_est; theta_pred; om_est];
-update = K * error;
-x = x_pred + update';
-
-% covariance update
-P = (eye(18) - K * H) * P_pred;
-%disp(x_new)
+function [x,P,K] = corrector_zvk(x,P,quat,R)
+
+ v = x(1:3)';
+ theta = x(4:6)';
+
+ A = [quat(1)^2 - quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(1)*quat(2) + quat(3)*quat(4)), 2*(quat(1)*quat(3) - quat(2)*quat(4));
+ 2*(quat(1)*quat(2) - quat(3)*quat(4)), -quat(1)^2 + quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(2)*quat(3) + quat(1)*quat(4)) ;
+ 2*(quat(1)*quat(3) + quat(2)*quat(4)), 2*(quat(2)*quat(3) - quat(1)*quat(4)), -quat(1)^2 - quat(2)^2 + quat(3)^2 + quat(4)^2];
+
+
+ % Measurment matrix
+ H = [eye(3) zeros(3);
+ zeros(3) eye(3)];
+
+ S = H * P * H' + R;
+ K = P * H' * inv(S);
+
+ % fake velocity
+ v_fake = zeros(3,1);
+ % fake attitude ramp
+ theta_fake = zeros(3,1);
+
+ % compute error and update
+ error = [v_fake; theta_fake] - [v; theta];
+ update = K * error;
+ x = x + update';
+
+ % covariance update
+ P = (eye(6) - K * H) * P;
end
diff --git a/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_acc.m b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_acc.m
new file mode 100644
index 0000000000000000000000000000000000000000..39740388f8e14d4b9cc1aa8ef9095cf2a9985260
--- /dev/null
+++ b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_acc.m
@@ -0,0 +1,31 @@
+function [x,P,K] = corrector_zvk_acc(x,P,quat,acc_meas,R)
+
+ a = x(1:3)';
+ bias_a = x(4:6)';
+
+ A = [quat(1)^2 - quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(1)*quat(2) + quat(3)*quat(4)), 2*(quat(1)*quat(3) - quat(2)*quat(4));
+ 2*(quat(1)*quat(2) - quat(3)*quat(4)), -quat(1)^2 + quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(2)*quat(3) + quat(1)*quat(4)) ;
+ 2*(quat(1)*quat(3) + quat(2)*quat(4)), 2*(quat(2)*quat(3) - quat(1)*quat(4)), -quat(1)^2 - quat(2)^2 + quat(3)^2 + quat(4)^2];
+
+
+ % Measurment matrix
+ H = [eye(3) eye(3)];
+
+ S = H * P * H' + R;
+ K = P * H' * inv(S);
+
+
+ % accelerometer correciton
+ g_meas = acc_meas' - (A * [0, 0, -9.81]');
+ g_est = a + bias_a;
+
+ % compute error and update
+ error = g_meas - g_est;
+ update = K * error;
+ x = x + update';
+
+ % covariance update
+ P = (eye(6) - K * H) * P;
+ %disp(x_new)
+end
+
diff --git a/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_gyro.m b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_gyro.m
new file mode 100644
index 0000000000000000000000000000000000000000..6fa50f05fb3a93ee5d9739efbab6ecfad6a52224
--- /dev/null
+++ b/commonFunctions/kalman/ZVK_Staccato/corrector_zvk_gyro.m
@@ -0,0 +1,32 @@
+function [x,P,K] = corrector_zvk_gyro(x,P,quat,om_meas,R)
+
+ om = x(1:3)';
+ bias_g = x(4:6)';
+
+
+ A = [quat(1)^2 - quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(1)*quat(2) + quat(3)*quat(4)), 2*(quat(1)*quat(3) - quat(2)*quat(4));
+ 2*(quat(1)*quat(2) - quat(3)*quat(4)), -quat(1)^2 + quat(2)^2 - quat(3)^2 + quat(4)^2, 2*(quat(2)*quat(3) + quat(1)*quat(4)) ;
+ 2*(quat(1)*quat(3) + quat(2)*quat(4)), 2*(quat(2)*quat(3) - quat(1)*quat(4)), -quat(1)^2 - quat(2)^2 + quat(3)^2 + quat(4)^2];
+
+
+ % Measurment matrix
+ H = [eye(3) eye(3)];
+
+ S = H * P * H' + R;
+ K = P * H' * inv(S);
+
+
+ % gyro correction
+ om_meas = om_meas';
+ om_est = om + bias_g;
+
+ % compute error and update
+ error = om_meas - om_est;
+ update = K * error;
+ x = x + update';
+
+ % covariance update
+ P = (eye(6) - K * H) * P;
+ %disp(x_new)
+end
+
diff --git a/commonFunctions/kalman/ZVK_Staccato/predictor_zvk.m b/commonFunctions/kalman/ZVK_Staccato/predictor_zvk.m
index ddcd476aab31f06dd88f4bd949762b04da742c4f..19d88a6c69ebdca26999d59fb95a1a9dba7e3d9d 100644
--- a/commonFunctions/kalman/ZVK_Staccato/predictor_zvk.m
+++ b/commonFunctions/kalman/ZVK_Staccato/predictor_zvk.m
@@ -1,35 +1,39 @@
-function [x,P] = predictor_zvk(x_,P_,dt,Q)
-v_ = x_(1:3)';
-a_ = x_(4:6)';
-bias_a_ = x_(7:9)';
-
-theta_ = x_(10:12)';
-om_ = x_(13:15)';
-bias_g_ = x_(16:18)';
-
-
-%%% Prediciton accelerometer
-v = v_ + dt * a_;
-a = a_;
-bias_a = bias_a_;
-
-%%% Prediction gyro
-theta = theta_ + dt * om_;
-om = om_;
-bias_g = bias_g_;
-
-
-% Assemble predicted global state
-x = [v; a; bias_a; theta; om; bias_g]';
-
-
-%%% Covariance prediction
-A = eye(18);
-A(1:3, 4:6) = dt*eye(3);% v / acc
-A(10:12, 13:15) = dt*eye(3);% theta / om
-
-
-P = A * P_ * A' + Q;
+function [x,P] = predictor_zvk(x_old,P_old,dt,Q)
+
+ v_old = x_old(1:3)';
+ a_old = x_old(4:6)';
+ bias_a_main_old = x_old(7:9)';
+ bias_a_payload_old = x_old(10:12)';
+
+ theta_old = x_old(13:15)';
+ om_old = x_old(16:18)';
+ bias_g_main_old = x_old(19:21)';
+ bias_g_payload_old = x_old(22:24)';
+
+
+ %%% Prediciton accelerometer
+ v_pred = v_old + dt * a_old;
+ a_pred = a_old;
+ bias_a_main_pred = bias_a_main_old;
+ bias_a_payload_pred = bias_a_payload_old;
+
+ %%% Prediction gyro
+ theta_pred = theta_old + dt * om_old;
+ om_pred = om_old;
+ bias_g_main_pred = bias_g_main_old;
+ bias_g_payload_pred = bias_g_payload_old;
+
+
+ % Assemble predicted global state
+ x = [v_pred; a_pred; bias_a_main_pred; bias_a_payload_pred; theta_pred; om_pred; bias_g_main_pred; bias_g_payload_pred]';
+
+ %%% Covariance prediction
+ A = eye(24);
+ A(1:3, 4:6) = dt*eye(3); % v / acc
+ A(13:15, 16:18) = dt*eye(3); % theta / om
+
+
+ P = A * P_old * A' + Q;
end