mathlib fixes

6 files changed, 82 insertions, 85 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index ecca04dd7..6533eb7cf 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -54,17 +54,17 @@ static const int8_t ret_error = -1; 	// error occurred
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// ekf matrices
-	F(9, 9),
-	G(9, 6),
-	P(9, 9),
-	P0(9, 9),
-	V(6, 6),
+	F(),
+	G(),
+	P(),
+	P0(),
+	V(),
 	// attitude measurement ekf matrices
-	HAtt(4, 9),
-	RAtt(4, 4),
+	HAtt(),
+	RAtt(),
 	// position measurement ekf matrices
-	HPos(6, 9),
-	RPos(6, 6),
+	HPos(),
+	RPos(),
 	// attitude representations
 	C_nb(),
 	q(),
@@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	using namespace math;
 
 	// initial state covariance matrix
-	P0 = Matrix::identity(9) * 0.01f;
+	P0.identity();
+	P0 *= 0.01f;
 	P = P0;
 
 	// initial state
@@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 		_sensors.magnetometer_ga[2]);
 
 	// initialize dcm
-	C_nb = Dcm(q);
+	C_nb.from_quaternion(q);
 
 	// HPos is constant
 	HPos(0, 3) = 1.0f;
@@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt)
 
 	// attitude prediction
 	if (_attitudeInitialized) {
-		Vector3 w(_sensors.gyro_rad_s);
+		Vector<3> w(_sensors.gyro_rad_s);
 
 		// attitude
 		q = q + q.derivative(w) * dt;
 
 		// renormalize quaternion if needed
-		if (fabsf(q.norm() - 1.0f) > 1e-4f) {
-			q = q.unit();
+		if (fabsf(q.length() - 1.0f) > 1e-4f) {
+			q.normalize();
 		}
 
 		// C_nb update
-		C_nb = Dcm(q);
+		C_nb.from_quaternion(q);
 
 		// euler update
-		EulerAngles euler(C_nb);
-		phi = euler.getPhi();
-		theta = euler.getTheta();
-		psi = euler.getPsi();
+		Vector<3> euler = C_nb.to_euler();
+		phi = euler.data[0];
+		theta = euler.data[1];
+		psi = euler.data[2];
 
 		// specific acceleration in nav frame
-		Vector3 accelB(_sensors.accelerometer_m_s2);
-		Vector3 accelN = C_nb * accelB;
+		Vector<3> accelB(_sensors.accelerometer_m_s2);
+		Vector<3> accelN = C_nb * accelB;
 		fN = accelN(0);
 		fE = accelN(1);
 		fD = accelN(2);
@@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt)
 	G(5, 4) = C_nb(2, 1);
 	G(5, 5) = C_nb(2, 2);
 
-	// continuous predictioon equations
-	// for discrte time EKF
+	// continuous prediction equations
+	// for discrete time EKF
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
+	P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
 
 	return ret_ok;
 }
@@ -577,13 +578,14 @@ int KalmanNav::correctAtt()
 
 	// compensate roll and pitch, but not yaw
 	// XXX take the vectors out of the C_nb matrix to avoid singularities
-	math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
+	math::Matrix<3,3> C_rp;
+	C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
 
 	// mag measurement
-	Vector3 magBody(_sensors.magnetometer_ga);
+	Vector<3> magBody(_sensors.magnetometer_ga);
 
 	// transform to earth frame
-	Vector3 magNav = C_rp * magBody;
+	Vector<3> magNav = C_rp * magBody;
 
 	// calculate error between estimate and measurement
 	// apply declination correction for true heading as well.
@@ -592,12 +594,12 @@ int KalmanNav::correctAtt()
 	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
 
 	// accel measurement
-	Vector3 zAccel(_sensors.accelerometer_m_s2);
-	float accelMag = zAccel.norm();
-	zAccel = zAccel.unit();
+	Vector<3> zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.length();
+	zAccel.normalize();
 
 	// ignore accel correction when accel mag not close to g
-	Matrix RAttAdjust = RAtt;
+	Matrix<4,4> RAttAdjust = RAtt;
 
 	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
@@ -611,14 +613,10 @@ int KalmanNav::correctAtt()
 	}
 
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
+	Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
 
 	// calculate residual
-	Vector y(4);
-	y(0) = yMag;
-	y(1) = zAccel(0) - zAccelHat(0);
-	y(2) = zAccel(1) - zAccelHat(1);
-	y(3) = zAccel(2) - zAccelHat(2);
+	Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
 
 	// HMag
 	HAtt(0, 2) = 1;
@@ -632,9 +630,9 @@ int KalmanNav::correctAtt()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
-	Matrix K = P * HAtt.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
+	Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correciton is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
@@ -669,7 +667,7 @@ int KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	if (beta > _faultAtt.get()) {
 		warnx("fault in attitude: beta = %8.4f", (double)beta);
@@ -678,7 +676,7 @@ int KalmanNav::correctAtt()
 
 	// update quaternions from euler
 	// angle correction
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	q.from_euler(phi, theta, psi);
 
 	return ret_ok;
 }
@@ -688,7 +686,7 @@ int KalmanNav::correctPos()
 	using namespace math;
 
 	// residual
