Finished documentation for pprz_algebra. There are still many notes in it, which can be easily removed in the headfile (command: mynote)

doc/pprz_algebra/Makefile

@@ -1,8 +1,8 @@
-doc_libeknav.pdf: headfile.tex
+doc_pprz_algebra.pdf: headfile.tex
 	pdflatex $<
 bib:
 	bibtex headfile
 
 clean:
-	rm -f *~ *.aux *.bbl *.blg *.log *.out *.toc *.pdf *.dvi *.ps
+	rm -f *~ *.aux *.bbl *.blg *.log *.out *.toc *.dvi *.ps
 	find . -name '*~' -exec rm -f {} \;

doc/pprz_algebra/euler.tex

@@ -9,7 +9,8 @@ It is available for the following simple types:\\
 type		& struct		\\ \hline
 int16\_t	& Int16Eulers	\\
 int32\_t	& Int32Eulers	\\
-float		& FloatEulers
+float		& FloatEulers	\\
+double		& DoubleEulers
 \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:
@@ -23,6 +24,7 @@ Because there are many definitions of euler angles (some say 12, wikipedia says
 \eu v = \begin{pmatrix} 0 \\ 0 \\ 0 \end{pmatrix}
 \end{equation}
 \inHfile{INT\_EULERS\_ZERO(e)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_EULERS\_ZERO(e)}{pprz\_algebra\_float}
 
 \subsubsection*{$\eu a = \transp{(\Roll,\Pitch,\Yaw)}$}
 \begin{equation}
@@ -122,6 +124,12 @@ or between
 \end{equation}
 \inHfile{INT32\_COURSE\_NORMALIZE(a)}{pprz\_algebra\_int}
 
+\subsubsection*{$\norm{\eu{e}} $ Norm}
+Calculates the 2-norm
+\begin{equation}
+\norm{\norm{\eu{e}}}_2 = \sqrt{\Roll^2+\Pitch^2+\Yaw^2}
+\end{equation}
+\inHfile{FLOAT\_EULERS\_NORM(e)}{pprz\_algebra\_float}
 
 \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}.

doc/pprz_algebra/introduction.tex

@@ -1,6 +1,6 @@
 \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.
+The Conversion between FLOAT and REAL to BFP(binary floating point)and vice versa 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}}.
@@ -112,6 +112,7 @@ 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	\\
+NORM		& \YES	& \YES	& \NO	& \YES	& \YES	& \YES	& \YES	\\
 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

doc/pprz_algebra/matrix.tex

@@ -18,7 +18,8 @@ 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
+float		& FloatMat33	& FloatRMat		\\
+double		& DoubleMat33	& DoubleRMat
 \end{tabular}
 
 

doc/pprz_algebra/quaternion.tex

@@ -10,7 +10,8 @@ It is available for the following simple types:\\
 \begin{tabular}{c|c}
 type		& struct		\\ \hline
 int32\_t	& Int32Quat		\\
-float		& FloatQuat
+float		& FloatQuat		\\
+double		& DoubleQuat		
 \end{tabular}
 
 
@@ -71,6 +72,7 @@ Sets a quaternion to the identity rotation (no rotation).
 
 
 \subsection{$\multiplication$ Multiplication}
+\mynote{FLOAT\_QUAT\_ROTATE\_FRAME is stil missing. The function seems useless to me.}
 \subsubsection*{$\quat{o} = s \multiplication \quat{i}$ With a scalar}
 \begin{equation}
 	\quat{o} = s \multiplication \quat{i}
@@ -134,6 +136,9 @@ Please note that due to the fact that it's done very often, the functions above
 
 
 \subsection{Transformation to Quaternions}
+\subsubsection*{from an axis and an angle}
+\input{transformations/axisangle2quaternion}
+
 \subsubsection*{from euler angles}
 \input{transformations/euler2quaternion}
 
@@ -169,4 +174,42 @@ It is possible to invert the quaternion if its real value is negative
 \end{matrix} \right.
 \end{equation}
 \inHfile{INT32\_QUAT\_WRAP\_SHORTEST(q)}{pprz\_algebra\_int}
