120 lines
4.4 KiB
Matlab
120 lines
4.4 KiB
Matlab
%% 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
|