mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-23 04:45:37 +08:00
added small documentation for the initialisation of the kalman-filter
This commit is contained in:
Martin Dieblich
@@ -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}
|
||||
Binary file not shown.
@@ -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}
|
||||
@@ -1,4 +1,6 @@
|
||||
#include "estimate_attitude.h"
|
||||
#include <stdio.h>
|
||||
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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<double, 12, 1> 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);
|
||||
}
|
||||
|
||||
@@ -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<<VI_GPS_DATA_VALID))
|
||||
|
||||
|
||||
|
||||
@@ -35,9 +37,11 @@ int main(int argc, char** argv) {
|
||||
struct raw_log_entry e;
|
||||
ssize_t nb_read = read(raw_log_fd, &e, sizeof(e));
|
||||
if (nb_read != sizeof(e)) break;
|
||||
print_raw_log_entry(&e);
|
||||
if(GPS_READY(e.data_valid)){
|
||||
print_raw_log_entry(&e);
|
||||
printf("\n");
|
||||
}
|
||||
//printf("%f %f %f %f", e.time, e.gyro.p, e.gyro.q, e.gyro.r);
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
Reference in New Issue
Block a user