%% Copyright (C) 2011, Gostai S.A.S.
%%
%% This software is provided "as is" without warranty of any kind,
%% either expressed or implied, including but not limited to the
%% implied warranties of fitness for a particular purpose.
%%
%% See the LICENSE file for more information.

\section{Matrix}

\subsection{Prototypes}
\begin{refObjects}
\item[Object]
\end{refObjects}

\subsection{Construction}
\label{sec:specs:matrix:ctor}

The \lstinline|init| function is overloaded, and its behavior depends on the
number of arguments and their types.

When there is a single argument, it can either be a List, or another Matrix.

If it's a List of ``Vectors and/or Lists of Floats'', then they must have
the same sizes and constitute the rows.

\begin{urbiscript}
var listList     = Matrix.new([           [0, 1],             [0, 1] ]);
[00071383] Matrix([
[:]  [0, 1],
[:]  [0, 1]])
var listVector   = Matrix.new([           [0, 1],  Vector.new([0, 1])])|;
var vectorList   = Matrix.new([Vector.new([0, 1]),            [0, 1] ])|;
var vectorVector = Matrix.new([Vector.new([0, 1]), Vector.new([0, 1])])|;

assert
{
  listList == listVector;
  listList == vectorList;
  listList == vectorVector;
};
Matrix.new([           [0],            [1, 2]]);
[00000030:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new([Vector.new([0]),           [1, 2]]);
[00000056:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new([           [0], Vector.new([1, 2])]);
[00000071:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new([Vector.new([0]), Vector.new([1, 2])]);
[00052403:error] !!! new: expecting rows of size 1, got size 2 for row 2
\end{urbiscript}

If it's a Matrix, then there is a deep-copy: they are not aliases.

\begin{urbiscript}
var m1 = Matrix.new([[1, 1], [1, 1]])|;
var m2 = Matrix.new(m1)|;
m2[0, 0] = 0|;
assert
{
  m1 == Matrix.new([[1, 1], [1, 1]]);
  m2 == Matrix.new([[0, 1], [1, 1]]);
};
\end{urbiscript}

When given two Float arguments, they must be the two integers, defining the
size of the null Matrix.
\begin{urbiscript}
Matrix.new(2, 3);
[00051329] Matrix([
[:]  [0, 0, 0],
[:]  [0, 0, 0]])
\end{urbiscript}

In other cases, the arguments are expected to be Lists of Floats and/or
Vectors.

\begin{urbiassert}
Matrix.new([1, 2])                   == Matrix.new([[1, 2]]);
Matrix.new([1, 2],           [3, 4]) == Matrix.new([[1, 2], [3, 4]]);
Matrix.new([1, 2], Vector.new(3, 4)) == Matrix.new([[1, 2], [3, 4]]);
\end{urbiassert}

These rows must have equal sizes.

\begin{urbiscript}
// Lists and Lists.
Matrix.new([0], [1, 2]);
[00000160:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new([0, 1], [2]);
[00000169:error] !!! new: expecting rows of size 2, got size 1 for row 2

// Lists and Vectors.
Matrix.new([0], Vector.new(1, 2));
[00000178:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new(Vector.new(0, 1), [2]);
[00000186:error] !!! new: expecting rows of size 2, got size 1 for row 2

// Vectors and Vectors.
Matrix.new(Vector.new(0), Vector.new(1, 2));
[00000195:error] !!! new: expecting rows of size 1, got size 2 for row 2
Matrix.new(Vector.new(0, 1), Vector.new(2));
[00000204:error] !!! new: expecting rows of size 2, got size 1 for row 2
\end{urbiscript}

\subsection{Slots}

\begin{urbiscriptapi}
\item['*'](<that>)%
  If \that is a Matrix, matrix product between \this and \that.
\begin{urbiassert}
Matrix.new([1, 2]) * Matrix.new([10], [20])
  == Matrix.new([50]);
Matrix.new([3, 4]) * Matrix.new([10], [20])
  == Matrix.new([110]);
Matrix.new([1, 2], [3, 4]) * Matrix.new([10], [20])
  == Matrix.new([50], [110]);
\end{urbiassert}

The sizes must be compatible ($\texttt{\this.size.second} =
\texttt{\that.size.first}$).
\begin{urbiscript}
Matrix.new([1, 2]) * Matrix.new([3, 4]);
[00081168:error] !!! *: incompatible sizes: 1x2, 1x2
\end{urbiscript}

  If \that is a Float, the scalar product.
\begin{urbiassert}
Matrix.new([1, 2], [3, 4]) * 3 == Matrix.new([3, 6], [9, 12]);
\end{urbiassert}

\item['*='](<that>)%
  In place (\autoref{sec:lang:op:ass}) product (\refSlot{'*'}).  The same
  constraints apply.
\begin{urbiassert}
var lhs1 = Matrix.new([1, 2], [3, 4]);
var lhs2 = Matrix.new(lhs1);
var rhs = Matrix.new([10], [20]);
var res = lhs1 * rhs;

(lhs1.'*='(rhs)) === lhs1;  lhs1 == res;
(lhs2  *=  rhs ) === lhs2;  lhs2 == res;

rhs *= rhs;
[00272182:error] !!! *=: incompatible sizes: 2x1, 2x1
\end{urbiassert}

\begin{urbiassert}
var v = Matrix.new([1, 2], [3, 4]);
var res = v * 3;
(v *= 3) === v; v == res;
\end{urbiassert}

\item['+'](<that>)%
  The sum of \this and \that.  Their sizes must be equal.
\begin{urbiassert}
Matrix.new([1, 2]) + Matrix.new([10, 20])
  == Matrix.new([11, 22]);

Matrix.new([1, 2], [3, 4]) + Matrix.new([10, 20], [30, 40])
  == Matrix.new([11, 22], [33, 44]);

Matrix.new([1, 2]) + Matrix.new([10, 20], [30, 40]);
[00002056:error] !!! +: incompatible sizes: 1x2, 2x2
\end{urbiassert}

  If \that is a Float, the scalar addition.
\begin{urbiassert}
Matrix.new([1, 2], [3, 4]) + 3 == Matrix.new([4, 5], [6, 7]);
\end{urbiassert}

\item['+='](<that>)%
  In place (\autoref{sec:lang:op:ass}) sum (\refSlot{'+'}).  The same
  constraints apply.
\begin{urbiassert}
var lhs1 = Matrix.new([ 1 , 2], [ 3,  4]);
var lhs2 = Matrix.new(lhs1);
var rhs = Matrix.new([10, 20], [30, 40]);
var res = lhs1 + rhs;

(lhs1.'+='(rhs)) === lhs1;  lhs1 == res;
(lhs2  +=  rhs ) === lhs2;  lhs2 == res;

lhs1 += Matrix.new([1, 2]);
[00338194:error] !!! +=: incompatible sizes: 2x2, 1x2
\end{urbiassert}

\begin{urbiassert}
var v = Matrix.new([3, 6], [9, 12]);
var res = v + 3;
(v += 3) === v; v == res;
\end{urbiassert}

\item['-'](<that>)%
  The difference of \this and \that.  Their sizes must be equal.
\begin{urbiassert}
Matrix.new([1, 2]) - Matrix.new([10, 20])
  == Matrix.new([-9, -18]);

Matrix.new([1, 2], [3, 4]) - Matrix.new([10, 20], [30, 40])
  == Matrix.new([-9, -18], [-27, -36]);

Matrix.new([1, 2]) - Matrix.new([10, 20], [30, 40]);
[00002056:error] !!! -: incompatible sizes: 1x2, 2x2
\end{urbiassert}

  If \that is a Float, the scalar difference.
\begin{urbiassert}
Matrix.new([1, 2], [3, 4]) - 3 == Matrix.new([-2, -1], [0, 1]);
\end{urbiassert}

\item['-='](<that>)%
  In place (\autoref{sec:lang:op:ass}) difference (\refSlot{'-'}).  The same
  constraints apply.
\begin{urbiassert}
var lhs1 = Matrix.new([ 1 , 2], [ 3,  4]);
var lhs2 = Matrix.new(lhs1);
var rhs = Matrix.new([10, 20], [30, 40]);
var res = lhs1 - rhs;

(lhs1.'-='(rhs)) === lhs1;  lhs1 == res;
(lhs2  -=  rhs ) === lhs2;  lhs2 == res;

lhs1 -= Matrix.new([1, 2]) ;
[00362383:error] !!! -=: incompatible sizes: 2x2, 1x2
\end{urbiassert}

\begin{urbiassert}
var v = Matrix.new([3, 6], [9, 12]);
var res = v - 3;
(v -= 3) === v; v == res;
\end{urbiassert}

\item['/'](<that>)%
  Same as \lstinline|this * that.inverse|.  \that must be invertible.
\begin{urbiassert}
var lhs = Matrix.new([20, 0], [0, 200]);
var rhs = Matrix.new([10, 0], [0, 100]);
var res = Matrix.new([ 2, 0], [0,   2]);

lhs / rhs == res;
(lhs / rhs) * rhs == lhs;
rhs * (lhs / rhs) == lhs;

lhs / Matrix.createZeros(2, 2);
[00160168:error] !!! /: non-invertible matrix: <<0, 0>, <0, 0>>
\end{urbiassert}

  If \that is a Float, the scalar division.
\begin{urbiassert}
Matrix.new([3, 6], [9, 12]) / 3 == Matrix.new([1, 2], [3, 4]);
\end{urbiassert}

\item['/='](<that>)%
  In place (\autoref{sec:lang:op:ass}) division (\refSlot{'/'}).  The same
  constraints apply.
\begin{urbiassert}
var lhs1 = Matrix.new([20, 0], [0, 200]);
var lhs2 = Matrix.new(lhs1);
var rhs = Matrix.new([10, 0], [0, 100]);
var res = Matrix.new([ 2, 0], [0,   2]);

(lhs1.'/='(rhs)) === lhs1;  lhs1 == res;
(lhs2  /=  rhs ) === lhs2;  lhs2 == res;

lhs1 /= Matrix.createZeros(2, 2);
[00207285:error] !!! /=: non-invertible matrix: <<0, 0>, <0, 0>>
\end{urbiassert}

\begin{urbiassert}
var v = Matrix.new([3, 6], [9, 12]);
var res = v / 3;
(v /= 3) === v; v == res;
\end{urbiassert}

\item['=='](<that>)%
  Whether \this and \that have the same dimensions and members.
\begin{urbiscript}
do (Matrix)
{
  assert
  {
      new([[1], [2], [3]]) == new([[1], [2], [3]]);
    !(new([[1], [2], [3]]) == new([[1], [2]]));
    !(new([[1], [2], [3]]) == new([[3], [2], [1]]));
  };
}|;
\end{urbiscript}

\item|'[]'|(<row>, <col>)%
  The element at \lstinline|\var{row}, \var{col}|.  The index \var{row} must
  verify $0 \le \textrm{\var{row}} < \texttt{\this.size.first}$, or
  $-\texttt{\this.size.first} \le \texttt{\var{row}} < 0$, in which case it
  is equivalent to using index $\textrm{\var{row}} +
  \texttt{\this.size.first}$: it counts ``backward''.  Similarly for
  \var{col}.
\begin{urbiscript}
var m = Matrix.new([1, 2, 3], [10, 20, 30])|;
assert
{
  m[0, 0] == 1;   m[0, -3] == 1;
  m[0, 1] == 2;   m[0, -2] == 2;
  m[0, 2] == 3;   m[0, -1] == 3;

  m[1, 2] == 30;  m[-1, -1] == 30;
};

m[2, 0];
[00127812:error] !!! []: invalid row: 2

m[-3, 0];
[00127824:error] !!! []: invalid row: -3

m[0, 3];
[00127836:error] !!! []: invalid column: 3

m[0, -4];
[00127850:error] !!! []: invalid column: -4
\end{urbiscript}
\begin{urbicomment}
  removeSlots("m");
\end{urbicomment}

\item|'[]='|(<row>, <col>, <val>)%
  Set the element at \lstinline|\var{row}, \var{col}| to \var{val}, and
  return \var{val}.  The index \var{row} must verify $0 \le
  \textrm{\var{row}} < \texttt{\this.size.first}$, or
  $-\texttt{\this.size.first} \le \texttt{\var{row}} < 0$, in which case it
  is equivalent to using index $\textrm{\var{row}} +
  \texttt{\this.size.first}$: it counts ``backward''.  Similarly for
  \var{col}.
\begin{urbiscript}
var m = Matrix.new([1, 2], [10, 20])|;
assert
{
  (m[0, 0]  = -1) == -1;   m[0, 0] == -1;
  (m[-1, -1] = -2) == -2;  m[1, 1] == -2;
};

m[2, 0] = -1;
[00127812:error] !!! []=: invalid row: 2

m[-3, 0] = -1;
[00127824:error] !!! []=: invalid row: -3

m[0, 2] = -1;
[00127836:error] !!! []=: invalid column: 2

m[0, -3] = -1;
[00127850:error] !!! []=: invalid column: -3
\end{urbiscript}
\begin{urbicomment}
  removeSlots("m");
\end{urbicomment}

\item[appendRow](<vector>)%
  Append \var{vector} to \this and return \this.
\begin{urbiassert}
var m2x1 = Matrix.new([0], [1]);
m2x1.appendRow(Vector.new(2)) === m2x1;
m2x1 == Matrix.new([0], [1], [2]);

var m2x2 = Matrix.new([0, 1], [10, 11]);
m2x2.appendRow(Vector.new(20, 21)) == m2x2;
m2x2 == Matrix.new([0, 1], [10, 11], [20, 21]);
\end{urbiassert}

  Sizes must match.
\begin{urbiscript}
Matrix.new([0], [1]).appendRow(Vector.new(10, 11));
[00017936:error] !!! appendRow: incompatible sizes: 2x1, 2

Matrix.new([0, 1]).appendRow(Vector.new(10));
[00050922:error] !!! appendRow: incompatible sizes: 1x2, 1
\end{urbiscript}

\item[asMatrix]%
  \this.
\begin{urbiassert}
Matrix.asMatrix() === Matrix;
var m = Matrix.new([1], [2]);
m.asMatrix() === m;
\end{urbiassert}

\item[asPrintable]%
  A String that denotes \this.
\begin{urbiassert}
Matrix                    .asPrintable() == "Matrix([])";
Matrix.new([1, 2], [3, 4]).asPrintable() == "Matrix([[1, 2], [3, 4]])";
\end{urbiassert}

\item[asString]%
  A String that denotes \this.
\begin{urbiassert}
Matrix                    .asString() == "<>";
Matrix.new([1, 2], [3, 4]).asString() == "<<1, 2>, <3, 4>>";
\end{urbiassert}

\item[asTopLevelPrintable]%
  A String that denotes \this.
\begin{urbiassert}
Matrix                    .asTopLevelPrintable()
  == "Matrix([])";
Matrix.new([1, 2], [3, 4]).asTopLevelPrintable()
 == "Matrix([\n  [1, 2],\n  [3, 4]])";
\end{urbiassert}

\item[column](<i>)%
  The \var{i}th column as a Vector.  See also \refSlot{row}.
\begin{urbiassert}
var m = Matrix.new([1, 2, 3], [4, 5, 6]);
m.column(0) == Vector.new(1, 4);  m.column(-3) == m.column(0);
m.column(1) == Vector.new(2, 5);  m.column(-2) == m.column(1);
m.column(2) == Vector.new(3, 6);  m.column(-1) == m.column(2);

m.column(3);
[00000232:error] !!! column: invalid column: 3
\end{urbiassert}

\item[createIdentity](<size>)%
  The unit Matrix of dimensions \var{size}.
\begin{urbiassert}
Matrix.createIdentity(0) == Matrix;
Matrix.createIdentity(3) == Matrix.new([1, 0, 0], [0, 1, 0], [0, 0, 1]);

Matrix.createIdentity(-2);
[00000328:error] !!! createIdentity: argument 1: expected non-negative integer: -2
\end{urbiassert}

\item[createOnes](<row>, <col>)%
  Same as \lstinline|createScalar(\var{row}, \var{col}, 1)|.
\begin{urbiassert}
Matrix.createOnes(0, 0) == Matrix;
Matrix.createOnes(2, 3) == Matrix.new([1, 1, 1], [1, 1, 1]);

Matrix.createOnes(-2, 2);
[00000328:error] !!! createOnes: argument 1: expected non-negative integer: -2
\end{urbiassert}

\item[createScalars](<row>, <col>, <scalar>)%
  A Matrix of size \lstinline|\var{row} * \var{col}| filled with
  \var{scalar}.
\begin{urbiassert}
Matrix.createScalars(0, 0, 99) == Matrix;
Matrix.createScalars(2, 3, 99) == Matrix.new([99, 99, 99], [99, 99, 99]);

Matrix.createScalars(-2, 2, 99);
[00000328:error] !!! createScalars: argument 1: expected non-negative integer: -2
\end{urbiassert}

\item[createZeros](<row>, <col>)%
  Same as \lstinline|createScalar(\var{row}, \var{col}, 0)|.
\begin{urbiassert}
Matrix.createZeros(0, 0) == Matrix;
Matrix.createZeros(2, 3) == Matrix.new([0, 0, 0], [0, 0, 0]);

Matrix.createZeros(-2, 2);
[00000328:error] !!! createZeros: argument 1: expected non-negative integer: -2
\end{urbiassert}

\item[distanceMatrix](<that>)%
  Considering that \this and \that are collections of Vectors that denote
  positions in an Euclidean space, produce a Matrix whose value at
  \lstinline|(\var{i}, \var{j})| is the distance between points
  \lstinline|this[\var{i}]| and \lstinline|\var{that}[\var{j}]|.
\begin{urbiassert}
// Left-hand side matrix.
var l0 = Vector.new([0, 0]);  var l1 = Vector.new([2, 1]);
var lhs = Matrix.new([l0, l1]);
// Right-hand side matrix.
var r0 = Vector.new([0, 1]);  var r1 = Vector.new([2, 0]);
var rhs = Matrix.new([r0, r1]);

lhs.distanceMatrix(rhs)
  == Matrix.new([l0.distance(r0), l0.distance(r1)],
                [l1.distance(r0), l1.distance(r1)]);
\end{urbiassert}

\item[init]%
  See \autoref{sec:specs:matrix:ctor}.

\item[inverse]%
  The inverse of \this if it exists, raise an error otherwise.
\begin{urbiassert}
var m = Matrix.new(
  [1, 3, 1],
  [1, 1, 2],
  [2, 3, 4]);

m * m.inverse() == Matrix.createIdentity(3);
m.inverse() * m == Matrix.createIdentity(3);

m.inverse() == Matrix.new(
  [ 2,  9, -5],
  [ 0, -2,  1],
  [-1, -3,  2]);

Matrix.createZeros(2, 2).inverse();
[00000534:error] !!! inverse: non-invertible matrix: <<0, 0>, <0, 0>>
\end{urbiassert}

\item[resize](<row>, <col>)%
  Change the dimensions of \this, using 0 for new members.  Return \this.
\begin{urbiscript}
// Check that <<1, 2><3, 4>> is equal to the Matrix composed of rows,
// when resized to the dimensions of rows.
function resized(var rows[])
{
  var m = Matrix.new([1, 2], [3, 4]);
  var res = Matrix.new(rows);
  // Resize returns this...
  m.resize(res.size.first, res.size.second) === m;
  // ...and does resize.
  m == res;
}|;
assert
{
  // Fewer rows/cols.
  resized([1], [3]);
  resized([1, 2]);
  resized([1]);
  resized([]);

  // As many rows and cols.
  resized([1, 2], [3, 4]);

  // More rows/cols.
  resized([1, 2, 0], [3, 4, 0]);
  resized([1, 2], [3, 4], [0, 0]);
  resized([1, 2, 0], [3, 4, 0], [0, 0, 0]);

  // More rows, less cols, and conversely.
  resized([1], [3], [0]);
  resized([1, 2, 0]);
};
\end{urbiscript}

\item[row](<i>)%
  The \var{i}th row as a Vector.  See also \refSlot{column}.
\begin{urbiassert}
var m = Matrix.new([1, 2, 3], [4, 5, 6]);
m.row(0) == Vector.new(1, 2, 3);  m.row(-2) == m.row(0);
m.row(1) == Vector.new(4, 5, 6);  m.row(-1) == m.row(1);

m.row(2);
[00195645:error] !!! row: invalid row: 2
\end{urbiassert}

\item[rowAdd](<vector>)%
  A Matrix whose rows (vectors) are the sum of each row of \this with
  \var{vector}.
\begin{urbiscript}
do (Matrix)
{
  assert
  {
    rowAdd(Vector) == Matrix;
    new([[1, 2]]).rowAdd(Vector.new(10))     == new([[11, 12]]);
    new([1], [2]).rowAdd(Vector.new(10, 20)) == new([11], [22]);
    new([1, 2], [3, 4]).rowAdd(Vector.new(10, 20)) == new([11, 12], [23, 24]);
  }
}|;
\end{urbiscript}

  The dimensions must be compatible:
  \lstinline|size.first == \var{vector}.size|.
\begin{urbiscript}
Matrix.new([1], [2]).rowAdd(Vector.new(10));
[00000415:error] !!! rowAdd: incompatible sizes: 2x1, 1

Matrix.new([1, 2]).rowAdd(Vector.new(10, 20));
[00000425:error] !!! rowAdd: incompatible sizes: 1x2, 2
\end{urbiscript}

\item[rowDiv](<vector>)%
  A Matrix whose rows (vectors) are the member-wise division of each row of
  \this with \var{vector}.
\begin{urbiscript}
do (Matrix)
{
  assert
  {
    rowDiv(Vector) == Matrix;
    new([[10, 20]]).rowDiv(Vector.new(10))     == new([[1, 2]]);
    new([10], [20]).rowDiv(Vector.new(10, 20)) == new([1], [1]);
    new([10, 30], [20, 40]).rowDiv(Vector.new(10, 20)) == new([1, 3], [1, 2]);
  }
}|;
\end{urbiscript}

  The dimensions must be compatible:
  \lstinline|size.first == \var{vector}.size|.
\begin{urbiscript}
Matrix.new([1], [2]).rowDiv(Vector.new(10));
[00000415:error] !!! rowDiv: incompatible sizes: 2x1, 1

Matrix.new([1, 2]).rowDiv(Vector.new(10, 20));
[00000425:error] !!! rowDiv: incompatible sizes: 1x2, 2
\end{urbiscript}

\item[rowMul](<vector>)%
  A Matrix whose rows (vectors) are the member-wise product of each row of
  \this with \var{vector}.
\begin{urbiscript}
do (Matrix)
{
  assert
  {
    rowMul(Vector) == Matrix;
    new([[10, 20]]).rowMul(Vector.new(10))     == new([[100, 200]]);
    new([10], [20]).rowMul(Vector.new(10, 20)) == new([100], [400]);
    new([1, 2], [3, 4]).rowMul(Vector.new(10, 20)) == new([10, 20], [60, 80]);
  }
}|;
\end{urbiscript}

  The dimensions must be compatible:
  \lstinline|size.first == \var{vector}.size|.
\begin{urbiscript}
Matrix.new([1], [2]).rowMul(Vector.new(10));
[00000415:error] !!! rowMul: incompatible sizes: 2x1, 1

Matrix.new([1, 2]).rowMul(Vector.new(10, 20));
[00000425:error] !!! rowMul: incompatible sizes: 1x2, 2
\end{urbiscript}

\item[rowNorm]%
  A Vector whose values are the (Euclidean) norms of the rows of \this.
\begin{urbiassert}
var m = Matrix.new([1, 2], [3, 3]);
m.rowNorm()[ 0] == m.row( 0).norm();
m.rowNorm()[-1] == m.row(-1).norm();
\end{urbiassert}

\item[rowSub](<vector>)%
  A Matrix whose rows (vectors) are the difference of each row of \this with
  \var{vector}.
\begin{urbiscript}
do (Matrix)
{
  assert
  {
    rowSub(Vector) == Matrix;
    new([[10, 20]])    .rowSub(Vector.new(1))      == new([[9, 19]]);
    new([11], [22])    .rowSub(Vector.new(10, 20)) == new([1], [2]);
    new([1, 2], [3, 4]).rowSub(Vector.new(1, 2))   == new([0, 1], [1, 2]);
  }
}|;
\end{urbiscript}

  The dimensions must be compatible:
  \lstinline|size.first == \var{vector}.size|.
\begin{urbiscript}
Matrix.new([1], [2]).rowSub(Vector.new(10));
[00000415:error] !!! rowSub: incompatible sizes: 2x1, 1

Matrix.new([1, 2]).rowSub(Vector.new(10, 20));
[00000425:error] !!! rowSub: incompatible sizes: 1x2, 2
\end{urbiscript}

\item[set](<vectors>)%
  Change \this to be equal to the Matrix defined by the list of vectors
  given as argument, and return \this.
\begin{urbiassert}
var m = Matrix.new([]);

var m1 = Matrix.new([0, 1], [0, 1]);
    m  ===    m.set([[0, 1], [0, 1]]);
    m  == m1;

var m2 = Matrix.new([2, 3]);
    m  ===    m.set([[2, 3]]);
    m  == m2;
\end{urbiassert}

\item[setRow](<index>, <vector>)%
  Set the \var{index}th row of \this to \var{vector} and return \this.
\begin{urbiassert}
var m2x1 = Matrix.new([0], [1]);
m2x1.setRow(0, Vector.new(2)) === m2x1;
m2x1 == Matrix.new([2], [1]);

var m2x2 = Matrix.new([0, 1], [10, 11]);
m2x2.setRow(0, Vector.new(20, 21)) == m2x2;
m2x2 == Matrix.new([20, 21], [10, 11]);
\end{urbiassert}

  Sizes and index must match.
\begin{urbiscript}
Matrix.new([0], [1]).setRow(0, Vector.new(10, 11));
[00017936:error] !!! setRow: incompatible sizes: 2x1, 2

Matrix.new([0, 1]).setRow(0, Vector.new(10));
[00050922:error] !!! setRow: incompatible sizes: 1x2, 1

Matrix.new([0], [1]).setRow(2, Vector.new(10));
[00017936:error] !!! setRow: invalid row: 2
\end{urbiscript}

\item[size](<arg>)%
  The dimensions of the \this, as a Pair of Floats.
\begin{urbiassert}
Matrix.size == Pair.new(0, 0);
Matrix.new([1, 2], [3, 4], [5, 6]).size == Pair.new(3, 2);
Matrix.new([1, 2, 3], [4, 5, 6])  .size == Pair.new(2, 3);
\end{urbiassert}

\item[transpose](<arg>)%
  The transposed of \this.
\begin{urbiassert}
Matrix                    .transpose() == Matrix;
Matrix.new([1])           .transpose() == Matrix.new([1]);
Matrix.new([1], [2])      .transpose() == Matrix.new([1, 2]);
Matrix.new([1, 2], [3, 4]).transpose() == Matrix.new([1, 3], [2, 4]);
\end{urbiassert}

\item[type]%
  The String \lstinline|Matrix|.
\begin{urbiassert}
Matrix.type         == "Matrix";
Matrix.new([]).type == "Matrix";
\end{urbiassert}
\end{urbiscriptapi}

%%% Local Variables:
%%% coding: utf-8
%%% mode: latex
%%% TeX-master: "../urbi-sdk"
%%% ispell-dictionary: "american"
%%% ispell-personal-dictionary: "../urbi.dict"
%%% fill-column: 76
%%% End:


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Stylish Article
% LaTeX Template
% Version 2.0 (13/4/14)
%
% This template has been downloaded from:
% http://www.LaTeXTemplates.com
%
% Original author:
% Mathias Legrand (legrand.mathias@gmail.com)
%
% License:
% CC BY-NC-SA 3.0 (http://creativecommons.org/licenses/by-nc-sa/3.0/)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%----------------------------------------------------------------------------------------
%   PACKAGES AND OTHER DOCUMENT CONFIGURATIONS
%----------------------------------------------------------------------------------------

\documentclass[fleqn,10pt]{SelfArx} % Document font size and equations flushed left

\usepackage{xspace}
\usepackage{color}
\usepackage{bm} % bold math

%----------------------------------------------------------------------------------------
%   COLUMNS
%----------------------------------------------------------------------------------------

\setlength{\columnsep}{0.55cm} % Distance between the two columns of text
\setlength{\fboxrule}{0.75pt} % Width of the border around the abstract

%----------------------------------------------------------------------------------------
%   COLORS
%----------------------------------------------------------------------------------------

\definecolor{color1}{RGB}{0,0,90} % Color of the article title and sections
\definecolor{color2}{RGB}{0,20,20} % Color of the boxes behind the abstract and headings

%----------------------------------------------------------------------------------------
%   PLOTTING and DRAWING
%----------------------------------------------------------------------------------------
\usepackage{tikz}
\usetikzlibrary{positioning}
\usetikzlibrary{shapes,arrows,backgrounds,fit,shapes.geometric,calc}
\usetikzlibrary{pgfplots.groupplots}
\usepackage{pgfplots}
\usepackage{pgfplotstable}

%----------------------------------------------------------------------------------------
%   HYPERLINKS
%----------------------------------------------------------------------------------------

\usepackage{hyperref} % Required for hyperlinks
\hypersetup{hidelinks,colorlinks,breaklinks=true,urlcolor=color2,citecolor=color1,linkcolor=color1,bookmarksopen=false,pdftitle={Title},pdfauthor={Author}}

%----------------------------------------------------------------------------------------
%   CUSTOM COMMANDS
%----------------------------------------------------------------------------------------

\newcommand{\todo}[1]{\textbf{\textcolor{blue}{TODO: #1}}} % add a comment to the article
\newcommand{\note}[1]{\textbf{\textcolor{red}{NOTE: #1}}} % add a note to the article

\newcommand{\tbl}[1]{\textbf{Table \ref{#1}}\xspace}
\newcommand{\fig}[1]{\textbf{Figure \ref{#1}}\xspace}
\newcommand{\eq}[1]{\textbf{(\ref{#1})}\xspace}
\newcommand{\ssec}[1]{\textbf{\S\ref{#1}}\xspace}

\newcommand{\deriv}[1]{~\text{d}{#1}}
\newcommand{\pder}[2]{\frac{\partial{#1}}{\partial{#2}}}
\newcommand{\dder}[2]{\frac{\deriv{#1}}{\deriv{#2}}}
\newcommand{\vv}[1]{\bm{#1}\xspace}
\newcommand{\cmi}{c_{m,i}}

\newcommand{\unit}[1]{\left[{#1}\right]}
\newcommand{\txtunit}[1]{$\left[{#1}\right]$}

%----------------------------------------------------------------------------------------
%   ARTICLE INFORMATION
%----------------------------------------------------------------------------------------

\JournalInfo{CSCS}      % name of "journal" on upper right corner
\Archive{working document}

\PaperTitle{Cable Equation}

%===============================================================================

\begin{document}

\flushbottom    % Makes all text pages the same height
\maketitle      % Print the title and authors
\tableofcontents

\thispagestyle{empty} % Removes page numbering from the first page

%------------------------------------------------
\section{Formulation}
\input{formulation.tex}

\section{Appendix}
\input{appendix.tex}

\section{Symbols and Units}
\input{symbols.tex}

%*************************************************
\bibliographystyle{abbrv}
\bibliography{bibliography}
%*************************************************

\end{document}


\chapter{Sensors}
\label{chapter:Sensors}

In this chapter will is briefly explained several notations used along the report and
important definitions that help the reader to walk through the work. Many parts of the text presented here are extracted from \cite{thesis_BT} where full description is available and user should refer to it, if necessary.
The notation system of leading superscripts and subscripts is used to denote
relative frames orientation or general physical quantities (vectors, points).
For frames orientations, leading subscript refers to the frame being represented
with respect to the frame in leading superscript. For example let R be a
rotation matrix. Using the notations stated before, ${}^a_bR$ describes
orientation of frame $b$ with respect to frame $a$. For physical quantities, a
vector is represented in the frame defined by is leading superscript,
${}^av$ and in similar way points follow the same rule,
${}^aP=[{}^ax,{}^ay,{}^az]$.

\section{Definition of frames} \label{section:frames}

The frames cited are \gls{ECEF}, \gls{WGS84},
\gls{ENU} and body frame. The \gls{ECEF}, \gls{WGS84} are just auxiliary frames
used to define the \gls{ENU} frame which will be considered the world frame.

\subsection{ECEF and ENU Frame}

\gls{ECEF} coordinate system defines a referential axis where the origin is
defined as the center of Earth, X axis is defined through the intersection of
the plan defined by zero latitude line (Equator) and plan defined by zero
longitude line (prime meridian). The X-axis orientation is considered positive
from center towards the point defined by zero latitude and zero longitude. Z axis
is defined by line intersecting origin and both Poles, being positive towards
North Pole. Y axis is perpendicular to the plan defined by X and Z axis and it
positive direction is defined by right hand rule.

\begin{figure}[!htb]
	\centering
	\includegraphics[width=0.3\linewidth]{figures/EarthTangentialPlane.png}
	\caption[ECEF frame and Local ENU frame.]{ECEF frame and Local ENU frame (\href{https://en.wikipedia.org/wiki/File:EarthTangentialPlane.png}{source:wikipedia})}.
	\label{fig:ecef_enu}
\end{figure}

\gls{ENU} coordinate system is a local coordinate system where the origin is
located at a user defined point in \gls{ECEF} coordinate system, with Y axis pointing
towards North Pole and X axis pointing towards East. The plan defined by X and Y
axis is tangent to the \gls{WGS84} frame on the origin of ENU. Z axis express
the altitude from defined local plane (see figure \ref{fig:ecef_enu}). The
\gls{ENU} frame is considered in this work as the reference frame and will be
denoted with superscript or subscript $w$.

Given a point of reference in \gls{ECEF} frame, it is necessary to find the
corresponding latitude and longitude of reference point ($X_r,Y_r,Z_r$). To do
it is necessary to use the parameters of \gls{WGS84} presented in the table
\ref{tab:wgs84_parameters}

\begin{table}[!htb]
	\centering
	\begin{tabular}{lll}
		\toprule
		\multicolumn{3}{c}{\textbf{WGS 84 Defining Parameters}\cite{wgs84_params}}\\
		\midrule
		\textbf{Parameter}        & \textbf{Notation} & \textbf{Value} \\
		\midrule
		Semi-major axis           & $a$   & 6 378 137.0 m   \\
		Reciprocal of flattening  & $1/f$ & 298.257 223 563 \\
		Semi-minor axis 		  & $b$   & 6 356 752.3142 m\\
		First eccentricity squared& $e^2$ & 6.694 379 990 14x10\textsuperscript{-3}\\
		Second eccentricity squared &${e'}^2$ &  6.739 496 742 28x10\textsuperscript{-3}\\
		\bottomrule
	\end{tabular}
	\caption[WGS84 parameters necessary to transform ECEF coordinates into ENU]{WGS84 parameters necessary to transform ECEF coordinates into ENU}
	\label{tab:wgs84_parameters}
\end{table}

Using set of equations \eqref{eq:ecef_enu_auxiliary} to estimate the latitude
($\lambda_r$) and longitude ($\varphi_r$) for the reference coordinate point.
The final transformation results in applying \eqref{eq:ecef_enu} to
\gls{ECEF} physical quantities \cite{ecef_enu}.

\begin{subequations}
	\label{eq:ecef_enu_auxiliary}
	\begin{equation}
	p = \sqrt{{X_r}^2 + {Y_r}^2}
	\end{equation}
	\begin{equation}
	\theta = \arctan\bigg(Z_r\frac{a}{pb}\bigg)
	\end{equation}
	\begin{equation}
	\lambda_r = \arctantwo(Y_r,X_r)
	\end{equation}
	\begin{equation}
	\varphi_r = \arctan\bigg(\frac{Z_r + e^2b\sin^3(\theta)}{p-{e'}^2a\cos^3(\theta)}\bigg)
	\end{equation}
\end{subequations}

\begin{equation}
\begin{bmatrix}
X\\
Y\\
Z
\end{bmatrix}_{ENU} =
\begin{bmatrix}
-\sin(\lambda_r)   				& \cos(\varphi_r) 				  & 0               \\
-\sin(\varphi_r)\cos(\lambda_r) & -\sin(\varphi_r)\sin(\lambda_r) & \cos(\varphi_r) \\
\cos(\varphi_r)\cos(\lambda_r)  & \cos(\varphi_r)\sin(\lambda_r)  & \sin(\varphi_r)
\end{bmatrix}
\begin{bmatrix}
X-X_r\\
Y-Y_r\\
Z-Z_r
\end{bmatrix}_{ECEF}
\label{eq:ecef_enu}
\end{equation}

\subsection{Body frame, \textit{b}}\label{subsection:body_frame}

Each sensor present in the razor board is aligned to match the sensor axes
printed in the board as seen in figure \ref{fig:razor9dof}. For simplicity, the
razor sensor is placed as possible near the middle point of the bisector
segment between the two wheel axes in such a away that YY axis as marked in the
figure \ref{fig:razor9dof} is pointing towards the front of vehicle, XX axis is
pointing to the right side of the car and ZZ axis is pointing to the top. This
way, if the Euler angles describing the orientation of body frame related to
world frame are all equal to zero, it means the axes in each frame are
coincident apart from an offset in origin.
Rotation angles are considered positive following the right hand rule in each axis.
The origin of body frame is equal to intersection of rear wheel axis with the bisector defined above.

\section{Euler angles and Rotation Matrix} \label{section:euler_matrix_def}

\begin{figure}[hb]
	\centering
	\subcaptionbox{3D view of axes frames example\label{subcap:3D_view_axis_example}}{
		\tdplotsetmaincoords{45}{70}
		\begin{tikzpicture}[scale=3, tdplot_main_coords]

		%draw a grid in the x-y plane
		\foreach \xgrid in {-1,-0.9,...,1.1}
		\foreach \ygrid in {-1,-0.9,...,1.1}
		{
			\draw[very thin,gray!30] (\xgrid,-1) -- (\xgrid,1);
			\draw[very thin,gray!30] (-1,\ygrid) -- (1,\ygrid);
		}

		%---------------------------------------------------------------------------------
		% Extend axis backwards
		%---------------------------------------------------------------------------------
		\draw[dashed] (0,0,0) -- ( -1, 0, 0);
		\draw[dashed] (0,0,0) -- (  0, -1, 0);
		%\draw[dashed] (0,0,0) -- ( 0, 0, -1);

		%---------------------------------------------------------------------------------
		% Plot new frame axis
		%---------------------------------------------------------------------------------
		\draw[->, bluemat] (0,0,0) -- ( 0.5000,  0.5000, 0.7071) node[anchor=south]{${}^bx$};
		\draw[->, redmat](0,0,0) -- (-0.8536,  0.1464, 0.5000) node[anchor=south]{${}^by$};
		\draw[->, yellowmat]  (0,0,0) -- ( 0.1464, -0.8536, 0.5000) node[anchor=south]{${}^bz$};

		%---------------------------------------------------------------------------------
		% guides for XX axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, bluemat] (0.5000, 0, 0) -- ( 0.5000, 0.5000, 0) node[pos=0.5, anchor=north]{\small $r_{xy}$};
		% dashed lines y
		\draw[dashed, bluemat] ( 0, 0.5000, 0) -- ( 0.5000, 0.5000, 0) node[pos=0.5, rotate=-60, anchor=north]{\small $r_{xx}$};
		% dashed lines z
		\draw[dashed, bluemat] ( 0.5000, 0.5000, 0) -- ( 0.5000,  0.5000, 0.7071) node[pos=0.5, rotate=90, anchor=north]{\small $r_{xz}$};
		%---------------------------------------------------------------------------------
		% guides for YY axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, redmat] (-0.8536, 0, 0) -- (-0.8536, 0.1464, 0) node[pos=0.5,anchor=north]{\small $r_{yy}$};
		% dashed lines y
		\draw[dashed, redmat] (0, 0.1464, 0) -- (-0.8536, 0.1464, 0) node[pos=0.5, rotate=-60, anchor=south]{\small $r_{yx}$};
		% dashed lines z
		\draw[dashed, redmat] (-0.8536, 0.1464, 0) -- (-0.8536,  0.1464, 0.5000) node[pos=0.5,rotate=90,anchor=south]{\small $r_{yz}$};
		%---------------------------------------------------------------------------------
		% guides for ZZ axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, yellowmat] (0, -0.8536, 0) -- ( 0.1464, -0.8536, 0) node[pos=0.5,anchor=east]{\small $r_{zx}$};
		% dashed lines y
		\draw[dashed, yellowmat] (0.1464, 0, 0) -- ( 0.1464, -0.8536, 0) node[pos=0.5,anchor=north]{\small $r_{zy}$};
		% dashed lines z
		\draw[dashed, yellowmat] ( 0.1464, -0.8536, 0) -- ( 0.1464, -0.8536, 0.5000) node[pos=0.5,rotate=90,anchor=north]{\small $r_{zz}$};

		% Draw main coordinate system
		\draw[thick,->] (0,0,0) -- (1,0,0) node[anchor=north east]{${}^ax$};
		\draw[thick,->] (0,0,0) -- (0,1,0) node[anchor=north west]{${}^ay$};
		\draw[thick,->] (0,0,0) -- (0,0,1) node[anchor=south]{${}^az$};

		\end{tikzpicture}}
	\hfill
	\subcaptionbox{XY plane of axes frame example\label{subcap:axis_example_xy_plane}}{
		\tdplotsetmaincoords{0}{0}
		\begin{tikzpicture}[scale=3, tdplot_main_coords]

		%draw a grid in the x-y plane
		\foreach \xgrid in {-1,-0.9,...,1.1}
		\foreach \ygrid in {-1,-0.9,...,1.1}
		{
			\draw[very thin,gray!30] (\xgrid,-1) -- (\xgrid,1);
			\draw[very thin,gray!30] (-1,\ygrid) -- (1,\ygrid);
		}

		%---------------------------------------------------------------------------------
		% Extend axis backwards
		%---------------------------------------------------------------------------------
		\draw[dashed] (0,0,0) -- ( -1, 0, 0);
		\draw[dashed] (0,0,0) -- (  0, -1, 0);
		%\draw[dashed] (0,0,0) -- ( 0, 0, -1);

		%---------------------------------------------------------------------------------
		% Plot new frame axis
		%---------------------------------------------------------------------------------
		\draw[->, bluemat] (0,0,0) -- ( 0.5000,  0.5000, 0.7071) node[anchor=south]{${}^bx$};
		\draw[->, redmat](0,0,0) -- (-0.8536,  0.1464, 0.5000) node[anchor=south]{${}^by$};
		\draw[->, yellowmat]  (0,0,0) -- ( 0.1464, -0.8536, 0.5000) node[anchor=west]{${}^bz$};

		%---------------------------------------------------------------------------------
		% guides for XX axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, bluemat] (0.5000, 0, 0) -- ( 0.5000, 0.5000, 0) node[pos=0.5, anchor=west]{\small $r_{xy}$};
		% dashed lines y
		\draw[dashed, bluemat] ( 0, 0.5000, 0) -- ( 0.5000, 0.5000, 0) node[pos=0.5, anchor=south]{\small $r_{xx}$};
		% dashed lines z
		%\draw[dashed, bluemat] ( 0.5000, 0.5000, 0) -- ( 0.5000,  0.5000, 0.7071) node[pos=0.5, rotate=90, anchor=north]{\small $r_{xz}$};
		%---------------------------------------------------------------------------------
		% guides for YY axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, redmat] (-0.8536, 0, 0) -- (-0.8536, 0.1464, 0) node[pos=0.5,anchor=east]{\small $r_{yy}$};
		% dashed lines y
		\draw[dashed, redmat] (0, 0.1464, 0) -- (-0.8536, 0.1464, 0) node[pos=0.5, anchor=south]{\small $r_{yx}$};
		% dashed lines z
		%\draw[dashed, redmat] (-0.8536, 0.1464, 0) -- (-0.8536,  0.1464, 0.5000) node[pos=0.5,rotate=90,anchor=south]{\small $r_{yz}$};
		%---------------------------------------------------------------------------------
		% guides for ZZ axis
		%---------------------------------------------------------------------------------
		% dashed lines x
		\draw[dashed, yellowmat] (0, -0.8536, 0) -- ( 0.1464, -0.8536, 0) node[pos=0.5,anchor=north]{\small $r_{zx}$};
		% dashed lines y
		\draw[dashed, yellowmat] (0.1464, 0, 0) -- ( 0.1464, -0.8536, 0) node[pos=0.5,anchor=west]{\small $r_{zy}$};
		% dashed lines z
		%\draw[dashed, yellowmat] ( 0.1464, -0.8536, 0) -- ( 0.1464, -0.8536, 0.5000) node[pos=0.5,rotate=90,anchor=north]{\small $r_{zz}$};

		% Draw main coordinate system
		\draw[thick,->] (0,0,0) -- (1,0,0) node[anchor=north east]{${}^ax$};
		\draw[thick,->] (0,0,0) -- (0,1,0) node[anchor=north west]{${}^ay$};
		%\draw[thick,->] (0,0,0) -- (0,0,1) node[anchor=south]{${}^az$};

		\end{tikzpicture}

	}
	\caption[Two frames axes example]{Two frames axes example}
	\label{fig:frame_axis_example}
\end{figure}

The rotation matrix is a tool used to describe transformation of coordinates
from one frame to another and this also describe orientation of one frame
relative to another frame. The convention used in this work is represented by \eqref{eq:rotation_matrix} and maps quantities described in frame $b$ to
frame $a$. Comparing the structure of \eqref{eq:rotation_matrix} with
figure \ref{fig:frame_axis_example} it is seen that columns of ${}^a_bR$
represent each unity vector defining all axes of frame $b$.

\begin{equation}
{}^a_bR=\begin{bmatrix}
{}^ar_{xx} & {}^ar_{yx} & {}^ar_{zx}\\
{}^ar_{xy} & {}^ar_{yy} & {}^ar_{zy}\\
{}^ar_{xz} & {}^ar_{yz} & {}^ar_{zz}
\end{bmatrix}
\label{eq:rotation_matrix}
\end{equation}

Rotation matrices belong to the orthonormal group and one important property
is that ${}^a_bR{}^a_bR^T=I$ meaning ${}^a_bR^T = {}^a_bR^{-1}$. Also
${}^a_bR^T$ is equal to ${}^b_aR$
\cite{Sequeira2016}\cite{wiki_rotationtheorem}.

Euler angles are another form to describe orientation of one frame relative to
another by defining three angles. The Euler angles convention adopted in this
work is the Tait–Bryan \gls{intrinsic} rotation sequence Z-Y-X, meaning
referential $a$ axes, represented in figure \ref{fig:frame_axis_example}, can be
mapped into referential $b$ by performing sequential rotations, first along ZZ
axis by an angle $\psi$, second along the resulting YY axis by an angle $\theta$
and final rotation along the resulting XX axis by an angle $\phi$. In figure
\ref{fig:frame_axis_example_euler} is represented the sequence necessary to
rotate frame $a$ into frame $b$ with the respective Euler angles to achieve the
same orientation as expressed in figure \ref{fig:frame_axis_example}. It is also
represented the intermediary axes resulting from each sequential rotation and it
positive direction. Each individual rotation is expressed by is own rotation
matrix using the respective Euler angle associated and the final orientation is
the result of successive matrices multiplications as shown in equation
\ref{eq:rotMat_composition} where the intermediary axes sequence is denoted as
expressed in the figure \ref{fig:frame_axis_example_euler}.

\begin{equation}
\begin{aligned}
{}^b_aR&={}^b_2R_x(\phi){}^2_1R_y(\theta){}^1_aR_z(\psi)\\
&=\begin{bmatrix}
\cos(\psi)\cos(\theta) 								  & \sin(\psi)\cos(\theta) 								  & -\sin(\theta)         \\
\cos(\psi)\sin(\theta)\sin(\phi)-\sin(\psi)\cos(\phi) & \cos(\psi)\cos(\phi)+\sin(\psi)\sin(\theta)\sin(\phi) & \cos(\theta)\sin(\phi)\\
\sin(\psi)sin(\phi)+\cos(\psi)\sin(\theta)\cos(\phi)  & \sin(\psi)\sin(\theta)\cos(\phi)-\cos(\psi)\sin(\phi) & \cos(\theta)\cos(\phi)
\end{bmatrix}
%				  \begin{bmatrix} % the usual transpose
%				  \cos(\psi)\cos(\theta) & \cos(\psi)\sin(\theta)\sin(\phi)-\sin(\psi)\cos(\phi) & \sin(\psi)sin(\phi)+\cos(\psi)\sin(\theta)\sin(\phi)\\
%				  \sin(\psi)\cos(\theta) & \cos(\psi)\cos(\phi)+\sin(\psi)\sin(\theta)\sin(\phi) & \sin(\psi)\sin(\theta)\cos(\phi)-\cos(\psi)\sin(\phi)\\
%				  -\sin(\theta)		   & \cos(\theta)\sin(\phi)								   & \cos(\theta)\cos(\phi)
%				  \end{bmatrix}
\end{aligned}
\label{eq:rotMat_composition}
\end{equation}

\begin{figure}[!hbt]
	\centering
	% Set the plot display orientation
	% Syntax: \tdplotsetdisplay{\theta_d}{\phi_d}
	\tdplotsetmaincoords{75}{120}

	\pgfmathsetmacro{\zRot}{45}
	\pgfmathsetmacro{\yRot}{-45}
	\pgfmathsetmacro{\xRot}{45}
	%%%%%% Change the rotation matrix in order to use Tait-Bryan angles
	\tdseteulerxyz
	%%%%%%%%%%%%% Z-Y-X
	\begin{tikzpicture}[scale=4,tdplot_main_coords]
	\foreach \xgrid in {-1,-0.9,...,1.1}
	\foreach \ygrid in {-1,-0.9,...,1.1}
	{
		\draw[very thin,gray!30] (\xgrid,-1) -- (\xgrid,1);
		\draw[very thin,gray!30] (-1,\ygrid) -- (1,\ygrid);
	}

	%---------------------------------------------------------------------------------
	% Extend axis backwards
	%---------------------------------------------------------------------------------
	\draw[dashed] (0,0,0) -- ( -1, 0, 0);
	\draw[dashed] (0,0,0) -- (  0, -1, 0);
	%\draw[dashed] (0,0,0) -- ( 0, 0, -1);
	% Set origin of main (body) coordinate system
	\coordinate (O) at (0,0,0);

	% Draw main coordinate system
	\draw[thick,->] (0,0,0) -- (1,0,0) node[anchor=north east]{\tiny ${}^ax$};
	\draw[thick,->] (0,0,0) -- (0,1,0) node[anchor=north west]{\tiny ${}^ay$};
	\draw[thick,->] (0,0,0) -- (0,0,1) node[anchor=south]{\tiny ${}^az$};

	%Draws arc representing the rotated angle.
	\tdplotdrawarc[thick,tdplot_main_coords,->, color=black]{(0,0,0)}{0.5}{0}{\zRot}{anchor=north,color=black}{\tiny $\psi$}
	%Draws circle representing the rotated planes. Each of these should be "pointed" by two arrows.
	\tdplotdrawarc[dashed,tdplot_main_coords,->, color=black]{(0,0,1)}{0.1}{0}{359}{}{}

	% First intrinsic rotation
	\tdplotsetrotatedcoords{\zRot}{0}{0}
	\draw[thick,tdplot_rotated_coords,->, yellowmat] (0,0,0) -- (0.9,0,0) node[anchor=west]{\tiny ${}^1x$};
	\draw[thick,tdplot_rotated_coords,->, yellowmat] (0,0,0) -- (0,0.9,0) node[anchor=west]{\tiny ${}^1y$};
	\draw[thick,tdplot_rotated_coords,->, yellowmat] (0,0,0) -- (0,0,0.9) node[anchor=north west]{\tiny ${}^1z$};

	% make xy plane in vertical to mark angle
	\tdplotsetrotatedcoords{\zRot}{0}{90}
	\tdplotdrawarc[thick,tdplot_rotated_coords,->,color=yellowmat]{(0,0,0)}{0.5}{0}{-\yRot}{anchor=west,color=yellowmat}{$\theta$}
	\tdplotdrawarc[dashed,tdplot_rotated_coords,->,color=yellowmat]{(0,0,-0.9)}{0.1}{0}{-359}{}{}
	%	% auxiliary axis to confirm correct plan
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (1,0,0) node[anchor=north east]{${}^tx$};
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (0,1,0) node[anchor=north west]{${}^ty$};
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (0,0,1) node[anchor=south]{${}^tz$};

	% second intrinsic rotation
	\tdplotsetrotatedcoords{\zRot}{\yRot}{0}
	\draw[thick,tdplot_rotated_coords,->, redmat] (0,0,0) -- (0.8,0,0) node[anchor=west]{\tiny ${}^2x$};
	\draw[thick,tdplot_rotated_coords,->, redmat] (0,0,0) -- (0,0.8,0) node[anchor=north]{\tiny ${}^2y$};
	\draw[thick,tdplot_rotated_coords,->, redmat] (0,0,0) -- (0,0,0.8) node[anchor=south]{\tiny ${}^2z$};

	% make xy plan in vertical to mark angle b
	\tdplotsetrotatedcoords{90+\zRot}{0}{\xRot}
	\tdplotdrawarc[thick,tdplot_rotated_coords,->,color=redmat]{(0,0,0)}{0.5}{0}{\xRot}{anchor=west,color=redmat}{$\phi$}
	\tdplotdrawarc[dashed,tdplot_rotated_coords,->,color=redmat]{(0,0,0.8)}{0.1}{0}{359}{}{}
	%	% auxiliary axis to confirm correct plan
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (1,0,0) node[anchor=north east]{${}^tx$};
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (0,1,0) node[anchor=north west]{${}^ty$};
	%	\draw[thick,tdplot_rotated_coords,->] (0,0,0) -- (0,0,1) node[anchor=south]{${}^tz$};

	% second intrinsic rotation
	\tdplotsetrotatedcoords{\zRot}{\yRot}{\xRot}
	\draw[thick,tdplot_rotated_coords,->, bluemat] (0,0,0) -- (1,0,0) node[anchor=west]{\tiny ${}^bx$};
	\draw[thick,tdplot_rotated_coords,->, bluemat] (0,0,0) -- (0,1,0) node[anchor=west]{\tiny ${}^by$};
	\draw[thick,tdplot_rotated_coords,->, bluemat] (0,0,0) -- (0,0,1) node[anchor=south]{\tiny ${}^bz$};

	\end{tikzpicture}
	\caption[Two frames axes example - Euler angles]{Two frames axes example - Euler angles. In this case, the angles are $\psi=45^o, \theta=-45^o, \phi=45^o$}
	\label{fig:frame_axis_example_euler}
\end{figure}

\section{Quaternion} \label{section:quaternion}

During the work an \gls{AHRS} algorithm based on \cite{Madgwick2010_report} and derived in \cite{thesis_BT} is implemented  and it uses quaternions. Basic definitions for understanding the filter terminology  are explained in this section following the same notation as present by \cite{thesis_BT} in case the user as no notion of quaternions at all.

Quaternion is a complex number in four dimension that can be used, similar to rotation matrices and Euler angles, to represent orientation of frames with respect to others. The Euler rotation theorem and the Rodriguez formula are the basis for quaternion representation of rotations but will not be discussed. The theorem states it is possible to describe an orientation of frame relative to other by performing a rotation of $\alpha$ along an axis $r$ that is defined by the points that remain static relative to the original frame. \cite{quaternions} quickly resumes the main idea about quaternions, why they can be used to express orientations and relation between them and Euler and Rodriguez formulations.

A quaternion can be represented by \eqref{eq:quaternion_definition} where $q_1$ is the norm of it, $q_2, q_3$ and $q_4$ are complex coordinates with \textit{i, j, k} being the axis versors. If a quaternion is normalized, it is denoted with a circumflex accent as shown in \eqref{eq:quaternion_normalized}.

\begin{equation}
\centering
\begin{aligned}
q &=q_1+q_2i+q_3j+q_4k\\
q &= [q_1\, q_2\, q_3\, q_4]
\end{aligned}
\label{eq:quaternion_definition}
\end{equation}

\begin{equation}
\centering
\hat{q} \implies \|q\|=1
\label{eq:quaternion_normalized}
\end{equation}

Using the same notation as stated before, ${}^w_bq$ represent the orientation of body frame with respect to world frame. With \eqref{eq:quaternion_definition} in mind, the following list of properties/operations are summarized in this \hyperref[list:quaternion_operations]{list}:

\begin{description}[]
	\label{list:quaternion_operations}
	\item [Identities] All quaternions multiplications must obey to the following set of properties described in equation \ref{eq:quaternion_identities}.
	\begin{subequations}
		\label{eq:quaternion_identities}
		\begin{equation}
		i^2=j^2 = k^2 = ijk = -1
		\end{equation}
		\begin{equation}
		ij =-ji=k
		\end{equation}
		\begin{equation}
		jk =-kj=i
		\end{equation}
		\begin{equation}
		ki =-ik= j
		\end{equation}
	\end{subequations}
	\item [Quaternion multiplication] The quaternions multiplication, denoted by $\otimes$, is defined by the Hamilton product as in equation \ref{eq:quaternion_product}
	\begin{equation}
	\begin{aligned}
	a \otimes b &= [a_1\, a_2\, a_3\, a_4] \otimes [b_1\, b_2\, b_3\, b_4]\\
	&= \begin{bmatrix}
	a_1b_1 -a_2b_2 -a_3b_3 -a_4b_4\\
	a_1b_2 +a_2b_1 +a_3b_4 -a_4b_3\\
	a_1b_3 -a_2b_4 +a_3b_1 +a_4b_2\\
	a_1b_4 +a_2b_3 -a_3b_2 +a_4b_1

	\end{bmatrix}^T
	\end{aligned}
	\label{eq:quaternion_product}
	\end{equation}
	\item [Quaternion conjugate] The quaternion conjugate describes the inverse rotation and is defined as equation \ref{eq:quaternion_conjugate}.
	\begin{equation}
	{}^w_bq^* = {}^b_wq = [q_1\, -q_2\, -q_3\, -q_4]
	\label{eq:quaternion_conjugate}
	\end{equation}
	\item [Vector rotation] Let's define the following quaternion representation of the same vector but in each referential by using is \gls{pure quaternion} as ${}^wv = [0\,{}^wx\,{}^wy\,{}^wz]$ and ${}^bv = [0\,{}^bx\,{}^by\,{}^bz]$. The rotation of vector $v$ from one frame to other, using a quaternion, is performed by equation \ref{eq:quaternion_rotation}.
	\begin{equation}
	{}^bv = {}^w_b\hat{q} \otimes {}^wv \otimes {}^w_b\hat{q}^*
	\label{eq:quaternion_rotation}
	\end{equation}

	\item [Composed rotations] The composition of rotations can be described in quaternions as the product between quaternions. For example the sequence ${}^a_b\hat{q} \rightarrow {}^b_c\hat{q}$ is equal to ${}^a_c\hat{q}$ and is defined as equation \ref{eq:quaternion_composed_rotation}.
	\begin{equation}
	{}^a_c\hat{q} = {}^b_c\hat{q} \otimes {}^a_b\hat{q}
	\label{eq:quaternion_composed_rotation}
	\end{equation}
\end{description}

%-------------------------------------------------------------------------------
% Novatel part
%-------------------------------------------------------------------------------
\section{Novatel OEM4-G2L FlexPak} \label{section:novatel}

\begin{figure}[!hb]
	\centering
	\subcaptionbox{Novatel OEM4-G2L FlexPak receiver.\label{subcap:novatel_receiver}}{\includegraphics[width=0.4\linewidth]{figures/novatel_flexpak.png}}
	\subcaptionbox{Novatel GPS-701-GG antenna\label{subcap:novatel_antenna}}{\includegraphics[width=0.4\linewidth]{figures/Novatel_701-GGL.png}}
	\caption[Novatel GNSS devices used]{Novatel GNSS devices used (\href{https://www.novatel.com}{source:NovAtel})}
	\label{fig:novatel_devices}
\end{figure}

Each  Novatel GPS sensor is composed with Novatel OEM4-G2L FlexPak receiver and
Novatel high performance GPS-701-GG antenna. The Novatel OEM4-G2L FlexPak
receiver provides position and velocity estimations using GPS broadcasted
satellites radio signals. It has two communications ports that can be configured
independently. Additional to common GPS protocols, it has support for custom
Novatel protocols (which will be used) providing the best possible estimation
the receiver can do making it as simple as possible from the point of view of
user. The receiver can handle both L1 and L2 signals provided by satellites
\cite{NovAtel2005_volume1}, however, antenna module only can handle L1 frequency
signal (1575.42MHz)\cite{NovAtel2013_antenna}. Power supply to FlexPak enclosure
should range from 6 to 18 volts DC with typical consumption of 5W. Without
differential GPS methods, the Novatel receiver can achieve up to 1.8 meters
precision \gls{CEP} with single point operation. Output logs data rate is up to
20Hz in binary mode and 10Hz in ASCII format.

Since the receivers used are industrial grade, no particular effort is made to modeling any error source or trying to improve the solution given by it. However is important to have minimal knowledge the limitations of those types of receivers. It is trusted that the reported position and velocity information is the best the sensor could possibly estimate.

The developed library interface full documentation is present in the attach appendix \ref{appendix:novatel}, however is advised to use the online version for granting the most recent \href{https://novatel-oem4-python.readthedocs.io/en/latest/}{updated documentation}.

\subsection{GNSS main errors}\label{subsection:GNSS_main_errors}

The main cause of \gls{GNSS} calculations errors are presented in table \ref{tab:gnss_error_source}.
While some can't be controlled by user (satellite clocks, orbit clocks), others can be mitigated using differential GNSS techniques, multi-frequency (currently civilians have access to more than one), satellite based augmentations system and multi-constellation or using modeling the error source. Multipath is probably the most user dependent but in some situations hard to avoid for example in streets surround with high buildings general denoted as urban canyons.

\begin{table}[!htb]
	\centering
	\begin{tabular}{lc}
		\toprule
		\textbf{Source}		& \textbf{Value (up to)}\\
		\midrule
		Satellite clocks	& $\pm2$m\\
		Orbit Errors		& $\pm2.5$m\\
		Inospheric Delays	& $\pm5$m\\
		Tropospheric Delays & $\pm0.5$m\\
		Receiver Noise		& $\pm0.3$m\\
		Multipath			& $\pm1$m\\
		\bottomrule
	\end{tabular}
	\caption[Main source of errors in calculations using GNSS]{Main source of errors in calculations using GNSS \cite{NovatelIntroGNSS}}
	\label{tab:gnss_error_source}
\end{table}
\vfill

%-------------------------------------------------------------------------------
% Razor related stuff
%-------------------------------------------------------------------------------
\section{Sparkfun Razor IMU 9DOF} \label{section:sparkfun}

\begin{figure}[!htb]
	\centering
	\includegraphics[width=0.5\linewidth]{figures/10125-04b.jpg}
	\caption[Razor 9DOF IMU.]{Razor 9DOF IMU.}
	\label{fig:razor9dof}
\end{figure}

Razor IMU \glsdisp{9DOF}{9DOF} is an electronics board, developed by
\href{http://www.sparkfun.com/}{Sparkfun Electronics}, which includes an
accelerometer, a gyroscope and a magnetometer, each one with three axis of
sensibility. Since this board is based on the
\href{https://www.arduino.cc/}{Arduino project}, the chip sensors are
interconnected by an Atmel\textsuperscript{\textregistered} ATmega328p \gls{MCU}
which allows the configuration of sensors, handles the readings and processing
of their respective outputs.

%-------------------------------------------------------------------------------
% ADXL345 part
%-------------------------------------------------------------------------------
\subsection{ADXL345 Digital Accelerometer} \label{subsection:adxl345}

The ADXL345 is a \gls{MEMS} sensor with three axis accelerometer made by
\href{http://http://www.analog.com/en/index.html}{Analog Devices} . It has an
adjustable acceleration scale range value up to $\pm16g$. It is possible to
measure both dynamic accelerations resulting from motion and shock or measure
static accelerations such as gravity, which allows this device to be used as a
tilt sensor also. Acceleration deflects the a proof mass attached to one differential capacitor plate and unbalances it, resulting in a sensor output whose amplitude is
proportional to acceleration \cite{adxl345_datasheet}.Conversion of outputs is made through 10 to 13 bit \gls{ADC} according to selected
range to maintain the same scale of 4mg/LSB.

\subsubsection{Accelerometer sensor model}\label{subsubsection:adxl345model}

The output of accelerometer is proportional to the sum of acceleration in each
axis. However the accelerometer is not capable of distinguish if the
acceleration is caused by a true acceleration produced by motion or produced by
a static force, generally gravity force. As so, equation \eqref{eq:accel_model}
describes the general output of accelerometer \cite{Vectornav_calibration}
where, ${}^bA_{ext}$ is the sum of real external acceleration due to
linear or rotational dynamics in the body frame, ${}^b_wR$ is the rotation
matrix mapping quantities in world frame into sensor frame, ${}^wg $
represents the fictitious acceleration due to gravity in the world frame, ${}^bA_{0}$ is an offset and
$\delta A_{\epsilon}$ describes addictive gaussian noise.
Ideally, $G$ should be equal to identity matrix but in fact it represents the
product of two matrices, one describing the cross-axis influence and the other
a scale factor for each axis \cite{Vectornav_calibration}.

\begin{equation}
{}^bA_{s}=G[{}^bA_{ext} + {}^b_wR\,{}^wg\,] + {}^bA_{0} + \delta A_{\epsilon}
\label{eq:accel_model}
\end{equation}

\subsubsection{Accelerometer calibration}\label{subsubsection:adxl345calibration}

The suggested calibration method by the manufacturer \cite{adxl345AN1077}
assumes that cross-axis influence is small enough and can be neglected, this
way, $G$ should only be represented by a diagonal matrix with the scale factors
for each axis. However, if more precision for the application is needed,
calibration using the ellipsoid fitting approach should be made
\cite{Pylvanainen2008}.

For this application it will be used the suggested calibration by manufacturer
resulting in exposing the sensor to six position combination while resting. This
means the ${}^bA_{ext}$ will be zero and accelerometer will be only
influenced by the gravity force, 1g.

\begin{figure}[!hbt]
	\begin{minipage}{0.49\textwidth}
		\centering
		\includegraphics[width=\textwidth]{figures/adxl_calibration.png}
		\caption[ADXL345 calibration poses and expected output.]{ADXL345 calibration
			poses and expected output \cite{adxl345AN1077}.}
		\label{fig:adxl_calibration}
	\end{minipage}
	\hfill
	\begin{minipage}{0.49\textwidth}
		\renewcommand{\arraystretch}{1.2} % more space between rows
		\centering
		\begin{tabular}{cc}
			\toprule
			\textbf{Gains and offset [LSB]} & \textbf{Razor }\\
			\midrule
			Z gain   &  253.66  \\
			Z offset & -3.47    \\
			Y gain	 & 266.27    \\
			Y offset & -3.18     \\
			X gain   & 266.04   \\
			X offset & 13.83	\\
			\bottomrule
		\end{tabular}
		\captionof{table}{Summary of values extracted and calculated for each axis of accelerometer in both sensors}
		\label{tab:adxl_summary}
	\end{minipage}
\end{figure}

Aligning one axis at a time with gravity vector, as seen in figure
\ref{fig:adxl_calibration}, the mean value of several samples is calculated.
Using these means for each axis and the known acceleration
values(\ensuremath{\pm}1g), it is calculated the slope and offset of the
respective line passing through the two points. Calculated values (see table \ref{tab:adxl_summary}) will be stored
in the internal \glsdisp{EEPROM}{EEPROM} memory of \gls{MCU} to be loaded at
boot time of each razor. The conversion between LSB and g unities is given by
the relation $254LSB/g$ or $3.9mg/LSB$ \cite{adxl345_datasheet}

%-------------------------------------------------------------------------------
% ITG3200 part
%-------------------------------------------------------------------------------
\subsection{ITG3200 Digital Gyroscope} \label{subsection:itg3200}

The ITG-3200 is a microelectronic mechanical integrated circuit chip with three
axis independent gyroscopes, built by
\href{http://www.invensense.com/}{Invensense} able of measuring angular
velocities up to $\pm2000\degree/s$. When the gyroscopes are rotated about any
of the sensor axes, the Coriolis effect causes a deflection that is  detected by
a capacitive pick-off circuit proportional to angular velocity. The measured signal is filtered and converted using internal \gls{ADC}. The scale factor that allow to convert the
output values to angular velocities in degrees per second is, according to its
datasheet \cite{itg3200_datasheet}, factory calibrated and equal to
$14.375LSB/\degree.s^{-1}$.

\subsubsection{Gyroscope sensor model} \label{subsubsection:itg3200model}

For a given axis, the gyroscope output is proportional the angular velocity
sensed of that axis and is given by equation \eqref{eq:gyro_model} where
${}^b\omega_{s}$ represents the vector output of the sensor in body
frame at instant $t$, ${}^b\omega_{real}$ is the real angular velocity
vector applied to sensor in body frame, ${}^b\omega_{0}$ is the bias
vector term and $\delta\omega_{\epsilon}$ addictive gaussian noise.
Since the sensor is described in \cite{itg3200_datasheet} as factory calibrated,
$G$ which represent a scaled factor correction, is expected to be equal to
identity matrix. Note that the equation model \eqref{eq:gyro_model} is in fact a
simplified version of a more accurate model described in
\cite{Vectornav_calibration}. As stated in \cite{Vectornav_calibration}
cross-axis misalignment and linear acceleration sensitivity are expected to be
small for the application purpose so they are considered irrelevant.

In general, the offset value depends on the temperature value inside chip. Once
internal stability is reached, offset values tends to be constant \cite{Woodman2007}. Electrical
temperature stability can be achieved after a few minutes of operation. At the
beginning of initialization of Razor, several samples are acquired internally in
the microprocessor and  mean value is calculated for each axis, defining this
way the offset constant for each axis.

\begin{equation}
{}^b\omega_{s}=G\,{}^b\omega_{real} + {}^b\omega_{0} + \delta\omega_{\epsilon}
\label{eq:gyro_model}
\end{equation}

%-------------------------------------------------------------------------------
% HMC5843 part
%-------------------------------------------------------------------------------
\subsection{HMC5843 Digital compass} \label{subsection:hmc5843}

The HCM5843 sensor is a digital compass chip designed by
\href{https://www.honeywell.com}{Honeywell}, designed for low field magnetic
sensing. This sensor uses anisotropic magnetic resistor technology, making it
immune to vibration noises, and the use of Wheatstone bridge makes it more
robust to noise. A material with anisotropic magnetic resistor properties
changes its resistance according to absolute value and direction of magnetic
field.

\subsubsection{Magnetometer model}

The magnetometer sensor model follows a similar structure as two previous
sensors and is given by equation \eqref{eq:mag_model}. The ${}^bH_{s}$ is
the value sensed by the sensor in body frame. $C$ is a matrix resulting from
the multiplication of matrices containing influence of cross-axis, gain scale
factor and soft-iron interference \cite{magAN4246}. ${}^wh_e$ represent
the geomagnetic vector of Earth, ${}^bh_{offset}$ is an offset vector which
describes the influence of zero-field offset and the ferromagnetic masses fixed
relative to body frame usually denoted as hard-iron. $\delta h_{\epsilon}$
represents gaussian additive noise.

\begin{equation}
{}^bH_{s}=C\,{}^b_wR{}^wh_e\, + {}^bh_{offset} + \delta h_{\epsilon}
\label{eq:mag_model}
\end{equation}

\subsubsection{Geomagnetic field}\label{subsubsection: geomagnetic_field}

The geomagnetic field can be described as 3D vector in each point as seen in
\cite{ipma_geomagnetism}. As previous defined, ${}^wh_e$ represent the
geomagnetic vector of Earth. Let us define now, ${}^wh_0$ as the
horizontal projection of ${}^wh_e$. The angle between ${}^wh_0$ and
${}^wh_e$ is denominated as inclination, $I$. As stated in
\cite{ipma_geomagnetism}, the horizontal projection of the geomagnetic vector
points to the magnetic north which may not be aligned with the north axis of
reference frame, ${}^wY$. This defines the magnetic declination angle, $D$, as the
angle between horizontal projection and the ${}^wY$ axis.

For Lisbon, the magnetic declination angle, in January 2017, defined in the reference
frame is equal to 2.48\degree, inclination angle is equal to 52.77\degree and
the ${}^wh_e$ is equal to $[-1121.5, 26479.8, -34884.7]^T$ nanoteslas
\cite{noaa}

\subsubsection{Hard and soft-iron compensation}

The compensation for hard and soft-iron effects can be done using a geometric
approach as described by \cite{magAN4246} \cite{Caruso2000}
\cite{Vasconcelos2011}. If the geomagnetic vector is known and sensor perfect
calibrated, free of any type ambient distortion, points collected are expected
to belong a surface of an origin centered sphere with radius equal to the
magnitude of geomagnetic field ${}^wh_e$. Distortions, cross-axis
influence, scale-factor inequalities between axis, causes the expected sphere to
be deformed into a ellipsoid. Collecting points in 3D space by performing
rotations of the sensor frame, allow us to fit the data to an ellipsoid surface
and after determinate the matrix $C$ and offset vector ${}^bh_{offset}$.

However since the body frame is a vehicle, it is not physically possible to
collect points based in rotations of YY and XX axis. Only ZZ axis rotations are
available and the data instead of belonging to an ellipsoid surfaces, will
belong to an ellipse in Z-plane as a result from that plan cutting the ellipsoid
in some undetermined z coordinate. Because of restriction stated before it will
be considered that the influence in ZZ axis is zero and the scale
factor is equal to one. Offset in ZZ axis will also be considered zero. In \cite{thesis_BT} is shown that this assumption is not relevant. \eqref{eq:mag_model} will be rearranged to equation \eqref{eq:mag_model1}
form.

\begin{equation}
\begin{aligned}
{}^bH_{s}&=C\,{}^b_wR{}^wh_e\, + {}^bh_{offset} + \delta h_{\epsilon}\\
{}^b_wR{}^wh_e &=C^{-1}[{}^bH_{s}-{}^bh_{offset} - \delta h_{\epsilon}] \\
\end{aligned}
\label{eq:mag_model1}
\end{equation}

Without loss of generality, $C^{-1}\delta h_{\epsilon}$ will result
also in a gaussian vector so it will be rewritten as $\delta
h_{\epsilon}$. Equation \eqref{eq:mag_model1} can take now be arranged into \eqref{eq:mag_model2} where ${}^bH_{c}$ is the corrected value
after calibration process.

\begin{equation}
\begin{aligned}
{}^b_wR(t){}^wh_e &=C^{-1}[{}^bH_{s}-{}^bh_{offset} - \delta h_{\epsilon}] \\
{}^bH_{c} &= C^{-1}[{}^bH_{s}-{}^bh_{offset}] + \delta h_{\epsilon}
\end{aligned}
\label{eq:mag_model2}
\end{equation}

The matrix $C^{-1}$ and ${}^bh_{offset}$ is obtained as described by
\cite{Caruso2000} using the
\textit{ellipse\_fit}\footnote{\href{https://www.mathworks.com/matlabcentral/fileexchange/22423-ellipse-fit}{https://www.mathworks.com/matlabcentral/fileexchange/22423-ellipse-fit}}. The function \textit{ellipse\_fit} returns the least square estimate that best fits the data collected from sensor and the parameters that define the estimated ellipse as described in \hyperref[list:ellipsfit_out]{this list}:

\begin{itemize}[noitemsep]
	\label{list:ellipsfit_out}
	\item \textbf{semi-major} - The length of major semi axis of ellipse.
	\item \textbf{semi-minor} - The length of minor semi axis of ellipse.
	\item $\bm{x_0}$ - The offset in the XX axis of ellipse.
	\item $\bm{y_0}$ - The offset in the YY axis of ellipse.
	\item $\bm{\alpha}$ - Rotation angle between semi-major axis and XX axis in reference frame.
\end{itemize}

\begin{figure}[!htb]
	\centering
	\includegraphics[width=0.7\linewidth]{figures/mag_calibration_steps.png}
	\caption[Calibration steps of ellipse data]{Simulation of the calibration steps applied to hypothetical ellipse data.}
	\label{fig:mag_cal_steps_fig}
\end{figure}

Take note the fact that since is not physically possible to perform rotations in
3D space, the calibration will only be reflect the XX and YY axis. Using the
values returned, the calibration process consists in the following sequential
steps:

\circled{1} Remove offset by using equation \eqref{eq:mag_cal_step1} with ${}^bh_{offset} = [{}^bx_0, {}^by_0, 0]^T$

\begin{equation}
{}^bH_{s}={}^bH_{s}-{}^bh_{offset}
\label{eq:mag_cal_step1}
\end{equation}

\circled{2} Align semi-major axis of ellipse with the XX axis of reference frame using a rotation of $-\alpha$ by using equation \eqref{eq:mag_cal_step2}

\begin{equation}
\begin{aligned}
{}^bH_{s}&=R_{cal}\,{}^bH_{s}\Leftrightarrow\\
\Leftrightarrow{}^bH_{s}&=\begin{bmatrix}
\cos(\alpha) & -\sin(\alpha) & 0\\
\sin(\alpha) & \cos(\alpha)  & 0\\
0			 & 0			 & 1
\end{bmatrix}{}^bH_{s}
\end{aligned}
\label{eq:mag_cal_step2}
\end{equation}

\circled{3} Apply a scaling matrix $S$ to make semi-major axis the same length as semi-minor axis using \eqref{eq:mag_cal_step3}

\begin{equation}
\begin{aligned}
{}^bH_{s}&=S\,{}^bH_{s}\Leftrightarrow\\
\Leftrightarrow{}^bH_{s}&=\begin{bmatrix}
\frac{semi-minor}{semi-major} & 0 & 0\\
0 & 1 & 0\\
0 & 0 & 1
\end{bmatrix}{}^bH_{s}
\end{aligned}
\label{eq:mag_cal_step3}
\end{equation}

\circled{4} Restore the initial rotation $\alpha$ to the scaled data using the transpose of $R_{cal}$ as seen in \eqref{eq:mag_cal_step4}

\begin{equation}
\begin{aligned}
{}^bH_{s}&=R^T_{cal}\,{}^bH_{s}\Leftrightarrow\\
\Leftrightarrow{}^bH_{s}&=\begin{bmatrix}
\cos(\alpha) & \sin(\alpha) & 0\\
-\sin(\alpha) & \cos(\alpha)  & 0\\
0			 & 0			 & 1
\end{bmatrix}{}^bH_{s}
\end{aligned}
\label{eq:mag_cal_step4}
\end{equation}

The ${}^bH_{s}$ resulting from step four is in fact the expected
corrected reading of sensor, ${}^bH_{c}$. In figure
\ref{fig:mag_cal_steps_fig} visually represented the steps applied to a
hypothetical ellipse. Resuming, all sequential steps in one equation results in
\eqref{eq:mag_cal_resume}.

\begin{equation}
{}^bH_{c}=R_{cal}\,S\,R^T_{cal}[{}^bH_{s}-{}^bh_{offset}]
\label{eq:mag_cal_resume}
\end{equation}

Comparing the structure of \eqref{eq:mag_cal_resume} with \eqref{eq:mag_model2} this implies that $C^{-1}$ is equal to $R_{cal}\,S\,R^T_{cal}$.

%The results of calibration are presented for one of the devices in figure
%\ref{fig:mag_cal_din0} and the obtained are expressed in the table
%\ref{tab:mag_cal_results}. The trajectory used is not important as long as it is
%able perform one or more rotation of the vehicle in the horizontal plan. In
%figure \ref{subcap:mag_cal_comparison} is shown the results before and after
%calibration for the estimation of Euler $\psi$ angle. It is also present the
%estimation of the same angle using the the one of the gps unities as a reference
%for the comparison.
%
%\begin{figure}[!htp]
%	\centering
%	\subcaptionbox{Trajectory used for calibration.\label{subcap:mag_cal_din0_gps}}{\includegraphics[width=0.32\linewidth]{Figures/mag_calibration_steps_din0_gps.pdf}}
%	\hfill
%	\subcaptionbox{Magnetometer calibration \label{subcap:mag_cal_din0}}{\includegraphics[width=0.32\linewidth]{Figures/mag_calibration_steps_din0.pdf}}
%	\hfill
%	\subcaptionbox{$\psi$ angle estimation before and after calibration of magnetometers\label{subcap:mag_cal_comparison}}{\includegraphics[width=0.32\linewidth]{Figures/mag_calibration_compare.pdf}}
%	\caption{Magnetometer calibration steps applied to real data}
%	\label{fig:mag_cal_din0}
%\end{figure}
%
%
%\begin{table}[!hbt]
%	\centering
%	\begin{tabular}{ccc}
%		\toprule
%		{} & \textbf{Razor 1} \\
%		\midrule
%		${}^bh_{offset}\quad[nT]$ &[-2.5828\, -2.1703]\textsuperscript{T}x10\textsuperscript{4} \\
%		\addlinespace[5pt]
%		$\alpha\quad [deg]$ & 107.5238 \\
%		\addlinespace[5pt]
%		$\frac{semi-minor}{semi-major}$ & 0.9567\\
%		\addlinespace[5pt]
%		$R_{cal}$ & $\begin{bmatrix}
%		-0.3011 & -0.9536 & 0 \\
%		+0.9536 & -0.3011 & 0 \\
%		0		 &  0	   & 1 \end{bmatrix}$ \\
%		\addlinespace[5pt]
%		$C^{-1} $ &$\begin{bmatrix}
%		+0.9961& +0.0124 & 0\\
%		+0.0124& +0.9606 & 0\\
%		0     & 0      & 1
%		\end{bmatrix} $		 \\
%		\addlinespace[5pt]
%		\bottomrule
%	\end{tabular}
%	\caption{Magnetometers calibration results.}
%	\label{tab:mag_cal_results}
%\end{table}


% !TEX root = ../../../proposal.tex

\section{Factoring Group Orders of Non-Safe Primes}
\label{sec:ecm}

Across all scans, we collected 41,847 unique groups with non-safe primes.
To measure the extent to which each group would facilitate a small subgroup
attack in a vulnerable implementation, we attempted to factor $(p-1)/2$. We
used the GMP-ECM~\cite{gmp-ecm-zimmerman-2012} implementation of the elliptic curve method for
integer factorization on a local cluster with 288 cores over a several-week
period to opportunistically find small factors of the group order for each of
the primes.

\ECMBreakableGroups

\ECMRFCFiveFiveOneFourGroups

Given a group with prime $p$ and a generator $g$, we can check whether the
generator generates the entire group or generates a subgroup by testing whether
$g^{q_i} \equiv 1 \bmod p$ for each factor $q_i$ of $(p-1)/2$.  When $g^{q_i}
\equiv 1 \bmod p$, then if $q_i$ is prime, we know that $q_i$ is the exact
order of the subgroup generated by $g$; otherwise $q_i$ is a multiple of the
order of the subgroup. We show the distribution of group order for groups using
non-safe primes in Table~\ref{tab:ecm-distribution}.  We were able to
completely factor $p-1$ for 4,701 primes.  For the remaining primes, we
did not obtain enough factors of $(p-1)/2$ to determine the group order.

%We show the number of groups for which the difference in size of $\lg(p)$ and
%$\lg(q_i)$ is at least 8 bits, since these subgroups are more likely to have
%been intentionally generated \todo{come up with better reason for why this is
%interesting}.

Of the groups where we were able to deduce the exact subgroup orders, several
thousand had a generator for a subgroup that was either 8, 32, or 64 bits
shorter than the prime itself.  Most of these were generated by the Xlight FTP
server, a closed-source implementation supporting SFTP.  It is not clear
whether this behavior is intentional or a bug in an implementation intending to
generate safe primes.  Primes of this form would lead to a more limited
subgroup confinement or key recovery attack.

Given the factorization of $(p-1)/2$, and a limit for the amount of online and
offline work an attacker is willing to invest, we can estimate the
vulnerability of a given group to a hypothetical small subgroup key recovery
attack. For each subgroup of order $q_i$, where $q_i$ is less than the online
work limit, we can learn $q_i$ bits of the secret key via an online brute-force
attack over all elements of the subgroup. To recover the remaining bits of the
secret key, an attacker could use the Pollard lambda algorithm, which runs in
time proportional to the square root of the remaining search space. If this
runtime is less than the offline work limit, we can recover the entire secret
key. We give work estimates for the primes we were able to factor and the
number of hosts that would be affected by such a hypothetical attack in
Table~\ref{tab:ecm-breakable}.

The DSA groups introduced in RFC 5114~\cite{rfc5114} are of particular
interest. We were able to completely factor $(p-1)/2$ for both Group 22 and
Group 24, and found several factors for Group 23. We give these factorizations
in Table~\ref{tab:group-order-factorization}.
In Table~\ref{tab:ecm-rfc5114}, we show the amount of online and offline work
required to recover a secret exponent for each of the RFC 5114 groups. In
particular, an exponent of the recommended size used with Group 23 is fully
recoverable via a small subgroup attack with 33 bits of online work and 47 bits
of offline work.

\ECMDistributionTable

\GroupOrderFactorization


% !TEX root = as_grf_sopt.tex
\newcommand{\bOmega}{\boldsymbol{\Omega}}
\newcommand{\bH}{\mathbf{H}}
\newcommand{\bq}{\mathbf{q}}
%\newcommand{\rhos}{\rho^{\frac{1}{2}}}
\newcommand{\rhos}{\rho}
\newcommand{\cC}{\mathcal{C}}
\newcommand{\blambda}{\boldsymbol{\lambda}}
%%%%%%%%%%%

\newcommand{\COMMENTEQ}[2][4.5em]{%
  \leavevmode\hfill\makebox[#1][l]{//~#2}}

\begin{algorithm}[ht]
	\caption{ \textsc{gp-sopt} and its variants}
	\label{alg:main}
	\begin{algorithmic}[1]
  	\INPUT $\mu_0$, $\bA$, $\omega_0$, $\sigma_n$, $\alpha_t$, $T$;
  	if warm start, $\{v_\tau,y(v_\tau)\}_{\tau=1}^{t_0}$
	\STATE Obtain initial $\mathcal{N}(\bmu_{0},\bC_{0})$
		\COMMENTEQ{\eqref{eq:generative}}

	\FOR{$t = t_0, \ldots, T-1,$}

 	\STATE Update to posterior $\mathcal{N}(\bmu_t, \bC_t)$
 		\COMMENTEQ{\eqref{eq:var_update}}

 	\STATE $v_{t+1} \leftarrow \argmax_{v \in V \setminus S_t} \mu_t(v) + \alpha_{t+1} s_t(v)$

  		\hfill \makebox[5em][r]{// (\ref{eq:sopt.orig}, \ref{eq:sopt.tt}, or \ref{eq:sopt.topk})}
 		% \COMMENTEQ{(\ref{eq:sopt.orig}, \ref{eq:sopt.tt}, or \ref{eq:sopt.topk})}

	\STATE Observe $y(v_{t+1})$; include $S_{t+1} \leftarrow S_t \cup \{v_{t+1}\}$
	\ENDFOR
	\OUTPUT $S_T$.
	\end{algorithmic}
\end{algorithm}
%
%\begin{tikzpicture}[every node/.style={draw=black,thick,circle,inner sep=0pt,minimum size=0.5cm, ellipse, align=center}]
%
%% start
%\node [draw=none] (aa){};
%\node [below right=3 and 3.5 of aa] (f) {$f(\cdot)$\\$f(v^*)$};
%
%\foreach \t [evaluate=\t as \tpo using int(\t+1)] in {0,...,3}{
%
%	% paras (t-1)
%	\node [right=2*\t of aa] (p\t) {$\mu_{\t}(\cdot)$ \\ $c_{\t}(\cdot,\cdot)$};
%
%	% actions and obs (t)
%	\node [below=.5 of p\t, fill=green!20] (v\tpo) {$v_{\tpo}$};
%	\node [below=.5 of v\tpo, fill=gray!20] (y\tpo) {$y_{\tpo}\mid v_{\tpo}$};
%
%	\draw [-latex] (p\t) -- (v\tpo);
%	\draw [-latex] (v\tpo) -- (y\tpo);
%
%	% obs
%	\draw [-latex] (f) -- (y\tpo);
%
%	% reward
%	\node [below=1.5 of y\tpo] (r\tpo) {$r_{\tpo}$};
%	\draw [-latex] (f) -- (r\tpo);
%	\draw [-latex] (v\tpo) to[out=-145, in=110, distance=1cm] (r\tpo);
%
%	\ifthenelse{\t>0}{
%		\draw [-latex]	(y\t) -- (p\t);
%	}{};
%};
%\end{tikzpicture}

% \begin{algorithm}[h]
	% \caption{General GP-style Active Search}
	% \caption{\textsc{gp-sopt} active search and its variants}
	% \label{alg:main}
	% \begin{algorithmic}[1]
	% \INPUT Adjacency matrix $\bA$, set of seed nodes $S_0$, query budget $T$, class imbalance prior $\mu_0$, regularization $\omega_0$ (default $0.01$), observation noise $\sigma_n$, and tuning parameters, $\alpha_t$.
 %        \OUTPUT A set of $T$ selected nodes.
 %   \STATE Compute augmented Laplacian $\widetilde{\cLap}_0:=\bD-\bA+\omega_0\bI$
	% % \STATE Compute the prior covariance $\bC_0 = (\cLap + \omega_0\bI)^{-1}$.
	% \STATE Use $S_0$ to obtain the initial $\mathcal{N}(\bmu_{t_0},\bC_{t_0})$ by \eqref{eq:post_prec} or \eqref{eq:var_update}
	% \FOR{$t = t_0 + 1, \ldots, T,$}
	% \STATE Solve $v_t := \arg \max_{v \in V \setminus S_{t-1}} \mu_{t-1}(v) + \alpha_{t} s_{t-1}(v)$ with $s_{t-1}(v)$ defined via \eqref{eq:sopt.tt}, \eqref{eq:sopt.topk}, or \eqref{eq:sopt.orig}
	% \STATE Query the label $y_t$ of $v_t$
	% \STATE Update $\bmu_t$ and  $\bC_t$ by \eqref{eq:post_prec} or \eqref{eq:var_update}
	% \STATE $S_t := S_{t-1} \cup \{v_t\}$
	% \ENDFOR
	% \STATE Return $S_T$.
	% \end{algorithmic}
% \end{algorithm}
%%%%%%%%%%%
Our proposed active search algorithms are described in Algorithm~\ref{alg:main}.
They resemble general exploration-exploitation style algorithms with GPs.
Here we focus on binary functions that assign value $1$ to relevant or target nodes,
and $0$ to all other nodes.
% The warm start seed nodes are assumed to be all relevant.
%Based on the posterior distribution of node labels, we consider a
At iteration $t+1$, Algorithm~\ref{alg:main} selects the next node to query
based on a deterministic selection rule of the form:
\begin{equation}
	\argmax_{v \in V \setminus S_t} \; \mu_{t}(v) + \alpha_{t+1} \cdot s_{t}(v),
	\label{eq:acquisition}
\end{equation}
where $\mu_{t}(v)$ is the usual exploitation term and $s_{t}(v)$ encourages exploration,
with the two being balanced by a possibly iteration-dependent parameter $a_{t+1} > 0$.

Examples from existing literature like
the popular GP-UCB algorithm and its extension to Active Search, GP-SELECT \citep{vanchinathanadaptively},
amount to setting $s_{t}(v)^2 = \sigma_t(v)^2$, the posterior (as well as predictive) variance of the reward value at node $v$.
Although this is a very reasonable choice in many situations, it may lead to undesirable exploration behaviors on graphs.
Under our model assumption, low-degree nodes, which usually lie at the periphery of a graph, tend to have high predictive variances.
Direct applications of GP-UCB may result in the selection of many such outliers, which fail to reveal much information
about the reward values of most other nodes at the core of the graph  (Figure~\ref{fig:comparison:ucb}).

\begin{figure*}[htb]
\centering
\subfigure[Choices from UCB]{
\begin{tikzpicture}%[scale=0.7, transform shape]
  \toyPlot
  \foreach \x in {17,1,9,15,14}
    % GP_UCB(A,.3,label_toy_bin',struct('obs_sigma',1));
    % kernel parameters: alpha, 0.300, omega0, 0.010, obs_sigma, 1.000, weight_scale, 1.000, k, Inf
  {
    \node[draw=none, text=red] at (n\x){$\times$};
    \foreach \y in \toytargets{
       \ifthenelse{\x = \y}{\node[target] at (n\x){$\checkmark$};}{}
  }
  \node[ucb] at (n\x){};
  }
  \foreach \x in {1,...,17}
  {
    \node [draw=none, below right=.1*\nodeDist of n\x.center, inner sep=0pt, minimum size=2pt, opacity=.5, text opacity=1, fill=white, circle] {{\scriptsize{\x}}};
  }
\end{tikzpicture}
\label{fig:comparison:ucb}
}
%
$\qquad$
\subfigure[Choices by our algorithm]{
\begin{tikzpicture}%[scale=0.7, transform shape]
  \toyPlot
  \foreach \x in {7,14,2,10,5}
    % GP_SOPT(A,.3,label_toy_bin',struct('obs_sigma',1));
    % kernel parameters: alpha, 0.300, omega0, 0.010, obs_sigma, 1.000, weight_scale, 1.000, k, Inf
  {
  \node[draw=none, text=red] at (n\x){$\times$};
   \foreach \y in \toytargets{
     \ifthenelse{\x = \y}{\node[target] at (n\x){$\checkmark$};}{}
  }
  \node[sopt] at (n\x) {};
  }
  \foreach \x in {1,...,17}
  {
    \node [draw=none, below right=.1*\nodeDist of n\x.center, inner sep=0pt, minimum size=2pt, opacity=.5, text opacity=1, fill=white, circle] {{\scriptsize{\x}}};
  }
\end{tikzpicture}
\label{fig:comparison:sopt}
}
\caption{For the toy graph example, choices from (a) direct application of UCB \citep{vanchinathanadaptively,valko2014spectral}  versus (b) our vanilla GP-SOPT. We observe that our method (b) tends to select more from cluster centers, which helps reduce variance of the unobserved values/rewards, whereas previous literature (a) tends to select the graph periphery.}
\label{fig:comparison}
\end{figure*}

Intuitively, a good exploration criterion should favor nodes that have high influences on other parts of the graph.
That is, the knowledge of the function values at these nodes should reveal a lot about the function values at other nodes.
Under our model assumption, this principle naturally connects with the predictive covariances of a node with others. Research in active learning
on graphs has already made use of predictive covariances to construct better selection rules. \cite{mingji}
proposed to select nodes based on their sums of squares of predictive covariances with other nodes, which is derived
from the minimization of squared prediction error, known as V-optimality in experiment design. \cite{ma_2013}
observed that V-optimality can still be undesirably sensitive to outliers and used \textbf{$\Sigma$-optimality criterion} instead,
which by itself selects a set of nodes $\bv_t$ to minimize the following Bayes \textit{survey risk} on the posterior model after the selection,
\begin{equation*}
	%\min_{\bv_t}
	\mathcal{R}^\Sigma_{t\mid\bv_t} = \mathbb{E} \Biggl(
	\sum_{v'\in\mathcal{V}} f_{t\mid\bv_t}(v') -
	\sum_{v'\in\mathcal{V}} \mu_{t\mid\bv_t}(v')
	\Biggr)^2
	=\bone^\top\bC_{t\mid\bv_t}\bone.
\end{equation*}
For active search, we use this criterion in a greedy sequential selection manner for exploration scoring, as
\begin{align}
  s_t(v) &= \sqrt{\mathcal{R}^\Sigma_{t\mid S_t} -	\mathcal{R}^\Sigma_{t+1\mid S_t\cup\{v\}} }
  % \nonumber
  % \\
 %  & =  \bone^\top (\bC_{t\mid S_t} - \bC_{t+1\mid S_t\cup\{v\}}) \bone
 % \nonumber
 % \\
  % &
  =
 	\frac{ \sum_{v'} C_t(v,v')}{ \sqrt{C_t(v,v) + \sigma_n^2}}
 	\nonumber
 	\\
 % 	&=
 %  	\frac{\bc_t(v)^\top \bq_{t,(1)}}
	% {\sqrt{\sigma_t(v)^2 + \sigma_n^2}}
	%  \nonumber
	%  \\
	&= \frac{1}{\sqrt{1+\nicefrac{\sigma_n^2}{\sigma_t^2(v)}}}
	\cdot
	\sum_{v'\in V}\rho_t(v,v')\sigma_t(v'),
	\label{eq:sopt}
\end{align}
where the second equality is easily derived from \eqref{eq:var_update}.
If we ignore $\sigma_n$ (set it to $0$), the $\Sigma$-optimality criterion \eqref{eq:sopt} considers the sum
of a node's correlation times standard deviation of all nodes on the graph. High score nodes by this criterion are likely to provide rich
information for exploration.

We propose three exploitation-exploration style algorithms with  exploration criteria motivated by $\Sigma$-optimality, which are \emph{vanilla $\Sigma$-optimality} and its two variants with an additional parameter $k$ that we will describe next.
All algorithms select the next node to query by the general rule \eqref{eq:acquisition}, but use different exploration terms:
%
\stepcounter{equation}

\textbf{GP-SOPT (Vanilla $\Sigma$-Optimality):} %Select the next node by \eqref{eq:acquisition}, where,
\begin{equation}
   % \eqref{eq:acquisition}, \quad \mbox{where }\;
	s_t(v)
	=
		\frac{1}{\sqrt{1+\nicefrac{\sigma_n^2}{\sigma_t^2(v)}}}
	\cdot
	\sum_{v'\in V}\rho_t(v,v')\sigma_t(v').
	\label{eq:sopt.orig} \tag{\theequation.a}
\end{equation}

% The first
% two are variants of \eqref{eq:sopt} with an additional parameter $k$ that we will describe next:

\textbf{GP-SOPT.TT (Thresholded Total Covariance):} %Select the next node by \eqref{eq:acquisition}, where,
		\begin{equation}
			s_{t}(v) = \min\Biggl(k \sigma_{t}(v),
			% \sum_{v' \in V}\frac{C_{t}(v,v')}{\sigma_{t}(v)}
			\sum_{v'\in V}\rho_t(v,v')\sigma_t(v')
			\Biggr).
			\label{eq:sopt.tt} \tag{\theequation.b}
		\end{equation}
\textbf{GP-SOPT.TOPK (Top-$k$ Covariance):} %Select the next node by \eqref{eq:acquisition}, where,
		\begin{equation}
			s_{t}(v) := \max_{B \subset V, |B| = k}
			% \sum_{v' \in B} \frac{C_{t}(v,v')}{\sigma_{t}(v)}.
			\sum_{v' \in B} \rho_{t}(v,v')\sigma_{t}(v').
			\label{eq:sopt.topk} \tag{\theequation.c}
		\end{equation}

As one can see in Figure~\ref{fig:comparison:sopt}, the nodes selected by vanilla GP-SOPT indeed reside in more central parts of the toy graph than the nodes selected by its competitor. In a large graph with many peripheral nodes, we believe that the improved exploration criteria of GP-SOPT and its variants contribute to a better recall rate of search targets in real graphs in Section~\ref{sec:exp}.

The reason we propose the latter two variants, \eqref{eq:sopt.tt} and \eqref{eq:sopt.topk}, is to both address proof difficulties and increase practical robustness. By Lemma \ref{lemma:nnC} in Appendix \ref{sec:appendix:cov}, we have that $s_t(v) \geq \sigma_t(v)$ for both criteria,
meaning that $s_t(v)$ maintains the UCB property.
Note that the observation noise, $\sigma_n$, is also dropped from \eqref{eq:sopt.tt} and \eqref{eq:sopt.topk}.
% There is no obvious difference when $\sigma_n$ is small.
% Note that if we keep the observation noise variance as in \eqref{eq:sopt}, this property might not always hold.
As we will show in our theoretical analysis, we put a threshold in \eqref{eq:sopt.tt} against $k \sigma_t(v)$, where $k$ is a tuning parameter,
in order to explicitly control the regret of the algorithm.
As implied by Lemma \ref{lemma:nnC_sub} in Appendix \ref{sec:appendix:cov}, the Top-$k$ Covariance criterion \eqref{eq:sopt.topk}
is also always upper-bounded by $k \sigma_t(v)$.
% Top-$k$ truncation only considers
% a node's influence in its local neighborhood.
% We refer to \autoref{alg:main} combined with these two criteria as \textbf{GP-SOPT.TT} and \textbf{GP-SOPT.TOPK}, respectively.

% As mentioned before, this criterion may not give an upper-confidence bound due to
% the observation noise variance. However, using analysis techniques recently proposed by \cite{contal2014gaussian},
% we are able to derive some theoretical guarantees regarding the \textit{optimization regret} of algorithms
% using this criterion. More generally, our analysis suggests a connection between the variance minimization principle
% in active learning and optimization regret in the bandit setting. We refer to \autoref{alg:main} combined with this criterion
% as \textbf{GP-SOPT}.

In the next two subsections we discuss in more details the properties of various exploration criteria,
and present our theoretical analysis.

%\textbf{The exploration heuristic} we propose, in its most general form, is the one-step look-ahead improvement of a non-increasing spectral function on the sorted eigen-values of the covariance matrix. Let $\bC_t$ be the covariance matrix with decreasingly sorted eigen-values $\blambda_t^2= (\lambda_{t,(1)}^2, \dotsc, \lambda_{t,(n)}^2)^\top$, the spectral function is then defined as,
%\begin{align}
%	\rho(\bC_t) &= \sum_{j=1}^n h(\lambda_{t,(j)}),
%	\quad {\rm s.t.} \;
%	h'(s)>0, \forall s>0.
%\end{align}
%Correspondingly, assuming an observation of $v$, the exploration heuristic is then,
%\begin{align}
%	s_t(v) =
%	g\left(
%	\sum_{k=1}^n h\Bigl(\lambda_{t,(j)}\Bigr)
%	-
%	\sum_{k=1}^n h\Bigl(\lambda_{t+1,(j)}\Bigr)
%	\right)
%	,
%\end{align}
%where $g=h^{-1}$ is the inverse function. The difference in side $g(\cdot)$ is nonnegative because for inference on Gaussian models, $\lambda_{t+1,(j)} < \lambda_{t(j)}$.
%
%For example, some reasonable choices of $s_t(v)$ are,
%\begin{numcases}{\hspace{-2em}}
%	\sqrt{ 1+\nicefrac{\sigma_t^2(v)}{\sigma_n^2}}
%	 & $h_{ig}(s) = -log(s)$
%	   \label{eq:s-ig}
%	\\
%	 \sqrt{\tr(\bC_{t}) - \tr(\bC_{t+1})},
%	   & $h_2(s) = s^2$
%	   \label{eq:s-variance}
%	\\
%	 \lambda_{\max}(\bC_{t}) - \lambda_{\max}(\bC_{t+1}),
%	 \hspace{-1em}
%	 & $h_\infty(s) = s^p, p\to\infty$
%	   \label{eq:s-spectral}
%\end{numcases}
%
%We promote the last heuristic, \eqref{eq:s-spectral}, over others, for the following reasons. Alternative \eqref{eq:s-ig}, which adds biases to maximize the differential information gain of the joint distribution of node values, turns out to pay too much attention to the graph periphery, which actually prevents information gathering in the true problem against intuition. More precisely, differential entropy is sensitive to tails of the distribution, which happens to be the place of the biggest model mismatch of our \textsc{grf} models. In fact, selecting nodes using \eqref{eq:s-ig} result in an almost linear trend in the growth of differential information gain.
%
%
%The other alternative, \eqref{eq:s-variance}, alleviates the situation by adding independence assumptions on the nodes and measure the sum of the marginal variances. However, it does not completely solve the issue that nodes preferred occur more often in graph periphery. An intuition is ... ???
%
%Finally, we apply \eqref{eq:s-spectral}, which aims to globally control the posterior marginal variances of every node, by upper-bounding them by $\lambda_{\max}^2$.
%%
%	Indeed,  for any node $k$ and any covariance matrix $\bC$,
%	\begin{equation}
%	C_{kk}=\be_k^\top \bC \be_k \leq \max \nicefrac{\bv^\top\bC\bv}{\bv^\top\bv} = \lambda_{\max}^2(\bC).
%	\end{equation}
%
%Define $\rho_t = \lambda_{\max}(\bC_t)$ and $\rho_{t+1}(v) =  \lambda_{\max}(\bC_{t+1}\mid v)$; the ideal objective we consider then takes the form,
%\begin{equation}
%	s_t^0(v) = s_t(v) =  \rhos_t - \rhos_{t+1}(v).
%	\label{eq:criterion0}
%\end{equation}
%%which still needs some approximations before real implementation.
%
%\subsection{Approximate Spectral Norm Decreases}
%
%First, we linearly approximate \eqref{eq:criterion0}, by computing its gradient,
%\begin{align}
%	\frac{(\bq_{t+1}^\top \bc_{t+1}(v))^2}{c_{t+1}(v,v)^2+\sigma_n^2}
%	\leq
%	\rho_t^2 - \rho_{t+1}^2|_v
%	\leq
%	\frac{(\bq_t^\top \bc_t(v))^2}{c_t(v,v)^2+\sigma_n^2}
%\end{align}
%
%Then, we seek approximations by realizing $\bC_t$ as a kernel in \textsc{grf} model. With, ??some analysis??, the principal component of the prior kernel $\bC_0$ is close to a vector of all ones,
%\begin{equation}
%	\bq_0 \approx \mathbf{1},
%\end{equation}
%which allows an easy computation of what we call \textbf{$\Sigma$-Optimality}.
%\begin{align*}
%%	&\lambda^2_{\max}(\bC_{t+1}) \approx \lambda^2_{\max}(\bC_t) - \mathbf{1}^\top \bc_0(v) (\sigma_0^2(v) + \sigma_n^2) \bc_0(v)^\top \mathbf{1}
%	&\rhos_{1}^2(v) \approx \rhos_{0}^2 - \frac{\left(\mathbf{1}^\top \bc_0(v)\right)^2}
%	{ (\sigma_0^2(v) + \sigma_n^2) }
%	\\
%	&s_0(v) \approx \frac{1}{2\rhos_0}
%	\frac{\bc_0(v)^\top \mathbf{1}}
%	{\sqrt{\sigma_0(v)^2 + \sigma_n^2}}
%\end{align*}
%
%In fact, this approximation can be valid for relatively large $t$. Further breaking the graph down to different (relatively isolated) connected components, where each individual component is relatively unexplored, and therefore its corresponding eigen-vector will be close to an indicator vector of ones on the nodes inside this component, which we call $\bq_{t,c}\approx\mathbf{1}_\cC$, where $c$ and $\cC$ are the index and subset of nodes of this connected component. The more under-explored that the component $c$ is, the more likely that $\bq_{t,c}$ becomes the principal eigen-vector, $\bq_t$, and simultaneously that $\bq_{t,c}$ gets close to $\mathbf{1}_\cC$.
%
%In the meantime, every column on the current covariance matrix $\bc_t(v)$ will also reflect independence between these (relatively isolated) components. Thus, the inner product can be roughly approximated as,
%\begin{equation}
%	\bq_t^\top \bc_t(v) \approx \mathbf{1}_\cC^\top \bc_t(v)  + \mathbf{1}_{\bar{\cC}}^\top \mathbf{0} = \mathbf{1}^\top \bc_t(v),
%\end{equation}
%where $\bar{\cC}$ is the complement of $\cC$.
%
%\begin{remark}
%	connection to survey problem.
%\end{remark}
%
%\subsection{Sigma-Optimality and Beyond}
%
%%
%%
%%\begin{lemma}[Trace Average]
%%	\label{lem:computation}
%%	With $h_1(s) = s^2$, the following holds,
%%	\begin{align*}
%%		s_t(v) = \sqrt{\frac{\sum_{v'}C_t(v,v')^2}{C_t(v,v) + \sigma_n^2} }
%%		\geq
%%		\frac{C_t(v,v)}{\sqrt{C_t(v,v) + \sigma_n^2}}
%%	\end{align*}
%%%
%%%	Let $\bq_t$, $\bq_{t+1}$ be the eigen-vector corresponding to the largest eigen-value of $\rho^2(\bC_t)$ and $\rho^2(\bC_{t+1}\mid v)$, respectively, the eigen-values satisfies the following,
%%%	\begin{align}
%%%		\frac{(\bq_{t+1}^\top \bc_{t+1}(v))^2}{c_{t+1}(v,v)^2+\sigma_n^2}
%%%		\leq
%%%		\rho_t^2 - \rho_{t+1}^2|_v
%%%		\leq
%%%		\frac{(\bq_t^\top \bc_t(v))^2}{c_t(v,v)^2+\sigma_n^2}
%%%	\end{align}
%%\end{lemma}
%
%
%
%%\begin{lemma}[Trace for Average]
%%	\label{lem:computation}
%%	Let $\bq_t$, $\bq_{t+1}$ be the eigen-vector corresponding to the largest eigen-value of $\rho^2(\bC_t)$ and $\rho^2(\bC_{t+1}\mid v)$, respectively, the eigen-values satisfies the following,
%%	\begin{align}
%%		\frac{(\bq_{t+1}^\top \bc_{t+1}(v))^2}{c_{t+1}(v,v)^2+\sigma_n^2}
%%		\leq
%%		\rho_t^2 - \rho_{t+1}^2|_v
%%		\leq
%%		\frac{(\bq_t^\top \bc_t(v))^2}{c_t(v,v)^2+\sigma_n^2}
%%	\end{align}
%%\end{lemma}
%%
%
%%\begin{lemma}[Spectral for Control]
%%	\label{lem:robustness}
%%	With $h_1(s) = s^\infty$,
%%	$C_{kk}\leq\rho^2(\bC)$ holds for any marginal variance of $\bff\sim\mathcal{N}(\bmu, \bC)$.
%%\end{lemma}
%%	Indeed,  $C_{kk}=\be_k^\top \bC \be_k \leq \max \nicefrac{\bv^\top\bC\bv}{\bv^\top\bv} = \rho^2(\bC)$. \qed
%
%%\subsection{Theoretical Properties}
%%
%%\subsection{Numerical Applications}
%
%
%%\todo[inline]{stuck here, because $\sigma_t^2(v)$ may be irrelevant to $s_t$?}
%
%
%\subsection{Properties}
%
%
%%%%%%%%%%%%%
%\begin{lemma}
%\label{lemma:nnC}
%	For connected graph Laplacian, $\bK=\cLap+\bOmega$, the posterior covariance matrix, $\bC_t$, is nonnegative element-wise.
%\end{lemma}%
%%%%%%%%%%%%%
%\begin{proof}
%	The posterior precision matrix for conjugate multivariate normal prior and likelihood pairs is additive. As the observations involve only one coordinate, $v_t$, at a time, the precision matrix of the likelihood model is diagonal. Let $h_k = \sum_{\tau=1}^t \llbracket v_\tau=v_k \rrbracket$ to be the count of queries on node $k$, further define its diagonal matrix, $\bH = \diag(h_1,\dotsc,h_n)$.
%	\begin{align}
%		(\bC_t)^{-1} = \bK^{-1} + \sigma_n^{-2}\bH
%		= \bD - \bA + \bOmega + \sigma_n^{-2}\bH.
%		\label{eq:precision}
%	\end{align}
%	Define $\bD_t = \bD + \bOmega + \sigma_n^{-2}\bH$,
%	we have
%	\begin{align*}
%		\bC_t = (\bD_t - \bA)^{-1}
%		=
%		\bD_t^{-\frac{1}{2}}
%		\left(\sum_{k=0}^\infty \left(\bD_t^{-\frac{1}{2}}\bA\bD_t^{-\frac{1}{2}}\right)^k \right)
%		\bD_t^{-\frac{1}{2}},
%	\end{align*}
%	where convergence is because $\|\bD_t^{-\frac{1}{2}} \bA \bD_t^{-\frac{1}{2}}\|_2 < 1$ (details in Appendix~\ref{app:sec:nnC}). Notice the right hand side is always nonnegative.
%\end{proof}
%
%%%%%%%%%%%%%
%\begin{remark}
%	Notice that Lemma~\ref{lemma:nnC} also applies to normalized graph Laplacians, when because the counterpart of \eqref{eq:precision} allows a similar derivation,
%	\begin{align*}
%		\widetilde{\bC}_t
%		&=
%		\left( \widetilde{\cLap} +
%		\widetilde{\bOmega} + \sigma_n^{-2}\bH
%		\right)^{-1}
%		\numberthis
%		\label{eq:nnC_normalized}
%		\\
%%		&=
%%		\bD^{\frac{1}{2}}
%%		\left( \bD - \bA
%%		+ \widetilde{\bOmega}\bD + \sigma_n^{-2}\bH\bD
%%		\right)^{-1}
%%		\bD^{\frac{1}{2}},
%%		\\
%		&=
%		\bD^{\frac{1}{2}}
%		\widetilde{\bD}^{-\frac{1}{2}}
%		\left( \sum_{k=0}^\infty \left(
%			\widetilde{\bD}_t^{-\frac{1}{2}}
%			\bA
%			\widetilde{\bD}_t^{-\frac{1}{2}}
%		\right)^k
%		\right)^{-1}
%		\widetilde{\bD}^{-\frac{1}{2}}
%		\bD^{\frac{1}{2}},
%	\end{align*}
%	when the counterpart of $\bD_t$ is defined as,
%	\begin{equation}
%		\widetilde{\bD}_t = \bD + \widetilde{\bOmega}\bD + \sigma_n^{-2}\bH\bD.
%	\end{equation}
%	Generally we require $\sigma_n > 0$, but it is not necessary for submatrix of covariance matrix on unqueried nodes (more in Appendix~\ref{app:sec:nnC}).
%\end{remark}
%
%
%%%%%%%%%%%%%%
%
%\begin{lemma}
%If  prior is defined with respect to an unnormalized graph Laplacian, we have for any $t$ and $(v,v')$ pair,
%\begin{equation}
%	C_t(v,v) \geq C_t(v,v'),
%\end{equation}
%which essentially means $\sigma_t(v) \geq \rho_t(v,v')\sigma_t(v'), \forall v'$.
%\end{lemma}
%
%\begin{proof}
%	It is really simple with spring analogy, but I have to find out how to translate it to words. There should not be restrictions of the form $v\not\in S_t$.
%\end{proof}
%
%\begin{remark}
%	We can unify bounds with both normalized and unnormalized graph Laplacians, by also considering \eqref{eq:nnC_normalized}, as,
%	\begin{equation}
%	\sigma_t(v) \leq s_{t,k}(v) \leq k\sqrt{d_{\max}}\sigma_t(v),
%%		\sqrt{d_{\max}}\tilde{\sigma}_t(v)
%%		\geq
%%		\tilde{\rho}_t(v,v') \tilde{\sigma}_t(v'), \forall v',
%	\end{equation}
%	where $d_{\max}$ is the maximal degree on the graph.%
%\footnote{Maybe this suggests that we shouldn't use normalized/unnormalized Laplacian??}
%\end{remark}

%%%%%%%%%%%%