Merge pull request #319 from PX4/kalman_nav

Kalman navigation fixes

5 files changed, 168 insertions, 101 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index c3836bdfa..197c2e36c 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +40,7 @@
 #include <poll.h>
 
 #include "KalmanNav.hpp"
+#include <systemlib/err.h>
 
 // constants
 // Titterton pg. 52
@@ -58,14 +59,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	P0(9, 9),
 	V(6, 6),
 	// attitude measurement ekf matrices
-	HAtt(6, 9),
-	RAtt(6, 6),
+	HAtt(4, 9),
+	RAtt(4, 4),
 	// position measurement ekf matrices
 	HPos(6, 9),
 	RPos(6, 6),
 	// attitude representations
 	C_nb(),
 	q(),
+	_accel_sub(-1),
+	_gyro_sub(-1),
+	_mag_sub(-1),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -123,8 +127,19 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-	// initialize quaternions
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	// gyro, accel and mag subscriptions
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+
+	struct accel_report accel;
+	orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
+
+	struct mag_report mag;
+	orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
+
+	// initialize rotation quaternion with a single raw sensor measurement
+	q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
 
 	// initialize dcm
 	C_nb = Dcm(q);
@@ -141,6 +156,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	updateParams();
 }
 
+math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    return math::Quaternion(q0, q1, q2, q3);
+
+}
+
 void KalmanNav::update()
 {
 	using namespace math;
@@ -181,8 +235,8 @@ void KalmanNav::update()
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
-			printf("[kalman_demo] initialized EKF attitude\n");
-			printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			warnx("initialized EKF attitude\n");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
 			       double(phi), double(theta), double(psi));
 			_attitudeInitialized = true;
 		}
@@ -202,8 +256,8 @@ void KalmanNav::update()
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
 		_positionInitialized = true;
-		printf("[kalman_demo] initialized EKF state with GPS\n");
-		printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("initialized EKF state with GPS\n");
+		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
 		       double(vN), double(vE), double(vD),
 		       lat, lon, alt);
 	}
