diff --git a/paper/sections/04-fusion.tex b/paper/sections/04-fusion.tex
index 9c7facc0cd5028b533ee6d3b2ca7a7ee2e051652..dcb1d2bd279a2356b2a026620ce0414e7ddc2664 100644
--- a/paper/sections/04-fusion.tex
+++ b/paper/sections/04-fusion.tex
@@ -79,8 +79,7 @@ the identity matrix of order 4,
\label{eq:integration}
\end{equation}
%
-Finally, $\Omega$ is used to denote \todo{write me -- only a textual
- description} and is defined in Equation~\eqref{eq:Omega},
+Finally, $\Omega$ is defined as
%
\begin{equation}
\Omega(k) =
@@ -107,32 +106,62 @@ Figure~\ref{fig:sensorfusionscheme}.
\label{fig:sensorfusionscheme}
\end{figure}
-We now want to correct the estimation made with the integration of the inertial sensors by using the more realiable measurements from the GPS. The assumption is that two different disturbances affect the IMU measures: a slowly varying bias $\delta u(k)$ and an additive white noise $w_1(k)$, with covariance matrix $Q_1$, those are shown in equation~\ref{eq:sensorerror}. The actual acceleration is $u(k)$ and $\tilde{u}(k)$ is the output of the IMU that we will feed in the integration equation after compensating it for the estimated bias. The dynamics of the slowly varying bias is instead assumed to be a \emph{random walk}, 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)
+Our aim is to correct the estimation made by the INS using the more
+reliable measurements obtained using the GPS receiver. We denote with
+$u(k)$ the actual acceleration and with $\tilde{u}(k)$ the output of
+the IMU that we will feed in the integration equation (after
+compensating it for the estimated bias).
+
+We assume the following about the disturbances that affect sensor
+measurements. Two different disturbances affect the IMU measurements:
+a slowly varying bias $\delta u(k)$, and an additive white noise
+$w_1(k)$, with covariance matrix $Q_1$, as shown in
+Equation~\eqref{eq:sensorerror}. The dynamics of the slowly varying
+bias $\delta u(k)$ is a \emph{random walk}, shown in
+equation~\ref{eq:bias}, where $w_2(k)$ is a white noise with
+covariance matrix $Q_2$.
+\begin{eqnarray}
+\tilde{u}(k) & = & u(k)-\delta u(k) + w_1(k)
+\label{eq:sensorerror} \\
+\delta u(k) & = & \delta u(k-1) + w_2(k)
\label{eq:bias}
-\end{equation}
-
-The GPS sensor is instead assumed to provide a measure of the position only affected by an additive white noise $e(k)$ with covariance $R$:
-
+\end{eqnarray}
+The GPS sensor provides a measurement of the position that only
+affected by an additive white noise $e(k)$ with covariance $R$.
\begin{equation}
- \tilde{p}_{GPS}(k) = p(k) + e(k) .
+\tilde{p}_{\text{GPS}}(k) = p(k) + e(k)
\end{equation}
-The idea of the Kalman filter here implemented is to measure the discrepancy betwen the output of the INS and the GPS measure and use it to correct the position and biases estimation. The correction is made based on the so called \emph{kalman gain} that is computed online and has been proved to be optimal in case of gaussian disturbances and noises. The gain is in fact computed accordingly to the uncertainty of the quantities to be estimated, which is formally defined as their covariance matrix, usually called $\mathbf{P}$. This matrix is also updated online: its evolution is based on the linearized model of the system dynamics (i.e. how uncertainty of the previous estimation propagates to the next one) and on the disturbances and noise models (i.e. their variance).
-
-It is important to remark that if the system was linear then the covariance matrix would be updated at every step with the same system dynamics and covariances of disturbances and noises. This would apparently make the evolution of the covariance matrix deterministic. Since instead this is not the case and at every time step the system needs to be newly linearized, the evolution of the covariance matrix is not deterministic.
-
-We can now go into the details of how the kalman filter is implemented and how the correction is done. The quantities we want to estimate are: the position error (used to correct the position estimation) and the biases of the sensors (to compensate future measures). The position error is defined as the difference betwen the real value and the estimated one (represented by the variables with the hat):
-
+The Kalman filter measures the discrepancy betwen the output of the
+INS and the GPS and uses it to correct the position and biases
+estimation. The correction is based on the \emph{Kalman gain},
+computed online. This technique was proved to be optimal in case of
+gaussian disturbances and noises. In fact, the Kalman gain is computed
+according to the uncertainty of the quantities to be estimated,
+formally defined as their covariance matrix, usually denoted with
+$\mathbf{P}$. The convariance matrix is also updated
+online\footnote{If the system was linear, the covariance matrix would
+ be updated at every step with the same system dynamics and
+ covariances of disturbances and noises. This would apparently make
+ the evolution of the covariance matrix deterministic. On the
+ contrary, this is not the case here and at every time step the
+ system needs to be newly linearized. As a conclusion, the evolution
+ of the covariance matrix is not deterministic.}, its evolution is
+based on the linearized model of the system dynamics (i.e., how the
+\emph{past} estimation uncertainty propagates to the following one),
+and on the disturbances and noise models (i.e., their variance).
+
+We now provide details about how the kalman filter is implemented, and
+how it corrects the estimates. We would like to obtain an estimate
+$\hat{x}(k)$ of $x(k)$\footnote{In general, we use $\hat{\cdot}$ to
+ indicate the estimate of the variable represented by $\cdot$.}. We
+therefore need to estimate the following quantities: the position
+error $\delta x(k)$ (used to correct the position estimation) and the
+biases of the sensors $\delta u(k)$ (to compensate the estimate
+obtained using Equation~\eqref{eq:integration} for future
+measurements).
\begin{equation}
-\delta x(k) =
+\delta x(k) = x(k) - \hat{x}(k) =
\begin{bmatrix}
\delta p(k) \\
\delta v(k) \\
@@ -143,11 +172,19 @@ We can now go into the details of how the kalman filter is implemented and how t
p(k)-\hat{p}(k) \\
v(k)-\hat{v}(k) \\
\epsilon (k)
-\end{bmatrix} ,
+\end{bmatrix}.
\end{equation}
-
-where the attitude error vector $\epsilon (k)$ is defined as the the small (Euler) angle sequence that rotates the attitude vector $\hat{q}(k)$ into $q(k)$ \footnote{Formally $q(k)=\Gamma(\hat{q}(k), \epsilon (k))$, where $\Gamma(\hat{q}(k),\epsilon (k))\triangleq\{ q \in \mathbb{S}^3 | R_b^n (q(k)) = (I_3 - \left[\epsilon(k)\right]_{\times}) R_b^n (\hat{q}(k)) \}$ and $\left[a\right]_{\times}$ defines the antisymmetric matrix representation of $a$ for which $\left[a\right]_{\times} \cdot b = a \cdot b$.}. Next we can define the vector collecting all the quantities to be estimated and the vector that collects the disturbances as:
-
+Here, the attitude error vector $\epsilon(k)$ is defined as the the
+small (Euler) angle sequence that rotates the attitude vector
+$\hat{q}(k)$ into
+$q(k)$\footnote{$q(k) = \Gamma(\hat{q}(k), \epsilon(k))$, where
+ $\Gamma(\hat{q}(k),\epsilon (k)) \triangleq \{ q \in \mathbb{S}^3 |
+ R_b^n (q(k)) = (I_3 - \left[\epsilon(k)\right]_{\times}) R_b^n
+ (\hat{q}(k)) \}$ and $\left[a\right]_{\times}$ defines the
+ antisymmetric matrix representation of $a$ for which
+ $\left[a\right]_{\times} \cdot b = a \cdot b$.}. We can then define
+the vector collecting all the quantities to be estimated and the
+vector that collects the disturbances as,
\begin{equation}
\delta z(k) =
\begin{bmatrix}
@@ -155,69 +192,79 @@ where the attitude error vector $\epsilon (k)$ is defined as the the small (Eule
\delta u(k)
\end{bmatrix}
\in \mathbb{R}^{15} ,
-\end{equation}
-
-\begin{equation}
-\delta w(k) =
+\,\,\,
+w(k) =
\begin{bmatrix}
-\delta w_1(k) \\
-\delta w_2(k)
+w_1(k) \\
+w_2(k)
\end{bmatrix}
\in \mathbb{R}^{12} .
\end{equation}
-The linearized state space model, that describes the evolution of $z(k)$ vector and is needed for defining how the covariance matrix of the estimation changes in time, is then:
-
+The linearized state space model, describing the evolution of $z(k)$,
+needed to define how the covariance matrix of the estimation changes
+in time, is
\begin{equation}
- z(k) = \mathbf{F}(x(k),u(k)) \cdot z(k-1) + \mathbf{G}(x(k)) \cdot w(k) ,
+ z(k) = \mathbf{F}(x(k),u(k)) \cdot z(k-1) +
+ \mathbf{G}(x(k)) \cdot w(k) ,
\end{equation}
-
-where the state transition matrix $\mathbf{F}(x(k),u(k))$ and the noise gain matrix $\mathbf{G}(x(k))$ are defined as:
-
-\begin{equation}
-\begin{split}
-\mathbf{F}(x(k),u(k))= \\
+where the state transition matrix $\mathbf{F}(x(k),u(k))$ and the
+noise gain matrix $\mathbf{G}(x(k))$ are defined as follows. $\mathbf{F}(x(k),u(k)) =$
+\begin{equation*}
+\begin{array}{l}
\begin{bmatrix}
I_3 & T_sI_3 & 0_{3,3} & 0_{3,3} & 0_{3,3} \\
0_{3,3} & I_3 & \left[T_sR_b^n (q(k))\right]_\times & -T_sR_b^n (q(k)) & 0_{3,3} \\
0_{3,3} & 0_{3,3} & I_3 & 0_{3,3} & -T_sR_b^n (q(k)) \\
0_{3,3} & 0_{3,3} & 0_{3,3} & I_3 & 0_{3,3} \\
0_{3,3} & 0_{3,3} & 0_{3,3} & 0_{3,3} & I_3 \\
-\end{bmatrix} ,
-\end{split}
-\end{equation}
-
-\begin{equation}
-\mathbf{G}(x(k))=
+\end{bmatrix},\\
+\qquad\\
+\mathbf{G}(x(k)) =
\begin{bmatrix}
0_{3,3} & 0_{3,3} & 0_{3,3} & 0_{3,3} \\
T_sR_b^n (q(k)) & 0_{3,3} & 0_{3,3} & 0_{3,3} \\
0_{3,3} & T_sR_b^n (q(k)) & 0_{3,3} & 0_{3,3} \\
0_{3,3} & 0_{3,3} & I_3 & 0_{3,3} \\
0_{3,3} & 0_{3,3} & 0_{3,3} & I_3 \\
-\end{bmatrix} .
-\end{equation}
-
-At every sampling step the inertial measurements are integrated according to equation~\ref{eq:integration} and a first estimation $\hat{x}$ is obtained. The $P$ matrix is therefore updated according to equation~\ref{eq:covarianceudate} given both the propagation of the uncertainty coming from the previous estimations (first part of the sum) and then the contribution of the new disturbances affecting the process (second part of the sum):
+\end{bmatrix}.
+\end{array}
+\end{equation*}
+At every sampling step $k$, the inertial measurements $\tilde{u}(k)$
+are compensated with respect to the estimated biases $\delta u(k)$,
+according to
\begin{equation}
-\mathbf{P} \leftarrow \mathbf{F}(\hat{x},\hat{u})\mathbf{P}\mathbf{F}^{\top}(\hat{x},\hat{u}) + \mathbf{G}(\hat{x})
-\begin{bmatrix}
-Q_1 & 0_{3,3} \\
-0_{3,3} & Q_2
-\end{bmatrix}
-\mathbf{G}^{\top}(\hat{x}) .
+ \tilde{u}(k) \leftarrow \tilde{u}(k) + \delta u(k).
+ \label{eq:deltauuse}
+\end{equation}
+The new value of $\tilde{u}$ is then integrated according to
+Equation~\eqref{eq:integration}, obtaining a first estimation
+$\hat{x}(k)$. The covariance matrix $\mathbf{P}$ as specified in
+Equation~\eqref{eq:covarianceupdate}, including both the propagation
+of the uncertainty coming from the previous estimations (first part of
+the sum) and the contribution of the new disturbances affecting the
+process (second part of the sum).
+\begin{equation}
+ \mathbf{P} \leftarrow \mathbf{F}(\hat{x}, \hat{u})
+ \mathbf{P} \mathbf{F}^{\top}(\hat{x},\hat{u}) + \mathbf{G}(\hat{x})
+ \begin{bmatrix} Q_1 & 0_{3,3} \\ 0_{3,3} & Q_2 \end{bmatrix}
+ \mathbf{G}^{\top}(\hat{x})
\label{eq:covarianceupdate}
\end{equation}
-
-Next, when the GPS measurements are then avilable the Kalman gain $\mathbf{K}$ is computed according to the following equation:
-
+In absence of GPS measurements, the obtained estimate $\hat{x}$ is
+used as the position estimate. On the contrary, when GPS measurements
+are available, the Kalman gain $\mathbf{K}$ is computed according to
+the following equation,
\begin{equation}
-\mathbf{K} \leftarrow \mathbf{P}\left[ I_3 , 0_{3,3}\right]^\top (\left[ I_3 , 0_{3,3}\right]\mathbf{P}\left[ I_3 , 0_{3,3}\right]^\top + R )^{-1} ,
+ \mathbf{K} \leftarrow \mathbf{P}\left[ I_3 , 0_{3,3}\right]^\top
+ (\left[ I_3 , 0_{3,3}\right]\mathbf{P}\left[ I_3 , 0_{3,3}\right]^\top
+ + R )^{-1} ,
\end{equation}
-
-which can be interpreted as a weight that depends on how much we are confident about our current estimation, defined by $\mathbf{P}$, with respect to how noisy are the new measurement, defined by $R$. This is used to estimate the perturbation state $\hat{z}(k)$:
-
+which can be interpreted as a weighted sum of: (i) our confidence in
+the current estimations, defined by $\mathbf{P}$; (ii) how noisy are
+the new measurement, encoded in the covariance of the additive noise
+$R$. This is used to estimate the perturbation state $\hat{z}(k)$:
\begin{equation}
\delta \hat{z}(k) =
\begin{bmatrix}
@@ -227,21 +274,24 @@ which can be interpreted as a weight that depends on how much we are confident a
\mathbf{K}
(\tilde{p}_{GPS}(k)-\hat{p}).
\end{equation}
-
-It can be seen from the previous equation that, given the reliability of the GPS, after the previous update of the Kalman filter the quantity $\delta \hat{x}(k)$ is considered zero (i.e. $0_{9,1}$), while instead the sensor bias $\delta \hat{u}(k)$ is propagated. Finally given the new error vector the state estimation is updated:
-
+Given the availability of the GPS measurements, the quantity
+$\delta \hat{x}(k)$ is considered zero (i.e., $0_{9,1}$). On the
+contrary, the sensor bias $\delta \hat{u}(k)$ is propagated and will
+then be used also when the GPS measurements are not available, in
+Equation~\eqref{eq:deltauuse}.
+
+When the GPS is turned on, the computation of the new error vector
+allows us to finally update the state estimation as
\begin{equation}
-\begin{split}
-\hat{p} \leftarrow \hat{p} + \delta \hat{p} \\
-\hat{v} \leftarrow \hat{v} + \delta \hat{v} \\
-\hat{q} \leftarrow \Gamma(\hat{q},\hat{\epsilon}) .
-\end{split}
+\hat{p} \leftarrow \hat{p} + \delta \hat{p}, \qquad
+\hat{v} \leftarrow \hat{v} + \delta \hat{v}, \qquad
+\hat{q} \leftarrow \Gamma(\hat{q},\hat{\epsilon}).
\end{equation}
-
%---------------------------------
%
-% The resulting algorithm for GPS-IMU navigation is showed in the following listing:
+% The resulting algorithm for GPS-IMU navigation is showed in the
+% following listing:
%
% \begin{lstlisting}[frame=single,mathescape=true,columns=fullflexible]
% $\hat{x}\leftarrow$ initialize state estimation
@@ -255,8 +305,13 @@ It can be seen from the previous equation that, given the reliability of the GPS
%
% end loop
% \end{lstlisting}
+%
+%---------------------------------
-
+With the sensor fusion algorithm just described, and the sensor model
+specified in Section~\ref{sec:gps}, we can now describe the dynamical
+limitations that the GPS sensor imposes and the opportunities
+available for optimizing its sampling.
diff --git a/paper/sections/05-control.tex b/paper/sections/05-control.tex
index a0baa38932e47ec9da21cecbfdb2441fe81eaaf9..0316362ffe89f0fc48edd2aa582bd5a2be0a4811 100644
--- a/paper/sections/05-control.tex
+++ b/paper/sections/05-control.tex
@@ -1,4 +1,4 @@
-Given the sensor fusion algorithm and the sensor model we can now describe which dynamical limitations the sensor imposes and therefore how it can be sampled. In this section we will discuss the general features that characterize an effective sampling strategy. Complementary in the next section a simulation evaluation of the available trade-offs will be performed.
+In this section we will discuss the general features that characterize an effective sampling strategy. Complementary in the next section a simulation evaluation of the available trade-offs will be performed.
\subsection{The dynamics}
The model discussed in section~\ref{sec:gps} points out two dynamics that characterize the sensor: the availability of the ephemeris data and the availability of ranging data. The two are caracterized by very different time scales, both in terms of acquisition time and validity.