vibration-table/matlab/meas_transformation.m

120 lines
4.4 KiB
Mathematica
Raw Normal View History

2021-04-19 12:15:41 +02:00
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Define accelerometers positions/orientations
% <<sec:accelerometer_pos>>
% Let's first define the position and orientation of all measured accelerations with respect to a defined frame $\{O\}$.
Opm = [-0.1875, -0.1875, -0.245;
-0.1875, -0.1875, -0.245;
0.1875, -0.1875, -0.245;
0.1875, -0.1875, -0.245;
0.1875, 0.1875, -0.245;
0.1875, 0.1875, -0.245]';
% #+name: tab:accelerometers_table_positions
% #+caption: Positions of the accelerometers fixed to the vibration table with respect to $\{O\}$
% #+attr_latex: :environment tabularx :width 0.55\linewidth :align Xcccccc
% #+attr_latex: :center t :booktabs t :float t
% #+RESULTS:
% | | $a_1$ | $a_2$ | $a_3$ | $a_4$ | $a_5$ | $a_6$ |
% |---+--------+--------+--------+--------+--------+--------|
% | x | -0.188 | -0.188 | 0.188 | 0.188 | 0.188 | 0.188 |
% | y | -0.188 | -0.188 | -0.188 | -0.188 | 0.188 | 0.188 |
% | z | -0.245 | -0.245 | -0.245 | -0.245 | -0.245 | -0.245 |
% We then define the direction of the measured accelerations (unit vectors):
Osm = [0, 1, 0;
0, 0, 1;
1, 0, 0;
0, 0, 1;
1, 0, 0;
0, 0, 1;]';
% Transformation matrix from motion of the solid body to accelerometer measurements
% <<sec:transformation_motion_to_acc>>
% Let's try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in $\{O\}$.
% For any point $p_i$ of the solid body (corresponding to an accelerometer), we can write:
% \begin{equation}
% \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + p_i \times \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% We can write the cross product as a matrix product using the skew-symmetric transformation:
% \begin{equation}
% \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + \underbrace{\begin{bmatrix}
% 0 & p_{i,z} & -p_{i,y} \\
% -p_{i,z} & 0 & p_{i,x} \\
% p_{i,y} & -p_{i,x} & 0
% \end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% If we now want to know the (scalar) acceleration $a_i$ of the point $p_i$ in the direction of the accelerometer direction $\hat{s}_i$, we can just project the 3d acceleration on $\hat{s}_i$:
% \begin{equation}
% a_i = \hat{s}_i^T \cdot \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% Which is equivalent as a simple vector multiplication:
% \begin{equation}
% a_i = \begin{bmatrix}
% \hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
% \end{bmatrix}
% \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix} = \begin{bmatrix}
% \hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
% \end{bmatrix} {}^O\vec{x}
% \end{equation}
% And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form.
% We obtain Eq. eqref:eq:M_matrix.
% #+begin_important
% The transformation from solid body acceleration ${}^O\vec{x}$ from sensor measured acceleration $\vec{a}$ is:
% \begin{equation} \label{eq:M_matrix}
% \vec{a} = \underbrace{\begin{bmatrix}
% \hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\
% \vdots & \vdots \\
% \hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]}
% \end{bmatrix}}_{M} {}^O\vec{x}
% \end{equation}
% with $\hat{s}_i$ the unit vector representing the measured direction of the i'th accelerometer expressed in frame $\{O\}$ and $P_{i,[\times]}$ the skew-symmetric matrix representing the cross product of the position of the i'th accelerometer expressed in frame $\{O\}$.
% #+end_important
% Let's define such matrix using matlab:
M = zeros(length(Opm), 6);
for i = 1:length(Opm)
Ri = [0, Opm(3,i), -Opm(2,i);
-Opm(3,i), 0, Opm(1,i);
Opm(2,i), -Opm(1,i), 0];
M(i, 1:3) = Osm(:,i)';
M(i, 4:6) = Osm(:,i)'*Ri;
end