Added position initialization.

3 files changed, 103 insertions, 82 deletions

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 6acf39be8..15254f7c7 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -46,6 +46,8 @@
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
 static const float R0 = 6378137.0f; // earth radius, m
 static const float g0 = 9.806f; // standard gravitational accel. m/s^2
+static const int8_t ret_ok = 0; 		// no error in function
+static const int8_t ret_error = -1; 	// error occurred
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
@@ -99,7 +101,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_g(this, "ENV_G"),
 	_faultPos(this, "FAULT_POS"),
 	_faultAtt(this, "FAULT_ATT"),
-	_positionInitialized(false)
+	_attitudeInitialized(false),
+	_positionInitialized(false),
+	_attitudeInitCounter(0)
 {
 	using namespace math;
 
@@ -146,7 +150,6 @@ void KalmanNav::update()
 	// poll for new data
 	int ret = poll(fds, 1, 1000);
 
-	// check return value
 	if (ret < 0) {
 		// XXX this is seriously bad - should be an emergency
 		return;
@@ -168,11 +171,24 @@ void KalmanNav::update()
 	// this clears update flag
 	updateSubscriptions();
 
-	// abort update if no new data
-	if (!(sensorsUpdate || gpsUpdate)) return;
+	// initialize attitude when sensors online
+	if (!_attitudeInitialized && sensorsUpdate &&
+	    _sensors.accelerometer_counter > 10 &&
+	    _sensors.gyro_counter > 10 &&
+	    _sensors.magnetometer_counter > 10) {
+		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",
+			       double(phi), double(theta), double(psi));
+			_attitudeInitialized = true;
+		}
+	}
 
-	// if received gps for first time, reset position to gps
+	// initialize position when gps received
 	if (!_positionInitialized &&
+	    _attitudeInitialized && // wait for attitude first
 	    gpsUpdate &&
 	    _gps.fix_type > 2 &&
 	    _gps.counter_pos_valid > 10) {
@@ -183,9 +199,7 @@ void KalmanNav::update()
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
 		_positionInitialized = true;
-		printf("[kalman_demo] initializing EKF state with GPS\n");
-		printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
-		       double(phi), double(theta), double(psi));
+		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",
 		       double(vN), double(vE), double(vD),
 		       lat, lon, alt);
@@ -194,7 +208,7 @@ void KalmanNav::update()
 	// prediciton step
 	// using sensors timestamp so we can account for packet lag
 	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
-	//printf("dt: %15.10f\n", double(dtFast));
+	//printf("dt: %15.10f\n", double(dt));
 	_predictTimeStamp = _sensors.timestamp;
 
 	// don't predict if time greater than a second
@@ -209,12 +223,15 @@ void KalmanNav::update()
 	if (dt > 0.01f) _miss++;
 
 	// gps correction step
-	if (gpsUpdate) {
+	if (_positionInitialized && gpsUpdate) {
 		correctPos();
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
+	if (_attitudeInitialized 								// initialized
+	    && sensorsUpdate 								// new data
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 20 	// 20 Hz
+	   ) {
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -278,34 +295,9 @@ void KalmanNav::updatePublications()
 	SuperBlock::updatePublications();
 }
 
-void KalmanNav::predictState(float dt)
+int KalmanNav::predictState(float dt)
 {
 	using namespace math;
-	Vector3 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();
-	}
-
-	// C_nb update
-	C_nb = Dcm(q);
-
-	// euler update
-	EulerAngles euler(C_nb);
-	phi = euler.getPhi();
-	theta = euler.getTheta();
-	psi = euler.getPsi();
-
-	// specific acceleration in nav frame
-	Vector3 accelB(_sensors.accelerometer_m_s2);
-	Vector3 accelN = C_nb * accelB;
-	fN = accelN(0);
-	fE = accelN(1);
-	fD = accelN(2);
 
 	// trig
 	float sinL = sinf(lat);
@@ -318,30 +310,63 @@ void KalmanNav::predictState(float dt)
 		else cosLSing = -0.01;
 	}
 
-	// position update
-	// neglects angular deflections in local gravity
-	// see Titerton pg. 70
-	float R = R0 + float(alt);
-	float LDot = vN / R;
-	float lDot = vE / (cosLSing * R);
-	float rotRate = 2 * omega + lDot;
-	float vNDot = fN - vE * rotRate * sinL +
-		      vD * LDot;
-	float vDDot = fD - vE * rotRate * cosL -
-		      vN * LDot + _g.get();
-	float vEDot = fE + vN * rotRate * sinL +
-		      vDDot * rotRate * cosL;
-
-	// rectangular integration
-	vN += vNDot * dt;
-	vE += vEDot * dt;
-	vD += vDDot * dt;
-	lat += double(LDot * dt);
-	lon += double(lDot * dt);
-	alt += double(-vD * dt);
+	// attitude prediction
+	if (_attitudeInitialized) {
+		Vector3 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();
+		}
+
+		// C_nb update
+		C_nb = Dcm(q);
+
+		// euler update
+		EulerAngles euler(C_nb);
+		phi = euler.getPhi();
+		theta = euler.getTheta();
+		psi = euler.getPsi();
+
+		// specific acceleration in nav frame
+		Vector3 accelB(_sensors.accelerometer_m_s2);
+		Vector3 accelN = C_nb * accelB;
+		fN = accelN(0);
+		fE = accelN(1);
+		fD = accelN(2);
+	}
+
+	// position prediction
+	if (_positionInitialized) {
+		// neglects angular deflections in local gravity
+		// see Titerton pg. 70
+		float R = R0 + float(alt);
+		float LDot = vN / R;
+		float lDot = vE / (cosLSing * R);
+		float rotRate = 2 * omega + lDot;
+		float vNDot = fN - vE * rotRate * sinL +
+			      vD * LDot;
+		float vDDot = fD - vE * rotRate * cosL -
+			      vN * LDot + _g.get();
+		float vEDot = fE + vN * rotRate * sinL +
+			      vDDot * rotRate * cosL;
+
+		// rectangular integration
+		vN += vNDot * dt;
+		vE += vEDot * dt;
+		vD += vDDot * dt;
+		lat += double(LDot * dt);
+		lon += double(lDot * dt);
+		alt += double(-vD * dt);
+	}
+
+	return ret_ok;
 }
 
