diff --git a/sw/airborne/fms/libeknav/doc/content.tex b/sw/airborne/fms/libeknav/doc/content.tex new file mode 100644 index 0000000000..6116042a37 --- /dev/null +++ b/sw/airborne/fms/libeknav/doc/content.tex @@ -0,0 +1,167 @@ +\section{ENU to NED transformations} +I had the problem very often that I have to transform form ENU no NED. The simple conversion: ``Flip x and y and negate z'' doesn't work for quaternions or if you want to use matrix algebra. + +\subsection{Matrix} +Flipping x and y and negating z is easy to express as a matrix: +\begin{equation} +R_{ENU2NED} = \begin{pmatrix} +0 & 1 & 0 \\ +1 & 0 & 0 \\ +0 & 0 & -1 +\end{pmatrix} +\end{equation} +This works in both directions, since $ R_{ENU2NED} = \transp R_{ENU2NED} $. + +\subsection{Quaternion} +It's easy to compute a quaternion out of the above rotation matrix. +\begin{equation} +\quat{ENU2NED} = \frac{1}{\sqrt 2} \begin{pmatrix} +0 \\ 1 \\ 1 \\ 0 +\end{pmatrix} = \frac{1}{\sqrt 2} (i + j) +\end{equation} +This makes sense, since the real value = 0 represents a rotation about $180^{\circ}$ and the three values for the axis $ \vect v = \transp{\begin{pmatrix}1&1&0\end{pmatrix}}$ represent the axis of rotation. + +\subsubsection*{Transforming a quaternion between ENU/NED} +If you want to cahnge a quaternion from NED to ENU or vice versa. It's not totally simple like for vectors. \\ +If your quaternion consist of the values: +\begin{equation} +\quat{ECEF2NED} = \begin{pmatrix}a \\ b \\ c\\d\end{pmatrix} +\end{equation} +Then a transformation to ENU (NED) is made as following: +\begin{equation} +\quat{ECEF2NED} = \frac{1}{\sqrt 2} \begin{pmatrix}-b-c \\ a+d \\ a-d\\-b+c\end{pmatrix} +\end{equation} + +Pay attention doing it twice! The multiplication of an NED to ENU quaternion with itself leads to +\begin{equation} +\quat{ECEF2NED} \quatprod \quat{ECEF2NED} \quatprod \begin{pmatrix}a \\ b \\ c\\d\end{pmatrix}= \begin{pmatrix}-a \\ -b \\ -c\\-d\end{pmatrix} . +\end{equation} +This is logically the same rotation, but mathemaically a different quaternion. So don't be confused if all values are negative :-) + +\section{Initialisation} + +\subsection{What about the standard deviation?} +\begin{figure}[h!]\begin{center} + \begin{tikzpicture}[->,>=stealth',shorten >=1pt,auto,node distance=2cm, + semithick] + \tikzstyle{every state}=[draw=black,text=white] + + \node (A) {Noise}; + \node (B) [below of=A] {Measurement}; + \node (C) [below of=B] {attitude profile matrix}; + \node (D) [below of=C] {``K'' - matrix}; + \node (E) [below of=D] {Eigenvector}; + \node (F) [below of=E] {Euler angle}; + + \path (A) edge node {Gaussian noise} (B) + (B) edge node {weight of a single measurement} (C) + (C) edge node {(see the section below)} (D) + (D) edge node {computational error} (E) + (E) edge node {computational error} (F); + + \end{tikzpicture} + \caption{Propagation of uncertainty} + \label{Propagation of uncertainty} +\end{center}\end{figure} +First of all, every sensor (accelerometers $\vect a$ and magnetometers $\vect m$) has gaussian noise, that can be expressed as an additive error: +\begin{equation} +\vect a + \vect{\sigma_a} \quad \quad \vect m + \vect{\sigma_m} +\end{equation} +It can be asssumed that the error follows a standard deviation (has zero mean and is time-invariant). +The attitude profile matrix $ \mat B $ is the sum of the measurements with specific weights. +\begin{equation}\label{attitude profile matrix} +\mat B = \sum_{k=1}^n w_k \cdot \vect{W}_k \cdot \transp{\vect{V}_k} = w_a \sum_{k=1}^{n_a} \frac{\vect a_k}{\norm{a_k}} \cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect m_k}{\norm{m_k}} \cdot \transp{\vect{h}} +\end{equation} +$n$ is the number of measurements, $w_k$ is the specific weight of a measurement, $\vect{W}_k$ the measured vector and $\vect{V}_k$ the reference direction, which belongs to the measured direction. Therefore $n_a$ is the number of acceleration measurements, $w_a$ is the (constant) weight of the acceleration measurements, $\vect{a}_k$ is a single acceleration observation and $\vect{g}$ is the gravity. $\vect{a}_k$ becomes normed. Similar for the magnetometer weight $w_m$, measurement $\vect {m}_k$, the magnetic field $\vect h$ and the amount of magnetometer measurements $n_m$. See the next section how the weight should be choosen. + +The resulting error is +\begin{equation} +\mat{\sigma_B} = \frac{n_a}{f_a} \frac{1}{\norm g_2} \vect{\sigma_a}\transp{\vect{g}} + \frac{n_m}{f_m} \frac{1}{\norm h_2} \vect{\sigma_m}\transp{\vect{m}} +\end{equation} + +The error for the ``K''-matrix is easy to get by inserting $ \mat B + \mat {\sigma_B} $ into +\begin{equation} +\mat K = \begin{bmatrix} +trace(\mat B) & \transp{\vect Z} \\ +\vect Z & \mat B + \transp{\mat B} - trace(\mat B) \mat I +\end{bmatrix} +\end{equation} + + + + + + + + + + + +\subsection{choosing the best weight for the attitude profile matrix} +If you replace the single measurements in equation (\ref{attitude profile matrix}) with the real (and normed) measurements +\begin{equation} +\frac{\vect a_k + \vect{\sigma_a}}{\norm{a_k}}_2 \quad \quad \frac{\vect m_k + \vect{\sigma_m}}{\norm{m_k}}_2 +\end{equation} +and assume that $\mat B$ has an error $\mat B +\mat{\sigma_B} $, you will get +\begin{equation} +\mat B +\mat{\sigma_B} = w_a \sum_{k=1}^{n_a} \frac{\vect a_k + \vect{\sigma_a}}{\norm{a_k}_2} \cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect m_k + \vect{\sigma_m}}{\norm{m_k}_2} \cdot \transp{\vect{h}} +\end{equation} +\begin{equation} +\mat B +\mat{\sigma_B} = w_a \sum_{k=1}^{n_a} \frac{\vect a_k}{\norm{a_k}}_2 \cdot \transp{\vect{g}} + \frac{\vect{\sigma_a}}{\norm{a_k}}_2 \cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect m_k}{\norm{m_k}}_2 \cdot \transp{\vect{h}} + \frac{\vect{\sigma_m}}{\norm{m_k}}_2 \cdot \transp{\vect{h}} +\end{equation} +\begin{equation} +\mat B +\mat{\sigma_B} = \underbrace{w_a \sum_{k=1}^{n_a} \frac{\vect a_k}{\norm{a_k}}_2\cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect m_k}{\norm{m_k}}_2 \cdot \transp{\vect{h}}}_{\mat B} + w_a \sum_{k=1}^{n_a} \frac{\vect{\sigma_a}}{\norm{a_k}}_2 \cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect{\sigma_m}}{\norm{m_k}}_2 \cdot \transp{\vect{h}} +\end{equation} +\begin{equation} +\mat{\sigma_B} = w_a \sum_{k=1}^{n_a} \frac{\vect{\sigma_a}}{\norm{a_k}}_2 \cdot \transp{\vect{g}} + w_m \sum_{k=1}^{n_m} \frac{\vect{\sigma_m}}{\norm{m_k}}_2 \cdot \transp{\vect{h}} +\end{equation} +$\norm{a_k}_2$ and $\norm{m_k}_2$ shouldn't vary that much and can be assumed as constant ($\norm{a}_2$ and $\norm{m}_2$). The equation reduces to: +\begin{equation} +\mat{\sigma_B} = w_a n_a \frac{\vect{\sigma_a}}{\norm{a}_2}\cdot \transp{\vect{g}} + w_m n_m \frac{\vect{\sigma_m}}{\norm{m}_2} \cdot \transp{\vect{h}} +\end{equation} + +It would be nice, if it's possible to reduce this to a single value. To do that, we need a matrix norm. In this case, I choosed the Frobenius Norm: +\begin{align} +\norm{\mat{\sigma_B}}_{F} &= \norm{w_a n_a \frac{\vect{\sigma_a}}{\norm{a}_2}\cdot \transp{\vect{g}} + w_m n_m \frac{\vect{\sigma_m}}{\norm{m}_2} \cdot \transp{\vect{h}}}_{F} \\ +&\le \norm{w_a n_a \frac{\vect{\sigma_a}}{\norm{a}_2}\cdot \transp{\vect{g}}}_{F} + \norm{w_m n_m \frac{\vect{\sigma_m}}{\norm{m}_2} \cdot \transp{\vect{h}}}_{F} \\ +&= w_a n_a \frac{1}{\norm{a}_2}\cdot \norm{\vect{\sigma_a} \transp{\vect{g}}}_{F} + w_m n_m \frac{1}{\norm{m}_2} \cdot \norm{\vect{\sigma_m} \transp{\vect{h}}}_{F} +\end{align} + +It is straight-forward to proove that $ \norm{\vect a \transp{\vect b}}_F = \norm a_2 \cdot \norm b_2 $ +\begin{equation} +\norm{\mat{\sigma_B}}_{F} \le w_a n_a \frac{\norm g_2}{\norm a_2}\cdot \norm{\sigma_a}_2 + w_m n_m \frac{\norm h_2}{\norm m_2} \cdot \norm{\sigma_m}_2 +\end{equation} + +As you can see, the uncertainty depends on the following parameters: +\begin{itemize} +\item The weight of a measurement $w_a$ and $w_m$. +\item The number of measurements $n_a$ and $n_m$. +\item Something that I call a "measurement gain", $\frac{\norm{a}_2}{\norm{g}_2}$ and $\frac{\norm{m}_2}{\norm{h}_2}$, since it's the ratio between the true value and the measured value. +\item The maximum of the error $\sigma_a$ and $\sigma_m$. +\end{itemize} + +This is not what I want. I don't want the error grow with the number of measurements or with the gain, that is related to the measruement device. If I choose +\begin{equation} +w_a = \frac{\norm a_2}{n_a \cdot \norm g_2} \quad and \quad w_m = \frac{\norm m_2}{n_m \cdot \norm h_2} +\end{equation} +I get something like +\begin{equation} +\norm{\mat{\sigma_B}}_{F} \le \norm{\sigma_a}_2 + \norm{\sigma_m}_2 \quad , +\end{equation} +which looks much better. For the Frobenius norm of the attitude profile matrix the choosen weight leads to +\begin{equation} +\norm{\mat B}_{F} \le \frac{1}{n_a} \sum_{k=1}^{n_a} \norm{a_k}_2 + \frac{1}{n_m} \sum_{k=1}^{n_m} \norm{m_k}_2 \quad . +\end{equation} +That is an acceptable fact, since it helps to keep the matrix bound. +But because I want to do live-update of the attitude profile matrix I don't know the real amount of measurements $n_a$ and $n_m$. But I know the measurement frequencies $f_a$ and $f_m$, which are directly linked to them ($f = \tfrac n T $). So my final decision for the measurement weight is +\begin{equation} +w_a = \frac{\norm a_2}{f_a \cdot \norm g_2} \quad and \quad w_m = \frac{\norm m_2}{f_m \cdot \norm h_2} \quad . +\end{equation} +The resulting error is then +\begin{equation} +\mat{\sigma_B} = \frac{n_a}{f_a} \frac{1}{\norm g_2} \vect{\sigma_a}\transp{\vect{g}} + \frac{n_m}{f_m} \frac{1}{\norm h_2} \vect{\sigma_m}\transp{\vect{m}} +\end{equation} +or +\begin{equation} +\norm{\mat{\sigma_B}}_{F} \le \frac{n_a}{f_a} \norm{\sigma_a}_2 + \frac{n_m}{f_m} \norm{\sigma_m}_2 \quad , +\end{equation} \ No newline at end of file diff --git a/sw/airborne/fms/libeknav/doc/headfile.pdf b/sw/airborne/fms/libeknav/doc/headfile.pdf new file mode 100644 index 0000000000..3684d041da Binary files /dev/null and b/sw/airborne/fms/libeknav/doc/headfile.pdf differ diff --git a/sw/airborne/fms/libeknav/doc/headfile.tex b/sw/airborne/fms/libeknav/doc/headfile.tex new file mode 100644 index 0000000000..a259f21a28 --- /dev/null +++ b/sw/airborne/fms/libeknav/doc/headfile.tex @@ -0,0 +1,100 @@ +\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} \\ +%} +%\newcommand{\inCfile}[2]{ +% Function \texttt{#1} in File \texttt{#2.c} \\ +%} + +\newcommand{\inHfile}[2]{Function \texttt{#1}\\ +\indent in File \texttt{#2.h} \\} + +\newcommand{\inCfile}[2]{Function \texttt{#1}\\ +\indent in File \texttt{#2.c} \\} + +%\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{\Vert{#1}\Vert}} +\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{\lon}[0]{\ensuremath{\lambda}} +\newcommand{\lat}[0]{\ensuremath{\varphi}} + + +\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 +This file is just a bunch of random stuff and notes for myself (and others, of course). +\include{content} +% +\end{document} \ No newline at end of file diff --git a/sw/airborne/fms/libeknav/estimate_attitude.c b/sw/airborne/fms/libeknav/estimate_attitude.c index e4f5c24eed..95f7ed1e46 100644 --- a/sw/airborne/fms/libeknav/estimate_attitude.c +++ b/sw/airborne/fms/libeknav/estimate_attitude.c @@ -1,4 +1,6 @@ #include "estimate_attitude.h" +#include + struct DoubleMat44 square_skaled(struct DoubleMat44 A){ double _1_max = 1/INFTY_NORM16(A); @@ -58,6 +60,7 @@ DoubleVect4 dominant_Eigenvector(struct DoubleMat44 A, unsigned int maximum_iter x_k = x_kp1; } + printf("KONVERGE: %i\n", k); return x_k; } diff --git a/sw/airborne/fms/libeknav/libeknav_from_log.cpp b/sw/airborne/fms/libeknav/libeknav_from_log.cpp index bb8f915704..421136555b 100644 --- a/sw/airborne/fms/libeknav/libeknav_from_log.cpp +++ b/sw/airborne/fms/libeknav/libeknav_from_log.cpp @@ -50,7 +50,7 @@ static void main_init(void) { set_reference_direction(); } - +#if 0 static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){ int imu_measurements = 0, // => Gyro + Accel magnetometer_measurements = 0, @@ -109,6 +109,68 @@ static struct raw_log_entry first_entry_after_initialisation(int file_descriptor orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body); return e; } +#else +static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){ + int imu_measurements = 0, // => Gyro + Accel + magnetometer_measurements = 0, + gps_measurements = 0; // only the position + + struct DoubleMat33 attitude_profile_matrix; + struct Orientation_Measurement gravity, + magneto, + fake; + struct DoubleQuat q_ned2body; + + /* Prepare the attitude profile matrix */ + FLOAT_MAT33_ZERO(attitude_profile_matrix); + + /* set the gravity measurement */ + VECT3_ASSIGN(gravity.reference_direction, 0,0,-1); + gravity.weight_of_the_measurement = 1; + + /* set the magneto - measurement */ + EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction); + magneto.weight_of_the_measurement = 1; + + uint8_t read_ok = 1; + struct raw_log_entry e = next_GPS(file_descriptor); + + while( (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, gps_measurements) ){ + if(IMU_READY(e.data_valid)){ + imu_measurements++; + + // update the estimated bias + bias_0 = NEW_MEAN(bias_0, RATES_AS_VECTOR3D(e.gyro), imu_measurements); + + // update the attitude profile matrix + VECT3_COPY(gravity.measured_direction,e.accel); + add_orientation_measurement(&attitude_profile_matrix, gravity); + } + if(MAG_READY(e.data_valid)){ + magnetometer_measurements++; + // update the attitude profile matrix + VECT3_COPY(magneto.measured_direction,e.mag); + add_orientation_measurement(&attitude_profile_matrix, magneto); + + // now, generate fake measurement with the last gravity measurement + fake = fake_orientation_measurement(gravity, magneto); + add_orientation_measurement(&attitude_profile_matrix, fake); + } + if(GPS_READY(e.data_valid)){ + gps_measurements++; + // update the estimated bias + pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.ecef_pos)/100, gps_measurements); + } + + e = read_raw_log_entry(file_descriptor, &read_ok); + } + q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6); + //printf("NED2BODY Quat: % 7.2f % 7.2f % 7.2f % 7.2f\n", q_ned2body.qi, q_ned2body.qx, q_ned2body.qy, q_ned2body.qz); + orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body); + return e; +} +#endif + static void main_run_from_file(int file_descriptor, struct raw_log_entry first_entry){ struct raw_log_entry e = first_entry; @@ -180,8 +242,11 @@ static void init_ins_state(void){ printf("\n"); Matrix diag_cov; + double angle_cov = 10*M_PI/180; + diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 , - Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 , + //Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 , + Vector3d::Ones() * angle_cov * angle_cov , Vector3d::Ones() * pos_cov_0 * pos_cov_0 , Vector3d::Ones() * speed_cov_0 * speed_cov_0; ins.cov = diag_cov.asDiagonal(); @@ -219,7 +284,7 @@ Quaterniond ecef2body_from_pprz_ned2body(Vector3d ecef_pos, struct DoubleQuat q_ DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned); - FLOAT_QUAT_COMP(q_ecef2body, q_ecef2ned, q_ned2body); + FLOAT_QUAT_COMP_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body); return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body); } diff --git a/sw/airborne/fms/libeknav/raw_log_to_ascii.c b/sw/airborne/fms/libeknav/raw_log_to_ascii.c index 4d58697f74..62cb311007 100644 --- a/sw/airborne/fms/libeknav/raw_log_to_ascii.c +++ b/sw/airborne/fms/libeknav/raw_log_to_ascii.c @@ -16,6 +16,8 @@ void print_raw_log_entry(struct raw_log_entry*); void build_fake_log(void); #define PRT(a) printf("%f ", a); +#define VI_GPS_DATA_VALID 2 +#define GPS_READY(data_valid) (data_valid & (1<