From 818f3cc48fb1d59e13a6f801bf72ef40b2c71cf3 Mon Sep 17 00:00:00 2001 From: Martin Dieblich Date: Mon, 20 Sep 2010 17:40:51 +0000 Subject: [PATCH] added documentation for the algebra stored in /sw/airborne/math --- doc/pprz_algebra/Makefile | 8 + doc/pprz_algebra/appendix.tex | 89 +++++++ doc/pprz_algebra/euler.tex | 136 ++++++++++ doc/pprz_algebra/headfile.bib | 25 ++ doc/pprz_algebra/headfile.tex | 98 +++++++ .../Crystal_Clear_mimetype_source_c.png | Bin 0 -> 5042 bytes .../Crystal_Clear_mimetype_source_h.png | Bin 0 -> 3893 bytes doc/pprz_algebra/images/atan2_alternate.png | Bin 0 -> 6735 bytes doc/pprz_algebra/images/ratiofunction.png | Bin 0 -> 6220 bytes doc/pprz_algebra/images/xyvalues.png | Bin 0 -> 6713 bytes doc/pprz_algebra/introduction.tex | 119 +++++++++ doc/pprz_algebra/matrix.tex | 126 +++++++++ doc/pprz_algebra/quaternion.tex | 172 ++++++++++++ doc/pprz_algebra/rates.tex | 144 ++++++++++ doc/pprz_algebra/scalar.tex | 73 +++++ .../transformations/axisangle2matrix.tex | 25 ++ .../transformations/euler2matrix.tex | 24 ++ .../transformations/euler2quaternion.tex | 14 + .../transformations/eulerdot2rates.tex | 31 +++ .../transformations/matrix2euler.tex | 19 ++ .../transformations/matrix2quaternion.tex | 46 ++++ .../transformations/quaternion2euler.tex | 26 ++ .../transformations/quaternion2matrix.tex | 13 + .../transformations/rates2eulerdot.tex | 30 +++ .../transformations/rates2quaternion.tex | 24 ++ doc/pprz_algebra/vector.tex | 251 ++++++++++++++++++ 26 files changed, 1493 insertions(+) create mode 100644 doc/pprz_algebra/Makefile create mode 100644 doc/pprz_algebra/appendix.tex create mode 100644 doc/pprz_algebra/euler.tex create mode 100644 doc/pprz_algebra/headfile.bib create mode 100644 doc/pprz_algebra/headfile.tex create mode 100644 doc/pprz_algebra/images/Crystal_Clear_mimetype_source_c.png create mode 100644 doc/pprz_algebra/images/Crystal_Clear_mimetype_source_h.png create mode 100644 doc/pprz_algebra/images/atan2_alternate.png create mode 100644 doc/pprz_algebra/images/ratiofunction.png create mode 100644 doc/pprz_algebra/images/xyvalues.png create mode 100644 doc/pprz_algebra/introduction.tex create mode 100644 doc/pprz_algebra/matrix.tex create mode 100644 doc/pprz_algebra/quaternion.tex create mode 100644 doc/pprz_algebra/rates.tex create mode 100644 doc/pprz_algebra/scalar.tex create mode 100644 doc/pprz_algebra/transformations/axisangle2matrix.tex create mode 100644 doc/pprz_algebra/transformations/euler2matrix.tex create mode 100644 doc/pprz_algebra/transformations/euler2quaternion.tex create mode 100644 doc/pprz_algebra/transformations/eulerdot2rates.tex create mode 100644 doc/pprz_algebra/transformations/matrix2euler.tex create mode 100644 doc/pprz_algebra/transformations/matrix2quaternion.tex create mode 100644 doc/pprz_algebra/transformations/quaternion2euler.tex create mode 100644 doc/pprz_algebra/transformations/quaternion2matrix.tex create mode 100644 doc/pprz_algebra/transformations/rates2eulerdot.tex create mode 100644 doc/pprz_algebra/transformations/rates2quaternion.tex create mode 100644 doc/pprz_algebra/vector.tex diff --git a/doc/pprz_algebra/Makefile b/doc/pprz_algebra/Makefile new file mode 100644 index 0000000000..2d24db9e47 --- /dev/null +++ b/doc/pprz_algebra/Makefile @@ -0,0 +1,8 @@ +doc_libeknav.pdf: headfile.tex + pdflatex $< +bib: + bibtex headfile + +clean: + rm -f *~ *.aux *.bbl *.blg *.log *.out *.toc *.pdf *.dvi *.ps + find . -name '*~' -exec rm -f {} \; diff --git a/doc/pprz_algebra/appendix.tex b/doc/pprz_algebra/appendix.tex new file mode 100644 index 0000000000..d8b1ab8587 --- /dev/null +++ b/doc/pprz_algebra/appendix.tex @@ -0,0 +1,89 @@ +\section{Optimization} +Functions can be re-written to make them run faster, more accurate or making them small (less memory). The result is not often easy-to-read, so the optimazation steps are written down here.\\ +The functions are in alphabetical order. + +\subsection*{INT32\_QUAT\_VMULT} +\subsubsection*{Step 1} +Starting from the original matrix +\begin{equation} +\mat R_m \multiplication \vect v= \begin{pmatrix} +1-2(q_y^2 + q_z^2) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ +2(q_xq_y + q_iq_z) & 1-2(q_x^2 + q_z^2) & 2(q_yq_z - q_iq_x) \\ +2(q_xq_z - q_iq_y) & 2(q_yq_z+q_iq_x) & 1-2(q_x^2 + q_y^2) +\end{pmatrix} \multiplication \vect v, +\end{equation} +the first step is to rewrite the diagonal elements. Since +\begin{equation} +1 = q_i^2+q_x^2+q_y^2+q_z^2 +\end{equation} +it is possible to rewrite the first element to +\begin{eqnarray} +1-2(q_y^2 + q_z^2) \\ += 1-2(q_y^2 + q_z^2) +1-1 \\ += 2-2(q_y^2 + q_z^2)-1 \\ += 2(1 - q_y^2 + q_z^2)-1 \\ += 2(q_i^2+q_x^2+q_y^2+q_z^2 - q_y^2 + q_z^2)-1 \\ += 2(q_i^2+q_x^2)-1 \\ += (2q_i^2-1)+2q_x^2 +\end{eqnarray} +The same can be done for the other two elements +\begin{equation} +1-2(q_x^2 + q_z^2) = (2q_i^2-1)+2q_y^2 +\end{equation} +\begin{equation} +1-2(q_x^2 + q_y^2) = (2q_i^2-1)+2q_z^2 +\end{equation} +Note that the diagonal elements differ only for the last summand. Additionaly you have nearly everywhere in the matrix a multiplication with two. A multiplication with two is the same like shifting one bit to the left and since this is in fixed point arithmetic, you have to shift anyway. +\subsubsection*{Step 2} +Since you're only interested in the output vector, it is not necessary to compute every single step. Mostly it's a good choice to have a single, big equation and letting the compiler decide how to deal with it (storing it into a register or onto the RAM). But be aware of Overflows with Fixed-Point Values! + +\subsection*{INT32\_RMAT\_OF\_QUAT} +\subsubsection*{Step 1} +Starting from the original matrix +\begin{equation} +\mat R_m = \begin{pmatrix} +1-2(q_y^2 + q_z^2) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ +2(q_xq_y + q_iq_z) & 1-2(q_x^2 + q_z^2) & 2(q_yq_z - q_iq_x) \\ +2(q_xq_z - q_iq_y) & 2(q_yq_z+q_iq_x) & 1-2(q_x^2 + q_y^2) +\end{pmatrix}, +\end{equation} +the first step is to rewrite the diagonal elements. Since +\begin{equation} +1 = q_i^2+q_x^2+q_y^2+q_z^2 +\end{equation} +it is possible to rewrite the first element to +\begin{eqnarray} +1-2(q_y^2 + q_z^2) \\ += 1-2(q_y^2 + q_z^2) +1-1 \\ += 2-2(q_y^2 + q_z^2)-1 \\ += 2(1 - q_y^2 + q_z^2)-1 \\ += 2(q_i^2+q_x^2+q_y^2+q_z^2 - q_y^2 + q_z^2)-1 \\ += 2(q_i^2+q_x^2)-1 \\ += (2q_i^2-1)+2q_x^2 +\end{eqnarray} +The same can be done for the other two elements +\begin{equation} +1-2(q_x^2 + q_z^2) = (2q_i^2-1)+2q_y^2 +\end{equation} +\begin{equation} +1-2(q_x^2 + q_y^2) = (2q_i^2-1)+2q_z^2 +\end{equation} +Note that the diagonal elements differ only for the last summand. Additionaly you have nearly everywhere in the matrix a multiplication with two. A multiplication with two is the same like shifting one bit to the left and since this is in fixed point arithmetic, you have to shift anyway. +\subsubsection*{Step2} +A further optimization step is to use the final matrix to store values in it: +\begin{equation} +\mat R_m = \begin{pmatrix} +2q_x^2 & 2q_xq_y & 2q_xq_z\\ +0 & 2q_y^2 & 2q_yq_z\\ +0 & 0 & 2q_z^2 +\end{pmatrix} +\end{equation} +And finally: +\begin{equation} +\mat R_m = \begin{pmatrix} +\mat R_m(1,1) + (2q_i^2-1) & \mat R_m(1,2) - 2q_iq_z & \mat R_m(1,3) + 2q_iq_y \\ +\mat R_m(1,2) + 2q_iq_z & \mat R_m(2,2) + (2q_i^2-1) & \mat R_m(2,3) - 2q_iq_x \\ +\mat R_m(1,3) - 2q_iq_y & \mat R_m(2,3) + 2q_iq_x & \mat R_m(3,3) + (2q_i^2-1) +\end{pmatrix} +\end{equation} +The last step can save much memory and a very small amount of time. \ No newline at end of file diff --git a/doc/pprz_algebra/euler.tex b/doc/pprz_algebra/euler.tex new file mode 100644 index 0000000000..efbb162356 --- /dev/null +++ b/doc/pprz_algebra/euler.tex @@ -0,0 +1,136 @@ +\section{Euler Angles} +\subsection{Definition} +The values are called +\begin{equation} +\eu{e} = \begin{pmatrix} \Roll \\\Pitch\\\Yaw\\\end{pmatrix} = \begin{pmatrix} phi \\Pitch\\Yaw\\\end{pmatrix} +\end{equation} +It is available for the following simple types:\\ +\begin{tabular}{c|c} +type & struct \\ \hline +int16\_t & Int16Eulers \\ +int32\_t & Int32Eulers \\ +float & FloatEulers +\end{tabular} +\textbf{IMPORTANT:}\label{paparazzi euler definition}\\ +Because there are many definitions of euler angles (some say 12, wikipedia says 24, the author tends to believe there are 48) and the choice of perspective, paparazzi choosed the following convention: + + + + +\subsection{= Assigning} +\subsubsection*{$\eu{e} = \eu{0}$} +\begin{equation} +\eu v = \begin{pmatrix} 0 \\ 0 \\ 0 \end{pmatrix} +\end{equation} +\inHfile{INT\_EULERS\_ZERO(e)}{pprz\_algebra\_int} + +\subsubsection*{$\eu a = \transp{(\Roll,\Pitch,\Yaw)}$} +\begin{equation} +\eu a = \transp{(\Roll,\Pitch,\Yaw)} +\end{equation} +\inHfile{EULERS\_ASSIGN(e, phi, theta, psi)}{pprz\_algebra} + +\subsubsection*{$\eu a = \eu b$} +\begin{equation} +\eu a = \eu b +\end{equation} +\inHfile{EULERS\_COPY(a, b)}{pprz\_algebra} + + + +\subsection{+ Addition} +\subsubsection*{$\eu a += \eu b$} +\begin{equation} +\eu a = \eu a + \eu b +\end{equation} +\inHfile{EULERS\_ADD(a, b)}{pprz\_algebra} +\mynote{No EULERS\_SUM function?} + + + +\subsection{- Subtraction} +\subsubsection*{$\eu a -= \eu b$} +\begin{equation} +\eu a = \eu a - \eu b +\end{equation} +\inHfile{EULERS\_SUB(a, b)}{pprz\_algebra} + +\subsubsection*{$\eu c = \eu a - \eu b$} +\begin{equation} +\eu c = \eu a - \eu b +\end{equation} +\inHfile{EULERS\_DIFF(c, a, b)}{pprz\_algebra} + + + + +\subsection{$\multiplication$ Multiplication} +\subsubsection*{$\eu{e_o} = s \multiplication \eu{e_i}$ With a scalar} +\begin{equation} +\eu e_o = s \multiplication \eu{e_i} +\end{equation} +\inHfile{EULERS\_SMUL(eo, ei, s)}{pprz\_algebra} + + + + +\subsection{$\division$ Division} +\subsubsection*{$\eu{e_o} = \frac 1 s \multiplication \eu{e_i}$ With a scalar} +\begin{equation} +\eu{e_o} = \frac 1 s \multiplication \eu{e_i} +\end{equation} +\inHfile{EULERS\_SDIV(eo, ei, s)}{pprz\_algebra} + + + +\subsection{Transformation from euler angles} +\subsubsection*{to a rotational matrix} +\input{transformations/euler2matrix} + +\subsubsection*{to a quaternion} +\input{transformations/euler2quaternion} + +\subsubsection*{to rates} +\input{transformations/eulerdot2rates} + + + + +\subsection{Transformation to euler angles} +\subsubsection*{form a rotational matrix} +\input{transformations/matrix2euler} + +\subsubsection*{from a quaternion} +\input{transformations/quaternion2euler} + +\subsubsection*{euler angles derivative from rates} +\input{transformations/rates2eulerdot} + + + +\subsection{Other} +\subsubsection*{$-\pi \leq \alpha \leq \pi$ Normalizing} +You have either the option to normalize a single angle to a value between +\begin{equation} +-\pi \leq \alpha \leq \pi +\end{equation} +\inHfile{INT32\_ANGLE\_NORMALIZE(a)}{pprz\_algebra\_int} +\inHfile{FLOAT\_ANGLE\_NORMALIZE(a)}{pprz\_algebra\_float} +or between +\begin{equation} +0 \leq \alpha \leq 2\pi +\end{equation} +\inHfile{INT32\_COURSE\_NORMALIZE(a)}{pprz\_algebra\_int} + + +\subsubsection*{$min \leq \eu v \leq max$ Bounding} +Bounds the euler angles so that every angle $\Roll$, $\Pitch$ and $\Yaw$ is between \textit{min} and \textit{max}. +\begin{equation} +\eu v \in \mathbb{I}^3, \qquad \mathbb{I} = [min; max] +\end{equation} +\textbf{WARNING:}\\ +The function ``\texttt{EULERS\_BOUND\_CUBE}'' works different than the function \texttt{VECT3\_BOUND\_CUBE} in the case of $min > max$. Here, the lower border \textit{min} has a higher priority than the upper border \textit{max}. So, if $ min > max$ and a value of $ \vect e $ is between those, the value is set to min. \\ +\inHfile{EULERS\_BOUND\_CUBE(v, min, max)}{pprz\_algebra} +\mynote{Better naming suggestion: choose e instead of v} +\mynote{The difference between EULERS\_BOUND\_CUBE and VECT3\_BOUND\_CUBE is not very good} +\mynote{No BOUND\_BOX ?} \ No newline at end of file diff --git a/doc/pprz_algebra/headfile.bib b/doc/pprz_algebra/headfile.bib new file mode 100644 index 0000000000..89d734d3c5 --- /dev/null +++ b/doc/pprz_algebra/headfile.bib @@ -0,0 +1,25 @@ +@book{kuipers:2002, + abstract = {

Ever since the Irish mathematician William Rowan Hamilton introduced quaternions in the nineteenth century--a feat he celebrated by carving the founding equations into a stone bridge--mathematicians and engineers have been fascinated by these mathematical objects. Today, they are used in applications as various as describing the geometry of spacetime, guiding the Space Shuttle, and developing computer applications in virtual reality. In this book, J. B. Kuipers introduces quaternions for scientists and engineers who have not encountered them before and shows how they can be used in a variety of practical situations.

The book is primarily an exposition of the quaternion, a 4-tuple, and its primary application in a rotation operator. But Kuipers also presents the more conventional and familiar 3 x 3 (9-element) matrix rotation operator. These parallel presentations allow the reader to judge which approaches are preferable for specific applications. The volume is divided into three main parts. The opening chapters present introductory material and establish the book's terminology and notation. The next part presents the mathematical properties of quaternions, including quaternion algebra and geometry. It includes more advanced special topics in spherical trigonometry, along with an introduction to quaternion calculus and perturbation theory, required in many situations involving dynamics and kinematics. In the final section, Kuipers discusses state-of-the-art applications. He presents a six degree-of-freedom electromagnetic position and orientation transducer and concludes by discussing the computer graphics necessary for the development of applications in virtual reality.

}, + author = {Kuipers, J. B.}, + citeulike-article-id = {2037738}, + citeulike-linkout-0 = {http://www.amazon.ca/exec/obidos/redirect?tag=citeulike09-20\&path=ASIN/0691102988}, + citeulike-linkout-1 = {http://www.amazon.de/exec/obidos/redirect?tag=citeulike01-21\&path=ASIN/0691102988}, + citeulike-linkout-2 = {http://www.amazon.fr/exec/obidos/redirect?tag=citeulike06-21\&path=ASIN/0691102988}, + citeulike-linkout-3 = {http://www.amazon.co.uk/exec/obidos/ASIN/0691102988/citeulike00-21}, + citeulike-linkout-4 = {http://www.amazon.com/exec/obidos/redirect?tag=citeulike07-20\&path=ASIN/0691102988}, + citeulike-linkout-5 = {http://www.worldcat.org/isbn/0691102988}, + citeulike-linkout-6 = {http://books.google.com/books?vid=ISBN0691102988}, + citeulike-linkout-7 = {http://www.amazon.com/gp/search?keywords=0691102988\&index=books\&linkCode=qs}, + citeulike-linkout-8 = {http://www.librarything.com/isbn/0691102988}, + day = {19}, + howpublished = {Paperback}, + isbn = {0691102988}, + keywords = {book, quaternion}, + month = {August}, + posted-at = {2007-12-01 14:48:42}, + priority = {0}, + publisher = {Princeton University Press}, + title = {Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace and Virtual Reality}, + url = {http://www.worldcat.org/isbn/0691102988}, + year = {2002} +} diff --git a/doc/pprz_algebra/headfile.tex b/doc/pprz_algebra/headfile.tex new file mode 100644 index 0000000000..09cbd77280 --- /dev/null +++ b/doc/pprz_algebra/headfile.tex @@ -0,0 +1,98 @@ +\documentclass[10pt,a4paper]{paper} + +\usepackage[latin1]{inputenc} +\usepackage[english]{babel} + +\usepackage[official,right]{eurosym} +\usepackage{hyperref} +\usepackage{graphicx} +\usepackage{a4wide} +\usepackage{calc} +%\usepackage{picins} +\usepackage{fancyhdr} +\usepackage{amssymb,amsmath} +\usepackage{gensymb} +%\usepackage{multicol} +\usepackage{url} + +% For drawings +\usepackage{pgf} +\usepackage{tikz} +\usetikzlibrary{arrows,automata} +\usepackage{color} + +% For quick and easy figures +\newcommand{\pic}[3]{ +\begin{figure}[h]\begin{center}\includegraphics[width=#2]{#1.png} + \caption{#3} + \label{#1} +\end{center}\end{figure} +} + +\newcommand{\inHfile}[2]{ + Function \texttt{#1} in File \texttt{#2.h} \\ +} + +%\numberwithin{equation}{section} +\newcommand{\vect}[1]{\ensuremath{\overrightarrow{#1}}} %% How to mark Vectors +\newcommand{\eu}[1]{\ensuremath{#1^{\phi}}} %% how to mark euler angles +\newcommand{\ra}[1]{\ensuremath{\omega_{#1}}} %% how to mark rates +\newcommand{\ew}[1]{\;. \! \!#1} %% how to mark element-wise operations +\newcommand{\mat}[1]{\ensuremath{\mathbf{#1}}} %% how to mark Matrices +\newcommand{\eye}[0]{\mat{I}} %% Identity matrix +\newcommand{\quat}[1]{\ensuremath{q_{#1}}} %% how to mark Quaternions +\newcommand{\transp}[1]{\ensuremath{#1^{T}}} +\newcommand{\est}[1]{\ensuremath{\hat{#1}}} +\newcommand{\err}[1]{\ensuremath{\tilde{#1}}} +\newcommand{\meas}[1]{\ensuremath{\tilde{#1}}} +\newcommand{\linpt}[1]{\ensuremath{\overline{#1}}} +\newcommand{\norm}[1]{\ensuremath{|{#1}|}} +\newcommand{\quatprod}[0]{\ensuremath{\bullet}} +\newcommand{\ddt}[2]{\ensuremath{#1^{(#2)}}} +\newcommand{\deriv}[2]{\ensuremath{{#1}^{(#2)}}} +\newcommand{\inv}[1]{\ensuremath{#1}^{-1}} +\newcommand{\comp}[1]{\ensuremath{#1}^*} +\newcommand{\atan}[1]{\ensuremath{\text{atan}\left({#1}\right)}} +\newcommand{\sign}[1]{\ensuremath{\text{sign}\left({#1}\right)}} +\newcommand{\cross}{\ensuremath{\times}} + +\newcommand{\division}[0]{\ensuremath{\div}} +\newcommand{\multiplication}[0]{\ensuremath{\cdot}} + + +%% Formatting in the right color for euler angles +\definecolor{rollcolor}{rgb}{0,0,1} +\definecolor{pitchcolor}{rgb}{0,0.5,0} +\definecolor{yawcolor}{rgb}{1,0,0} +\newcommand{\Rollc}[1]{\color{rollcolor}#1\color{black}{}} +\newcommand{\Pitchc}[1]{\color{pitchcolor}#1\color{black}{}} +\newcommand{\Yawc}[1]{\color{yawcolor}#1\color{black}{}} +\newcommand{\Roll}[0]{\ensuremath{\Rollc \phi}} +\newcommand{\Pitch}[0]{\ensuremath{\Pitchc \theta }} +\newcommand{\Yaw}[0]{\ensuremath{\Yawc \psi}} + + +\newcommand{\mynote}[1]{\begin{flushright}\fbox{Martin: ``\textit{#1}''}\end{flushright}} +%\newcommand{\mynote}[1]{} + +\graphicspath{{./images/},{tmp/}} + +\title{Documentation for pprz_algebra} +\author{Martin Dieblich} + +\begin{document} + +%\maketitle +\include{introduction} +\tableofcontents +\include{scalar} +\include{vector} +\include{matrix} +\include{euler} +\include{rates} +\include{quaternion} +\bibliographystyle{plain} +\bibliography{pprz} +\include{appendix} +% +\end{document} \ No newline at end of file diff --git a/doc/pprz_algebra/images/Crystal_Clear_mimetype_source_c.png b/doc/pprz_algebra/images/Crystal_Clear_mimetype_source_c.png new file mode 100644 index 0000000000000000000000000000000000000000..ec07f8eb2333012ec3969d9c3a70d1ed788decf9 GIT binary patch literal 5042 zcmV;j6HV-iP)9&Vbf%N<{n|g!5B)3o z)PK;K{D59E?LaUX8%PEN2?kS>gfb2dV8YeLfbkvgN!akEZH|n)@e8F3;|5nk2ZVfGE@fxOS{&`?v;P;z1Zk!_WVU9UrI37z6OU+FYk@j@YoeWk`$e|B3z5>-pw>p>}Y zcABiCv&5>^Dyr!c02*M?)dJE4z?W-!wozbBmwXKPPif#>PEg znLxH2_hMX+#^WG`QQK#p zbDiT=yPlk(*Rx$RxWsY1tPQYh&y0O05WS_P(I2l?EVilFWJ`+;57*%eGgBpiuZrO~ zuM@z14vW_Jqzkd&QBgUmojWyLWatzC%&+QOYWg2|n7~W3u-ScCm$}Dm!<1f&KJ&jSXxm@i3&y zdV9-|*VwEqN_viDS@mM3xsLAQvdtb(uQ}#mZOvwv#+RAJZErYr2s=sY{RzmxU#me}fRn>~24#_mq7vX0Vv z0BsipNXHk98FGb&fmQC+qb~Be{=&5-R^~SY0dUIPyv0WOv1)ZagHkqaUe_g`GH4B% zz-p9Cpzyi1k*=-F1X>E2!0N{5lnE5i&1NRBv7p_71+z60Km;Ge+O@sRojsvzw3d&z z8ybECfOW%B%ig))s{w$IEmohquN@O}{`N-Nmu=+ynF|;WShnZ5T~7?g#N59NDrF_b zRIr=(7t_pj$Lz)k%->wfcR<;!`8C6?2Lg2Wm~3EMIT;gB=BoU++_SerdNHW+jD}AP z|9H{;-0+5z#>T$ke%;rE*kTMU5K6dft&^vAA)zVjyY{^QI;6LS7p5~BpQW*@(043^ zAA6WU=(>=;v8%O#0KzldZd>*BcO>TnAfXeSg;2>@q?Q*`I35I;=M0HKE=tFOfVS1I zhc4h!JOxls0F@zBtt_e|XQBe8`+B;MRVo%MmEvJ$vNkWWjjQ{5N#6ti>kwG89RQ8n z;40Fu`x&F&2kJi&0@!U7z_&{J)6gaWUk3ro?Fwj|b9LM9B<`V0P|ZApp9Ry=ihGd# zH#7{qFq3-Fy!r!Kaq`0M0sH?(X@IOre_T@(_dQ4pKm)|p0=W$XTPgi%NP5tH$@w!h zjk`0GaxPjS{b@+1Zjz|pBT)jp$qr#25=J_mzg!R?De2D&Nuo$U96+6=Tu6VqAnK5Q z(g1B82DZEOtKI`!NWYQXaLz{iKGrerpK03uHucij&YVNrCT6fObd*QH+d* z6R=%dTXPmrSXx?QtE;O{W71Y6Nk>OVt!~=1$*EmkT{Qq?7o5n8^fwS73*!OeN+54%w{B%UJw3kPOGEncEMc1Mjd22l z8kZVletw=kefred`bn;Meo&%CB*^R6uN}>SYxef`vORnDIGQ0UxKX5Ecj%W{0>oJ( z2S8z&l;_W%OC?KWz@zUI8D7153=#dK(fa$cn$jq~G=KeB~|h1fZRIQATx2hReN;1^m( zA)>DFe#(iUR)v94V(r_vZ`T~XRu1+6=no%0bfOKN(w`O%xyuBi!SCI>=fw4`PB4jn z2ux9sHj2{Sn^rNi3Up9;{P;0@`SN9AphmqG&bfd8zT>q=y@z+8hfHltA0Q}hhssB& z*9?%L?_&wv?c29ITZC|sTxxVAj&OY>k08m-GH-rKBxogsp*wc$Q1%DXR{fbEfXd4P z5U76DbSywdsP@)dZ?W#~?mG1i!|>3{ref#KgPa{Z3WRlgm>S4m+jrVx27dzZ92Y>yo5?eQkq_W zW(c6dcGy)UUsR+YdHaJ04>~H}&n`Dp%>V>3J}M+zr7r>zfghkj6FhVe^2j^^R2(-* z;PmvglygPjhqNC!aDZ*uvL&)pM$!z^fkY2=#xx@i`6}o?P)RPuscjhsiq8bcN>=&- z6r>*D2T7Lf}-@?0!o%fQs~^ArbN+?J9d+sD9+=A^AEQ z7uUe}fPsMl$FU3gB@;X=Oa+Z4@aw*9WxSuOFwjc?SuKS|hZnE%>mWhW?==M=0GvBW zHh`w_0(eH495{{$FHu1N(jI=-%L@1n1V~yPMOoMZF?qAbqVGdO0hDglifbWn0C1s= z05&uYfW|TWFM9J~eg{39jwDW7B!a?#f+Hn1W;0MRU<1+_$My#mbTvb}ASxhJAo>Zi zkww=0ta28B4q~^MTbCIZfP_!nMh*j!k~>i*XJ!%n1_Gr0kYS+GxIUHm8yXntW$Zxpu9nxJV2KE?Nf0AMf-;B^769Nu(E>| zbxzWt0T;5@!*ezpg(LGU;zAOB=70u)yb(SCk}+rqyfpgR5o?D5qr;mQ3r5<2gfa!s ziQYpzXOu8BY@R=X4@Z**gizus>%D!k2taHniJ+xQh!ChrI9_xNy?frt8DX<(Xz_+1 zPd#Yhr_9>BcxPHEgffp95ri&ij0*De7t-(nQduHKoaRI;R$wcVfyg4B8`ma7(s13d z5jWy-3?RXXVAwL59KJV^CJU+PlWIrx2`%* z4h<^Ha9jr(APDegBa527Gt3J#0KQLV1||W%2S|bLXpjL!!4Ps9Aewc5p&VBQc~zfC z2+@p)pEe6fooR_05CY&II&>)C z1W>Ut-t)XpmH4^eQl~-p0hmy|Uc2tMk1cHUkt0WHi6LF1Z{dqT+C?Wb=>74m6vpO? z06IH2ye}@}$oY{7!@Qq7c_L*`$qZm_p2|#~X0{*KBWF|wPQ_MW~=rWb+K<6y`1wl+_|Gz$8=PtkGHmQKgXX2kisz1 zDynmQLTUtvtNsn==jF?_96?x>1(ejOszkvV#42LY2tjr`O!8?3F!&U*+JsDmxI##- z!$4Ih5Z*7Qa)&iIg;8iET6V@qhrbS^zl#%+tq05h?|#WUSx?Nrq}h^)3|y zumBzijOPb&XiTC?9s=(TbCn0@FOUE_`t6&|&W(BxO#22nNXn=bfC+Hn8(@_&5D+9F z4TtcKW+6o#&kq7YONEg?7hz!53)cLZoScmGtYiW}2r;Q6_60;KxF$3j%uL>T3Ul|X zVK5t5^eJvb>l)!hi&4Gv&O6RFZzY#M8wY;?B*;SE0uC+KBZ5iE<5)qhQGo2iMLHHa zLZ%NX-@AA3###s}4*2jTknaZ}TUE3!5|y`s4QTkIM~^z&G@l*!g2cBP{5RGFhy`{R z=HTtO-%iB=gXANJfrTWYh4Nl<|Epo729YB`jE{IP;b`{SGVnOO@oPoG34YGwG zf!Ci=*>CovR$Rg9~#x;7cvsz0Rs9S#L?3-E1LvuRSKdIm_elAqbxaF)~O_s9CQc4Pvi zJu@h+aLwoX;b zRU8-S{guxoBNO-@7cN|2BO@aV=g*%%aqZf*aejCKAGstb2#^+uytAs3O5AwCKYRA< z;^^q;AP4_H{4lyopSdKcL?VF7Sot@-h@+joWDq@k-M{?@@GoAx$cBf97x;GY>#x7Q z#t%Q{^(S7RMkYWz&3jL~c;^!ak-$p`@BhsRLExW0efkw7|H_ps*In?Zczww0b6(M3 zZbi}nMTwUC2wE*nl>}jgfTQ`%H{Y;Rr%o+!sUPxy|A5z7UYBI;e?fo*`~ezWD@nAViFD8O&sz^EjE+DB%Qt9!@c zhr&rXYX@GvKH8#asY+^M3o{c_0FTKRi4{3*Uao}|q$2#^9H#2<$Ya{efL2=F=Z7|d}x1pLItycYyWz$|!8 zfOnq~NAUIm{^;oF$tDh;T@WCLn!qo~L(U*@Ul2fZIOt6V ziT(aa@uF9cgTFF7Jp6Ai`543Zq?O{0N_tdOsozK4Smi@@U1KP_gV=) zN|6dq&;g{*9eDfSefQlO2mfDL0>97zngRDZftWD_oB#dy-&Y0)2S0b!erCWg2#_El zp!Na&&6_vZJm61d1^j{l2{?h+Lh@nrZ{508IdS5|->+Z4{;dH1Pgw!K$OC8q-upl1 z93sx2oSd{yo;>-_EQ4QY0L^M2)3i^WIt9tMzWCyczj3uck!A3G2_PYXE?M zgMa)eOWwW=0qptt`Jo#(ZcOtR*ixxfy6}z#%*|Cl5Bm)6^_Q;kGBGjnUsvtlcfo&_ zRq&YyV65D{lh;GM?&Gx|R-k}bo(sHUL82FIeUj$Gtb^~>09E&4r`>%l&|hQ<<%{JK z;H3-vi>!n1)c}xoEOxuu)duCl1Sqv52!J{H%P#P%SqC5G|5U_jdka81O8@`>07*qo IM6N<$f}N#?QUCw| literal 0 HcmV?d00001 diff --git a/doc/pprz_algebra/images/Crystal_Clear_mimetype_source_h.png b/doc/pprz_algebra/images/Crystal_Clear_mimetype_source_h.png new file mode 100644 index 0000000000000000000000000000000000000000..0a5bb3c2ffe3ecd5a4294a9b996604722f1958e9 GIT binary patch literal 3893 zcmV-556bX~P)M9Lo! zDfvsd=agH7lchB^rs;7Ef zC6%VTr@N}Vf4}cp)!hbZEXLu(hyQBZ_8zFjfPx!wIj98v7v~r(j^jvayV)^8|0g}q zxaZWWJ)d(ipsqrlI&|pJKd=ehOl#AU_pF=H$e#xW2EN&{WlO_F`dzqpJG>1kjo)0h_}{=$bf{`6PJx5gdfCaWuR z#I7v5+l^u$*vTRgW(LT!>;lYyYXph|fnNtwv725egJn~vh!Vqdyz8t&-?q(x+t8*} z2b26%_I*JZ{~_MtpL)SgY`nsr|@?{#e1epSPc-yIblwgt0BM?%V{#__e;PStdYG9@+rVyFd9H?7Tr% z=BL+UWAOeh3(sHBUiD8P;Td&wtX{EmG3xz}TE4#2K1k@+{{pb1`)^6Yc`uSM?22?>u`Lo&u2)o8TbwlzPAs*w!DEq9GJuKTXrHSAm1EH9j2`!ozTO$1~*)6 z@PTf_eRtN6S<~SPC*HO&eMD(rK+YvkGFYO_o(a3nUsL!TYscRfrjID?N1ZwrKMc$$ z_(|GL(q0mRg)KM%lwIO0`pmtew2Xa<>hrZBM8)Du}BJq zf>24sepgmjTy^Y(7_v4^lXP`;B_gIQ=~tQne$VDFEiI83FJ6#^g$0t!F|HO68TW@GfXd0Bd_M2qOw%-5 z5}*}H;*t^DMa>dM1K|03Rdp2U7oGZTNdOhVmvIiV9XMG<)Tm#_C4$l-pJ&}@ zFh+Urpi}~=4up*M(ge3vpD6ih5+L7+Kxggzc-0@p-|6W20G%TJVfti1re*h)G67Ng zOM?_?M*{7z`^swo(L=1No0zoizA^-eD*Ym3VpXI+jclKiPGDL#S>^T^_;HCX#R<&v zoWLwezbdB5>~!guZ8qYzql%z)%$z`#rC(L>+u{SL6pyDxM8C-$A~aLSB!b%M5T;4I zc6$3t5FqKzj&lQz zR5fw=tVn;Hx%&wI$B!Syt4-5PWr9jaI)Q20egsL>%;2Qwp(&0W_#Y&g@=ki6GIhHPO0iq5fbyOaJzrb?2Oe&R1D9w=8 z%baDl#pda$I5n;|{^flJPnU#=ApSXa{#`am1g1H> z!?B{j;S=T@n>lov00N~e=#AE_A}Lrz;TKtm28d}XT@nx=sr}-K;rGTXBm!@PgZhf& zsL8OsaoWt34=Vt zIDz;c-KnW5GCx1x@>m3Z&z?Q*z)dsq#dV})IDu(6j%i(+)cF8mE?hfw3i<##VX}b< z&`GmhbkR|OE}8(kq#gyxNR@g=8N&|UwmS;o$3j$$0<;Fu8bBA>Mb>b1vyWjVB7le! zWka!HzOff|>K3*e=7UYL!ddx@L z_K%;_LxRO=Q;HLaH)H$u?V*oj($s8!{2vL0-vW$s9-{LacplrfZL8mx>FH^A+p}lS z+z(2!YvC*ZE@1vS1MhMiCy>p{ER{+*SVue!p#+FqWX!|~n&+Q(Vb{i_8SBfJFUh`r z`&_N#B>+E>#S1)7iZ}$oIho_*weEIT80RIte zeh+m9>JrpyYyx;6q|_sJ!IIBBao&crNwI7@|1hd|4rJVN`t)fsGBPp;B=})+a`H#m z^cw0k)SsX(qNUO%AVng09lD^brPtZx$q|+iLqkJkbaZt7%$YOa1Nf5^{3)ofs0LV$ z?E~l%9egja-j6?pSq@7G1b=L7Y-wdBFcb z)qdpITWbIbc@LHXZ}yN&GZOIY_-Ng!Q>PI8<)cTB3I$(UCUB!g0leK0V*;9)4*rok z%q^Ted6L|}f4>6Y|LYm18`r>pl@xqm0&LXhL;0*90{CBB+-HST9_-fZ#9DBu^^fYYiYF5fmj1WbZtCe;j?o(L~pC&*GC2SbTCUcC8jF8*6Z_f=44IN7xd`$q!BnXNCFuRA~U%!5x+_-TA zhjhP(Q13AjnBmg|XtP7e#{K=`e4INtHa3Q}1nPf~8`4b`d`$p>d4Mp|kEB5GZ{EC# z(myjiJbVg5zK87sza;_mI6!OggYx^{bUK2MQ^s%tC#d}o0RCwo@MqElUu%Grd;li- z2>$5k=*-otSA)PWrwP6$fCMw(ofd`;A3nTs_wHR6eDM0u+`D&A3Gg)mBuYPfZ!$_g zgn4N9KU}+ZZ3u|)Ko#&^?W7@-69r(w$F=U1`sIq{4z`OOiWCm z^s|RHl#+Z+07*UoF5}~C060oNf`3X?@Iw(mhk@bm4O<75PaZvbG>b_d&fA|-4ScNu zB$$FrmoAZq4kl;tM{-F(Jzz-xqu~@7xdq4P~OJl(|oU)c$;rui z%K?i-Hh3wdIvZ3F<|uY%K38Xq6; zwJfU$7p9$n5D@^}D|>8g>^gvtqk3<$0KQ9N&pN(SWjoX_q3(vd6|I1VIb8Tpf&M`4 z{cDo)z>WwFfG3L2mcOGH)T;>)P7%<1hXnXY@87KI;QMI+1iK6BX7q2M7D&W*&TL=; zV69LREYfA=zgT7XtecbwISqa+5>%=7)ZVi$>X-imKIigFeo|Ml00000NkvXXu0mjf DyeV#! literal 0 HcmV?d00001 diff --git a/doc/pprz_algebra/images/atan2_alternate.png b/doc/pprz_algebra/images/atan2_alternate.png new file mode 100644 index 0000000000000000000000000000000000000000..c25e8b481d12c978f23bdfcaf8033102123bf4cb GIT binary patch literal 6735 zcmZ{JcQ{;K_qGTzGYAoV9%W*L7|~1g7;O-QC?Od=5fMa>o?wWQL?=WEBf>C5h%$Qb zqW9>c45D|wBhUAJzdzphcb)58*O|TcT6>?f@B7|s?FqfFtws%Dg^-YtP^+Vq^+-rA z2?O^H5Gl~YuHt-Qxa6Xzh9tpv|FaDIxMHcPrc82vaiv#f#{ezhmndTw5|XQp7x$$& z#8oKJNa3okr9v?VrU$c;nY3faNJvGVq`W%4k+8AVD^#M0aLHI~!|_hVg@%Tp+5gEe)(LP*qy_HJff z{*ZXxfRXKVe!-y&3@aXVakMXg{*;!?DdD|-n~BfaZ+pbkcOQ!iYE?gd74c{&>%|2u zvftt__g@ajA7{Tai?MIV{hR2tU?h3Y{k_M`oqP914p)A2 zfy}UEO_9O%d76MSoWm%;mX5=9`CJ%bEUcQ1dF-r2s$lP#HpsTG!f|T|ck@w0QMeB> zQf&xl@Vd;uG>$PY(J}1~JAr3T7umQW^P5VGmA!XuwkQ?WeM8+>G*-`ExwrEjCz@T^ zX_qIZRg^4Of<$;6 z&|42q8!*3vU9(vH#YX6nFQlUzLL7>pu1ir?*Bt$Es>^Y6Nm$%T6p_A33l8Fxk~5AX zoTh}fL0#IxFpE?9(S4PTx&BlMF}v+qrubzA;>SsD&|x6w`U9=%eq+zY7TWpkW|uYk zln=i?NMt-)RE-qax0!Fj1D~$&)m-Z}B1BQIJc<^HuC$Y)66IeIekeJcG5 z_)%670@0$Qa8O7`41@OiRKyBlGkM6p>c`9!+0zcUgW|Nz-chPXXum*HXl;%cWX%BvEFW< zA-&z76ZqV3r|B}wIuaV%H+CtjILUnVEL5o3rfgEFtaZa9zf&_sVy+&`p-mN*-mF@c zIC#>!S?i~%_i1#qc9RW0MLqM1-wZ^j+C44$?Y*;ywd#&UhW8~cQ+dP-j_++dMB2j-*rf?b<%ZNC z&NUbUSX9{LsP=Q60CXK5m2d!2XW)+pkit;(nO;>60Gbk?&^t0h06I!QH#qh-8Za$H zg$ZL>IcbQH!w}dctF|8impZ)PC%h^v002b|Lli;3$=%$jwSYQj;8B#hQ6cU;t7H8~ zGxWmTr;qx)l}&ug2E^9M?TEZED`a)Z06}1_HGJ@k2@O62Fg7ws)7R7n*g>R4fbAW? zi8N8bOw8?d^{fykxj?J|svwdPct})|h(=HD*#2N-Q)lVF9j&JkI7XdV&E3&RU&&F3 ztI5_BQOXTLlOKtVL;9jsvnPB#ZScMNt~aw9eo3AePv?OJ2Wk1R)~WPM{g1tzes9)( z>ySU#Pb~b&*mKWh%Wq;bjb_0(w)~k`*w&OQ*cTI0fYqFa{G*?gI&sry;Eh^dbzYW- z%XV0YeS~l`-*ycf+co*;`@WA(L&_`U%w!{H1d5}0jcJsv^A>fVL+MM0aB;1vh058E zY?(Ykg?)C%hMm4P#?6ttEg>qpGYKp^LEKB=j@8;<9DQpKa20QJUGFz`nm{*AL84}w z-j3!iHczuX(C#bFE7+@VAVcnjxG9yWRj#P8@3;|Z_4e;ZAC{8oWChon370c3$5lNO zliDTl#?m!UgiMQf>7Sg+M)@XXMyM?cw*PamFypkQx$Q8)8AOXlb?n(ElhcBt%VgSXBjt*ZyTuA`3!c#>YPd6a~;6@`yKRC^rK$-IOSaVg{AkEm+Wz%1uB}A{mH~`OAbbaqJu& zF1~L;^`Qr5;1Nu`rV-+f@v*&{WB(V7E8%^wl#ajz%Si?#hKzs|!cJX)&)qA*fef5hLqYmi_z=)SIMy0 z-~qh~c#=+%w)TcB-2cI^RM^Z`H&1W!-7H;DR59|X?K|RbnE87_De-*dA^QLs5>ywM zWb+#PjTr2@mz~SNMiqwI+{aS+COwh)i>0qgkkRztPWGOSW#Nz2>r*drXFi)(&4vCL zI%eBvU?Z};=Dg0!fn z%Cx}%SwUWbFY%~%=Y#5~6c<#04)Vx8rLh8MsY@>o-yCYSf<+0K0;8k;DS{-^lM;{`ov=FG1*Q9RbJL{7A3aQwPlyyKM8NGZWO)FKnJ-p> zJVMH(Fe;dgutm2=b+?;p`Gf~Vgw*|&g7f;sKytZ*c#=Ydpv5GEvn484tq_5QC|N3y zTyQ@CI%5rp!-eg}SHJ^>9tZ;it3;Lg9y;^^ue?aTpE?3f|^HY`Y5GC7AtX}TMpZdIke?(1}efD-gmMyDO+!ADi z1ts>}W|3OEf3lJ{JBzdPX7lu(_kS|5`FLR?nQrRj%i!#FjH=a4sLE{*IJd)Dm0o$c zsv{{)=6wzDc9~oGRFy}w zrZC?6@gX$tETQ$=_%?rM&f}{2^h3BV{t(U;MElf{Zh18@>%{*lT_1m$yUI}X@#I+lHx7UK5~J|+bjBWS z^qDSkxrE@L{~B$;k+E`uO8AgZ==&p3WkUa{Q)G-K8@{?z#aLAlB=a~?ARSy{Y%h-a z`JAibrnC=3)k?xcFyljjO5(*Wv5iYs;qe@|iyp4CnN}a9<(sAT$qnoYxoo_jwBdND zJ{`jqQ(==`N!YVKvzkk}Zg*y7>;AY;la2J-dhRb;gM!_`=^)p`pB1;llk2QP5X$CqQCs3m3gugM4A)1&ueF|>0quQQ4bTYgPF z*+PN%u)$a8i4Y~Exsec68f{6ia)nqU{vh=d2R<^w07|FwSCj%_-yQ&x;%*?Xe2Twx z<3j%kNV@nI6X@)!Yc1Z~h?$O*K-#==k5lgN1_;?Kh%8}O281mDZP17#!{uEt&KdTK z`iQ|Py4nVSDpQt5K8Bk4l^TVBSdtTRTi*TKgEKy}mL9ociUH+Zuu zmgF5By96F=W%wS+tgtD}%#7q4NGoMXzJX+dJoUcnh~{)o3OVfLrkAThG}q0JGFH!Q z=Z5DW*5XrHi~Qb7b2K6=PEycdt4>#E)knVd?@19>5yq5yW&@=uo_+H_YtxB+vjSZd z8j*X2~?L1q%iA^V*x(d(~;n>T_CtW6ZiV8wfA zjU0y%6bCX@akI{;z~z1qE1rHDuj6w4*zrY{3iskT&-<43wpX#Vo5$T!N-@D|u7t8Y zo425U5;9s!PUEo-nQLVxYm=VOzZG|<@n$)TU$${e{&l#NelS#ay>zXAazyW5+EZ3q zy04;ee4+xsglywa-`QwSr_$=FZ1c3IQzn*f@;Z&lk&$*fkUiTW@r{$%H}Q{hq2lBG zhID}2#Jv%tp(!%zoGgrT9%1XT2g6GLy?QV1nlDpwHg)?(9P$EpN*vbvKCwSgZQ&ix z9_1ODYSc&Fz74mWjQfoXcq`u}F?ZdaQPr}ELfB^#=H0QGCf*NZB5Uf?`}imThf z^zXJ}&znccKmy&(((cwr0`t+X_$rL;;5{(`zXm4Gx`~ZupbMzQ-!%jgS3HO-JAptl zjWWLoT=Kbaov+*OwF&CiA@-%t2%oXDB$aSur@WQnPKMO%H>lEV$KD_#xts+wm)PFE z*-s;%Hvl1206f%%BY=Fl5a;s0>HITujlF};G@iT{N6#M@O314Sx_g11W32bF<~k3N zpl@=rM;5a`4odu?=gR4`OkG)Xap|zN+c&tyFZjDM5QF!wA8Iuz^DykIgC!eXbsSVy zUQVdc`94g6tvJjcr+C2UcKyj9Pdf(<7D~VAn?&wiZ@8GHo$O@MN5gYP<-V1b+0%ic zg3`tN({)8+y79%_LI8Wy*82HJt`E6#uiZau(m8?#1O{XTpPw9b8~NU_`oiu_^d|B# zs@&opq;4azGB+Y0dKKhVF9F?g>&|~vE{Qj*@jE^F@&=ZFbE7=*%sB0f{Ev0My|jOR zGE#;<^f36c_;v~WC{yh?B)BBmZEQXeEu(T8Q%U^@>{WjQ?KghikiKNro(82lv^{l- zC)FZ*tN;?a;%@LlK4;m8OD4_xoD&qis*ba8XXwBQ!O7g?-Uw7m!p*4wY^h*^IBypr zsK9Rk^~1k=)uXiJ^ja8L#W*F*yuK<7L~L)1Jb=)Eh`A7VdqA)L8Vwj2`UL3JfksB9 z022T?RIV^aV>uBZ<}xY4>jv=&22ef=Bi*#U*HFMLyU!H}yQqRkN`xE3AQywx|AkI& zt_*B&YMZzGUn>KEDW8zP=|=9#aa0{J8kUl{VhjF@ROgF%9>NF!i~plGflobU^KgY_ zUBpRnCh`qrj~YhCi%Bo6cgVQB`;sAgi*2v^Fq_;)%bBGwYZ;JrtuW!I(o&iE@)l}u z;)I*?$a4f&#=B*gN3LP#r$=5Y&01uh@^tLh?18+zYz;BRRg55~@foG~FIQ>d{x!7q ziTUOo+jS|7ro7+JzYtGOMyCD7A@(ISMe)D7x)1`T=U+t5$9u2MWV%n4N`27p`IE}7 z|FL`7Dse{(BsGcIg1t;nRkVHOmkoJ2oLyeQ^IdyNT2%*Rku|)RpLpC|6aicPKog!J zKv}RaX&9KN&8`HBYl;^Ud!eH9umr=ovIItqWPhYZCLWrMd0&>17nP}6EqajffX59K z>&m+9cQy28_8Ci)j#vGaJA1b8DZg|BK-EjF@=~6Po%Xbq{Kdx1#=A@oW{qy$y`^Qm z*KaouoSvjI)iQG}MtDvhZ?GNr3*Nds^6i5e^EdCEP|A+fTOAYroTXjUQB|1{6sDnC zc_&+c!u+~&JM8`~b^cUycu?kFecBX5l{(-|f6X_kLquWa;rZ08(SpfA0ioi=F|P>e z_kBG#!&g=yXVAFRu<}o)_H*&Dm3@Mx70JAfo@R=xgB&wmU+$*9DG$^R^_!GL7(<;Ji>=>Uk66ErMQy%HEe-FAp8jlLJ)HptcXQm6O}oOUzbXj&|z zd;W%eab{pG9x~K+^msbzFGhk##qatl4YATzLKGnvz*6M1G^%z{t!3^bcV9@CW{3t# zlYm&OVd+>P<%kR$LQApG0Vg1~iD);Gk;3-CuaUIaEAIJQa6@AlikisxH!Rc0;C36A z?$Ol?;@r)rg^Lp)67S+dP5y#Ej`qfcn4z{3l6E96QKyUeDY9g?S)o~N^=l||#fS8#{?MxVXGd4Fw)55!6FpAyK=dh4PaSP!c>!s$%t;;q4Ad^}N;ZxGhcL z?qfGR2ow>7Un|Tb%Yol-=JeQB9d{Kz{p`!g-YK3GCL+oGqra)ryFxz~A!&A&r$Yj> zw@HyukK*X)atll^Q0-)!1IaH%AYssz&Vg9G1v4%b2|!_@&H;E2=^oqghSpB${8IJ}j(oC*;wY6%;Isb*B*#^;b z?!|XiQVLh8x1X7lWsdyUgX3hfO!KyJ`I|+oA8)ic^vvmV;|wm&Yd!_?kass*EvcZt z4=V{WH!b}1mP&K2l0&zk-H&_pKSzi8NhmkL zAAK2g+iM%|TLd~@S^K?Q@7MN@2^MVQpG)}`W@pC(&*sUKd8BV@x-(u8WHyyrcr&}6 zX;|S7RaIhym%>i{_B|t|jVvDMH@TDzsgM6Dhx2S%B7$yX5tK1MZbRu6vj|N8JzMn1 z3~;2^T=%fkY4JNb95B5d5MUxJ<94X_Eqi#qVkZ2{ywFa-tjJD)v(%31-p#L*sq9Sm zr0jK=oVvG&gIq^n$a*vho9nepcWpBg@mRL@nzEXj8BvYICOvgjTC1ERdu64QIdcnd z#bQdD$WLcYH?8l+d2mj6q;vPbr5i3#=xNC{^z{@$zp&R0KPkgHeo`j*Bq(j1vMy7k zAS=#(&v-L1U_yRvu=X7`P{qK17c-_E3-5-A?ju;d54Out4VsTc3x<~#Q|Ksv^Ga-= zj+f@TdiK{w8pil{R&S?wHXoWMZXR^rmdT1@{>%E@rt=l!vmPD3x-)A?7Ixr3CObxi%s z2b$qh*-~?-JnF8_Ubhe(Cts~jnNki6-kFVga5U+D*z=a}Ws0yAO*PwB)wv7{3UX(y zn9i1A&{O_4&+$-pHb)2v0rdRWse@~5#Zk* O5_J`AWjxY6;Qs)B_V8x_ literal 0 HcmV?d00001 diff --git a/doc/pprz_algebra/images/ratiofunction.png b/doc/pprz_algebra/images/ratiofunction.png new file mode 100644 index 0000000000000000000000000000000000000000..a15ff86a89fc4cafc4c6bc487423b5f052464aa9 GIT binary patch literal 6220 zcmaiY2{e@N-~MRGWCl~nmS#kevWvztma$}CvL{PO-(>mPD!VWe!YD%c4rR$!_MHeb zNMudMB>Oh@Wh~=;^#7gz`JMOuz2`l1<~+~+na^`SbMEW&A^k|s-B z?>Ck8{}M+Qv+Ymwna$KXqsXGF;N8iQb^rmMb13)E6Vq1H?z@6 z>Vm;7n|qPQQ&f(iaF3_u8)4O$L~+<9Nw9d34I`#B{mZ9|H5$f8`RM~#x=097McYN+8UF1_cE|=4YP$~ z-DBKP-Ei=2KYnsUOszvguT)ZZfMzlG@lv7qk1WN%{z-G-(p2&^@tW0dOxa`P0%2H_ z;%Q!8&~Ag+aVs0U5Xas5q;9*t(r2=jKet@1zhAT4p71QOE-Xe@H;sg~YunKto;p^! z^vZE%gp;CQ`ozys=U%KPThbexJYGtmxUXqn+RHb=1aGoy#7LhXkpG&=Il;PjO^I|@ z(QNIx94wM1sbKYCC(cYW#PY$%U>TdcobNBB7`uHse)%cz_>un7V%nFZnU0r^zu&Hy z9Wba__O9B~K}*DatceIBenpO#2PTMACvCW-$_s|h^TPZld%nJ}pDVtJXr421yM24* zTfpycVp-Ko_sgMq$t#Wr3_tG)kw1}@3^1IW_QGbE;YvlTsIk>NCiAmxm>^x>>iJ_s zLsQi*EwWTH-&h7>o0j}Or|sydEz7o!+L(VxB9FW8n8-0;xcpxpr2cx-KWX-~-8vbr zHj4dyn6}n0`Pt7#Ea^bXq=+QrZ0}8`y)n+;SDmSwg^X#XLr_IhT zvU#wYF$Y(xbfztdBYF=h_cEfL)7>9&=fJY43)F)#Y=Db$>e<=jt=ZpKgvM9hS^Vc* z;h#5#9Uz!w=N~)A4u3W5P_q;9Fn)Duul_lWxbw+FoDD}eqb>h#N_4gwwoEK{=txje zgPe&?{e;fTwjz+={$&2KA#J7QS=Ywi`^pM7tXBgi*oXV(D4reL;sn=?+A8ue_?Jv* zzUu)W9Kwc_*cyu!+G9LL-RoFmI0PH(pjW6TL=pKzP6PBW#Pf$6bf;SqOg28Pv*NP& zb%-ZS)JG4`B7n3-LZ{!U(TkxG$hwEkF1^BvsLMeZ;$U!CfQBanLCdE2#%u_3vRB5q zhgwlt$qYt5w@~s?CMLkWNel6_U$7D|ftcraGHvgziR?lwS3h$gv#NgUND1baiO=?o zO&-q@7qNA&Sumx#N0gFu%{5j*O_Zkmbny$*PDH*``StF!LMfp_0gJhXdXqdk`B?FoWj*i%3BTgPtHX40i%lElRV;m9C7?vbfA*U{r2sdSi?a?A3Z`SPcq80vv$Q1Ih)2 ze=#M*%P`PC6TXb#J%%eXU_}gEaQrTfj25AR@2;=0s$Zp!3c&Xmr!kz=3IrGgRzb5T z#>ap`kZA#{4Z7pd!2By&ELttVJgjb_bk>;X=s(e$KZ7ZPZRfeWn{Gk)KF(&_3QiAX zhP3?Qho01<#L4}t+H3L0_s=;vhU(fIbynKh)yOse{tKz~pu69GUnEKuD5t%6KoZJY*U5?Ix(v*=qALDW7iv@DGu z(s7E~r;Q#4k5YL+90~ES&O*vnkO&@P=f$uVWhA1G(CGk&l>q56D!+gi#7%?gaS~Kd zU`bpJb9*o5I&fjvSLGaLn(px}Rg%`OVj|So#&9rEGpiVv_vSRJeX6-dfTXYe&b5&J zUpN*BeRTQZ*nh0?AIjAeo{Ew;Wv*Jd9yv8^uS1T7WY5dR&WD7 z=RR`4f8Ou+^u7Da%SN;Eu6=avr?R4kCgMKl?e>v!wP!BDc5{OLUnXUewoYB!=3^gF zo6~R5cNgOB8Vov}e}?~&<3eJwM^jY+a&l98pp`3+FD6t7w7$aLG<=62t|*))h+(A6 z=}8Gjz|HN_wuir{8Wy9JeFh@4d#GRz+i)2>*^2YrX7zgNnNIV4JIXpzv1U?ym$}b< zH!bt-kS^$|w5le*;ld+rU#)6&LfoVG_`VGbciMgDEaF3>71l==2G1-Tt{SGT9~F5= zre{jopwey+KkkA$wpO6TH7}>4zmnv|@+%LFwiglHf}LURH`v$YL4X1?lk}uyY@T`~ zsVAI{I|-cbDBT;dj_r# z|2%%n%P)|;2o8z$KOl~O=lh!uiJ07r`o-XY)}fK={?qOFgp5VocGk zEK2bUKaDxq6$lFqd=dCoi%l#P-eSMRF z%B&ETfU&5JaM~>Bjk_HnH2ADh`QiDPgm{&WFB+f()>2ef=viIdrC{r3s%de*Wm5gq zw&wlomQso!$7}s>H{kJOhNm>$_8blnG?Y*)S^0N6TgWJ_ElQx#l)36#7y zwkmiXg0{>Z8nV^ndvny4g|2yrfr9DJ&Cxv+xI6LTmhWMhh;(;ZQLTt~nY@>8W-kTG z+{qS>N%E>3Hte{Ah+hl2=lHkuPurERc=xk2Gkw}hKDWxtPII8iSMt}K3XYSY|KMrl}^gi;5k2GPQ2N?6n_pGdnoUvOhC z%Dv9$6)NPPR*s^wpf752q7~`_-#>Pv|u&+=f@=u4oDeIxR9`DVN}cyc}w9(C4wqsdBdX41i7R^PoA*p{3j^cDQ_rmHS#f z?fho8CbWAIIuZOS#ajt1{qjXI*r}l}MgsZ%D1oaa??JPleb1f9>B`-^A z=0t1jX=fOvCYq0y?3Xv&9WFJvDcQa-ld=^BoyU?Mj81-NYVIx>33kOz?#XInfueS+ z?%eoU@Hk0IlR|q$ah&Mg40G7!sY1}{MhDg;!J=#B6`roIM~GMJudd9sW+I73_prku z^MK~Pb`iT{c2n7Lz?(l-w~-46d;f-QwColjz@zBTfxgWxSIL9cmdVbdHyE>6mv_e) z>ghM5P|{#XB9VhBvXzJ2A&ju|vR;alxyH5s68}|muzsAP?DK!v`O5hv3dzmbcud`F zRU=B5qAX!)RGlyF^MH2Q_yC22t!%H&54Hposq!8xrz6zW1-V=M@?aVlnBoosk!SUd znHm>rzAKXhkx5ypn|1O8H9i z9eAG1d@g6~a$3><@ZP|_?EW9T_mdBN8$*nf3I$P=E8Jyc*-101Y2~GHOu(7+B6l+_ z_Nriuf}@LPBs~m8?Mz96iN;kj?*~Nns=J9i$q(_Q=FBg*58N){db@fC2Xhck6~t(k zm=M&Gs!Gz&a}+W00SDQ!(R%X5T2fD(89jHX<(fG>J4PGprREgGIW2qr0pFIbFmq|w zwc)Z)I@piQ?bI8SIgZZ|8Omju;ZYG%`W?d)E%#*DYIQz8i4KG}3vgpx*#5E25Pl38 zL+45tbLCurDXP)mja5<}2R#xqZ$#zN64)S^Fl_S|zaNx2q@^^hv1DcJOS1yed;t-g?(27h>iV;2{S8w z1uWQ!r3(}6Qho+3^FHo~m~{FBH`ZYgEOgmr0w`Gf`FboRfz<+sUl^%)XVJ9SIB8R9 zekaI1s1zKN)O0u$ahhr4jiT1^%l~Ym)eaWF5r$NCB6Ua@KKMTryt&9L)rY95_c<-C zaO^1^%Gv<)Wd_!LKeHCOwVs<9*kPcd zve^oF-S0Ma0TVAa*8{|cWN!@78iZ)e8&B)qWfjP5K&cqPF-vFq8nv+cv#B!X54 z$d`{}H<$BXkKs4+i0rt+0WntMI8TmKfC$G6HHdwv?+ z-wmL7Oa5@c!52(zG1|x8eLz!1!`n#=1V)_f@sqT`>y=jG>R)MDfUAPl|LhZ`OK`4pYYRPmHZ4ZRjS^o_l4H8>{C-&L$qTet)S4_*b~nd9zfdg=_ReXdb;y;nn5Z{< zYYY8aob4*jWnlOc^rS(ru&@!AJZLEUEACK}u1d>pYxgFKKM|1IOoayx<#V-}M{_Hp z`R@7Gx@-mwQ4d|UrIih*GBO<5C&g*IGY^Za_BSR_4lhE;X7`tM*56#TK_xdi5A7k( zNl4G(J+IZ>6)SO%?w)D=YyO>0MA1=QgIVI@9@y-Dva^-9*{qZA^rB*hXQA8$Ll}#G z*HG1RUva$Fp7N(pHK}tumKD?`&8_$ll=+YNC1GzKo)Wj`3#b|UxF3r}rM2FL+5J7= z223+$Q6E>ImFiSnNvfJwG0AG#6JJfOGePgWDvVseTjpu~swgU;GRslpad&&A=-)Xu zH<>eD$AngrO)5Q#r1sm?Fx}#||LUrpedO!y=xp^gC}~?f_eH5oB>lI;rruW9fS&Ez z!eh6V*5&l5ywhHM&ITCWl@T`Z{_)(Bc-*`;WPNF~=fxJUJajeu{&lkQoq@4%*K@lm zyj1qvfBL1Q!lMh<>a4`X+@cju7Nf$3ema=)_-MoCZZ1XopXQD%2VULs+xO$|`doFG ztNP0U7QZ5C;UXlC&RmRp!SZEI^xILAdeDeIx-US~h<927iL2pK5_6_)RRFRH%6;{Wg~6`wA~?%a#W|K;#N z^2hZS+T;Ra1T9uGk$X?d>u6QX|gP~9@s`+i1><*<`%Uy1B-Dj17NV@#EEAloa z8JhPtIVSY+P3g~)Il$!N=gHiyJaJdl ze#SGp!^PN|e)K zE>Ql-i1@dm6VLR&NQmNblE2GWf;)@ZVDI$ zIs|ul?*7eVU170%{Q`#$grBbew(?EC=F~ac`t@f{rNhCtGv8|+IJ+VTK2CX2LkPFr Wo{adYivq2FkhX?_dWo9-v;PAA4(ka3 literal 0 HcmV?d00001 diff --git a/doc/pprz_algebra/images/xyvalues.png b/doc/pprz_algebra/images/xyvalues.png new file mode 100644 index 0000000000000000000000000000000000000000..06c377c16fadee43f6541db8b8a9a500275101eb GIT binary patch literal 6713 zcmZvBcQjjX{QpI5MXg$;M%8LYJeYtrVBxlsD| z$sX))VW}3vOH zL#a`)hUJ>~01&7$8)DlUB#)aa!rhXCNLDI7wn1(q&b|#F>ak5}tH~P9`R;6ZKg9rJ zoWR)#UEFDy&xG4T&EBWwAC)VwrNM&&l&>Esde}bDFrLXQywg~>YkPf@2KdmcH~he* z%td(8aII&1pgDu#0=H9d1CthPS*#i$-Q+xD*Dx-91Wc)+kxD7rkb_88`+m3v&#Ed} zkel~R=P(C2V7I@Q2uJe8Y=t(VAfJ%&d8ufV1qpig%Mp(zd|^gjX03WA-kkFl0Uhum zPXhd6s`-S}KQ#F8P43&bDhO+-5ZvKPRpQP<)o~EIdEh=xpuL;hZ9!UKnFsOF7t`_` zQD;?oHzr1Y|NGbRXdfp<7pm>kGZ*ZZ#~~`2M)jPaBlXA63B6TqB0-~Nb&d2C@(5KD z&7-}76i5($HN>UIr!aD$!-o&>M0g^MA}QvC0RYMbG*~oPlqwVd&l#}fxSX9&2%-d0 z0{=}AG>Xh220BoLz||mQ>DAeiBSAz?Y+fXV>3?VV2~eg=azRf79+4w_;?tD3Vz)ii(euVSQ_g_d}H0za1UMujQ9<>2sM-yLep~pfX1WGuw-y$r#m(dGiSHyRXg%bbNL-+WbJGs_3+m#pKB$GKB{I z+uG7Ol;F4w(SJD5kVL+tlBZHp-tO}r{05paGLB0a<$mN6)-%i4=ow*T?C3z4A6WPR zl!(#GPv7mrKll||0HcC`7=SYUUvRksPQ)+hgxU}!*v#OxA?yZ&hOx?g+|YuxbFqdn zjcI-X9cbXK&x~=3l)Kw0D#Je`d(g~jG?+c}qNLji_3JONBqPq^Z7_M!BnR?5`*URd z>N@$G7a=+`Q5T?gB@;(ROo5EpU{XAE&w$=kz3?Xr651}~Dowxtd0J^Lz4_ScB^Q=* z2;1cEEMDi73`X}Jy{6k}bzOr+C5W3cpOV~lF&x@tmFB~7Y*Y8158#wI&a2yI%UB+> z&0K=)Y1S?xpqE`f*`^sv?y|aZ!%H1MccW%(EsVDbs~JT!6=qCo0`~sO9}L~NHrmXbpEbmHYDoIZZfPF-u)4Kb zA6iVKz6S{;^2^nApu9^7L4utR$Rom`=0?$i;F7Op+3(U>x=`L^3xuw0F$QcsCf3{R zLbWygpTTMH0%YGc<-Oa%Q8QqA7b+-ISWm3ex^@v*kW?o^?m9GGO%yaxoMN?j3hV|> z+8Gk5^U?k|w|jZNR9V9S%H)HGTDnqaXp{nW5a%t8%iA-L^}qpn0&*bi z@L2W9*wzA1e|`Ek+l|AHeJYgkcmoIaRSX6d`Yb8;BiH)V5EL(PQ4k_s&DH5|E z9Im=dGbgP%;52g?S3>mFb#VLP5+;k$$lh&9Q9N`&X8xi>jmZV0x$?WCQE64Q!Y?On zT>UNYMCT-PP7ThgjG|Obb2#ZeNy`wo=f-j*oR(W$rhQhFlnk#ifq>9;e1ZQkl?O)v zIN(IuvEK_i5ze?!$4b{Ed^YVph|)RY(%%VN|Jqy!snjJn@%cyyDx*;1!LqJ;Y+pN+ z1UeG6s3Q7IYx0)0=@lROL(PSHn^Bh~KGLD>@BC|fnq(kw{%slhe@byje(S4Kf91h@ zX%Kdwqt5#CvB&S+Vj%$nYnEPC00RF{CzL)}F$Ov&WqIlw8Q12RZRik%$WZfAALhhf zDaNu>gVCOywqOX~TzQk5;6w^x8}uH*!mz3L0hc&|(jJ~q7)u}iKs_rW_@`BP+~C3S z#}DCR0g3o}y#qD+N)InVK~#>F*8I^#F$wjyc=Zw(WzIr2?zS)LB~2JHj6-&lDlT22 z5*?%IE4iPC4@8X^G+189XJiLWbC=$52YCwg#yzX}oNboLjrP9p{&$dv97|bNmMBf# zALRPCH;6|4!}d{y0J?p%<*$0tchjI}+9hDLN!>YAt)4H<_0QttE~oxclqE%-L)xXC z#mw>Xt5?T+kPS7Dohi#D_UyC?S~%qxn=Erw^z~a*P0HX>6Lm@|j-jO!3v*J@sRy#MH1QhUH;8vr zE}IWM8$LqBAKH`A@TF(VYGGfp>PQ=8`-wx))9}+%xQ+CkTj8#waRb+P+^BUCAPcDX4|9)(muz2davGBTN zRZ%!wj9*8$k6yw&r26Aj^5!5hOu{Jb4T=!Gb-keiaZZ=GgJX;)TC=JP~D;!xVx_!D!6kAvL{kd&rp z#*i}_t?gKm5X+>36tE8W^wG##`C!N&9xnXvPM6oh+uWj} z`qMbXK~*DGQ0EC~;C?7=m(vupZ8?J2YO(1FsBEW3S;PN`c>+avTVw2<0s+1bxvLs2 z?lf3%9@@m_#3p>3FV{ib6{UtVq!a5_Qo6dArP~t^f9DD&oD-fQhMFrHbje_ZtEgG1 zjZlUxI*ED!1C99X3zHkRHX>JQjNkJPRDZFsWtdJk&WLAQ)68#=V6OZDlu0qQmEvV? zS_hbIuDm3rcm8PxUYf6L+VD+giDlqp0Uu+LMzL*}X2g|fdm{SdukI}}oE5@G5RYT2 zx}c_}g_<|?t>J!HWh&_Bue4KeX6wMkHZ+T05s3gMnrpY3s;x*$WzVYBZkpzEt1!}B zmDtsifEF)=o=ge(m&J=cH2Ha#K*Ff{-M06}R-~XZT1IL_XFj;W|7u2jh3|gM7i`o5 z>T`KxsGVP)n#5gbgWFG4>xtO~g6TJ=-Vcp6(8!Z97_-`FC50S*16sNFgVYA+9yyjU zQ2O0W?+IXU!=<;1kth6|7+>sn-ClVfZJQyc3X{K6qZZ3}Czm_;)lGe$p$#sYHJIY# zXWsqh&P|z*m{`3+7CJl=3Xo#jy58=~Us?A~c^mlJ%tBn{kd*b%bTJI_Qysj=JJbU@ z_opQ|T@ItT?Pohviae7w)bPH5oGU!G2%d(GsJ+$|Bp18E8;pEegJ{>EfB1k@be`_J zNu`+d5lK}5Wey(VOfn?`6^cgodKR_L4px~9$5U&_*z>B;&|RJA=J*L|#gF6Cazzxy zIcTMN4&+Hv9x2D1+`vAvo;^X{_M~sMjrhF6X@kL{dcd=-R$iUYeh8x9yO7NM`GxB~ z|K`8Iz`x);JcagIQC~vw;`E~dL&FukF2K-8fq)&g)3(pSlx6xL!-`wm2vllv{H~0gWu*sUkX^oEXe`ldJmTlD z-QA1x&Yq8Lxwz5wv$~%jy_rilJx%#sxbd7@Z)l@O%PL@)BZ>UOv<=y+4v#@PleT&+ zxK_f7B$zQh_RrQq;JHatsKxOUZNAfHN6U+O@wVx7dS{XNh;zq}+}@@L&OJ-M%iPPD zl6L|whD6zX4BhbJwG~M#Ss&>8=cjRPIej0Ui;^1gt~8x0zj4{I)ig!p@R~oLMp0u& z$Orb$FoYsbue8)uL;EOmb52-~AtJ}!m5o8dBhJc}!V~f5UZQZX9?iZZd)USn6lz{w zQ>q?&ZX+2+heHuorQi9wLwH~1z9`;`BwSe$BjcKcVK#j`sQF39FG3BC;+Jfefl6IA zzF!GCQGc+GPeY;r+@sSC$tP`uGdG+}sDS9W)HrME! z_l=-cfpO5BaBm7kVCtRm#mncA)m0wumEMvfn$|{pSC!_G!ryr(*g~mW)*Dsyt)J}h zu4XZ+Qjx@igi(wqRSYA^74u++BUtIVR%ee0pCSX9&9w+1<$I=2Hf`|F$T7%EEw`r< za33IXs8~UL7lyvz9Fe?t4nuRC29GiC@m!awlIr2ZAk#I5AWtpU2W5yKwaM(cJ$U|S zI=}wEROJ<@nn60WD77D;3=@~d9b$IitWpz|bYwWW)ZeW3e+S*X4RS=D-x9byNafN5 z8yT&Y{{FrAB!~Hi+-eEVlO5PS%-2Orep$O0;?@_rw|JhZn0B$zg?ZP0E4aY+WYzEF zWsPn#HK)RqbL%7P=TLSN{mT#H&UVA|jRSG=bK_HC_aAD@I03Y_AL9L$?tbUP_);EG zw=$33J3v~jvy6QtWQ;HKl>cU@kMK2(2_L82?)*qGfr6_NZ*w>Mf5q=lX+GFt^8OTB zxnPL4lznTY=>DX-vAjS3_CTjrG6{%s@){`7U&VnCSmVg@L~IQu`A6o&x<=(_G<#C4 zJDxyowUZo`{cWM??-SncXZ^wB1fP@Rw?8=!Tu_DFb@s7fQmm806Zh^cV9$rQw|H~p zc(tl#i*iXo`CV5s`{MAmz#&u3jkLqCs|gIh9mPP2Ur8U{D3>(Kr3hCbn~7Chy1GW7 z$zt$IimFHd~%R<9zy&f3)5Dxq6bfaPOzr^0=G@YW_h`{tisD*>e$;X^4s! zhh%tU(IZ2jTL~rHBDozv9y``VeP06?g!b(^g%9P~n>$*~bpIwik zZK_*af(Kyk4YTb%-|2hUPsz3f6EwX~TGJL7KgP_3Y?iym*v*`kgqJ0*n7XtruzcuH zF*bVr-ldHFO2ivh4>`6q7@zzaEIsgbBa4Kw{h%Sj5(7h@`=yn^&@p2-s|>?r!S$lp zez~p!B8@zwxZ#+Dm3d)2$)*%Zn^$0>yeAzVR4^y=J^+Cq7kp9$nv2Sk`H`i?a6IRGx_=elV!M&65gQGYW=KK>bdtT80i3hR$Yd1EYI2R zM-}w&7Ojl=o9g~CRID#MbTy2|-&2j_p-yP!w`)$sfJb!}!7~G$`C>NOwIF3jL5`sq z*3@DOE(ZJAc($zacP5}@9JBV9C+zOM1a5O~t|ptB9$0PxslaM3u@Z`Fuk`5TmtLp+ z%1bYQVE(*zLAL4H#-(r2_Uqm#;~=0F-jJdlMS6d}{_r~|pE@bAMtjKG_gb8yjwy)A(ffDw0-YIZT~(_lCwWFPQPQNTu*W7u7PjH{+ybHu0;$ z>k@urB?g96#10&m9b}nV)hlk_UAJL6q~1DN|4^{GS)kx?NVRpaU?H)sq*qeAc;@2Z z?75=#QdfT+epVqcFZty+tnw#z<=)N9^wK-LdLeGKI1?chd7a$J;`4Ht1aI24|Gp0TLR{gAQ>qNaBJ#j?FEG%vh(Kv~W4@2wbYA?jfaK_P%6!^d>u-;%Ih} z^qN9bd39uRVnC*W2x&$qwipOzdTQA_4TAy4@B0JK8xz^TH(PoxURioxOAoR;5Ikx33(_5cW7)#(g)T8uyVYWg3;v$rdxx8!d@gwo z4~R03+L>1#&5FVwz%OKk8*pVF?c(`7T)g=MXoPseAT)#^yvm7 zYbF)Z6zBDC4Fv}l;JU@DHPjuPhYb0)Pvv6JE_l{z*1Bfw>;63w>1C*|AouJ%J0*o- z@}@_X{9D=W-B<3ents$vWx{V};H(1&t&Ls-`#TmpFdT9+67p&E54NH5OWeEHsgAjN zGro23wzka-eeK@GY~-?cw4RgJ1Lsib_nYBZ38nBR*H9%MbYk}oEHAw-&Eb5q`a|-t z(zMgb@J)%5zhrBc-R9{sJ_cXYZVlvJFF6*XEE*?uNfIbydYwdxX5-9()m2~CR?%2P zmdv{?n*vrh8fYF5F*7hSFdTeKV&wdmyU>71&{Io|o^XmuopAc*j>R?77-63sUC1Uh zckc)rJhf?2JlXJ_?<^PkO_fN!rqvxHfPHvU^)in6QuD@)OYMz!%wIfCmlL1H#d?+& zY)s*9@R)hHb1br(%l+6S5_sdPca;VY9dBgU$-UxJ=0Q)=;h#_Q0VH&Nc@h70O7W-v zI}kQx<@kA71|Kw@XDz}S-9K$KFB-lQy^!I#tY@lRDE;DHzEVBc7FH3T`1Iw5oZ-Ap z$f`kZ^lHsW@mwR#2CjJLUb*zVe67D;A#}ID;6`t zc*fWR@fSmDEe9;74_rcvYZ_cb+j&y`$E=UNU7}LBW7K)|&i_2Ch)y;4^0U-kh-7h7kojgFng#-j%~T5>p&z(98sSrl6I7?h~_T-Jypi%%WFq3y!HY>tj9)Q(w#)~ z!+2w8RaI9~GaNENBgiD~RNTZ-d8gS`ok7npNvjN_NiSL|tnmNq0NNS`>XoXtQU4E` CiWBJo literal 0 HcmV?d00001 diff --git a/doc/pprz_algebra/introduction.tex b/doc/pprz_algebra/introduction.tex new file mode 100644 index 0000000000..47fef62e8a --- /dev/null +++ b/doc/pprz_algebra/introduction.tex @@ -0,0 +1,119 @@ +\section{Introduction} +This is the documentation for the algebra files of the paparazzi project (paparazzi.nongnu.org). It should be a reference for the functions which are defined in the directory \texttt{(paparazzi)/sw/airborne/math}. The structure of this documentation is in the way how it should make most sense in a mathematical content. This documentation might be redundant.\\ +The Conversion between FLOAT, REAL and BFP(binary floating point) is not in this documentation yet. + +\section{Important definition} \label{Important definition} +Unfortunately there are a lot of different definitions for rotations. There are 24 (some say 12, the author tends to think that there are much more) different ways to define euler angles. Therefore, paparazzi uses the convention, which is shown in \textit{figure \ref{Normal euler order}}. +\begin{figure}[h!]\begin{center} + \begin{tikzpicture}[->,>=stealth',shorten >=1pt,auto,node distance=2cm, + semithick] + \tikzstyle{every state}=[draw=black,text=white] + + \node (A) {$\begin{matrix}Reference\\frame\end{matrix}$}; + \node[state, fill=yawcolor] (B) [right of=A] {\Large $\psi$}; + \node[state, fill=pitchcolor] (C) [right of=B] {\Large $\theta$}; + \node[state, fill=rollcolor] (D) [right of=C] {\Large $\phi$}; + \node (E) [right of=D] {$\begin{matrix}Aircraft\\frame\end{matrix}$}; + + \path (A) edge node {} (B) + (B) edge node {} (C) + (C) edge node {} (D) + (D) edge node {} (E); + + \end{tikzpicture} + \caption{The order of rotation from the reference frame to the body frame is first \Yawc{Yaw}, then \Pitchc{Pitch} and finally \Rollc{Roll}.} + \label{Normal euler order} +\end{center}\end{figure} +For instance, a resulting rotational matrix would be +\begin{equation} +\mat R(\Yaw) \multiplication \mat R(\Pitch) \multiplication \mat R(\Roll) +\end{equation} +\begin{equation} +\begin{pmatrix} +cos(\Yaw) & -sin(\Yaw) & 0 \\ +sin(\Yaw) & cos(\Yaw) & 0 \\ +0 & 0 & 1 +\end{pmatrix} \multiplication \begin{pmatrix} + cos(\Pitch) & 0 & sin(\Pitch) \\ + 0 & 1 & 0 \\ +-sin(\Pitch) & 0 & cos(\Pitch) +\end{pmatrix} \multiplication \begin{pmatrix} +1 & 0 & 0 \\ +0 & cos(\Roll) & -sin(\Roll) \\ +0 & sin(\Roll) & cos(\Roll) +\end{pmatrix} +\end{equation} +\begin{equation} +\begin{pmatrix} +cos(\Pitch)cos(\Yaw) & sin(\Roll)sin(\Pitch)cos(\Yaw) - cos(\Roll)cos(\Yaw) & cos(\Roll)sin(\Pitch)cos(\Yaw) + sin(\Roll)sin(\Yaw)\\ +cos(\Pitch)sin(\Yaw) & sin(\Roll)sin(\Pitch)sin(\Yaw) + cos(\Roll)cos(\Yaw) & cos(\Roll)sin(\Pitch)sin(\Yaw) - sin(\Roll)cos(\Yaw) \\ +-sin(\Pitch) & sin(\Roll)cos(\Pitch) & cos(\Roll)cos(\Pitch) +\end{pmatrix} +\end{equation} + +An equivalent multiplication from a quaternion with a vector would be +\begin{equation} +\begin{pmatrix}0\\\vect v_o\end{pmatrix} = \quat{} \quatprod \begin{pmatrix}0\\\vect v_i\end{pmatrix} \quatprod \comp{\quat{}} +\end{equation} + + +But, since paparazzi is a library for aerospace, the \emph{choosen perspective is from the vehicle}. This is an important difference, because the attitude representation changes slightly, but it can mess up everything. In detail this means, that the order of the euler angles changes and also the sign\footnote{If you have problems understanding this, pages 123f and 130-134 of [1] help a lot!}(figure \ref{Aerospace euler order}). +\mynote{I still need to fix the citation} +\begin{figure}[h]\begin{center} + \begin{tikzpicture}[->,>=stealth',shorten >=1pt,auto,node distance=2cm, + semithick] + \tikzstyle{every state}=[draw=black,text=white] + + \node (A) {$\begin{matrix}Reference\\frame\end{matrix}$}; + \node[state, fill=yawcolor] (B) [right of=A] {\Large $-\psi$}; + \node[state, fill=pitchcolor] (C) [right of=B] {\Large $-\theta$}; + \node[state, fill=rollcolor] (D) [right of=C] {\Large $-\phi$}; + \node (E) [right of=D] {$\begin{matrix}Aircraft\\frame\end{matrix}$}; + + \path (C) edge node {} (B) + (D) edge node {} (C) + (E) edge node {} (D) + (B) edge node {} (A); + + \end{tikzpicture} + \caption{From the perspective of the vehicle the order and the sign of the euler angles change.} + \label{Aerospace euler order} +\end{center}\end{figure} +As a result the rotational matrix changes to the transposed/inverted. +\begin{equation} \label{order of matrix multiplication} +\mat R(-\Roll) \multiplication \mat R(-\Pitch) \multiplication \mat R(-\Yaw) +\end{equation} +\begin{equation} +\begin{pmatrix} +cos(\Pitch)cos(\Yaw) & cos(\Pitch)sin(\Yaw) & -sin(\Pitch) \\ +sin(\Roll)sin(\Pitch)cos(\Yaw) - cos(\Roll)cos(\Yaw) & sin(\Roll)sin(\Pitch)sin(\Yaw) + cos(\Roll)cos(\Yaw) & sin(\Roll)cos(\Pitch) \\ +cos(\Roll)sin(\Pitch)cos(\Yaw) + sin(\Roll)sin(\Yaw) & cos(\Roll)sin(\Pitch)sin(\Yaw) - sin(\Roll)cos(\Yaw) & cos(\Roll)cos(\Pitch) +\end{pmatrix} +\end{equation} +Same for the quaternion multiplication +\begin{equation} +\begin{pmatrix}0\\\vect v_o\end{pmatrix} = \comp{\quat{}} \quatprod \begin{pmatrix}0\\\vect v_i\end{pmatrix} \quatprod \quat{} +\end{equation} + +\section{Overview} +An Overview of the implemented functions\\ +\newcommand{\YES}{$\surd$} +\newcommand{\NO}{} +\begin{tabular}{c|ccccccc} +function & VECT2 & VECT3 & MAT33 & RMAT & EULER & RATES & QUAT \\ \hline +ZERO & \YES & \YES & \NO & \NO & \NO & \NO & \NO \\ +ASSIGN & \YES & \YES &(\YES) &(\YES) & \YES & \YES & \YES \\ +COPY & \YES & \YES & \YES & \YES & \YES & \YES & \YES \\ +ADD & \YES & \YES & \NO & \NO & \YES & \YES & \YES \\ +SUM & \YES & \YES & \NO & \NO & \NO & \YES & \NO \\ +SUB & \YES & \YES & \NO & \YES & \YES & \YES & \NO \\ +DIFF & \YES & \YES & \NO & \NO & \YES & \YES & \YES \\ +SMUL & \YES & \YES & \NO & \NO & \YES & \YES & \YES \\ +EW\_MUL & \NO & \YES & \NO & \NO & \NO & \YES & \NO \\ +SDIV & \YES & \YES & \NO & \NO & \YES & \YES & \NO \\ +EW\_DIV & \NO & \YES & \NO & \NO & \NO & \NO & \NO \\ +STRIM & \YES & \YES & \NO & \NO & \NO & \NO & \NO \\ +BOUND\_CUBE & \NO & \YES & \NO & \NO & \YES & \YES & \NO \\ +BOUND\_BOX & \NO & \YES & \NO & \NO & \NO & \YES & \NO +\end{tabular} +\mynote{Add Compiler \#warning or \#error for wrong use of Bound and Strim?} \ No newline at end of file diff --git a/doc/pprz_algebra/matrix.tex b/doc/pprz_algebra/matrix.tex new file mode 100644 index 0000000000..1f19093cba --- /dev/null +++ b/doc/pprz_algebra/matrix.tex @@ -0,0 +1,126 @@ +\section{Matrix $3\times3$ / Rotation Matrix} +The indices of a matrix are zero-indexed, i.e. the first element in the matrix has the row and column number \textbf{Zero}. +\mynote{I choosed to start the indices with 1.} +\subsection{Definition} +The matrix is represented as an array with the length 9. +\begin{equation} +\mat M = \begin{pmatrix} +m_0 & m_1 & m_2 \\ +m_3 & m_4 & m_5 \\ +m_6 & m_7 & m_8 +\end{pmatrix} = \begin{pmatrix} +m[0] & m[1] & m[2] \\ +m[3] & m[4] & m[5] \\ +m[6] & m[7] & m[8] +\end{pmatrix} +\end{equation} +It is available for the following simple types:\\ +\begin{tabular}{c|c|c} +type & struct Mat & struct RMat \\ \hline +int32\_t & Int32Mat33 & Int32RMat \\ +float & FloatMat33 & FloatRMat +\end{tabular} + + + +\subsection{= Assigning} +\subsubsection*{$\mat M = \mat 0$} +\begin{equation} +\mat M = \begin{pmatrix} +0 & 0 & 0 \\ +0 & 0 & 0 \\ +0 & 0 & 0 +\end{pmatrix} +\end{equation} +\inHfile{FLOAT\_MAT33\_ZERO(m)}{pprz\_algebra\_float} +\inHfile{FLOAT\_RMAT\_ZERO(m)}{pprz\_algebra\_float} + +\subsubsection*{$a_{ij}$ elements} +Accessing an element is able with +\inHfile{MAT33\_ELMT(m, row, col)}{pprz\_algebra} +\inHfile{RMAT\_ELMT(m, row, col)}{pprz\_algebra} + +\subsubsection*{$\mat M = diag(d_{00}, d_{11}, d_{22})$} +\begin{equation} +\mat M = \begin{pmatrix} +d_{00} & 0 & 0 \\ +0 & d_{11} & 0 \\ +0 & 0 & d_{22} +\end{pmatrix} +\end{equation} +\inHfile{FLOAT\_MAT33\_DIAG(m, d00, d11, d22)}{pprz\_algebra\_float} + +\subsubsection*{$\mat{A} = \mat{B}$} +\begin{equation} +\mat {mat1} = \mat {mat2} +\end{equation} +\inHfile{MAT33\_COPY(mat1, mat2)}{pprz\_algebra} +\inHfile{RMAT\_COPY(o, i)}{pprz\_algebra} + + +\subsubsection*{$\mat M_{b2a} = \inv{\mat M_{a2b}} = \transp{\mat M_{a2b}}$} +\begin{equation} +\mat M_{b2a} = \inv{\mat M_{a2b}} = \transp{\mat M_{a2b}} +\end{equation} +\inHfile{FLOAT\_RMAT\_INV(m\_b2a, m\_a2b)}{pprz\_algebra\_float} + +\subsection{- Subtraction} +\subsubsection*{$\mat C = \mat A - \mat B$} +\begin{equation} +\mat C = \mat A - \mat B +\end{equation} +\inHfile{RMAT\_DIFF(c, a, b)}{pprz\_algebra} + + + +\subsection{$\multiplication$ Multiplication} +\subsubsection*{$\mat M_{a2c} = \mat M_{b2c} \multiplication \mat M_{a2b}$ with a Matrix (composition)} +Makes a matrix-multiplication with additional Right-Shift about the decimal point. +\mynote{Not quite sure about that} +\begin{equation} +\mat M_{a2c} = \mat M_{b2c} \multiplication \mat M_{a2b} +\end{equation} +\inHfile{INT32\_RMAT\_COMP(m\_a2c, m\_a2b, m\_b2c)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_COMP(m\_a2c, m\_a2b, m\_b2c)}{pprz\_algebra\_float} +and with the inverse matrix +\begin{equation} +\mat M_{a2b} = \inv{\mat M_{b2c}} \multiplication \mat M_{a2c} +\end{equation} +\inHfile{INT32\_RMAT\_COMP\_INV(m\_a2b, m\_a2c, m\_b2c)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_COMP\_INV(m\_a2b, m\_a2c, m\_b2c)}{pprz\_algebra\_float} + + +\subsection{Transformation from a Matrix} +\subsubsection*{to euler angles} +\input{transformations/matrix2euler} + +\subsubsection*{to a quaternion} +\input{transformations/matrix2quaternion} + + + +\subsection{Tranformation to a Matrix} +\subsubsection*{from an axis and an angle} +\input{transformations/axisangle2matrix} + +\subsubsection*{from euler angles} +\input{transformations/euler2matrix} + +\subsubsection*{from a quaternion} +\input{transformations/quaternion2matrix} + + + +\subsection{Other} +\subsubsection*{Trace} +\begin{equation} +tr(\mat{R}_m) = a_{11} + a_{22} + a_{33} +\end{equation} +\inHfile{RMAT\_TRACE(rm)}{pprz\_algebra} + +\subsubsection*{$\norm{\norm{\mat M}}_F$ Norm (Frobenius)} +Calculates the Frobenius Norm of a matrix +\begin{equation} +\norm{\norm{\mat M}}_F = \sqrt{\sum_{i=1}^3 \sum_{i=1}^3 m_{ij}^2 } +\end{equation} +\inHfile{FLOAT\_RMAT\_NORM(m)}{pprz\_algebra\_float} diff --git a/doc/pprz_algebra/quaternion.tex b/doc/pprz_algebra/quaternion.tex new file mode 100644 index 0000000000..e9f51a1832 --- /dev/null +++ b/doc/pprz_algebra/quaternion.tex @@ -0,0 +1,172 @@ +\section{Quaternion} +\mynote{I hate the naming convention for the real part.} +\subsection{Definition} +The values are called +\begin{equation} +\quat{} = q_i + \mathrm{i} q_x + \mathrm{j} q_y + \mathrm{k} q_z + = \begin{pmatrix} q_i \\q_x\\q_y\\q_z\end{pmatrix} +\end{equation} +It is available for the following simple types:\\ +\begin{tabular}{c|c} +type & struct \\ \hline +int32\_t & Int32Quat \\ +float & FloatQuat +\end{tabular} + + + + +\subsection{= Assigning} +\subsubsection*{$\quat{} = $ Identity} +Sets a quaternion to the identity rotation (no rotation). +\begin{equation} +\quat{} = 1 = \begin{pmatrix}1 \\ 0 \\0\\0\end{pmatrix} +\end{equation} +\inHfile{INT32\_QUAT\_ZERO(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_ZERO(q)}{pprz\_algebra\_float} + +\subsubsection*{$\quat{} = \transp{(\quat i,\quat x,\quat y,\quat z)}$} +\begin{equation} +\textit{i} is the real part of the quaternion. + \quat a = \begin{pmatrix}i\\ x \\ y \\ z \end{pmatrix} +\end{equation} +\inHfile{QUAT\_ASSIGN(q, i, x, y, z)}{pprz\_algebra} + +\subsubsection*{$\quat{o} = \quat{i}$} +\begin{equation} +\quat o = \quat i +\end{equation} +\inHfile{QUAT\_COPY(qo, qi)}{pprz\_algebra} +\inHfile{FLOAT\_QUAT\_COPY(qo, qi)}{pprz\_algebra\_float} + +\subsubsection*{$\quat{b} = - \quat{a}$} +\begin{equation} +\quat b = - \quat a +\end{equation} +\inHfile{QUAT\_EXPLEMENTARY(b, a)}{pprz\_algebra} +\inHfile{FLOAT\_QUAT\_EXPLEMENTARY(b, a)}{pprz\_algebra\_float} +\mynote{Naming the other way round?} + + + +\subsection{+ Addition} +\subsubsection*{$\quat{o} += \quat{i}$} +\begin{equation} +\quat o = \quat o + \quat i +\end{equation} +\inHfile{QUAT\_ADD(qo, qi)}{pprz\_algebra} +\inHfile{FLOAT\_QUAT\_ADD(qo, qi)}{pprz\_algebra\_float} +\mynote{No SUM function?} + + + +\subsection{- Subtraction} +\subsubsection*{$\quat{c} = \quat{a} - \quat{b}$} +\begin{equation} +\quat c = \quat a - \quat b +\end{equation} +\inHfile{QUAT\_DIFF(qc, qa, qb)}{pprz\_algebra} +\mynote{no SUB function?} + + + +\subsection{$\multiplication$ Multiplication} +\subsubsection*{$\quat{o} = s \multiplication \quat{i}$ With a scalar} +\begin{equation} + \quat{o} = s \multiplication \quat{i} +\end{equation} +\inHfile{QUAT\_SMUL(vo, vi, s)}{pprz\_algebra} +\inHfile{FLOAT\_QUAT\_SMUL(vo, vi, s)}{pprz\_algebra\_float} + +\subsubsection*{$\quat{a2c} = \quat{b2c} \quatprod \quat{a2b}$ With a quaternion (composition)} +Returns the multiplication/composition of two quaternions. +\begin{equation} +\quat{a2c} = \quat{b2c} \quatprod \quat{a2b} +\end{equation} +\begin{equation} +\quat{a2c} = \begin{pmatrix} +\quat{b2c,i} & -\quat{b2c,x} & -\quat{b2c,y} & -\quat{b2c,z} \\ +\quat{b2c,x} & \quat{b2c,i} & -\quat{b2c,z} & \quat{b2c,y} \\ +\quat{b2c,y} & \quat{b2c,z} & \quat{b2c,i} & -\quat{b2c,x} \\ +\quat{b2c,z} & -\quat{b2c,y} & \quat{b2c,x} & \quat{b2c,i} +\end{pmatrix}\multiplication \begin{pmatrix} +\quat{a2b,i}\\\quat{a2b,x}\\\quat{a2b,y}\\\quat{a2b,z} +\end{pmatrix} +\end{equation} +\inHfile{INT32\_QUAT\_COMP(a2c, a2b, b2c) }{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_COMP(a2c, a2b, b2c) }{pprz\_algebra\_float} +\inHfile{FLOAT\_QUAT\_MULT(a2c, a2b, b2c) }{pprz\_algebra\_float} +Also available with inversions/conjugations (please note, that a inversion and a conjugation is the same for a unit quaternion): +\begin{equation} +\quat{a2b} = \comp{\quat{b2c}} \quatprod \quat{a2c} +\end{equation} +\inHfile{INT32\_QUAT\_COMP\_INV(a2b, a2c, b2c) }{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_COMP\_INV(a2b, a2c, b2c) }{pprz\_algebra\_float} +\begin{equation} +\quat{b2c} = \quat{a2c} \quatprod \comp{\quat{a2b}} +\end{equation} +\inHfile{INT32\_QUAT\_INV\_COMP(b2c, a2b, a2c)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_INV\_COMP(b2c, a2b, a2c)}{pprz\_algebra\_float} +\emph{Note} +Please note that due to the fact that it's done very often, the functions above are also available with Normalisation: +\inHfile{FLOAT\_QUAT\_COMP\_INV\_NORM\_SHORTEST(a2b, a2c, b2c)}{pprz\_algebra\_float} +\inHfile{FLOAT\_QUAT\_INV\_COMP\_NORM\_SHORTEST(b2c, a2b, a2c)}{pprz\_algebra\_float} + +\mynote{no Division?} + +\subsection{$\comp{}$ Complementary} +\begin{equation} +\quat o = \comp{\quat i} +\end{equation} +\inHfile{QUAT\_INVERT(qo, qi)}{pprz\_algebra} +\inHfile{INT32\_QUAT\_INVERT(qo, qi)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_INVERT(qo, qi)}{pprz\_algebra\_float} + + + +\subsection{Transformation from Quaternions} +\subsubsection*{to a rotational matrix} +\input{transformations/quaternion2matrix} + +\subsubsection*{to euler angles} +\input{transformations/quaternion2euler} + + + +\subsection{Transformation to Quaternions} +\subsubsection*{from euler angles} +\input{transformations/euler2quaternion} + +\subsubsection*{from a rotational matrix} +\input{transformations/matrix2quaternion} + +\subsubsection*{from measured rates} +\input{transformations/rates2quaternion} + + + +\subsection{Other} +\subsubsection*{$\norm{\quat{}}$ Norm} +Returns the 2-norm of a quaternion +\begin{equation} +n = \norm{\quat{}} = \sqrt{\quat{}\comp{\quat{}}} = \sqrt{\quat i^2 + \quat x^2 + \quat y^2 + \quat z^2} +\end{equation} +\inHfile{INT32\_QUAT\_NORM(n, q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORM(n, q)}{pprz\_algebra\_float} +It is also possible to directly normalise the quaternion +\begin{equation} +\quat{} := \frac{\quat{}}{\norm{\quat{}}} +\end{equation} +\inHfile{INT32\_QUAT\_NORMALISE(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_NORMALISE(q)}{pprz\_algebra\_float} + +\subsection*{Making the real value positive} +It is possible to invert the quaternion if its real value is negative +\begin{equation} +\quat{} = \left\lbrace \begin{matrix} +\quat{} \quad \quat i>0 \\ +-\quat{} \quad \quat i<0 +\end{matrix} \right. +\end{equation} +\inHfile{INT32\_QUAT\_WRAP\_SHORTEST(q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_QUAT\_WRAP\_SHORTEST(q)}{pprz\_algebra\_float} \ No newline at end of file diff --git a/doc/pprz_algebra/rates.tex b/doc/pprz_algebra/rates.tex new file mode 100644 index 0000000000..9771395dbc --- /dev/null +++ b/doc/pprz_algebra/rates.tex @@ -0,0 +1,144 @@ +\section{Rates} +\subsection{Definition} +The values are called +\begin{equation} +\ra{} = \begin{pmatrix} p \\q\\r\end{pmatrix} +\end{equation} +It is available for the following simple types:\\ +\begin{tabular}{c|c} +type & struct \\ \hline +int16\_t & Int16Rates \\ +int32\_t & Int32Rates \\ +float & FloatRates +\end{tabular} + +\subsection{= Assigning} +\subsubsection*{$\ra{} = 0$} +\begin{equation} +\ra{} = \begin{pmatrix}p\\q\\r\end{pmatrix} = \begin{pmatrix}0\\0\\0\end{pmatrix} +\end{equation} +\inHfile{FLOAT\_RATES\_ZERO(r)}{pprz\_algebra\_float} + +\subsubsection*{$\ra{} = \transp{(p, q, r)}$} +\begin{equation} +\ra = \transp{(p, q, r)} +\end{equation} +\inHfile{RATES\_ASSIGN(ra, p, q, r)}{pprz\_algebra} + +\subsubsection*{$\ra a = \ra b$} +\begin{equation} +\ra a = \ra b +\end{equation} +\inHfile{RATES\_COPY(a, b)}{pprz\_algebra} + + + +\subsection{+ Addition} +\subsubsection*{$\ra a += \ra b$} +\begin{equation} +\ra a = \ra a + \ra b +\end{equation} +\inHfile{RATES\_ADD(a, b)}{pprz\_algebra} + +\subsubsection*{$\ra c = \ra a + \ra b$} +\begin{equation} +\ra c = \ra a + \ra b +\end{equation} +\inHfile{RATES\_SUM(c, a, b)}{pprz\_algebra} + + + +\subsection{- Subtraction} +\subsubsection*{$\ra a -= \ra b$} +\begin{equation} +\ra a = \ra a - \ra b +\end{equation} +\inHfile{RATES\_SUB(a, b)}{pprz\_algebra} + +\subsubsection*{$\ra c = \ra a - \ra b$} +\begin{equation} +\ra c = \ra a - \ra b +\end{equation} +\inHfile{RATES\_DIFF(c, a, b)}{pprz\_algebra} + + + +\subsection{$\multiplication$ Multiplication} +\subsubsection*{$\ra{ro} = s \multiplication \ra{ri}$ With a scalar} +\begin{equation} +\ra{ro} = s \multiplication \ra{ri} +\end{equation} +\inHfile{RATES\_SMUL(ro, ri, s)}{pprz\_algebra} + +\subsubsection*{$\ra c = 2^{-s} \multiplication \ra a \ew \multiplication \ra b$ Element-wise with bit-shift} +Makes an element-wise multiplication (also known as ``Dot-Multiplication'' from languages like MATLAB, FreeMat or Octave) and an additional bitshift to the right about s. The bitwise shift operation often results in a multiplication like $2^{-s}$, but especially for the divisions of integer values it's not the same. +\begin{equation} +\ra c = \begin{pmatrix}p_c\\q_c\\r_c\end{pmatrix} = +\begin{pmatrix} +2^{-s} \; p_a \multiplication p_b \\ +2^{-s} \; q_a \multiplication q_b \\ +2^{-s} \; r_a \multiplication r_b +\end{pmatrix} +\end{equation} + +\subsubsection*{$ \ra {vb} = \transp{\mat M_{b2a}} \multiplication \ra {va}$ With a rotational matrix} +\begin{equation} +\ra {vb} = \transp{\mat M_{b2a}} \multiplication \ra {va} +\end{equation} +\inHfile{INT32\_RMAT\_TRANSP\_RATEMULT(vb, m\_b2a, va)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_TRANSP\_RATEMULT(vb, m\_b2a, va)}{pprz\_algebra\_float} +or without the not-transposed matrix +\begin{equation} +\ra {vb} = \mat M_{a2b} \multiplication \ra {va} +\end{equation} +\inHfile{FLOAT\_RMAT\_RATEMULT(vb, m\_a2b, va)}{pprz\_algebra\_float} + + + +\subsection{$\division$ Division} +\subsubsection*{$\ra{ro} = \frac 1 s \multiplication \ra{ri}$ With a scalar} +\begin{equation} +\ra{ro} = \frac 1 s \multiplication \ra{ri} +\end{equation} +\inHfile{EULERS\_SDIV(ro, ri, s)}{pprz\_algebra} + + +\subsection{Transformation form rates} +\subsubsection*{to euler angles (derivative)} +\input{transformations/rates2eulerdot} + +\subsubsection*{to a quaternion} +\input{transformations/rates2quaternion} + + +\subsection{Transformation to rates} +\subsubsection*{from euler angles (derivative)} +\input{transformations/eulerdot2rates} + + + +\subsection{Other} +\subsubsection*{$\norm{\ra{}}$ Norm} +Computes the 2-norm of the rates (the length). +\begin{equation} +n = \norm{\ra{}} = \sqrt{p^2 +q^2+r^2} +\end{equation} +\inHfile{FLOAT\_RATES\_NORM(v)}{pprz\_algebra\_float} + +\subsubsection*{$min \leq \ra v \leq max$ Bounding} +Bounds the rates so that every value (p, q, r) is between \textit{min} and \textit{max}. +\begin{equation} +\ra v \in \mathbb{I}^3, \quad \mathbb{I} = [min; max] +\end{equation} +The lower border \textit{min} has a higher priority than \textit{max}. So, if $ min > max$ and a value of $ \ra v $ is between those, the value is set to \textit{min}. \\ +\inHfile{RATES\_BOUND\_CUBE(v, min, max)}{pprz\_algebra} +\mynote{See the note for euler angles and the naming is bad.} + +\subsubsection*{$\ra{v_{min}} \leq \ra v \leq \ra{v_{max}}$ Bounding} +Ensures that +\begin{equation} +\ra {v_{min}} \leq \ra v \leq \ra{v_{max}} \Leftrightarrow \begin{pmatrix} p_{min} \\ q_{min} \\ r_{min} \end{pmatrix} \leq \begin{pmatrix} p \\ q \\ r \end{pmatrix} \leq \begin{pmatrix} p_{max} \\ q_{max} \\ r_{max} \end{pmatrix} +\end{equation} +The upper border \textit{max} has a higher priority than \textit{min}. So, if $ min > max$ and a value of $ \ra v $ is between those, the value is set to \textit{max}. \\ +\inHfile{RATES\_BOUND\_BOX(v, v\_min, v\_max}{pprz\_algebra} +\mynote{Not very consequent with the priority} \ No newline at end of file diff --git a/doc/pprz_algebra/scalar.tex b/doc/pprz_algebra/scalar.tex new file mode 100644 index 0000000000..5ac7688dee --- /dev/null +++ b/doc/pprz_algebra/scalar.tex @@ -0,0 +1,73 @@ +\section{Scalar} +For scalar values are a few functions available + +\subsection{Multiplication and Rightshift} +Represents $ a \multiplication b $ with a right shift about \textit{r}. This becomes close to +\begin{equation} +2^{-r} a \multiplication b +\end{equation} +but it is not the same. +\inHfile{INT\_MULT\_RSHIFT(a, b, r)}{pprz\_algebra\_int} + +\subsection{$\sqrt x$ Squareroot} +Calculates the squareroot $y = \sqrt x$. The function uses the Babylonian method. +\begin{equation} +y_{n+1} = \frac 1 2 \left( y_n + \frac{x}{y_n} \right) +\end{equation} +\inHfile{INT32\_SQRT(out,in)}{pprz\_algebra\_int} + +\subsection{atan2() 4-quadrant arctangent} +Calculates the 4-quadrant arctangent of two values, x and y: +\begin{equation} +a = atan2(y,x) +\end{equation} +The function uses a trick, which is desribed in detail at +\begin{itemize} +\item http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm +\end{itemize} +In short: + + +\begin{figure}[h!] + \centering + \begin{tabular}{ccc} + + \begin{minipage}{4cm} + \centering + \includegraphics[width=4cm]{xyvalues} + \end{minipage} + & + \begin{minipage}{4cm} + \centering + \includegraphics[width=4cm]{ratiofunction} + \end{minipage} + & + \begin{minipage}{5cm} + \centering + \includegraphics[width=4cm]{atan2_alternate} + \end{minipage} + \\ + (a) x/y-values & + (b) ratio function & + (c) comparison of the result (blue) + \\ + & & and the real value (red) + \end{tabular} + + \caption{alternate atan2 function} \label{alternate atan2 function} +\end{figure} + +If you have a set of x/y values (figure \ref{alternate atan2 function}a), you can compute the ratio (figure \ref{alternate atan2 function}b) of them: +\begin{equation} + r = \frac{x+y}{x-y} +\end{equation} +and transform this ratio very close to the real values (figure \ref{alternate atan2 function}c) using +\begin{equation} + \alpha = \tfrac \pi 4 (1-r) +\end{equation} +or (more accurate) using +\begin{equation} + \alpha_2 = 0.1963 \multiplication r^3 -0.9817 \multiplication r + \tfrac \pi 4 +\end{equation} +\inHfile{INT32\_ATAN2(a, y, x)}{pprz\_algebra\_int} +\inHfile{INT32\_ATAN2\_2(a, y, x)}{pprz\_algebra\_int} \ No newline at end of file diff --git a/doc/pprz_algebra/transformations/axisangle2matrix.tex b/doc/pprz_algebra/transformations/axisangle2matrix.tex new file mode 100644 index 0000000000..bc6776e568 --- /dev/null +++ b/doc/pprz_algebra/transformations/axisangle2matrix.tex @@ -0,0 +1,25 @@ +With a known axis of rotation $\vect u$ and an angle $\alpha$ it is possible to compute a rotational matrix with +\begin{equation} +\mat R_m = \begin{pmatrix} +0 & -u_z & u_y \\ +u_z & 0 & -u_x \\ +-u_y & u_x & 0 +\end{pmatrix} \sin \alpha + \left( \eye - \vect u \transp{\vect u} \right)\cos \alpha + \vect u \transp{\vect u}. +\end{equation} +Please note again, that all angles are from the perspective of the aircraft (see section \ref{Important definition}). Therefore the angle is defined ngeative, leading to +\begin{equation} +\mat R_m = \begin{pmatrix} +0 & u_z & -u_y \\ +-u_z & 0 & u_x \\ +u_y & -u_x & 0 +\end{pmatrix} \sin \alpha + \left( \eye - \vect u \transp{\vect u} \right)\cos \alpha + \vect u \transp{\vect u}. +\end{equation} +Rearranging this equation leads to +\begin{equation} +\mat R_m = \begin{pmatrix} +u_x^2+(1-u_x^2)\cos \alpha & u_xu_y(1-\cos \alpha) + u_z \sin \alpha & u_xu_z(1-\cos \alpha) - u_y \sin \alpha \\ +u_xu_y(1-\cos \alpha) - u_z \sin \alpha & u_y^2+(1-u_y^2)\cos \alpha & u_yu_z(1-\cos \alpha) + u_x \sin \alpha \\ +u_xu_z(1-\cos \alpha) + u_y \sin \alpha & u_yu_z(1-\cos \alpha) - u_x \sin \alpha & u_z^2+(1-u_z^2)\cos \alpha +\end{pmatrix}. +\end{equation} +\inHfile{FLOAT\_RMAT\_OF\_AXIS\_ANGLE(rm, uv, an)}{pprz\_algebra\_float} \ No newline at end of file diff --git a/doc/pprz_algebra/transformations/euler2matrix.tex b/doc/pprz_algebra/transformations/euler2matrix.tex new file mode 100644 index 0000000000..bd60955bbc --- /dev/null +++ b/doc/pprz_algebra/transformations/euler2matrix.tex @@ -0,0 +1,24 @@ +The transformation from euler angles $ \eu e$ to a rotational matrix depends on the order of rotation. Here, the default order is 321, which means first \Yawc{Yaw} (about the \emph{third} axis), then \Pitchc{Pitch} (the \emph{second} axis) and finally \Rollc{Roll}(the \emph{first} axis). Please note the important definition about perspectives (page \ref{Important definition}). +\begin{equation} +\mat R_m = \begin{pmatrix} +cos(\Pitch)cos(\Yaw) & cos(\Pitch)sin(\Yaw) & -sin(\Pitch) \\ +sin(\Roll)sin(\Pitch)cos(\Yaw) - cos(\Roll)cos(\Yaw) & sin(\Roll)sin(\Pitch)sin(\Yaw) + cos(\Roll)cos(\Yaw) & sin(\Roll)cos(\Pitch) \\ +cos(\Roll)sin(\Pitch)cos(\Yaw) + sin(\Roll)sin(\Yaw) & cos(\Roll)sin(\Pitch)sin(\Yaw) - sin(\Roll)cos(\Yaw) & cos(\Roll)cos(\Pitch) +\end{pmatrix}\end{equation} + +\inHfile{INT32\_RMAT\_OF\_EULERS(rm, e)}{pprz\_algebra\_int} +\inHfile{INT32\_RMAT\_OF\_EULERS\_321(rm, e)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_OF\_EULERS(rm, e)}{pprz\_algebra\_float} +\inHfile{FLOAT\_RMAT\_OF\_EULERS\_321(rm, e)}{pprz\_algebra\_float} +You can also choose the 312 definition (First \Yawc{Yaw}, then \Rollc{Roll} then \Pitchc{Pitch} $\Rightarrow \mat R(\Yaw) \mat R(\Roll) \mat R(\Pitch)$). Again, remember the different order and sign: +\begin{equation} +\mat R_m = \mat R(-\Pitch) \mat R(-\Roll) \mat R(-\Yaw) +\end{equation} +\begin{equation} +\mat R_m = \begin{pmatrix} +cos(\Pitch)cos(\Yaw)-sin(\Roll)sin(\Pitch)sin(\Yaw) & cos(\Pitch)sin(\Yaw) + sin(\Roll)sin(\Pitch)cos(\Yaw) & -cos(\Roll)sin(\Pitch) \\ +-cos(\Roll)sin(\Yaw) & cos(\Roll)cos(\Yaw) & sin(\Roll) \\ +sin(\Pitch)cos(\Yaw) + sin(\Roll)cos(\Pitch)sin(\Yaw) & sin(\Pitch)sin(\Yaw)-sin(\Roll)cos(\Pitch)cos(\Yaw) & cos(\Roll)cos(\Pitch) +\end{pmatrix}\end{equation} +\inHfile{INT32\_RMAT\_OF\_EULERS\_312(rm, e)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_OF\_EULERS\_312(rm, e)}{pprz\_algebra\_float} diff --git a/doc/pprz_algebra/transformations/euler2quaternion.tex b/doc/pprz_algebra/transformations/euler2quaternion.tex new file mode 100644 index 0000000000..a27146f399 --- /dev/null +++ b/doc/pprz_algebra/transformations/euler2quaternion.tex @@ -0,0 +1,14 @@ +The transformation is given by +\begin{equation} +\quat{} = [\cos \tfrac{\Yaw}{2} + \mathbf{k} \sin \tfrac{\Yaw}{2}][\cos \tfrac{\Pitch}{2} + \mathbf{j} \sin \tfrac{\Pitch}{2}][\cos \tfrac{\Roll}{2} + \mathbf{i} \sin \tfrac{\Roll}{2}] +\end{equation} +In matrix notation: +\begin{equation} +\quat = \begin{pmatrix} +\cos \tfrac{\Roll}{2} \cos \tfrac{\Pitch}{2} \cos \tfrac{\Yaw}{2} + \sin \tfrac{\Roll}{2} \sin \tfrac{\Pitch}{2} \sin \tfrac{\Yaw}{2} \\ +\sin \tfrac{\Roll}{2} \cos \tfrac{\Pitch}{2} \cos \tfrac{\Yaw}{2} - \cos \tfrac{\Roll}{2} \sin \tfrac{\Pitch}{2} \sin \tfrac{\Yaw}{2} \\ +\cos \tfrac{\Roll}{2} \sin \tfrac{\Pitch}{2} \cos \tfrac{\Yaw}{2} + \sin \tfrac{\Roll}{2} \cos \tfrac{\Pitch}{2} \sin \tfrac{\Yaw}{2} \\ +\cos \tfrac{\Roll}{2} \cos \tfrac{\Pitch}{2} \sin \tfrac{\Yaw}{2} - \sin \tfrac{\Roll}{2} \cos \tfrac{\Pitch}{2} \sin \tfrac{\Yaw}{2} +\end{pmatrix} +\end{equation} +\inHfile{INT32\_QUAT\_OF\_EULERS(q, e)}{pprz\_algebra\_int} diff --git a/doc/pprz_algebra/transformations/eulerdot2rates.tex b/doc/pprz_algebra/transformations/eulerdot2rates.tex new file mode 100644 index 0000000000..361c445e20 --- /dev/null +++ b/doc/pprz_algebra/transformations/eulerdot2rates.tex @@ -0,0 +1,31 @@ +This function requires the euler angles e and also their derivative ed.\\ +\begin{equation} +\ra{r} = \begin{pmatrix} p\\q\\r \end{pmatrix} = +\eye \multiplication \eye \multiplication\begin{pmatrix} \dot{\Roll}\\0\\0 \end{pmatrix} + +\mat R(\Roll)\multiplication \eye \multiplication\begin{pmatrix} 0\\\dot{\Pitch}\\0 \end{pmatrix} + +\mat R(\Roll)\multiplication \mat R(\Pitch)\multiplication \begin{pmatrix} 0\\0\\\dot{\Yaw} \end{pmatrix} +\end{equation} +\begin{equation} +\mat R(\Roll) = \begin{pmatrix} +1 & 0 & 0 \\ +0 & cos(\Roll) & -sin(\Roll) \\ +0 & sin(\Roll) & cos(\Roll) +\end{pmatrix} +\end{equation} +\begin{equation} +\mat R(\Pitch) = \begin{pmatrix} + cos(\Pitch) & 0 & sin(\Pitch) \\ + 0 & 1 & 0 \\ +-sin(\Pitch) & 0 & cos(\Pitch) +\end{pmatrix} +\end{equation} +\begin{equation} +\ra r = \begin{pmatrix} p\\q\\r \end{pmatrix} = +\begin{pmatrix} +- \sin (\Roll) \dot{\Yaw} + \dot{\Roll} \\ +\sin (\Roll) \cos (\Pitch) \dot{\Yaw} + \cos (\Roll) \dot{\Pitch} \\ +\cos (\Roll) \cos (\Pitch) \dot{\Yaw} - \sin (\Roll) \dot{\Pitch} \\ +\end{pmatrix} +\end{equation} +\inHfile{INT32\_RATES\_OF\_EULERS\_DOT(r, e, ed)}{pprz\_algebra\_int} +\inHfile{INT32\_RATES\_OF\_EULERS\_DOT\_321(r, e, ed)}{pprz\_algebra\_int} diff --git a/doc/pprz_algebra/transformations/matrix2euler.tex b/doc/pprz_algebra/transformations/matrix2euler.tex new file mode 100644 index 0000000000..5aafcacc31 --- /dev/null +++ b/doc/pprz_algebra/transformations/matrix2euler.tex @@ -0,0 +1,19 @@ +\mynote{This is only for the 321-convention} +The rotation matrix from euler angles is known +\begin{equation} +\mat R_m = \begin{pmatrix} +cos(\Pitch)cos(\Yaw) & cos(\Pitch)sin(\Yaw) & -sin(\Pitch) \\ +sin(\Roll)sin(\Pitch)cos(\Yaw) - cos(\Roll)cos(\Yaw) & sin(\Roll)sin(\Pitch)sin(\Yaw) + cos(\Roll)cos(\Yaw) & sin(\Roll)cos(\Pitch) \\ +cos(\Roll)sin(\Pitch)cos(\Yaw) + sin(\Roll)sin(\Yaw) & cos(\Roll)sin(\Pitch)sin(\Yaw) - sin(\Roll)cos(\Yaw) & cos(\Roll)cos(\Pitch) +\end{pmatrix} +\end{equation} +and the extraction is done vice versa. +\begin{equation} +\eu e = \begin{pmatrix}\Roll \\ \Pitch \\ \Yaw \end{pmatrix} = +\begin{pmatrix} +\arctan2(r_{23}, r_{33}) \\ +-\arcsin(r_{13}) \\ +\arctan2(r_{12}, r_{11}) +\end{pmatrix} +\end{equation} +\inHfile{INT32\_EULERS\_OF\_RMAT(e, rm)}{pprz\_algebra\_int} diff --git a/doc/pprz_algebra/transformations/matrix2quaternion.tex b/doc/pprz_algebra/transformations/matrix2quaternion.tex new file mode 100644 index 0000000000..a3e61089af --- /dev/null +++ b/doc/pprz_algebra/transformations/matrix2quaternion.tex @@ -0,0 +1,46 @@ +Since the construction of a matrix from a quaternion is known +\begin{equation} +\mat R_m = \begin{pmatrix} +1-2(q_y^2 + q_z^2) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ +2(q_xq_y + q_iq_z) & 1-2(q_x^2 + q_z^2) & 2(q_yq_z - q_iq_x) \\ +2(q_xq_z - q_iq_y) & 2(q_yq_z+q_iq_x) & 1-2(q_x^2 + q_y^2) +\end{pmatrix}, +\end{equation} +the extraction of a quaternion is done vice versa. But there are obviously many opportunities to extract the quaternion. They differ in the way which element of the quaternion is extracted from the diaognal elements $r_{11}$, $r_{22}$ and $r_{33}$ of the matrix. +\begin{equation} +1 = q_i^2+q_x^2+q_y^2+q_z^2 +\end{equation} +\textbf{First case} +\begin{eqnarray} +\zeta = \sqrt{1 + (r_{11}+r_{22}+r_{33})} = \sqrt{1 + (3q_i^2 -q_x^2 -q_y^2 -q_z^2)} = \sqrt{4 q_i^2} \\ +q_i = \tfrac 1 2 \zeta \\ +q_x = \tfrac 1 {2 \zeta} (r_{23}-r_{32}) \\ +q_y = \tfrac 1 {2 \zeta} (r_{31}-r_{13}) \\ +q_z = \tfrac 1 {2 \zeta} (r_{12}-r_{21}) +\end{eqnarray} +\textbf{Second case} +\begin{eqnarray} +\zeta = \sqrt{1 + (r_{11}-r_{22}-r_{33})} = \sqrt{1 + (-q_i^2+3q_x^2 -q_y^2 -q_z^2)} = \sqrt{4 q_x^2} \\ +q_i = \tfrac 1 {2 \zeta} (r_{23}-r_{32}) \\ +q_x = \tfrac 1 2 \zeta \\ +q_y = \tfrac 1 {2 \zeta} (r_{12}+r_{21}) \\ +q_z = \tfrac 1 {2 \zeta} (r_{31}+r_{13}) +\end{eqnarray} +\textbf{Third case} +\begin{eqnarray} +\zeta = \sqrt{1 + (-r_{11}+r_{22}-r_{33})} = \sqrt{1 + (-q_i^2 -q_x^2+3q_y^2 -q_z^2)} = \sqrt{4 q_y^2} \\ +q_i = \tfrac 1 {2 \zeta} (r_{31}-r_{13}) \\ +q_x = \tfrac 1 {2 \zeta} (r_{12}+r_{21}) \\ +q_y = \tfrac 1 2 \zeta \\ +q_z = \tfrac 1 {2 \zeta} (r_{23}+r_{32}) +\end{eqnarray} +\textbf{Fourth case} +\begin{eqnarray} +\zeta = \sqrt{1 + (-r_{11}-r_{22}+r_{33})} = \sqrt{1 + (-q_i^2 -q_x^2 -q_y^2+3q_z^2)} = \sqrt{4 q_z^2} \\ +q_i = \tfrac 1 {2 \zeta} (r_{12}-r_{21}) \\ +q_x = \tfrac 1 {2 \zeta} (r_{31}+r_{13}) \\ +q_y = \tfrac 1 {2 \zeta} (r_{23}+r_{32}) \\ +q_z = \tfrac 1 2 \zeta +\end{eqnarray} +All are mathematicaly equivalent but numerically different. To avoid complex numbers and singularities the case with the biggest $\zeta$ should be choosen. +\inHfile{INT32\_QUAT\_OF\_RMAT(q, r)}{pprz\_algebra\_int} \ No newline at end of file diff --git a/doc/pprz_algebra/transformations/quaternion2euler.tex b/doc/pprz_algebra/transformations/quaternion2euler.tex new file mode 100644 index 0000000000..40cf8a320d --- /dev/null +++ b/doc/pprz_algebra/transformations/quaternion2euler.tex @@ -0,0 +1,26 @@ +This is done by constructing a rotational matrix out of a quaternion (note: not all elements need to be generated), +\begin{equation} +\mat R_m = \begin{pmatrix} +1-2(q_y^2 + q_z^2) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ + & & 2(q_yq_z - q_iq_x) \\ + & & 1-2(q_x^2 + q_y^2) +\end{pmatrix}, +\end{equation} +which is equivalent to a rotational matrix, that is constructed from euler angles +\begin{equation} +\mat R_m = \begin{pmatrix} +cos(\Pitch)cos(\Yaw) & cos(\Pitch)sin(\Yaw) & -sin(\Pitch) \\ + & & sin(\Roll)cos(\Pitch) \\ + & & cos(\Roll)cos(\Pitch) +\end{pmatrix}. +\end{equation} +The euler angles are then +\begin{equation} +\eu e = \begin{pmatrix}\Roll \\ \Pitch \\ \Yaw \end{pmatrix} = +\begin{pmatrix} +\arctan2(r_{23}, r_{33}) \\ +-\arcsin(r_{13}) \\ +\arctan2(r_{12}, r_{11}) +\end{pmatrix} +\end{equation} +\inHfile{INT32\_EULERS\_OF\_QUAT(e, q)}{pprz\_algebra\_int} diff --git a/doc/pprz_algebra/transformations/quaternion2matrix.tex b/doc/pprz_algebra/transformations/quaternion2matrix.tex new file mode 100644 index 0000000000..8f416a004b --- /dev/null +++ b/doc/pprz_algebra/transformations/quaternion2matrix.tex @@ -0,0 +1,13 @@ +The most common definition for this transformation is +\begin{equation} +\mat R_m = \begin{pmatrix} +1-2(q_y^2 + q_z^2) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ +2(q_xq_y + q_iq_z) & 1-2(q_x^2 + q_z^2) & 2(q_yq_z - q_iq_x) \\ +2(q_xq_z - q_iq_y) & 2(q_yq_z+q_iq_x) & 1-2(q_x^2 + q_y^2) +\end{pmatrix}. +\end{equation} + +\inHfile{INT32\_RMAT\_OF\_QUAT(rm, q)}{pprz\_algebra\_int} +\inHfile{FLOAT\_RMAT\_OF\_QUAT(rm, q)}{pprz\_algebra\_float} +\mynote{I called the quicker function "INT32\_RMAT\_OF\_QUAT\_QUICKER"} + diff --git a/doc/pprz_algebra/transformations/rates2eulerdot.tex b/doc/pprz_algebra/transformations/rates2eulerdot.tex new file mode 100644 index 0000000000..fd44fe5317 --- /dev/null +++ b/doc/pprz_algebra/transformations/rates2eulerdot.tex @@ -0,0 +1,30 @@ +The transformation from euler angles derivative to rates can be written as a matrix multiplication +\begin{equation} +\begin{pmatrix} p\\q\\r \end{pmatrix} = +\begin{pmatrix} +- \sin (\Roll) \dot{\Yaw} + \dot{\Roll} \\ +\sin (\Roll) \cos (\Pitch) \dot{\Yaw} + \cos (\Roll) \dot{\Pitch} \\ +\cos (\Roll) \cos (\Pitch) \dot{\Yaw} - \sin (\Roll) \dot{\Pitch} +\end{pmatrix} \Leftrightarrow \begin{pmatrix} p\\q\\r \end{pmatrix} = +\begin{pmatrix} +1 & 0 & -\sin (\Roll) \\ +0 & \cos (\Roll) & \sin (\Roll) \cos (\Pitch) \\ +0 & -\sin (\Roll) & \cos (\Roll) \cos (\Pitch) +\end{pmatrix} \multiplication \begin{pmatrix} +\dot{\Roll} \\ +\dot{\Pitch} \\ +\dot{\Yaw} +\end{pmatrix}. +\end{equation} +This can be solved easily to +\begin{equation} +\begin{pmatrix}\dot{\Roll} \\ \dot{\Pitch} \\ \dot{\Yaw} \end{pmatrix} = +\begin{pmatrix} +1 & \frac{ \sin^2 \Roll }{\cos \Pitch} & \frac{\sin \Roll \cos \Roll}{\cos \Pitch} \\ +0 & \cos \Roll & -\sin \Roll \\ +0 & \frac{\sin \Roll}{\cos \Pitch} & \frac{\cos \Roll}{\cos \Pitch} +\end{pmatrix} \multiplication \begin{pmatrix} p\\q\\r \end{pmatrix}. +\end{equation} +Please note the singularity at the \emph{gimbal lock} ($\Pitch = \pm 90^{\circ }$)! +\inHfile{INT32\_EULERS\_DOT\_OF\_RATES(ed, e, r)}{pprz\_algebra\_int} +\inHfile{INT32\_EULERS\_DOT\_321\_OF\_RATES(ed, e, r)}{pprz\_algebra\_int} diff --git a/doc/pprz_algebra/transformations/rates2quaternion.tex b/doc/pprz_algebra/transformations/rates2quaternion.tex new file mode 100644 index 0000000000..e879f002df --- /dev/null +++ b/doc/pprz_algebra/transformations/rates2quaternion.tex @@ -0,0 +1,24 @@ +This function computes the differential quaternion of measured rates after an amount of time. \\ +Let $ \ra{} $ be the measured rates, then $\norm {\ra {}}$ represents the absolute value of rates. Therefore, +\begin{equation} +\Delta \alpha = \norm {\ra {}} \multiplication \Delta t +\end{equation} +is the rotational angle. The (normalized) axis of the rotation is then +\begin{equation} +\vect v = \frac{\ra{}}{\norm {\ra {}}}. +\end{equation} +The construction of a quaternion from an axis and an angle is +\begin{equation} +\quat{} = \begin{pmatrix} +\cos \tfrac \alpha 2 \\ +\vect v \sin \tfrac \alpha 2 +\end{pmatrix}, +\end{equation} +so that the resulting quaternion of measured rates becomes +\begin{equation} +\quat{} = \begin{pmatrix} +\cos \tfrac{\norm {\ra {}} \multiplication \Delta t} 2 \\ + \frac{\ra{}}{\norm {\ra {}}} \sin \tfrac{\norm {\ra {}} \multiplication \Delta t} 2 +\end{pmatrix}. +\end{equation} +\inHfile{FLOAT\_QUAT\_DIFFERENTIAL(q\_out, w, dt)}{pprz\_algebra\_float} \ No newline at end of file diff --git a/doc/pprz_algebra/vector.tex b/doc/pprz_algebra/vector.tex new file mode 100644 index 0000000000..c5a8e99da4 --- /dev/null +++ b/doc/pprz_algebra/vector.tex @@ -0,0 +1,251 @@ +\section{Vector} +\subsection{Definition} +The main definition for every vector struct is that the values are called x, y and z (if it's a 3D-vector): +\begin{equation} +\vect v = \begin{pmatrix}x\\y\end{pmatrix} \quad oor \quad \vect v = \begin{pmatrix}x\\y\\z\end{pmatrix} +\end{equation} +It is available for the following simple types:\\ +\begin{tabular}{c|c|c} +type & struct 2D & struct 3D\\ \hline +uint16\_t & & Uint16Vect3 \\ +int16\_t & & Int16Vect3 \\ +int32\_t & Int32Vect2 & Int32Vect3 \\ +int64\_t & Int32Vect2 & Int32Vect3 \\ +float & FloatVect2 & FloatVect3 +\end{tabular} + + +\subsection{= Assigning} +\subsubsection*{$\vect{v} = \vect{0}$} +\begin{equation} + \vect v = \begin{pmatrix} 0 \\ 0 \end{pmatrix} \qquad or \qquad \vect v = \begin{pmatrix} 0 \\ 0 \\ 0 \end{pmatrix} +\end{equation} +\inHfile{INT\_VECT2\_ZERO(v)}{pprz\_algebra\_int} +\inHfile{INT\_VECT3\_ZERO(v)}{pprz\_algebra\_int} +\inHfile{INT32\_VECT3\_ZERO(v)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT2\_ZERO(v)}{pprz\_algebra\_float} +\inHfile{FLOAT\_VECT3\_ZERO(v)}{pprz\_algebra\_float} + +\subsubsection*{$\vect{a} = \transp{(x,y)}$ or $\vect a = \transp{(x,y,z)}$} +\begin{equation} + \vect a = \begin{pmatrix} x \\ y \end{pmatrix} \qquad or \qquad \vect a = \begin{pmatrix} x \\ y \\ z \end{pmatrix} +\end{equation} +\inHfile{VECT2\_ASSIGN(a, x, y)}{pprz\_algebra} +\inHfile{VECT3\_ASSIGN(a, x, y, z)}{pprz\_algebra} +\inHfile{FLOAT\_VECT2\_ASSIGN(a, x, y)}{pprz\_algebra\_float} +\inHfile{FLOAT\_VECT3\_ASSIGN(a,x,y,z)}{pprz\_algebra\_float} + + +\subsubsection*{$\vect{a} = \vect{b}$} +\begin{equation} +\vect a = \vect b +\end{equation} +\inHfile{VECT2\_COPY(a, b)}{pprz\_algebra} +\inHfile{VECT3\_COPY(a, b)}{pprz\_algebra} +\inHfile{INT32\_VECT3\_COPY(o, i)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT2\_COPY(a, b)}{pprz\_algebra\_float} + + + + +\subsection{+ Addition} +\subsubsection*{$\vect{a} += \vect{b}$} +\begin{equation} +\vect a = \vect a + \vect b +\end{equation} +\inHfile{VECT2\_ADD(a, b)}{pprz\_algebra} +\inHfile{VECT3\_ADD(a, b)}{pprz\_algebra} +\inHfile{INT32\_VECT3\_ADD(a, b)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT2\_ADD(a, b)}{pprz\_algebra\_float} + + +\subsubsection*{$\vect{c} = \vect{a} + \vect{b}$} +\begin{equation} +\vect c = \vect a + \vect b +\end{equation} +\inHfile{VECT2\_SUM(c, a, b)}{pprz\_algebra} +\inHfile{VECT3\_SUM(c, a, b)}{pprz\_algebra} +\inHfile{INT32\_VECT3\_SUM(c, a, b)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT2\_SUM(c, a, b)}{pprz\_algebra\_float} + + + +\subsection{- Subtraction} +\subsubsection*{$\vect{a} -= \vect{b}$} +\begin{equation} +\vect a = \vect a - \vect b +\end{equation} +\inHfile{VECT2\_SUB(a, b)}{pprz\_algebra} +\inHfile{VECT3\_SUB(a, b)}{pprz\_algebra} +\mynote{no INT32 vect3 sub?} +\inHfile{FLOAT\_VECT2\_SUB(a, b)}{pprz\_algebra\_float} +\inHfile{FLOAT\_VECT3\_SUB(a, b)}{pprz\_algebra\_float} + +\subsubsection*{$\vect{c} = \vect{a} - \vect{b}$} +\begin{equation} +\vect c = \vect a - \vect b +\end{equation} +\inHfile{VECT2\_DIFF(c, a, b}{pprz\_algebra} +\inHfile{VECT3\_DIFF(c, a, b)}{pprz\_algebra} +\inHfile{INT32\_VECT3\_DIFF(c, a, b)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT2\_DIFF(c, a, b)}{pprz\_algebra\_float} +\inHfile{FLOAT\_VECT3\_DIFF(c, a, b)}{pprz\_algebra\_float} + + + + + + +\subsection{$\multiplication$ Multiplication} +\subsubsection*{$\vect{v_o} = s \multiplication \vect{v_i}$ With a scalar} +\begin{equation} + \vect{v_o} = s \multiplication \vect{v_i} +\end{equation} +\inHfile{VECT2\_SMUL(vo, vi, s)}{pprz\_algebra} +\inHfile{VECT3\_SMUL(vo, vi, s)}{pprz\_algebra} +\inHfile{FLOAT\_VECT2\_SMUL(vo, vi, s)}{pprz\_algebra\_float} +\inHfile{FLOAT\_VECT3\_SMUL(vo, vi, s)}{pprz\_algebra\_float} +or with a fraction +\begin{equation} + \vect{a} = \frac{num}{den} \multiplication \vect{b} +\end{equation} +\inHfile{INT32\_VECT2\_SCALE\_2(a, b, num, den)}{pprz\_algebra\_int} +\inHfile{INT32\_VECT3\_SCALE\_2(a, b, num, den)}{pprz\_algebra\_int} + +\subsubsection*{$\vect{v_o} = \vect{v_a} \ew\multiplication \vect{v_b} $ Element-wise} +Also known as the ``Dot-Multiplication'' from MATLAB, Octave or FreeMat. +\begin{equation} + \begin{pmatrix}x_o\\y_o\\z_o\end{pmatrix} = + \begin{pmatrix}x_a \multiplication x_b\\y_a \multiplication y_b\\z_a \multiplication z_b\end{pmatrix} +\end{equation} +\inHfile{VECT3\_EW\_MUL(vo, va, vb)}{pprz\_algebra} +\mynote{Nothing for VECT2?} + +\subsubsection*{$\vect v_o = \vect v_1 \cross \vect v_2$ Cross-Product} +\begin{equation} +\vect v_o = \vect v_1 \cross \vect v_2 = \begin{pmatrix} + 0 & -z_1 & y_1 \\ + z_1 & 0 & -x_1 \\ +-y_1 & x_1 & 0 +\end{pmatrix} \multiplication \begin{pmatrix}x_2\\y_2\\z_2\end{pmatrix} +\end{equation} +\inHfile{FLOAT\_VECT3\_CROSS\_PRODUCT(vo, v1, v2)}{pprz\_algebra\_float} + +\subsubsection*{$\vect v_{out} = \mat{A} \multiplication \vect v_{in}$ With a Matrix} +\begin{equation} +\vect v_{out} = \mat A \multiplication \vect v_{in} +\end{equation} +\inHfile{MAT33\_VECT3\_MUL(vout, mat, vin)}{pprz\_algebra} +\inHfile{RMAT\_VECT3\_MUL(vout, rmat, vin)}{pprz\_algebra} +\inHfile{FLOAT\_RMAT\_VECT3\_MUL(vout, rmat, vin)}{pprz\_algebra\_float} +\begin{equation} +\vect v_{out} = \transp{\mat A} \multiplication \vect v_{in} +\end{equation} +\inHfile{MAT33\_VECT3\_TRANSP\_MUL(vout, mat, vin)}{pprz\_algebra} +For rotational matrices, with additional right shift about the decimal point position: +\begin{equation} +\vect v_b = \mat M_{a2b} \multiplication \vect v_a +\end{equation} +\inHfile{INT32\_RMAT\_VMULT(vb, m\_a2b, va)}{pprz\_algebra\_int} +With the transposed matrix +\begin{equation} +\vect v_b = \transp{\mat M_{b2a}} \multiplication \vect v_a +\end{equation} +\inHfile{INT32\_RMAT\_TRANSP\_VMULT(vb, m\_b2a, va)}{pprz\_algebra\_int} + +With choosable right-shift: +\begin{equation} +\vect v_{out} = 2^{-f} \mat M \multiplication \vect v +\end{equation} +\inHfile{INT32\_MAT33\_VECT3\_MULT(o, m, v, f)}{pprz\_algebra\_int} + +\subsubsection*{$\vect v_{out} = \quat{} \quatprod \vect v_{in}$ With a quaternion} +The quaternion is transformed to a rotational matrix and then the vector is multiplied with the matrix +\begin{equation} +\vect v_{out} = \quat{} \quatprod \vect v_{in} +\end{equation} +\begin{equation} +\vect v_{out} = \mat R_m(\quat{}) \multiplication \vect v_{in} +\end{equation} +\begin{equation} +\vect v_{out} = \begin{pmatrix} +1-2(q_y + q_z) & 2(q_xq_y-q_iq_z) & 2(q_xq_z + q_iq_y) \\ +2(q_xq_y + q_iq_z) & 1-2(q_x + q_z) & 2(q_yq_z - q_iq_x) \\ +2(q_xq_z - q_iq_y) & 2(q_yq_z+q_iq_x) & 1-2(q_x + q_y) +\end{pmatrix} + \multiplication \vect v_{in} +\end{equation} +\inHfile{INT32\_QUAT\_VMULT(v\_out, q, v\_in)}{pprz\_algebra\_int} + + + +\subsection{$\division$ Division} +\subsubsection*{$\vect{v_o} = \frac 1 s \multiplication \vect{v_i}$ With a scalar} +\begin{equation} + \vect{v_o} = \frac 1 s \multiplication \vect{v_i} +\end{equation} +\inHfile{VECT2\_SDIV(vo, vi, s)}{pprz\_algebra} +\inHfile{VECT3\_SDIV(vo, vi, s)}{pprz\_algebra} +\inHfile{INT32\_VECT3\_SDIV(a, b, s)}{pprz\_algebra\_int} + +\subsubsection*{$\vect{v_o} = \vect{v_a} \ew\division \vect{v_b} $ Element-wise} +Also known as the ``Dot-Division'' from MATLAB, Octave or FreeMat. +\begin{equation} + \begin{pmatrix}x_o\\y_o\\z_o\end{pmatrix} = + \begin{pmatrix}x_a \division x_b\\y_a \division y_b\\z_a \division z_b\end{pmatrix} +\end{equation} +\inHfile{VECT3\_EW\_DIV(vo, va, vb)}{pprz\_algebra} +\mynote{Nothing for VECT2?} + + + + + +\subsection{Other} +\subsubsection*{$\norm{\vect{v}}$ Norm} +Computes the 2-norm of a vector (the length). +\begin{equation} +n = \norm{\vect{v}} = \sqrt{\vect v \multiplication \vect v} = \sqrt{x \multiplication x + y \multiplication y} \quad or \quad \sqrt{x \multiplication x + y \multiplication y + z \multiplication z} +\end{equation} +\inHfile{INT32\_VECT2\_NORM(n, v)}{pprz\_algebra\_int} +\inHfile{INT32\_VECT3\_NORM(n, v)}{pprz\_algebra\_int} +\inHfile{FLOAT\_VECT3\_NORM(v)}{pprz\_algebra\_float} +\mynote{float differs from int!} +Alternatively you can normalize a 3D - vector directly using +\inHfile{FLOAT\_VECT3\_NORMALIZE(v)}{pprz\_algebra\_float} + +\subsubsection*{Right-Shift} +Makes an bitwise right-shift with every value. This is close to the multiplication with $ 2^{-r} $, but not the same. +\begin{equation} +\vect v_o = 2^{-r} \vect v_i +\end{equation} +\inHfile{INT32\_VECT2\_RSHIFT(o, i, r)}{pprz\_algebra\_int} + +\subsubsection*{Left-Shift} +Makes an bitwise left-shift with every value. This is close to the multiplication with $ 2^{l} $, but not the same. +\begin{equation} +\vect v_o = 2^{l} \vect v_i +\end{equation} +\inHfile{INT32\_VECT2\_LSHIFT(o, i, l)}{pprz\_algebra\_int} + +\subsubsection*{$min \leq \vect v \leq max$ Bounding} +Bounds the vector so that every value is between \textit{min} and \textit{max}. +\begin{eqnarray} +\vect v \in \mathbb{I}^2 \qquad or \qquad \vect v \in \mathbb{I}^3, \\ +\mathbb{I} = [min; max] +\end{eqnarray} +\textbf{WARNING:}\\ +The functions ``\texttt{STRIM}'' have a higher priority for the lower border. So, if $ min > max$ and a value of $ \vect v $ is between those, the value is set to min. \\ +The function ``\texttt{BOUND\_CUBE}'' does that the other way round.\\ +\inHfile{VECT2\_STRIM(v, min, max)}{pprz\_algebra} +\inHfile{VECT3\_STRIM(v, min, max)}{pprz\_algebra} +\inHfile{VECT3\_BOUND\_CUBE(v, min, max)}{pprz\_algebra} +\mynote{VECT3\_STRIM and VECT3\_BOUND\_CUBE do nearly the same.} + +\subsubsection*{$\vect{v}_{min} \leq \vect v \leq \vect{v}_{max}$ Bounding} +Ensures that +\begin{equation} +\vect v_{min} \leq \vect v \leq \vect v_{max} \Leftrightarrow \begin{pmatrix} x_{min} \\ y_{min} \\ z_{min} \end{pmatrix} \leq \begin{pmatrix} x \\ y \\ z \end{pmatrix} \leq \begin{pmatrix} x_{max} \\ y_{max} \\ z_{max} \end{pmatrix} +\end{equation} +\inHfile{VECT3\_BOUND\_BOX(v, v\_min, v\_max}{pprz\_algebra} +\mynote{Nothing for VECT2?} \ No newline at end of file