Added support for attitude_estimator_q functions.

matrix/Matrix.hpp

@@ -342,6 +342,22 @@ public:
         }
     }
 
+    void setRow(size_t i, const Matrix<Type, N, 1> &row)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t j = 0; j < N; j++) {
+            self(i, j) = row(j, 0);
+        }
+    }
+
+    void setCol(size_t j, const Matrix<Type, M, 1> &col)
+    {
+        Matrix<Type, M, N> &self = *this;
+        for (size_t i = 0; i < M; i++) {
+            self(i, j) = col(i, 1);
+        }
+    }
+
     void setZero()
     {
         memset(_data, 0, sizeof(_data));

matrix/Quaternion.hpp

@@ -29,6 +29,11 @@ public:
     typedef Matrix<Type, 4, 1> Matrix41;
     typedef Matrix<Type, 3, 1> Matrix31;
 
+    Quaternion(const Type *data_) :
+        Vector<Type, 4>(data_)
+    {
+    }
+
     Quaternion() :
         Vector<Type, 4>()
     {
@@ -199,6 +204,7 @@ public:
 };
 
 typedef Quaternion<float> Quatf;
+typedef Quaternion<float> Quaternionf;
 
 } // namespace matrix
 

matrix/Vector.hpp

@@ -66,8 +66,8 @@ public:
         return Type(sqrt(a.dot(a)));
     }
 
-    inline void normalize() {
-        (*this) /= norm();
+    Vector normalize() const {
+        return  (*this) / norm();
     }
 
     Vector pow(Type v) const {