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 {

test/attitude.cpp

@@ -49,7 +49,7 @@ int main()
     TEST(fabs(q(3) - 4) < eps);
 
     // quat normalization
-    q.normalize();
+    q = q.normalize();
     TEST(isEqual(q, Quatf(0.18257419f,  0.36514837f,
                           0.54772256f,  0.73029674f)));
 

test/matrixAssignment.cpp

@@ -77,6 +77,10 @@ int main()
     Matrix<float, 1, 1> m5 = s;
     TEST(fabs(m5(0,0) - s) < 1e-5);
 
+    Matrix<float, 2, 2> m6;
+    m6.setRow(0, Vector2f(1, 1));
+    m6.setCol(0, Vector2f(1, 1));
+
     return 0;
 }