From 5de3eafedd5c34b6096623e63560d8e9b2a9fd0b Mon Sep 17 00:00:00 2001
From: "Claudio (xjobb Martina)" <claudio@neumann.control.lth.se>
Date: Wed, 26 Sep 2018 09:39:55 +0200
Subject: [PATCH] sensor fusion wip
---
paper/main.tex | 1 +
paper/sections/04-fusion.tex | 51 +++++++++++++++++++++++++++++++++++-
2 files changed, 51 insertions(+), 1 deletion(-)
diff --git a/paper/main.tex b/paper/main.tex
index 35b5a30..2aa4e03 100644
--- a/paper/main.tex
+++ b/paper/main.tex
@@ -32,6 +32,7 @@
\usepackage{pst-geo}
\usepackage{soul}
\usepackage[english]{babel}
+\usepackage{amsmath}
% TiKz stuff
\usepackage{tikz,xstring,siunitx,pgfplots,psfrag}
diff --git a/paper/sections/04-fusion.tex b/paper/sections/04-fusion.tex
index 7575c12..3a4783e 100644
--- a/paper/sections/04-fusion.tex
+++ b/paper/sections/04-fusion.tex
@@ -12,4 +12,53 @@ Inertial measurement units (IMUs) are sensors that usually include one accelerom
The main drawback of those sensors is that, given the relative nature of the provided measurement, the ouput must be integrated. This exposes the estimation to low frequency errors introduced by the biases. By rule of thumb, those are usually considered to be growing cubically in time. For this reason inertial sensor are considered only reliable on a short time scale. The idea of GPS-IMU sensor fusion is to use the inertial sensor unit to provide continuous and low-power positioning, while the GPS, being sampled at a lower rate can compensate and not affected by biases can compensate for the low frequency erors.
-In this framework the presented model can capture the delays in the position measure of the GPS and its power consumption. This allows rigorous study on what is the optimal sampling method for the GPS sensor.
+In this framework the presented model can capture the delays in the position measure of the GPS and its power consumption. This allows rigorous study on what is the optimal (in terms of power consumption over accuracy trade-off) sampling method for the GPS sensor.
+
+\subsection{The Sensor Fusion Algorithm}
+In this section the actual implementation of the sensor fusion algorithm is discussed. The algorithm integrates two times the measurements in order to obtain the trajectory profile and, when available, uses the measurement from the GPS sensor in order to estimate and compensate the bias of the inertial estimation.
+
+\begin{figure}
+\includegraphics[scale=0.35]{images/sensor-fusion-scheme.png}
+\label{fig:sensorfusionscheme}
+\caption{Block scheme of the GPS-IMU navigation system.}
+\end{figure}
+
+The quantities we want to estimate are: the position $p(k)\in \!R^3 [m]$, the velocity $v(k)\in \!R^3 [m/s]$ and the attitude $q(k)\in \!R^4 [-]$ in quaternion representation. The measurements available from the IMU are instead: the acceleration along the three axis $s(k)\in \!R^3 [m/s]$ and the angular rates $\omega(k)\in \!R^3 [red/s]$. The integration of the inertial measurements is represented in equation \ref{eq:integration} where $\Omega(k)$ is defined in equation~\ref{eq:Omega} and $R_b^n(q(k))$ is the directional cosine matrix that rotates a vector from the body coordinate frame $b$ to the navigation coordinate frame $n$. Note that the equation includes also compensation of gravitational force $g$.
+\begin{equation}
+\begin{split}
+p(k) = & p(k-1) + T_sv(k-1) + (R_b^n(q(k-1))s(k)-g)\frac{T_s^2}{2} \\
+v(k) = & v(k-1) + (R_b^n(q(k-1))s(k)-g)T_s \\
+q(k) = & \Big(cos(0.5T_s\|\omega(k)\|)I_4+sin(0.5T_s\|\omega(k)\|)\frac{\Omega(k)}{\|\omega(k)\|}\Big)q(k-1)
+\label{eq:integration}
+\end{split}
+\end{equation}
+
+\begin{equation}
+\Omega(k) =
+\begin{bmatrix}
+ 0 & [\omega(k)]_z & -[\omega(k)]_y & [\omega(k)]_x \\
+-[\omega(k)]_z & 0 & [\omega(k)]_x & [\omega(k)]_y \\
+ [\omega(k)]_y & -[\omega(k)]_x & 0 & [\omega(k)]_z \\
+-[\omega(k)]_x & -[\omega(k)]_z & -[\omega(k)]_z & 0
+\end{bmatrix}
+\label{eq:Omega}
+\end{equation}
+
+ The equation can also be seen in the form of a dynamical system $x(k)=f(x(k-1),u(k))$, where $x(k)=[p(k),v(k),q(k)]'$ and $u(k)=[s(k),\omega(k)]'$.
+
+We now want to correct the measures from the inertial sensors. The assumption is that wo different disturbances -- shown in equation~\ref{eq:sensorerror} -- affect the measures: a slowly varying bias $\delta u(k)$ and an additive white noise $w_1(k)$ with covariance matrix $Q_1$. $\tilde{u}(k)$ is therefore the actual measurement and therefore $u(k)$ is the measurement compensated by the estimated bias that we will feed in the integration equation. The dynamics of the slowly varying bias is instead assumed to be a \emph{random walk}, as shown in equation~\ref{eq:bias}, where $w_2(k)$ is a white noise with covariance matrix $Q_2$.
+
+\begin{equation}
+\tilde{u}(k)=u(k)-\delta u(k) + w_1(k)
+\label{eq:sensorerror}
+\end{equation}
+
+\begin{equation}
+\delta u(k)= \delta u(k-1) + w_2(k)
+\label{eq:bias}
+\end{equation}
+
+
+
+
+
--
GitLab