-\inHfile{FLOAT\_QUAT\_WRAP\_SHORTEST(q)}{pprz\_algebra\_float}
+\inHfile{FLOAT\_QUAT\_WRAP\_SHORTEST(q)}{pprz\_algebra\_float}
+
+\subsection*{Derivative}
+Calculates the derivative of a quaternion using the rates. The resulting quaternion still needs to be normalized
+\begin{equation}
+\dot{\quat{}} = -\tfrac 1 2 \mat \Omega(\ra{}) \quatprod \quat{}
+\end{equation}
+\begin{equation}
+\dot{\quat{}} = -\tfrac 1 2 \begin{pmatrix} 
+  0		&  \ra p &  \ra q &  \ra r	\\
+-\ra p	&	0	 & -\ra r &  \ra q	\\
+-\ra q	&  \ra r &	0	  & -\ra p	\\
+-\ra r	& -\ra q &  \ra p &		0
+\end{pmatrix}
+\multiplication \quat{}
+\end{equation}
+\inHfile{FLOAT\_QUAT\_DERIVATIVE(qd, r, q)}{pprz\_algebra\_float}
+You can also use a method, which slightly normalizes the quaternion by itself. The intention is that you calculate a quaternion, which represents the difference to a unit quaternion
+\begin{eqnarray}
+\Delta n = \norm{\norm{\quat{}}}_2-1 \\
+\Delta \quat {} = \Delta n \multiplication \quat{}.
+\end{eqnarray}
+Now you substract this difference from the result
+\begin{eqnarray}
+\dot{\quat{}} = -\tfrac 1 2 \mat \Omega(\ra{}) \quatprod \quat{} - \Delta \quat {} \\
+\dot{\quat{}} = -\tfrac 1 2 \mat \Omega(\ra{}) \quatprod \quat{} - \Delta n \multiplication \quat{} \\
+\dot{\quat{}} = -\tfrac 1 2 \left( 2 \Delta n \eye + \mat \Omega(\ra{}) \right) \quatprod \quat{}
+\end{eqnarray}
+leading to
+\begin{equation}
+\dot{\quat{}} = -\tfrac 1 2 \begin{pmatrix} 
+2 \Delta n&  \ra p &  \ra q &  \ra r	\\
+-\ra p	&2 \Delta n& -\ra r &  \ra q	\\
+-\ra q	&  \ra r &2 \Delta n & -\ra p	\\
+-\ra r	& -\ra q &  \ra p &2 \Delta n
+\end{pmatrix}
+\multiplication \quat{}
+\end{equation}
+\inHfile{FLOAT\_QUAT\_DERIVATIVE\_LAGRANGE(qd, r, q)}{pprz\_algebra\_float}

doc/pprz_algebra/rates.tex

@@ -9,7 +9,8 @@ It is available for the following simple types:\\
 type		& struct		\\ \hline
 int16\_t	& Int16Rates	\\
 int32\_t	& Int32Rates	\\
-float 		& FloatRates
+float 		& FloatRates	\\
+double 		& DoubleRates
 \end{tabular}
 
 \subsection{= Assigning}

doc/pprz_algebra/transformations/axisangle2quaternion.tex

@@ -0,0 +1,13 @@
+A quaternion can be easily constructed from an axis $\vect u_v$ and an angle $\alpha $ using
+\begin{equation}
+\quat {} = \begin{pmatrix}
+\cos \left( \tfrac \alpha 2 \right) \\
+\sin \left( \tfrac \alpha 2 \right) \vect u_v
+\end{pmatrix} = \begin{pmatrix}
+\cos \left( \tfrac \alpha 2 \right) \\
+\sin \left( \tfrac \alpha 2 \right) u_x \\
+\sin \left( \tfrac \alpha 2 \right) u_y \\
+\sin \left( \tfrac \alpha 2 \right) u_z
+\end{pmatrix}
+\end{equation}
+\inHfile{FLOAT\_QUAT\_OF\_AXIS\_ANGLE(q, uv, an)}{pprz\_algebra\_float}

doc/pprz_algebra/transformations/euler2matrix.tex

@@ -22,3 +22,4 @@ sin(\Pitch)cos(\Yaw) + sin(\Roll)cos(\Pitch)sin(\Yaw)	& sin(\Pitch)sin(\Yaw)-sin
 \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}
+\inHfile{DOUBLE\_RMAT\_OF\_EULERS\_312(rm, e)}{pprz\_algebra\_float}

doc/pprz_algebra/transformations/euler2quaternion.tex