@@ -233,7 +287,7 @@ void KalmanNav::update()
 	// attitude correction step
 	if (_attitudeInitialized 								// initialized
 	    && sensorsUpdate 								// new data
-	    && _sensors.timestamp - _attTimeStamp > 1e6 / 20 	// 20 Hz
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
 	   ) {
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
@@ -480,26 +534,32 @@ int KalmanNav::correctAtt()
 	// trig
 	float cosPhi = cosf(phi);
 	float cosTheta = cosf(theta);
-	float cosPsi = cosf(psi);
+	// float cosPsi = cosf(psi);
 	float sinPhi = sinf(phi);
 	float sinTheta = sinf(theta);
-	float sinPsi = sinf(psi);
-
-	// mag measurement
-	Vector3 zMag(_sensors.magnetometer_ga);
-	//float magNorm = zMag.norm();
-	zMag = zMag.unit();
+	// float sinPsi = sinf(psi);
 
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
 	float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
-	float bN = cosf(dip) * cosf(dec);
-	float bE = cosf(dip) * sinf(dec);
-	float bD = sinf(dip);
-	Vector3 bNav(bN, bE, bD);
-	Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
+
+	// 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();
+
+	// mag measurement
+	Vector3 magBody(_sensors.magnetometer_ga);
+
+	// transform to earth frame
+	Vector3 magNav = C_rp * magBody;
+
+	// calculate error between estimate and measurement
+	// apply declination correction for true heading as well.
+	float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
+	if (yMag > M_PI_F) yMag -= 2*M_PI_F;
+	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
 
 	// accel measurement
 	Vector3 zAccel(_sensors.accelerometer_m_s2);
@@ -512,9 +572,9 @@ int KalmanNav::correctAtt()
 	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
 	if (ignoreAccel) {
+		RAttAdjust(1, 1) = 1.0e10;
+		RAttAdjust(2, 2) = 1.0e10;
 		RAttAdjust(3, 3) = 1.0e10;
-		RAttAdjust(4, 4) = 1.0e10;
-		RAttAdjust(5, 5) = 1.0e10;
 
 	} else {
 		//printf("correcting attitude with accel\n");
@@ -523,58 +583,25 @@ int KalmanNav::correctAtt()
 	// accel predicted measurement
 	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
 
-	// combined measurement
-	Vector zAtt(6);
-	Vector zAttHat(6);
+	// 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);
 
-	for (int i = 0; i < 3; i++) {
-		zAtt(i) = zMag(i);
-		zAtt(i + 3) = zAccel(i);
-		zAttHat(i) = zMagHat(i);
-		zAttHat(i + 3) = zAccelHat(i);
-	}
+	// HMag
+	HAtt(0, 2) = 1;
 
-	// HMag , HAtt (0-2,:)
-	float tmp1 =
-		cosPsi * cosTheta * bN +
-		sinPsi * cosTheta * bE -
-		sinTheta * bD;
-	HAtt(0, 1) = -(
-			     cosPsi * sinTheta * bN +
-			     sinPsi * sinTheta * bE +
-			     cosTheta * bD
-		     );
-	HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
-	HAtt(1, 0) =
-		(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
-		(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
-		cosPhi * cosTheta * bD;
-	HAtt(1, 1) = sinPhi * tmp1;
-	HAtt(1, 2) = -(
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
-		     );
-	HAtt(2, 0) = -(
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
-			     (sinPhi * cosTheta) * bD
-		     );
-	HAtt(2, 1) = cosPhi * tmp1;
-	HAtt(2, 2) = -(
-			     (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
-			     (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
-		     );
-
-	// HAccel , HAtt (3-5,:)
-	HAtt(3, 1) = cosTheta;
-	HAtt(4, 0) = -cosPhi * cosTheta;
-	HAtt(4, 1) = sinPhi * sinTheta;
-	HAtt(5, 0) = sinPhi * cosTheta;
-	HAtt(5, 1) = cosPhi * sinTheta;
+	// HAccel
+	HAtt(1, 1) = cosTheta;
+	HAtt(2, 0) = -cosPhi * cosTheta;
+	HAtt(2, 1) = sinPhi * sinTheta;
+	HAtt(3, 0) = sinPhi * cosTheta;
+	HAtt(3, 1) = cosPhi * sinTheta;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Vector y = zAtt - zAttHat; // residual
 	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
 	Matrix K = P * HAtt.transpose() * S.inverse();
 	Vector xCorrect = K * y;
@@ -585,7 +612,7 @@ int KalmanNav::correctAtt()
 
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in att correction\n");
+			warnx("numerical failure in att correction\n");
 			// reset P matrix to P0
 			P = P0;
 			return ret_error;
@@ -615,11 +642,8 @@ int KalmanNav::correctAtt()
 	float beta = y.dot(S.inverse() * y);
 
 	if (beta > _faultAtt.get()) {
-		printf("fault in attitude: beta = %8.4f\n", (double)beta);
-		printf("y:\n"); y.print();
-		printf("zMagHat:\n"); zMagHat.print();
-		printf("zMag:\n"); zMag.print();
-		printf("bNav:\n"); bNav.print();
+		warnx("fault in attitude: beta = %8.4f", (double)beta);
+		warnx("y:\n"); y.print();
 	}
 
 	// update quaternions from euler
@@ -652,9 +676,9 @@ int KalmanNav::correctPos()
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
 
-		if (isnan(val) || isinf(val)) {
+		if (!isfinite(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in gps correction\n");
+			warnx("numerical failure in gps correction\n");
 			// fallback to GPS
 			vN = _gps.vel_n_m_s;
 			vE = _gps.vel_e_m_s;
@@ -685,8 +709,8 @@ int KalmanNav::correctPos()
 
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
-		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("fault in gps: beta = %8.4f", (double)beta);
+		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
 		       double(y(0) / sqrtf(RPos(0, 0))),
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
@@ -722,8 +746,6 @@ void KalmanNav::updateParams()
 	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
 
 	RAtt(0, 0) = noiseMagSq; // normalized direction
-	RAtt(1, 1) = noiseMagSq;
-	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
 	float noiseAccelSq = _rAccel.get() * _rAccel.get();
@@ -731,9 +753,9 @@ void KalmanNav::updateParams()
 	// bound noise to prevent singularities
 	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
 
-	RAtt(3, 3) = noiseAccelSq; // normalized direction
-	RAtt(4, 4) = noiseAccelSq;
-	RAtt(5, 5) = noiseAccelSq;
+	RAtt(1, 1) = noiseAccelSq; // normalized direction
+	RAtt(2, 2) = noiseAccelSq;
+	RAtt(3, 3) = noiseAccelSq;
 
 	// gps noise
 	float R = R0 + float(alt);
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index c2bf18115..49d0d157d 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -56,6 +56,10 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+
 #include <drivers/drv_hrt.h>
 #include <poll.h>
 #include <unistd.h>
@@ -78,6 +82,9 @@ public:
 	 */
 
 	virtual ~KalmanNav() {};
+
+	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
+
 	/**
 	 * The main callback function for the class
 	 */
@@ -136,6 +143,11 @@ protected:
 	// publications
 	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
 	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+
+	int			_accel_sub;		/**< Accelerometer subscription */
+	int			_gyro_sub;		/**< Gyroscope subscription */
+	int			_mag_sub;		/**< Magnetometer subscription */
+
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
 	uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -151,7 +163,8 @@ protected:
 	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
 	float phi, theta, psi;                  /**< 3-2-1 euler angles */
 	float vN, vE, vD;                       /**< navigation velocity, m/s */
-	double lat, lon, alt;                   /**< lat, lon, alt, radians */
+	double lat, lon;                   	/**< lat, lon radians */
+	float alt;                   		/**< altitude, meters */
 	// parameters
 	control::BlockParam<float> _vGyro;      /**< gyro process noise */
 	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 890184427..4befdc879 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	James Goppert
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,8 +33,10 @@
  ****************************************************************************/
 
 /**
- * @file kalman_demo.cpp
- * Demonstration of control library
+ * @file kalman_main.cpp
+ * Combined attitude / position estimator.
+ *
+ * @author James Goppert
  */
 
 #include <nuttx/config.h>
@@ -51,7 +53,7 @@
 
 static bool thread_should_exit = false;     /**< Deamon exit flag */
 static bool thread_running = false;     /**< Deamon status flag */
-static int deamon_task;             /**< Handle of deamon task / thread */
+static int daemon_task;             /**< Handle of deamon task / thread */
 
 /**
  * Deamon management function.
@@ -102,9 +104,9 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 
-		deamon_task = task_spawn_cmd("att_pos_estimator_ekf",
+		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
+					 SCHED_PRIORITY_MAX - 30,
 					 4096,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -134,7 +136,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 int kalman_demo_thread_main(int argc, char *argv[])
 {
 
-	warnx("starting\n");
+	warnx("starting");
 
 	using namespace math;
 
@@ -146,7 +148,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
 		nav.update();
 	}
 
-	printf("exiting.\n");
+	warnx("exiting.");
 
 	thread_running = false;
 
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk
index 21b7c9166..8d4a40d95 100644
--- a/src/modules/att_pos_estimator_ekf/module.mk
+++ b/src/modules/att_pos_estimator_ekf/module.mk
@@ -37,9 +37,6 @@
 
 MODULE_COMMAND	= att_pos_estimator_ekf
 
-# XXX this might be intended for the spawned deamon, validate
-MODULE_PRIORITY	 = "SCHED_PRIORITY_MAX-30"
-
 SRCS		= kalman_main.cpp \
 		  KalmanNav.cpp \
 		  params.c
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c
index 50642f067..4d21b342b 100644
--- a/src/modules/att_pos_estimator_ekf/params.c
+++ b/src/modules/att_pos_estimator_ekf/params.c
@@ -1,3 +1,36 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/