mathlib minor fixes

src/lib/mathlib/math/Matrix.hpp

@@ -129,7 +129,7 @@ public:
 	 */
 	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
 		memcpy(data, m.data, sizeof(data));
-		return *reinterpret_cast<Matrix<M, N>*>(this);
+		return *static_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
@@ -158,7 +158,7 @@ public:
  		for (unsigned int i = 0; i < N; i++)
  			for (unsigned int j = 0; j < M; j++)
  				data[i][j] += m.data[i][j];
-		return *reinterpret_cast<Matrix<M, N>*>(this);
+		return *static_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
@@ -176,7 +176,7 @@ public:
  		for (unsigned int i = 0; i < N; i++)
  			for (unsigned int j = 0; j < M; j++)
  				data[i][j] -= m.data[i][j];
-		return *reinterpret_cast<Matrix<M, N>*>(this);
+		return *static_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
@@ -194,7 +194,7 @@ public:
  		for (unsigned int i = 0; i < M; i++)
  			for (unsigned int j = 0; j < N; j++)
  				data[i][j] *= num;
-		return *reinterpret_cast<Matrix<M, N>*>(this);
+		return *static_cast<Matrix<M, N>*>(this);
 	}
 
 	Matrix<M, N> operator /(const float num) const {

src/lib/mathlib/math/Quaternion.hpp

@@ -73,42 +73,6 @@ public:
 	Quaternion(const float *v) : Vector(v) {
 	}
 
-	/**
-	 * access to elements by index
-	 */
-	/*
-	inline float &operator ()(unsigned int i) {
-		return *(&a + i);
-	}
-	*/
-
-	/**
-	 * access to elements by index
-	 */
-	/*
-	inline const float &operator ()(unsigned int i) const {
-		return *(&a + i);
-	}
-	*/
-
-	/**
-	 * addition
-	 */
-	/*
-	const Quaternion operator +(const Quaternion &q) const {
-		return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d);
-	}
-	*/
-
-	/**
-	 * subtraction
-	 */
-	/*
-	const Quaternion operator -(const Quaternion &q) const {
-		return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d);
-	}
-	*/
-
 	Quaternion derivative(const Vector<3> &w) {
 	        float dataQ[] = {
 	                data[0], -data[1], -data[2], -data[3],

src/lib/mathlib/math/Vector.hpp

@@ -135,7 +135,7 @@ public:
 	 */
 	const Vector<N> &operator =(const Vector<N> &v) {
 		memcpy(data, v.data, sizeof(data));
-		return *reinterpret_cast<const Vector<N>*>(this);
+		return *static_cast<const Vector<N>*>(this);
 	}
 
 	/**
@@ -180,7 +180,7 @@ public:
 	 * uniform scaling
 	 */
 	const Vector<N> operator /(const float num) const {
-		Vector<N> temp(*reinterpret_cast<const Vector<N>*>(this));
+		Vector<N> temp(*static_cast<const Vector<N>*>(this));
 		return temp /= num;
 	}
 
@@ -190,7 +190,7 @@ public:
 	const Vector<N> &operator +=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] += v(i);
-		return *reinterpret_cast<const Vector<N>*>(this);
+		return *static_cast<const Vector<N>*>(this);
 	}
 
 	/**
@@ -199,7 +199,7 @@ public:
 	const Vector<N> &operator -=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] -= v(i);
-		return *reinterpret_cast<const Vector<N>*>(this);
+		return *static_cast<const Vector<N>*>(this);
 	}
 
 	/**
@@ -208,7 +208,7 @@ public:
 	const Vector<N> &operator *=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] *= num;
-		return *reinterpret_cast<const Vector<N>*>(this);
+		return *static_cast<const Vector<N>*>(this);
 	}
 
 	/**
@@ -217,7 +217,7 @@ public:
 	const Vector<N> &operator /=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] /= num;
-		return *reinterpret_cast<const Vector<N>*>(this);
+		return *static_cast<const Vector<N>*>(this);
 	}
 
 	/**
@@ -241,7 +241,7 @@ public:
 	 * gets the length of this vector
 	 */
 	float length() const {
-		return sqrtf(*this * *reinterpret_cast<const Vector<N>*>(this));
+		return sqrtf(*this * *static_cast<const Vector<N>*>(this));
 	}
 
 	/**

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -53,21 +53,6 @@ static const int8_t ret_error = -1; 	// error occurred
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
-	// ekf matrices
-	F(),
-	G(),
-	P(),
-	P0(),
-	V(),
-	// attitude measurement ekf matrices
-	HAtt(),
-	RAtt(),
-	// position measurement ekf matrices
-	HPos(),
-	RPos(),
-	// attitude representations
-	C_nb(),
-	q(),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz