%% 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