@@ -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.
\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$.
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$.
