@@ -15,7 +15,7 @@ The main drawback of those sensors is that, given the relative nature of the pro
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.
In this section the actual implementation of the sensor fusion algorithm is discussed. The proposed algorithm integrates two times the inertial measurements in order to obtain the trajectory profile.When GPS positioning is available a \emph{Kalman filter} is implemented to compensate the inertial estimation and estimates its biases.
@@ -23,12 +23,12 @@ In this section the actual implementation of the sensor fusion algorithm is disc
\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 quantities we want to estimate are: the position $p(k)\in\!R^3[m]$, the velocity $v(k)\in\mathbb{R}^3[m/s]$ and the attitude $q(k)\in\mathbb{R}^4[-]$ in quaternion representation. The measurements available from the IMU are instead: the acceleration along the three axis $s(k)\in\mathbb{R}^3[m/s]$ and the angular rates $\omega(k)\in\mathbb{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)]'$.
Equation~\ref{eq:integration} can also intuitively 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$.
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 -- 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$. The actual measurement is $\tilde{u}(k)$ and $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}, 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)
...
...
@@ -58,6 +58,32 @@ We now want to correct the measures from the inertial sensors. The assumption is
\label{eq:bias}
\end{equation}
We now define the discrepancy between the state estimated with the integration of the IMU measures and the actual position
\begin{equation}
\delta x(k) =
\begin{bmatrix}
\delta p(k) \\
\delta v(k) \\
\epsilon (k) \\
\end{bmatrix}
=
\begin{bmatrix}
p(k)-\hat{p}(k) \\
v(k)-\hat{v}(k) \\
\epsilon (k)
\end{bmatrix} ,
\end{equation}
where the attitude perturbation 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}* b = a*b$.}.