@@ -12,3 +12,5 @@ In matrix notation:
 \end{pmatrix}
 \end{equation}
 \inHfile{INT32\_QUAT\_OF\_EULERS(q, e)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_QUAT\_OF\_EULERS(q, e)}{pprz\_algebra\_float}
+\inHfile{DOUBLE\_QUAT\_OF\_EULERS(q, e)}{pprz\_algebra\_double}

doc/pprz_algebra/transformations/matrix2euler.tex

@@ -17,3 +17,4 @@ and the extraction is done vice versa.
 \end{pmatrix}
 \end{equation}
 \inHfile{INT32\_EULERS\_OF\_RMAT(e, rm)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_EULERS\_OF\_RMAT(e, rm)}{pprz\_algebra\_float}

doc/pprz_algebra/transformations/matrix2quaternion.tex

@@ -43,4 +43,5 @@ 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}
+\inHfile{INT32\_QUAT\_OF\_RMAT(q, r)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_QUAT\_OF\_RMAT(q, r)}{pprz\_algebra\_float}

doc/pprz_algebra/transformations/quaternion2euler.tex

@@ -24,3 +24,5 @@ The euler angles are then
 \end{pmatrix}
 \end{equation}
 \inHfile{INT32\_EULERS\_OF\_QUAT(e, q)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_EULERS\_OF\_QUAT(e, q)}{pprz\_algebra\_float}
+\inHfile{DOUBLE\_EULERS\_OF\_QUAT(e, q)}{pprz\_algebra\_float}

doc/pprz_algebra/vector.tex

@@ -11,7 +11,8 @@ uint16\_t	& 				& Uint16Vect3	\\
 int16\_t 	& 				& Int16Vect3	\\
 int32\_t	& Int32Vect2	& Int32Vect3	\\
 int64\_t	& Int32Vect2	& Int32Vect3	\\
-float		& FloatVect2	& FloatVect3	
+float		& FloatVect2	& FloatVect3	\\
+double		& DoubleVect2	& DoubleVect3	
 \end{tabular}
 
 
@@ -67,6 +68,7 @@ float		& FloatVect2	& FloatVect3
 \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}
+\inHfile{DOUBLE\_VECT3\_SUM(c, a, b)}{pprz\_algebra\_double}
 
 
 
@@ -130,6 +132,7 @@ Also known as the ``Dot-Multiplication'' from MATLAB, Octave or FreeMat.
 \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}
+\inHfile{DOUBLE\_VECT3\_CROSS\_PRODUCT(vo, v1, v2)}{pprz\_algebra\_double}
 
 \subsubsection*{$\vect v_{out} = \mat{A} \multiplication \vect v_{in}$ With a Matrix}
 \begin{equation}
@@ -138,10 +141,12 @@ Also known as the ``Dot-Multiplication'' from MATLAB, Octave or FreeMat.
 \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}
+\inHfile{DOUBLE\_MAT33\_VECT3\_MUL(vout, mat, vin)}{pprz\_algebra\_double}
 \begin{equation}
 \vect v_{out} = \transp{\mat A} \multiplication \vect v_{in}
 \end{equation}
 \inHfile{MAT33\_VECT3\_TRANSP\_MUL(vout, mat, vin)}{pprz\_algebra}
+\inHfile{DOUBLE\_MAT33\_VECT3\_TRANSP\_MUL(vout, mat, vin)}{pprz\_algebra\_double}
 For rotational matrices, with additional right shift about the decimal point position:
 \begin{equation}
 \vect v_b = \mat M_{a2b} \multiplication \vect v_a
@@ -176,6 +181,7 @@ The quaternion is transformed to a rotational matrix and then the vector is mult
  \multiplication \vect v_{in}
 \end{equation}
 \inHfile{INT32\_QUAT\_VMULT(v\_out, q, v\_in)}{pprz\_algebra\_int}
+\inHfile{FLOAT\_QUAT\_VMULT(v\_out, q, v\_in)}{pprz\_algebra\_float}
 
 
 
@@ -248,4 +254,11 @@ Ensures that
 \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?}
+\mynote{Nothing for VECT2?}
+
+\subsubsection*{Rounding}
+Rounds the values of a double vector to integer values.
+\begin{equation}
+\vect v_{out} = rint(\vect v_{in})
+\end{equation}
+\inHfile{DOUBLE\_VECT3\_RINT(vout, vin)}{pprz\_algebra\_double}