-void KalmanNav::predictStateCovariance(float dt)
+int KalmanNav::predictStateCovariance(float dt)
 {
 	using namespace math;
 
@@ -434,18 +459,14 @@ void KalmanNav::predictStateCovariance(float dt)
 	// for discrte time EKF
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
+
+	return ret_ok;
 }
 
-void KalmanNav::correctAtt()
+int KalmanNav::correctAtt()
 {
 	using namespace math;
 
-	// check for valid data, return if not ready
-	if (_sensors.accelerometer_counter < 10 ||
-	    _sensors.gyro_counter < 10 ||
-	    _sensors.magnetometer_counter < 10)
-		return;
-
 	// trig
 	float cosPhi = cosf(phi);
 	float cosTheta = cosf(theta);
@@ -489,12 +510,8 @@ void KalmanNav::correctAtt()
 		//printf("correcting attitude with accel\n");
 	}
 
-	// account for banked turn
-	// this would only work for fixed wing, so try to avoid
-	//Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
-
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get()) /*+ zCentrip*/).unit();
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
 
 	// combined measurement
 	Vector zAtt(6);
@@ -561,7 +578,7 @@ void KalmanNav::correctAtt()
 			printf("[kalman_demo] numerical failure in att correction\n");
 			// reset P matrix to P0
 			P = P0;
-			return;
+			return ret_error;
 		}
 	}
 
@@ -595,14 +612,14 @@ void KalmanNav::correctAtt()
 	// update quaternions from euler
 	// angle correction
 	q = Quaternion(EulerAngles(phi, theta, psi));
+
+	return ret_ok;
 }
 
-void KalmanNav::correctPos()
+int KalmanNav::correctPos()
 {
 	using namespace math;
 
-	if (!_positionInitialized) return;
-
 	// residual
 	Vector y(5);
 	y(0) = _gps.vel_n - vN;
@@ -633,7 +650,7 @@ void KalmanNav::correctPos()
 			setAltE3(_gps.alt);
 			// reset P matrix to P0
 			P = P0;
-			return;
+			return ret_error;
 		}
 	}
 
@@ -661,6 +678,8 @@ void KalmanNav::correctPos()
 		       double(y(3) / sqrtf(RPos(3, 3))),
 		       double(y(4) / sqrtf(RPos(4, 4))));
 	}
+
+	return ret_ok;
 }
 
 void KalmanNav::updateParams()
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index da1a7ee10..7709ac697 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -93,23 +93,23 @@ public:
 	 * State prediction
 	 * Continuous, non-linear
 	 */
-	void predictState(float dt);
+	int predictState(float dt);
 
 	/**
 	 * State covariance prediction
 	 * Continuous, linear
 	 */
-	void predictStateCovariance(float dt);
+	int predictStateCovariance(float dt);
 
 	/**
 	 * Attitude correction
 	 */
-	void correctAtt();
+	int correctAtt();
 
 	/**
 	 * Position correction
 	 */
-	void correctPos();
+	int correctPos();
 
 	/**
 	 * Overloaded update parameters
@@ -166,7 +166,9 @@ protected:
 	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
 	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
 	// status
-	bool _positionInitialized; 			    /**< status, if position has been init. */
+	bool _attitudeInitialized;
+	bool _positionInitialized;
+	uint16_t _attitudeInitCounter;
 	// accessors
 	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
 	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index bb18d5b92..77225b4c7 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -8,7 +8,7 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
 PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);