From 0bdac0f21ade37f9f57dedccdf38bf4d39b48d60 Mon Sep 17 00:00:00 2001
From: Filippo Cataldo <filippo.cataldo@skywarder.eu>
Date: Wed, 17 Jul 2024 16:46:47 +0200
Subject: [PATCH] [ASD] Fix IMU position
---
.../postprocessing/ASD/postProcessDrogue.m | 20 ++++++++++------
.../postprocessing/ASD/postProcessMain.m | 23 +++++++++++--------
.../postprocessing/ASD/src/internalLoads.m | 13 +++++++----
3 files changed, 35 insertions(+), 21 deletions(-)
diff --git a/RoccarasoFlight/postprocessing/ASD/postProcessDrogue.m b/RoccarasoFlight/postprocessing/ASD/postProcessDrogue.m
index b9792f3..18b4c42 100644
--- a/RoccarasoFlight/postprocessing/ASD/postProcessDrogue.m
+++ b/RoccarasoFlight/postprocessing/ASD/postProcessDrogue.m
@@ -29,9 +29,14 @@ load("data\geminiFlightRoccaraso.mat");
% Pressure distribution type, only 'uniform' supported for now
aeroDistribution = 'uniform';
-indexOpen = 975; % Drogue opening esimtated index in quaternions time
+indexOpen = 968; % Drogue opening esimtated index in quaternions time
gNED = [0; 0; 9.81]; % Gravity acceleration in NED [m/s^2]
-xImu = 0.1854; % [m] x position of IMU from top of ELC bay
+imu2bay = 0.1854; % IMU longitudinal distance from top of its bay (ELC) [m]
+
+% IMU y and z position from longitudinal axis, sign according to IMU
+% frame
+yImu = 0; % [m]
+zImu = -0.075; % diameter/2 [m]
% Options for non-linear system
guess = [200*ones(2,1); ones(2,1)]; % Initial guess
@@ -59,9 +64,9 @@ absolutePositions = values(rocket.absolutePositions);
absolutePositions = absolutePositions(indexFirstBay:end); % [m]
diameter = rocket.diameter; % [m]
-xCg = rocket.xCg(end); % [m]
+xCg = rocket.xCg(end); % [m]
armImu = xCg - (rocket.absolutePositions('electronics') +...
- xImu); % Accelerometer position from xCg [m]
+ imu2bay); % Accelerometer position from xCg [m]
%% Get flight data
% Time [s]
@@ -117,7 +122,8 @@ omegaNED = squeeze(omegaNED); % Transform to NED to differentiate
omegaDotNED = [zeros(3,1) diff(omegaNED, [], 2)/dt];
omegaDotTemp = zeros(3,1,nTime);
-omegaDotTemp(:,:,:) = [omegaDotNED(1,:); omegaDotNED(2,:); omegaDotNED(3,:)];
+omegaDotTemp(:,:,:) = [omegaDotNED(1,:); omegaDotNED(2,:);...
+ omegaDotNED(3,:)];
omegaDotBody = pagemtimes(Rnb, omegaDotTemp);
omegaDotBody = squeeze(omegaDotBody);
@@ -146,8 +152,8 @@ for i = 1:nTime
% CHECK FOR SIGN OF RADIAL DISTANCE; SHOULD BE POSITIVE IN CENTRIFUGAL
accBody(:,i) = accImu(:,i) - cross(omegaDotBody(:,i),...
- [armImu; 0; diameter/2]) - cross(omegaBody(:,i),...
- cross(omegaBody(:,i), [armImu; 0; diameter/2]));
+ [armImu; yImu; zImu]) - cross(omegaBody(:,i),...
+ cross(omegaBody(:,i), [armImu; yImu; zImu]));
dStateBody(:,i) = [velBody(:,i)
accBody(:,i)
diff --git a/RoccarasoFlight/postprocessing/ASD/postProcessMain.m b/RoccarasoFlight/postprocessing/ASD/postProcessMain.m
index 7b1c56d..37f9b97 100644
--- a/RoccarasoFlight/postprocessing/ASD/postProcessMain.m
+++ b/RoccarasoFlight/postprocessing/ASD/postProcessMain.m
@@ -4,8 +4,8 @@
% loads.
% NAS time is used - with lower sampling frequency.
% No specification of reference frame means 2D inertial
-% frame. Body frame can be 2D or 3D depending on the
-% context. Anyway, frames are only used to rotate
+% frame. Body frame can be 2D or 3D (IMU frame) depending
+% on the context. Anyway, frames are only used to rotate
% vectors: velocities or acceleration are always with
% respect to an inertial frame.
%
@@ -31,7 +31,12 @@ aeroDistribution = 'uniform';
indexOpen = 3585; % Main opening esimtated index in quaternions time
gNED = [0; 0; 9.81]; % Gravity acceleration in NED [m/s^2]
-xImu = 0.1854; % [m] x position of IMU from top of ELC bay
+imu2bay = 0.1854; % IMU longitudinal distance from top of its bay (ELC) [m]
+
+% IMU y and z position from longitudinal axis, sign according to IMU
+% frame
+yImu = 0; % [m]
+zImu = -0.075; % diameter/2 [m]
% Options for non-linear system
guess = [200*ones(2,1); ones(2,1)]; % Initial guess
@@ -59,9 +64,9 @@ absolutePositions = values(rocket.absolutePositions);
absolutePositions = absolutePositions(indexFirstBay:end); % [m]
diameter = rocket.diameter; % [m]
-xCg = rocket.xCg(end); % [m]
+xCg = rocket.xCg(end); % [m]
armImu = xCg - (rocket.absolutePositions('electronics') +...
- xImu); % Accelerometer position from xCg [m]
+ imu2bay); % Accelerometer x position from xCg [m]
%% Get flight data
% Time [s]
@@ -117,7 +122,8 @@ omegaNED = squeeze(omegaNED); % Transform to NED to differentiate
omegaDotNED = [zeros(3,1) diff(omegaNED, [], 2)/dt];
omegaDotTemp = zeros(3,1,nTime);
-omegaDotTemp(:,:,:) = [omegaDotNED(1,:); omegaDotNED(2,:); omegaDotNED(3,:)];
+omegaDotTemp(:,:,:) = [omegaDotNED(1,:); omegaDotNED(2,:);...
+ omegaDotNED(3,:)];
omegaDotBody = pagemtimes(Rnb, omegaDotTemp);
omegaDotBody = squeeze(omegaDotBody);
@@ -144,10 +150,9 @@ measuredLoads = zeros(3, nBays + 1, nTime);
for i = 1:nTime
- % CHECK FOR SIGN OF RADIAL DISTANCE; SHOULD BE POSITIVE IN CENTRIFUGAL
accBody(:,i) = accImu(:,i) - cross(omegaDotBody(:,i),...
- [armImu; 0; diameter/2]) - cross(omegaBody(:,i),...
- cross(omegaBody(:,i), [armImu; 0; diameter/2]));
+ [armImu; yImu; zImu]) - cross(omegaBody(:,i),...
+ cross(omegaBody(:,i), [armImu; yImu; zImu]));
dStateBody(:,i) = [velBody(:,i)
accBody(:,i)
diff --git a/RoccarasoFlight/postprocessing/ASD/src/internalLoads.m b/RoccarasoFlight/postprocessing/ASD/src/internalLoads.m
index c4ee8be..ade1fa8 100644
--- a/RoccarasoFlight/postprocessing/ASD/src/internalLoads.m
+++ b/RoccarasoFlight/postprocessing/ASD/src/internalLoads.m
@@ -60,14 +60,17 @@ absolutePositions =... % [m]
xCg = rocket.xCg(end); % [m]
%%% Initial state
-alt0 = state0(1); % Initial altitude [m]
-vel0 = state0(2:3); % Initial CG velocity [m/s]
-AoA0 = state0(4); % Initial attack angle [rad]
+alt0 = state0(1); % Initial altitude [m]
+vel0 = state0(2:3); % Initial CG velocity [m/s]
+AoA0 = state0(4); % Initial attack angle [rad]
theta0 = atan2(vel0(2), vel0(1)) + AoA0; % Initial pitch angle [rad]
-omega0 = g(2)*vel0(1)/norm(vel0)^2; % Initial angular velocity [rad/s]
-state0 = [alt0; vel0; theta0; omega0]; % Rearrange initial state
+% Approximated initial angular velocity (really correct only in
+% true apogee)
+omega0 = g(2)*vel0(1)/norm(vel0)^2; % [rad/s]
+
+state0 = [alt0; vel0; theta0; omega0]; % Rearrange initial state
%%% Solve differential equation
[time, state] = ode113(@(time, state)...
--
GitLab