-	Vector y(6);
+	Vector<6> y;
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
 	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@@ -698,9 +696,9 @@ int KalmanNav::correctPos()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
-	Matrix K = P * HPos.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
+	Matrix<9,6> K = P * HPos.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correction is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
@@ -735,7 +733,7 @@ int KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index a69bde1a6..46ee4b6c8 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -125,17 +125,17 @@ public:
 	virtual void updateParams();
 protected:
 	// kalman filter
-	math::Matrix F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
-	math::Matrix G;             /**< noise shaping matrix for gyro/accel */
-	math::Matrix P;             /**< state covariance matrix */
-	math::Matrix P0;            /**< initial state covariance matrix */
-	math::Matrix V;             /**< gyro/ accel noise matrix */
-	math::Matrix HAtt;          /**< attitude measurement matrix */
-	math::Matrix RAtt;          /**< attitude measurement noise matrix */
-	math::Matrix HPos;          /**< position measurement jacobian matrix */
-	math::Matrix RPos;          /**< position measurement noise matrix */
+	math::Matrix<9,9> F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
+	math::Matrix<9,6> G;             /**< noise shaping matrix for gyro/accel */
+	math::Matrix<9,9> P;             /**< state covariance matrix */
+	math::Matrix<9,9> P0;            /**< initial state covariance matrix */
+	math::Matrix<6,6> V;             /**< gyro/ accel noise matrix */
+	math::Matrix<4,9> HAtt;          /**< attitude measurement matrix */
+	math::Matrix<4,4> RAtt;          /**< attitude measurement noise matrix */
+	math::Matrix<6,9> HPos;          /**< position measurement jacobian matrix */
+	math::Matrix<6,6> RPos;          /**< position measurement noise matrix */
 	// attitude
-	math::Dcm C_nb;             /**< direction cosine matrix from body to nav frame */
+	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
 	math::Quaternion q;         /**< quaternion from body to nav frame */
 	// subscriptions
 	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 5eeca5a1a..36b75dd58 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix board_rotation(3, 3);
+		math::Matrix<3,3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix board_rotation_t = board_rotation.transpose();
-		math::Vector3 accel_offs_vec;
-		accel_offs_vec.set(&accel_offs[0]);
-		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix accel_T_mat(3, 3);
-		accel_T_mat.set(&accel_T[0][0]);
-		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
+		math::Vector<3> accel_offs_vec(&accel_offs[0]);
+		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
+		math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
 
 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..653d4b6b3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg)
 
 			/* Calculate Rotation Matrix */
 			math::Quaternion q(hil_state.attitude_quaternion);
-			math::Dcm C_nb(q);
-			math::EulerAngles euler(C_nb);
+			math::Matrix<3,3> C_nb;
+			C_nb.from_quaternion(q);
+			math::Vector<3> euler = C_nb.to_euler();
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg)
 			hil_attitude.q[3] = q(3);
 			hil_attitude.q_valid = true;
 
-			hil_attitude.roll = euler.getPhi();
-			hil_attitude.pitch = euler.getTheta();
-			hil_attitude.yaw = euler.getPsi();
+			hil_attitude.roll = euler(0);
+			hil_attitude.pitch = euler(1);
+			hil_attitude.yaw = euler(2);
 			hil_attitude.rollspeed = hil_state.rollspeed;
 			hil_attitude.pitchspeed = hil_state.pitchspeed;
 			hil_attitude.yawspeed = hil_state.yawspeed;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f6c44444a..04307d96b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -422,11 +422,11 @@ Navigator::task_main()
 
 			mission_poll();
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
+			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
 			// Current waypoint
-			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
+			math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
 			// Global position
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
 
 			/* AUTONOMOUS FLIGHT */
 
@@ -436,19 +436,19 @@ Navigator::task_main()
 				if (_mission_valid) {
 
 					// Next waypoint
-					math::Vector2f prev_wp;
+					math::Vector<2> prev_wp;
 
 					if (_global_triplet.previous_valid) {
-						prev_wp.setX(_global_triplet.previous.lat / 1e7f);
-						prev_wp.setY(_global_triplet.previous.lon / 1e7f);
+						prev_wp(0) = _global_triplet.previous.lat / 1e7f;
+						prev_wp(1) = _global_triplet.previous.lon / 1e7f;
 
 					} else {
 						/*
 						 * No valid next waypoint, go for heading hold.
 						 * This is automatically handled by the L1 library.
 						 */
-						prev_wp.setX(_global_triplet.current.lat / 1e7f);
-						prev_wp.setY(_global_triplet.current.lon / 1e7f);
+						prev_wp(0) = _global_triplet.current.lat / 1e7f;
+						prev_wp(1) = _global_triplet.current.lon / 1e7f;
 
 					}
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6d38b98ec..5baab4a5d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -211,8 +211,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3,3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3,3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
 	struct {
@@ -457,8 +457,8 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3, 3),
-	_external_mag_rotation(3, 3),
+	_board_rotation(),
+	_external_mag_rotation(),
 	_mag_is_external(false)
 {
 
@@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z};
 		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
@@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z};
 		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
@@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
 		if (_mag_is_external)
 			vect = _external_mag_rotation * vect;