mathlib fixes

2 files changed, 55 insertions, 57 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. */