From 5745cfae385e5a7fe7948977ea90facb9360c2c7 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Sun, 13 Jan 2013 21:12:24 -0500
Subject: Tracking down gps ekf bug, not enough precision for GPS in rad.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 42 +++++++++++++++++++++++++--------
 1 file changed, 32 insertions(+), 10 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 240564b56..18bca6d95 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -316,7 +316,11 @@ void KalmanNav::predictFast(float dt)
 	float cosL = cosf(lat);
 	float cosLSing = cosf(lat);
 
-	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+    // prevent singularity
+	if (fabsf(cosLSing) < 0.01f) {
+        if (cosLSing > 0) cosLSing = 0.01;
+        else cosLSing = -0.01;
+    }
 
 	// position update
 	// neglects angular deflections in local gravity
@@ -577,11 +581,11 @@ void KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse() * y);
+	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > 1.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
-		//printf("y:\n"); y.print();
+		printf("y:\n"); y.print();
 	}
 
 	// update quaternions from euler
@@ -592,7 +596,7 @@ void KalmanNav::correctAtt()
 void KalmanNav::correctPos()
 {
 	using namespace math;
-	Vector y(6);
+	Vector y(5);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
 	y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
@@ -604,7 +608,10 @@ void KalmanNav::correctPos()
 	float R = R0 + float(alt);
 
 	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+	if (fabsf(cosLSing) < 0.01f) {
+        if (cosLSing > 0) cosLSing = 0.01;
+        else cosLSing = -0.01;
+    }
 
 	float noiseLat = _rGpsPos.get() / R;
 	float noiseLon = _rGpsPos.get() / (R * cosLSing);
@@ -650,11 +657,23 @@ void KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse() * y);
+	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > 1.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		//printf("y:\n"); y.print();
+		printf("y:\n"); y.print();
+		printf("R:\n"); RPos.print();
+		printf("S:\n"); S.print();
+		printf("S*S^T:\n"); (S*S.transpose()).print();
+		printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
+		printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
+        printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
+                double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d),
+                double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
+                double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG,
+                double(_gps.alt)/ 1.0e3);
+        printf("x  : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
+                double(vN), double(vE), double(vD), lat, lon, alt);
 	}
 }
 
@@ -692,7 +711,10 @@ void KalmanNav::updateParams()
 	float R = R0 + float(alt);
 
 	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+    if (fabsf(cosLSing) < 0.01f) {
+        if (cosLSing > 0) cosLSing = 0.01;
+        else cosLSing = -0.01;
+    }
 
 	float noiseVel = _rGpsVel.get();
 	float noiseLat = _rGpsPos.get() / R;
-- 
cgit v1.2.3


From 3db216380bed13fd25c21e49a1fbd66680968937 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 01:09:02 -0500
Subject: Changing measurement units for gps, not working well yet.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 26 +++++++++++++-------------
 1 file changed, 13 insertions(+), 13 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 18bca6d95..788cb5a71 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -141,8 +141,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// HPos is constant
 	HPos(0, 3) = 1.0f;
 	HPos(1, 4) = 1.0f;
-	HPos(2, 6) = 1.0f;
-	HPos(3, 7) = 1.0f;
+	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
+	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
 	HPos(4, 8) = 1.0f;
 
 	// initialize all parameters
@@ -599,9 +599,9 @@ void KalmanNav::correctPos()
 	Vector y(5);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
-	y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
-	y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
-	y(4) = double(_gps.alt) / 1.0e3 - alt;
+	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
+	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
+	y(4) = double(_gps.alt)/1.0e3 - alt;
 
 	// update gps noise
 	float cosLSing = cosf(lat);
@@ -613,10 +613,10 @@ void KalmanNav::correctPos()
         else cosLSing = -0.01;
     }
 
-	float noiseLat = _rGpsPos.get() / R;
-	float noiseLon = _rGpsPos.get() / (R * cosLSing);
-	RPos(2, 2) = noiseLat * noiseLat;
-	RPos(3, 3) = noiseLon * noiseLon;
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R;
+	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
+	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
+	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -717,12 +717,12 @@ void KalmanNav::updateParams()
     }
 
 	float noiseVel = _rGpsVel.get();
-	float noiseLat = _rGpsPos.get() / R;
-	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R;
+	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	float noiseAlt = _rGpsAlt.get();
 	RPos(0, 0) = noiseVel * noiseVel; // vn
 	RPos(1, 1) = noiseVel * noiseVel; // ve
-	RPos(2, 2) = noiseLat * noiseLat; // lat
-	RPos(3, 3) = noiseLon * noiseLon; // lon
+	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
+	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
 	RPos(4, 4) = noiseAlt * noiseAlt; // h
 }
-- 
cgit v1.2.3


From f2d2aafb8d340bdad7e75e9df27b8fd002da1344 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 01:32:34 -0500
Subject: Fault detection working, but GPS velocity measurement causing fault.

Possible error in HIL script or progpagation/ F matrix of EKF.
---
 apps/examples/kalman_demo/KalmanNav.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 788cb5a71..c3093fe7f 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -613,7 +613,7 @@ void KalmanNav::correctPos()
         else cosLSing = -0.01;
     }
 
-	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R;
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
@@ -662,11 +662,11 @@ void KalmanNav::correctPos()
 	if (beta > 1.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
 		printf("y:\n"); y.print();
-		printf("R:\n"); RPos.print();
-		printf("S:\n"); S.print();
-		printf("S*S^T:\n"); (S*S.transpose()).print();
-		printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
-		printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
+		//printf("R:\n"); RPos.print();
+		//printf("S:\n"); S.print();
+		//printf("S*S^T:\n"); (S*S.transpose()).print();
+		//printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
+		//printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
         printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
                 double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d),
                 double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
@@ -717,7 +717,7 @@ void KalmanNav::updateParams()
     }
 
 	float noiseVel = _rGpsVel.get();
-	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG * _rGpsPos.get() / R;
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	float noiseAlt = _rGpsAlt.get();
 	RPos(0, 0) = noiseVel * noiseVel; // vn
-- 
cgit v1.2.3


From a13cf90e0ac89527830b6426e70913d67ec093b2 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 13:12:01 -0500
Subject: Increased KF process noise.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 21 ++++++---------------
 apps/examples/kalman_demo/params.c      |  4 ++--
 2 files changed, 8 insertions(+), 17 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index c3093fe7f..d28cba42c 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -583,7 +583,7 @@ void KalmanNav::correctAtt()
 	// fault detection
 	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1.0f) {
+	if (beta > 10.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		printf("y:\n"); y.print();
 	}
@@ -613,8 +613,10 @@ void KalmanNav::correctPos()
         else cosLSing = -0.01;
     }
 
+	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
+	float noiseAlt = _rGpsAlt.get();
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
 
@@ -659,21 +661,10 @@ void KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot((S*S.transpose()).inverse() * y);
 
-	if (beta > 1.0f) {
+	if (beta > 10.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		printf("y:\n"); y.print();
-		//printf("R:\n"); RPos.print();
-		//printf("S:\n"); S.print();
-		//printf("S*S^T:\n"); (S*S.transpose()).print();
-		//printf("(S*S^T)^-1:\n"); ((S*S.transpose()).inverse()).print();
-		//printf("(S*S^T)^-1*y:\n"); ((S*S.transpose()).inverse()*y).print();
-        printf("gps: vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
-                double(_gps.vel_n), double(_gps.vel_e), double(_gps.vel_d),
-                double(_gps.lat)/ 1.0e7 / M_RAD_TO_DEG,
-                double(_gps.lon)/ 1.0e7 / M_RAD_TO_DEG,
-                double(_gps.alt)/ 1.0e3);
-        printf("x  : vN: %8.5f, vE: %8.4f, vD: %8.4f, lat: %15.10f, lon: %15.10f, alt: %15.2f\n", 
-                double(vN), double(vE), double(vD), lat, lon, alt);
+        printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+                y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt);
 	}
 }
 
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 03cdca5ed..f36ac334f 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,8 +1,8 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
-- 
cgit v1.2.3


From c49320a03ef7f57ae01e9943fc1b4c340c648050 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 13:49:30 -0500
Subject: Working on fault detection tolerances.

---
 apps/examples/control_demo/params.c     |  4 ++--
 apps/examples/kalman_demo/KalmanNav.cpp | 40 ++++++++++++++++++---------------
 2 files changed, 24 insertions(+), 20 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 8f923f5a1..aea260330 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -24,7 +24,7 @@ PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
 PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
@@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
 
 
 // theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index d28cba42c..e804fbca5 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -316,11 +316,11 @@ void KalmanNav::predictFast(float dt)
 	float cosL = cosf(lat);
 	float cosLSing = cosf(lat);
 
-    // prevent singularity
+	// prevent singularity
 	if (fabsf(cosLSing) < 0.01f) {
-        if (cosLSing > 0) cosLSing = 0.01;
-        else cosLSing = -0.01;
-    }
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
 
 	// position update
 	// neglects angular deflections in local gravity
@@ -581,9 +581,9 @@ void KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot((S*S.transpose()).inverse() * y);
+	float beta = y.dot((S * S.transpose()).inverse() * y);
 
-	if (beta > 10.0f) {
+	if (beta > 1000.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		printf("y:\n"); y.print();
 	}
@@ -601,7 +601,7 @@ void KalmanNav::correctPos()
 	y(1) = _gps.vel_e - vE;
 	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
 	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = double(_gps.alt)/1.0e3 - alt;
+	y(4) = double(_gps.alt) / 1.0e3 - alt;
 
 	// update gps noise
 	float cosLSing = cosf(lat);
@@ -609,9 +609,9 @@ void KalmanNav::correctPos()
 
 	// prevent singularity
 	if (fabsf(cosLSing) < 0.01f) {
-        if (cosLSing > 0) cosLSing = 0.01;
-        else cosLSing = -0.01;
-    }
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
 
 	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
@@ -659,12 +659,16 @@ void KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot((S*S.transpose()).inverse() * y);
+	float beta = y.dot((S * S.transpose()).inverse() * y);
 
-	if (beta > 10.0f) {
+	if (beta > 1000.0f) {
 		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",
-                y(0)/noiseVel, y(1)/noiseVel, y(2)/noiseLatDegE7, y(3)/noiseLonDegE7, y(4)/noiseAlt);
+		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		       double(y(0) / noiseVel),
+		       double(y(1) / noiseVel),
+		       double(y(2) / noiseLatDegE7),
+		       double(y(3) / noiseLonDegE7),
+		       double(y(4) / noiseAlt));
 	}
 }
 
@@ -702,10 +706,10 @@ void KalmanNav::updateParams()
 	float R = R0 + float(alt);
 
 	// prevent singularity
-    if (fabsf(cosLSing) < 0.01f) {
-        if (cosLSing > 0) cosLSing = 0.01;
-        else cosLSing = -0.01;
-    }
+	if (fabsf(cosLSing) < 0.01f) {
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
 
 	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
-- 
cgit v1.2.3


From 4613d1247d0dc7091be4ac477053e73866455e2f Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 17:15:43 -0500
Subject: Added param comments for FWB controller.

---
 apps/examples/control_demo/params.c | 36 ++++++++++++++++++------------------
 1 file changed, 18 insertions(+), 18 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index aea260330..422e9b90e 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -24,40 +24,40 @@ PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
 PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f);      // velocity to pitch angle PID, prop gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);      // integral gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);      // derivative gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);   // derivative low-pass
+PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);  // integrator wind up guard
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);     // the max commanded pitch angle
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);      // the min commanded pitch angle
 
 
 // theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f);      // pitch angle to pitch-rate PID
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
 
 //  h -> thr
-PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);     // altitude to throttle PID
 PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
 
 // crosstrack
-PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f);  // cross-track to yaw angle limit 90 deg
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);     // cross-track to yaw angle gain
 
 // speed command
-PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
-PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
-PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
+PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);       // minimum commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);       // commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);       // maximum commanded velocity
 
 // trim
-PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);     // trim aileron, normalized (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);   // trim elevator (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);     // trim rudder (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);    // trim throttle (0,1)
-- 
cgit v1.2.3


From 5b0aa490d68741fc067923e7ef801f672dcb5819 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 14 Jan 2013 18:38:17 -0500
Subject: Added P0. Hid some printing. Corrected fault detection.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 26 +++++++++++++++++++-------
 apps/examples/kalman_demo/KalmanNav.hpp |  1 +
 2 files changed, 20 insertions(+), 7 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index e804fbca5..a735de406 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -45,8 +45,6 @@
 // Titterton pg. 52
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
 static const float R0 = 6378137.0f; // earth radius, m
-
-static const float RSq = 4.0680631591e+13; // earth radius squared
 static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
@@ -55,6 +53,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	F(9, 9),
 	G(9, 6),
 	P(9, 9),
+	P0(9, 9),
 	V(6, 6),
 	// attitude measurement ekf matrices
 	HAtt(6, 9),
@@ -100,7 +99,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	using namespace math;
 
 	// initial state covariance matrix
-	P = Matrix::identity(9) * 1.0f;
+	P0 = Matrix::identity(9) * 1.0f;
+    P = P0;
 
 	// wait for gps lock
 	while (1) {
@@ -132,6 +132,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	setLonDegE7(_gps.lon);
 	setAltE3(_gps.alt);
 
+    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("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);
+
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
 
@@ -335,6 +342,10 @@ void KalmanNav::predictFast(float dt)
 		      vN * LDot + g;
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
+    printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
+            double(vNDot), double(vEDot), double(vDDot));
+    printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
+            double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
 	//printf("dt: %8.4f\n", double(dt));
@@ -360,6 +371,7 @@ void KalmanNav::predictSlow(float dt)
 
 	// prepare for matrix
 	float R = R0 + float(alt);
+    float RSq = R*R;
 
 	// F Matrix
 	// Titterton pg. 291
@@ -558,7 +570,7 @@ void KalmanNav::correctAtt()
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
 			// reset P matrix to identity
-			P = Matrix::identity(9) * 1.0f;
+			P = P0;
 			return;
 		}
 	}
@@ -581,7 +593,7 @@ void KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot((S * S.transpose()).inverse() * y);
+	float beta = y.dot(S.inverse()*y);
 
 	if (beta > 1000.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
@@ -641,7 +653,7 @@ void KalmanNav::correctPos()
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
 			// reset P matrix to identity
-			P = Matrix::identity(9) * 1.0f;
+			P = P0;
 			return;
 		}
 	}
@@ -659,7 +671,7 @@ void KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot((S * S.transpose()).inverse() * y);
+	float beta = y.dot(S.inverse()*y);
 
 	if (beta > 1000.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index 1ea46d40f..c074cb7c3 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -81,6 +81,7 @@ protected:
 	math::Matrix F;
 	math::Matrix G;
 	math::Matrix P;
+	math::Matrix P0;
 	math::Matrix V;
 	math::Matrix HAtt;
 	math::Matrix RAtt;
-- 
cgit v1.2.3


From d02a24ec82e35016545d519ea88d46881c42df2d Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 11:00:52 -0500
Subject: Adding comments to ekf.

---
 apps/examples/kalman_demo/KalmanNav.cpp |  39 ++++++-----
 apps/examples/kalman_demo/KalmanNav.hpp | 117 ++++++++++++++++++++++----------
 2 files changed, 102 insertions(+), 54 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index a735de406..5f71da58e 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// miss counts
 	_missFast(0),
 	_missSlow(0),
-	// state
+	// accelerations
 	fN(0), fE(0), fD(0),
+	// state
 	phi(0), theta(0), psi(0),
 	vN(0), vE(0), vD(0),
 	lat(0), lon(0), alt(0),
@@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsVel(this, "R_GPS_VEL"),
 	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
-	_rAccel(this, "R_ACCEL")
+	_rAccel(this, "R_ACCEL"),
+	_magDip(this, "MAG_DIP"),
+	_magDec(this, "MAG_DEC")
 {
 	using namespace math;
 
 	// initial state covariance matrix
 	P0 = Matrix::identity(9) * 1.0f;
-    P = P0;
+	P = P0;
 
 	// wait for gps lock
 	while (1) {
@@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	setLonDegE7(_gps.lon);
 	setAltE3(_gps.alt);
 
-    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("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);
+	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("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);
 
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
@@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt)
 		      vN * LDot + g;
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
-    printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-            double(vNDot), double(vEDot), double(vDDot));
-    printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-            double(LDot), double(lDot), double(rotRate));
+	printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
+	       double(vNDot), double(vEDot), double(vDDot));
+	printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
+	       double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
 	//printf("dt: %8.4f\n", double(dt));
@@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt)
 
 	// prepare for matrix
 	float R = R0 + float(alt);
-    float RSq = R*R;
+	float RSq = R * R;
 
 	// F Matrix
 	// Titterton pg. 291
@@ -472,8 +475,8 @@ void KalmanNav::correctAtt()
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
-	static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
+	static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	static const 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);
@@ -593,7 +596,7 @@ void KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse()*y);
+	float beta = y.dot(S.inverse() * y);
 
 	if (beta > 1000.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
@@ -671,7 +674,7 @@ void KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse()*y);
+	float beta = y.dot(S.inverse() * y);
 
 	if (beta > 1000.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index c074cb7c3..af4588e20 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -68,53 +68,98 @@
 class KalmanNav : public control::SuperBlock
 {
 public:
+	/**
+	 * Constructor
+	 */
 	KalmanNav(SuperBlock *parent, const char *name);
+
+	/**
+	 * Deconstuctor
+	 */
+
 	virtual ~KalmanNav() {};
+	/**
+	 * The main callback function for the class
+	 */
 	void update();
+
+	/**
+	 * Fast prediction
+	 * Contains: state integration
+	 */
 	virtual void updatePublications();
 	void predictFast(float dt);
+
+	/**
+	 * Slow prediction
+	 * Contains: covariance integration
+	 */
 	void predictSlow(float dt);
+
+	/**
+	 * Attitude correction
+	 */
 	void correctAtt();
+
+	/**
+	 * Position correction
+	 */
 	void correctPos();
+
+	/**
+	 * Overloaded update parameters
+	 */
 	virtual void updateParams();
 protected:
-	math::Matrix F;
-	math::Matrix G;
-	math::Matrix P;
-	math::Matrix P0;
-	math::Matrix V;
-	math::Matrix HAtt;
-	math::Matrix RAtt;
-	math::Matrix HPos;
-	math::Matrix RPos;
-	math::Dcm C_nb;
-	math::Quaternion q;
-	control::UOrbSubscription<sensor_combined_s> _sensors;
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;
-	control::UOrbSubscription<parameter_update_s> _param_update;
-	control::UOrbPublication<vehicle_global_position_s> _pos;
-	control::UOrbPublication<vehicle_attitude_s> _att;
-	uint64_t _pubTimeStamp;
-	uint64_t _fastTimeStamp;
-	uint64_t _slowTimeStamp;
-	uint64_t _attTimeStamp;
-	uint64_t _outTimeStamp;
-	uint16_t _navFrames;
-	uint16_t _missFast;
-	uint16_t _missSlow;
-	float fN, fE, fD;
+	// 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 */
+	// attitude
+	math::Dcm 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. */
+	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	// publications
+	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
+	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+	// time stamps
+	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
+	uint64_t _fastTimeStamp;    /**< fast prediction time stamp */
+	uint64_t _slowTimeStamp;    /**< slow prediction time stamp */
+	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
+	uint64_t _outTimeStamp;     /**< output time stamp */
+	// frame count
+	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
+	// miss counts
+	uint16_t _missFast;         /**< number of times fast prediction loop missed */
+	uint16_t _missSlow;         /**< number of times slow prediction loop missed */
+	// accelerations
+	float fN, fE, fD;           /**< navigation frame acceleration */
 	// states
-	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
-	float phi, theta, psi;
-	float vN, vE, vD;
-	double lat, lon, alt;
-	control::BlockParam<float> _vGyro;
-	control::BlockParam<float> _vAccel;
-	control::BlockParam<float> _rMag;
-	control::BlockParam<float> _rGpsVel;
-	control::BlockParam<float> _rGpsPos;
-	control::BlockParam<float> _rGpsAlt;
-	control::BlockParam<float> _rAccel;
+	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 */
+	// parameters
+	control::BlockParam<float> _vGyro;      /**< gyro process noise */
+	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
+	control::BlockParam<float> _rMag;       /**< magnetometer measurement noise  */
+	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
+	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
+	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
+	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
+	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	// 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; }
 	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
-- 
cgit v1.2.3


From 8b6660fc369806da509da5b5b98acc51da83811f Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 11:16:28 -0500
Subject: Fixed param issue.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 5f71da58e..081664d17 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -475,8 +475,8 @@ void KalmanNav::correctAtt()
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
-	static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
+	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);
-- 
cgit v1.2.3


From 281372ef3ad14d61eb720ddbe05e701e82aabad0 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 11:36:49 -0500
Subject: Added mag dip/dec as parameters.

---
 apps/examples/kalman_demo/params.c | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index f36ac334f..93571d433 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -8,3 +8,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
+PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f);
-- 
cgit v1.2.3


From 022c30ea4f8355799b8ef25d8fb31037df527bb9 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 12:17:09 -0500
Subject: Enabled kf to run w/o gps.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 70 +++++++++++++++------------------
 apps/examples/kalman_demo/KalmanNav.hpp |  2 +
 2 files changed, 34 insertions(+), 38 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 081664d17..b5c8a1073 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -97,50 +97,27 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsAlt(this, "R_GPS_ALT"),
 	_rAccel(this, "R_ACCEL"),
 	_magDip(this, "MAG_DIP"),
-	_magDec(this, "MAG_DEC")
+	_magDec(this, "MAG_DEC"),
+	_positionInitialized(false)
 {
 	using namespace math;
 
 	// initial state covariance matrix
-	P0 = Matrix::identity(9) * 1.0f;
+	P0 = Matrix::identity(9) * 0.01f;
 	P = P0;
 
-	// wait for gps lock
-	while (1) {
-		struct pollfd fds[1];
-		fds[0].fd = _gps.getHandle();
-		fds[0].events = POLLIN;
-
-		// poll 10 seconds for new data
-		int ret = poll(fds, 1, 10000);
-
-		if (ret > 0)  {
-			updateSubscriptions();
-
-			if (_gps.fix_type > 2) break;
-
-		} else if (ret == 0) {
-			printf("[kalman_demo] waiting for gps lock\n");
-		}
-	}
-
 	// initial state
 	phi = 0.0f;
 	theta = 0.0f;
 	psi = 0.0f;
-	vN = _gps.vel_n;
-	vE = _gps.vel_e;
-	vD = _gps.vel_d;
-	setLatDegE7(_gps.lat);
-	setLonDegE7(_gps.lon);
-	setAltE3(_gps.alt);
-
-	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("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);
+	vN = 0.0f;
+	vE = 0.0f;
+	vD = 0.0f;
+	lat = 0.0f;
+	lon = 0.0f;
+	alt = 0.0f;
+
+
 
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
@@ -199,6 +176,23 @@ void KalmanNav::update()
 	// abort update if no new data
 	if (!(sensorsUpdate || gpsUpdate)) return;
 
+	// if received gps for first time, reset position to gps
+	if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) {
+		vN = _gps.vel_n;
+		vE = _gps.vel_e;
+		vD = _gps.vel_d;
+		setLatDegE7(_gps.lat);
+		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("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);
+	}
+
 	// fast prediciton step
 	// note, using sensors timestamp so we can account
 	// for packet lag
@@ -345,10 +339,10 @@ void KalmanNav::predictFast(float dt)
 		      vN * LDot + g;
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
-	printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-	       double(vNDot), double(vEDot), double(vDDot));
-	printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-	       double(LDot), double(lDot), double(rotRate));
+	//printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
+		   //double(vNDot), double(vEDot), double(vDDot));
+	//printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
+		   //double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
 	//printf("dt: %8.4f\n", double(dt));
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index af4588e20..ba3756f46 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -159,6 +159,8 @@ protected:
 	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
 	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
 	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	// status
+	bool _positionInitialized; 			    /**< status, if position has been init. */
 	// 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; }
-- 
cgit v1.2.3


From edf0a6bae7fa5ae8c6d4df63489dcf75ec008517 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 12:37:12 -0500
Subject: Added check for valid attitude data.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 25 +++++++++++++++++--------
 apps/examples/kalman_demo/params.c      |  4 ++--
 2 files changed, 19 insertions(+), 10 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index b5c8a1073..68deae185 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -177,7 +177,10 @@ void KalmanNav::update()
 	if (!(sensorsUpdate || gpsUpdate)) return;
 
 	// if received gps for first time, reset position to gps
-	if (!_positionInitialized && gpsUpdate && _gps.fix_type > 2 && _gps.counter_pos_valid > 5) {
+	if (!_positionInitialized &&
+	    gpsUpdate &&
+	    _gps.fix_type > 2 &&
+	    _gps.counter_pos_valid > 10) {
 		vN = _gps.vel_n;
 		vE = _gps.vel_e;
 		vD = _gps.vel_d;
@@ -187,10 +190,10 @@ void KalmanNav::update()
 		_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));
+		       double(phi), double(theta), double(psi));
 		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);
+		       double(vN), double(vE), double(vD),
+		       lat, lon, alt);
 	}
 
 	// fast prediciton step
@@ -340,9 +343,9 @@ void KalmanNav::predictFast(float dt)
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
 	//printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-		   //double(vNDot), double(vEDot), double(vDDot));
+	//double(vNDot), double(vEDot), double(vDDot));
 	//printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-		   //double(LDot), double(lDot), double(rotRate));
+	//double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
 	//printf("dt: %8.4f\n", double(dt));
@@ -455,6 +458,12 @@ void 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);
@@ -566,7 +575,7 @@ void KalmanNav::correctAtt()
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
-			// reset P matrix to identity
+			// reset P matrix to P0
 			P = P0;
 			return;
 		}
@@ -649,7 +658,7 @@ void KalmanNav::correctPos()
 			setLatDegE7(_gps.lat);
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
-			// reset P matrix to identity
+			// reset P matrix to P0
 			P = P0;
 			return;
 		}
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 93571d433..55be116fa 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,8 +1,8 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
 PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
-- 
cgit v1.2.3


From 9cf3d51aec4e73f626cfdea29b19bdfe80eea384 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 13:12:00 -0500
Subject: Made fault tolerances adjustable.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 6 ++++--
 apps/examples/kalman_demo/KalmanNav.hpp | 2 ++
 apps/examples/kalman_demo/params.c      | 2 ++
 3 files changed, 8 insertions(+), 2 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 68deae185..918dfd9df 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -98,6 +98,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rAccel(this, "R_ACCEL"),
 	_magDip(this, "MAG_DIP"),
 	_magDec(this, "MAG_DEC"),
+	_faultPos(this, "FAULT_POS"),
+	_faultAtt(this, "FAULT_ATT"),
 	_positionInitialized(false)
 {
 	using namespace math;
@@ -601,7 +603,7 @@ void KalmanNav::correctAtt()
 	// fault detection
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > _faultAtt.get()) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		printf("y:\n"); y.print();
 	}
@@ -679,7 +681,7 @@ void KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > _faultPos.get()) {
 		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",
 		       double(y(0) / noiseVel),
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index ba3756f46..4af8bbf5c 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -159,6 +159,8 @@ protected:
 	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
 	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
 	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	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. */
 	// accessors
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 55be116fa..b98dd8fd7 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -10,3 +10,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
 PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
-- 
cgit v1.2.3


From 68b92cd4fc2c4df3de15ef19e723edb67a108ea0 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 13:21:13 -0500
Subject: Slowed HIL status updates. Also prevented posCor. when gps not init.

---
 apps/examples/kalman_demo/KalmanNav.cpp |  8 ++++++--
 apps/mavlink/mavlink_receiver.c         | 12 ++++++------
 2 files changed, 12 insertions(+), 8 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 918dfd9df..38cdb6ca0 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -239,9 +239,10 @@ void KalmanNav::update()
 	}
 
 	// output
-	if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
+	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
-		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
+		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n",
+				_navFrames/10, _missFast/10, _missSlow/10);
 		_navFrames = 0;
 		_missFast = 0;
 		_missSlow = 0;
@@ -616,6 +617,9 @@ void KalmanNav::correctAtt()
 void KalmanNav::correctPos()
 {
 	using namespace math;
+
+	if (!_positionInitialized) return;
+
 	Vector y(5);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index fa63c419f..0b63c30a4 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -367,8 +367,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil imu at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -412,8 +412,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil gps at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil gps at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -454,8 +454,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil pressure at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil pressure at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
-- 
cgit v1.2.3


From 28ef7aa1bea97593637537c832b07459a2520b86 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 14:03:04 -0500
Subject: Refactored RPos. Increased global pos output rate for debugging.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 46 ++++++++++++---------------------
 apps/mavlink/orb_listener.c             |  2 +-
 2 files changed, 18 insertions(+), 30 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 38cdb6ca0..0f3069d11 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -574,7 +574,6 @@ void KalmanNav::correctAtt()
 	// check correciton is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
-
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
@@ -589,9 +588,7 @@ void KalmanNav::correctAtt()
 		phi += xCorrect(PHI);
 		theta += xCorrect(THETA);
 	}
-
 	psi += xCorrect(PSI);
-
 	// attitude also affects nav velocities
 	vN += xCorrect(VN);
 	vE += xCorrect(VE);
@@ -603,7 +600,6 @@ void KalmanNav::correctAtt()
 
 	// fault detection
 	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();
@@ -620,6 +616,7 @@ void KalmanNav::correctPos()
 
 	if (!_positionInitialized) return;
 
+	// residual
 	Vector y(5);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
@@ -627,23 +624,6 @@ void KalmanNav::correctPos()
 	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
 	y(4) = double(_gps.alt) / 1.0e3 - alt;
 
-	// update gps noise
-	float cosLSing = cosf(lat);
-	float R = R0 + float(alt);
-
-	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) {
-		if (cosLSing > 0) cosLSing = 0.01;
-		else cosLSing = -0.01;
-	}
-
-	float noiseVel = _rGpsVel.get();
-	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
-	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
-	float noiseAlt = _rGpsAlt.get();
-	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7;
-	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7;
-
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
@@ -653,7 +633,6 @@ void KalmanNav::correctPos()
 	// check correction is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
-
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in gps correction\n");
@@ -684,15 +663,14 @@ void KalmanNav::correctPos()
 
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
-
 	if (beta > _faultPos.get()) {
 		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",
-		       double(y(0) / noiseVel),
-		       double(y(1) / noiseVel),
-		       double(y(2) / noiseLatDegE7),
-		       double(y(3) / noiseLonDegE7),
-		       double(y(4) / noiseAlt));
+		       double(y(0) / sqrtf(RPos(0,0))),
+		       double(y(1) / sqrtf(RPos(1,1))),
+		       double(y(2) / sqrtf(RPos(2,2))),
+		       double(y(3) / sqrtf(RPos(3,3))),
+		       double(y(4) / sqrtf(RPos(3,3))));
 	}
 }
 
@@ -702,7 +680,6 @@ void KalmanNav::updateParams()
 	using namespace control;
 	SuperBlock::updateParams();
 
-
 	// gyro noise
 	V(0, 0) = _vGyro.get();   // gyro x, rad/s
 	V(1, 1) = _vGyro.get();   // gyro y
@@ -714,13 +691,17 @@ void KalmanNav::updateParams()
 	V(5, 5) = _vAccel.get();   // accel z
 
 	// magnetometer noise
+	float noiseMin = 1e-6f;
 	float noiseMagSq = _rMag.get() * _rMag.get();
+	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();
+	// bound noise to prevent singularities
+	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
 	RAtt(3, 3) = noiseAccelSq; // normalized direction
 	RAtt(4, 4) = noiseAccelSq;
 	RAtt(5, 5) = noiseAccelSq;
@@ -739,9 +720,16 @@ void KalmanNav::updateParams()
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	float noiseAlt = _rGpsAlt.get();
+	// bound noise to prevent singularities
+	if (noiseVel < noiseMin) noiseVel = noiseMin;
+	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
+	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
+	if (noiseAlt < noiseMin) noiseAlt = noiseMin;
 	RPos(0, 0) = noiseVel * noiseVel; // vn
 	RPos(1, 1) = noiseVel * noiseVel; // ve
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
 	RPos(4, 4) = noiseAlt * noiseAlt; // h
+	// XXX, note that RPos depends on lat, so updateParams should
+	// be called if lat changes significantly
 }
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index f5253ea35..a7b43e2b9 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -702,7 +702,7 @@ uorb_receive_start(void)
 
 	/* --- GLOBAL POS VALUE --- */
 	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 1000);	/* 1Hz active updates */
+	orb_set_interval(mavlink_subs.global_pos_sub, 5);	/* 200 Hz active updates */
 
 	/* --- LOCAL POS VALUE --- */
 	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-- 
cgit v1.2.3


From afb69cd05450d6df1bf2233b95030c9b93daaf1e Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 15:11:24 -0500
Subject: Reducing pos/att correction update rates for debugging.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 8 +++-----
 apps/mavlink/orb_listener.c             | 2 +-
 2 files changed, 4 insertions(+), 6 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 0f3069d11..3966b4fcd 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -65,8 +65,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	C_nb(),
 	q(),
 	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
@@ -208,7 +208,6 @@ void KalmanNav::update()
 		predictFast(dtFast);
 		// count fast frames
 		_navFrames += 1;
-
 	} else _missFast++;
 
 	// slow prediction step
@@ -216,7 +215,6 @@ void KalmanNav::update()
 
 	if (dtSlow > 1.0f / 100) { // 100 Hz
 		_slowTimeStamp = _sensors.timestamp;
-
 		if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
 		else _missSlow ++;
 	}
@@ -227,7 +225,7 @@ void KalmanNav::update()
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index a7b43e2b9..43bb4fb28 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -702,7 +702,7 @@ uorb_receive_start(void)
 
 	/* --- GLOBAL POS VALUE --- */
 	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 5);	/* 200 Hz active updates */
+	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
 
 	/* --- LOCAL POS VALUE --- */
 	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-- 
cgit v1.2.3


From 68a6a64213b5af66771a6a302bf06c4c588dc719 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 18:25:08 -0500
Subject: Working on velocity errors.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 86 +++++++++++++++++++--------------
 apps/examples/kalman_demo/KalmanNav.hpp |  1 +
 apps/examples/kalman_demo/params.c      |  1 +
 apps/mavlink/mavlink_receiver.c         |  7 ++-
 4 files changed, 57 insertions(+), 38 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 3966b4fcd..41e948ae2 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -45,7 +45,7 @@
 // Titterton pg. 52
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
 static const float R0 = 6378137.0f; // earth radius, m
-static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
+static const float g0 = 9.806f; // standard gravitational accel. m/s^2
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
@@ -98,6 +98,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rAccel(this, "R_ACCEL"),
 	_magDip(this, "MAG_DIP"),
 	_magDec(this, "MAG_DEC"),
+	_g(this, "G"),
 	_faultPos(this, "FAULT_POS"),
 	_faultAtt(this, "FAULT_ATT"),
 	_positionInitialized(false)
@@ -119,8 +120,6 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-
-
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
 
@@ -142,16 +141,12 @@ void KalmanNav::update()
 {
 	using namespace math;
 
-	struct pollfd fds[3];
+	struct pollfd fds[1];
 	fds[0].fd = _sensors.getHandle();
 	fds[0].events = POLLIN;
-	fds[1].fd = _param_update.getHandle();
-	fds[1].events = POLLIN;
-	fds[2].fd = _gps.getHandle();
-	fds[2].events = POLLIN;
 
-	// poll 20 milliseconds for new data
-	int ret = poll(fds, 2, 20);
+	// poll for new data
+	int ret = poll(fds, 1, 1000);
 
 	// check return value
 	if (ret < 0) {
@@ -202,22 +197,25 @@ void KalmanNav::update()
 	// note, using sensors timestamp so we can account
 	// for packet lag
 	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
+	printf("dtFast: %15.10f\n", double(dtFast));
 	_fastTimeStamp = _sensors.timestamp;
 
-	if (dtFast < 1.0f / 50) {
+	if (dtFast < 1.0f) {
 		predictFast(dtFast);
+		predictSlow(dtFast);
 		// count fast frames
 		_navFrames += 1;
+
 	} else _missFast++;
 
 	// slow prediction step
-	float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
+	//float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
 
-	if (dtSlow > 1.0f / 100) { // 100 Hz
-		_slowTimeStamp = _sensors.timestamp;
-		if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
-		else _missSlow ++;
-	}
+	//if (dtSlow > 1.0f / 100) { // 100 Hz
+	//_slowTimeStamp = _sensors.timestamp;
+	//if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
+	//else _missSlow ++;
+	//}
 
 	// gps correction step
 	if (gpsUpdate) {
@@ -240,7 +238,7 @@ void KalmanNav::update()
 	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
 		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n",
-				_navFrames/10, _missFast/10, _missSlow/10);
+		       _navFrames / 10, _missFast / 10, _missSlow / 10);
 		_navFrames = 0;
 		_missFast = 0;
 		_missSlow = 0;
@@ -340,18 +338,18 @@ void KalmanNav::predictFast(float dt)
 	float vNDot = fN - vE * rotRate * sinL +
 		      vD * LDot;
 	float vDDot = fD - vE * rotRate * cosL -
-		      vN * LDot + g;
+		      vN * LDot + _g.get();
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
-	//printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-	//double(vNDot), double(vEDot), double(vDDot));
-	//printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-	//double(LDot), double(lDot), double(rotRate));
+	printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
+	       double(vNDot), double(vEDot), double(vDDot));
+	printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
+	       double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
-	//printf("dt: %8.4f\n", double(dt));
-	//printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
-	//printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
+	printf("dt: %8.4f\n", double(dt));
+	printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
+	printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
 	vN += vNDot * dt;
 	vE += vEDot * dt;
 	vD += vDDot * dt;
@@ -495,7 +493,7 @@ void KalmanNav::correctAtt()
 	// ignore accel correction when accel mag not close to g
 	Matrix RAttAdjust = RAtt;
 
-	bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
+	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
 	if (ignoreAccel) {
 		RAttAdjust(3, 3) = 1.0e10;
@@ -511,7 +509,7 @@ void KalmanNav::correctAtt()
 	//Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
 
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get()) /*+ zCentrip*/).unit();
 
 	// combined measurement
 	Vector zAtt(6);
@@ -572,6 +570,7 @@ void KalmanNav::correctAtt()
 	// check correciton is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
+
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
@@ -586,11 +585,12 @@ void KalmanNav::correctAtt()
 		phi += xCorrect(PHI);
 		theta += xCorrect(THETA);
 	}
+
 	psi += xCorrect(PSI);
 	// attitude also affects nav velocities
-	vN += xCorrect(VN);
-	vE += xCorrect(VE);
-	vD += xCorrect(VD);
+	//vN += xCorrect(VN);
+	//vE += xCorrect(VE);
+	//vD += xCorrect(VD);
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -598,6 +598,7 @@ void KalmanNav::correctAtt()
 
 	// fault detection
 	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();
@@ -631,6 +632,7 @@ void KalmanNav::correctPos()
 	// check correction is sane
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
+
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in gps correction\n");
@@ -661,14 +663,15 @@ void KalmanNav::correctPos()
 
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
+
 	if (beta > _faultPos.get()) {
 		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",
-		       double(y(0) / sqrtf(RPos(0,0))),
-		       double(y(1) / sqrtf(RPos(1,1))),
-		       double(y(2) / sqrtf(RPos(2,2))),
-		       double(y(3) / sqrtf(RPos(3,3))),
-		       double(y(4) / sqrtf(RPos(3,3))));
+		       double(y(0) / sqrtf(RPos(0, 0))),
+		       double(y(1) / sqrtf(RPos(1, 1))),
+		       double(y(2) / sqrtf(RPos(2, 2))),
+		       double(y(3) / sqrtf(RPos(3, 3))),
+		       double(y(4) / sqrtf(RPos(4, 4))));
 	}
 }
 
@@ -691,22 +694,26 @@ void KalmanNav::updateParams()
 	// magnetometer noise
 	float noiseMin = 1e-6f;
 	float noiseMagSq = _rMag.get() * _rMag.get();
+
 	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();
+
 	// bound noise to prevent singularities
 	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
+
 	RAtt(3, 3) = noiseAccelSq; // normalized direction
 	RAtt(4, 4) = noiseAccelSq;
 	RAtt(5, 5) = noiseAccelSq;
 
 	// gps noise
-	float cosLSing = cosf(lat);
 	float R = R0 + float(alt);
+	float cosLSing = cosf(lat);
 
 	// prevent singularity
 	if (fabsf(cosLSing) < 0.01f) {
@@ -718,11 +725,16 @@ void KalmanNav::updateParams()
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	float noiseAlt = _rGpsAlt.get();
+
 	// bound noise to prevent singularities
 	if (noiseVel < noiseMin) noiseVel = noiseMin;
+
 	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
+
 	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
+
 	if (noiseAlt < noiseMin) noiseAlt = noiseMin;
+
 	RPos(0, 0) = noiseVel * noiseVel; // vn
 	RPos(1, 1) = noiseVel * noiseVel; // ve
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index 4af8bbf5c..d7068beec 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -159,6 +159,7 @@ protected:
 	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
 	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
 	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	control::BlockParam<float> _g;          /**< gravitational constant */
 	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
 	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
 	// status
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index b98dd8fd7..2cdbc1435 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -12,3 +12,4 @@ PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
 PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_G, 9.806f);
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index 0b63c30a4..9491ce7e3 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -318,9 +318,11 @@ handle_message(mavlink_message_t *msg)
 			static uint16_t hil_frames = 0;
 			static uint64_t old_timestamp = 0;
 
+			/* sensors general */
+			hil_sensors.timestamp = imu.time_usec;
+
 			/* hil gyro */
 			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.timestamp = timestamp;
 			hil_sensors.gyro_counter = hil_counter;
 			hil_sensors.gyro_raw[0] = imu.xgyro;
 			hil_sensors.gyro_raw[1] = imu.ygyro;
@@ -429,6 +431,9 @@ handle_message(mavlink_message_t *msg)
 			static uint16_t hil_frames = 0;
 			static uint64_t old_timestamp = 0;
 
+			/* sensors general */
+			hil_sensors.timestamp = press.time_usec;
+
 			/* baro */
 			/* TODO, set ground_press/ temp during calib */
 			static const float ground_press = 1013.25f; // mbar
-- 
cgit v1.2.3


From ce3f835c637338086a18307c61deb35ccddbee05 Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Tue, 15 Jan 2013 23:36:01 -0500
Subject: Mag and velocity measurement fix. Fault detection working.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 79 ++++++++++++++-------------------
 apps/examples/kalman_demo/KalmanNav.hpp | 23 +++++-----
 apps/examples/kalman_demo/params.c      | 25 ++++++-----
 3 files changed, 59 insertions(+), 68 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 41e948ae2..d93a7bdc6 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -73,15 +73,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_att(&getPublications(), ORB_ID(vehicle_attitude)),
 	// timestamps
 	_pubTimeStamp(hrt_absolute_time()),
-	_fastTimeStamp(hrt_absolute_time()),
-	_slowTimeStamp(hrt_absolute_time()),
+	_predictTimeStamp(hrt_absolute_time()),
 	_attTimeStamp(hrt_absolute_time()),
 	_outTimeStamp(hrt_absolute_time()),
 	// frame count
 	_navFrames(0),
 	// miss counts
-	_missFast(0),
-	_missSlow(0),
+	_miss(0),
 	// accelerations
 	fN(0), fE(0), fD(0),
 	// state
@@ -96,9 +94,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
 	_rAccel(this, "R_ACCEL"),
-	_magDip(this, "MAG_DIP"),
-	_magDec(this, "MAG_DEC"),
-	_g(this, "G"),
+	_magDip(this, "ENV_MAG_DIP"),
+	_magDec(this, "ENV_MAG_DEC"),
+	_g(this, "ENV_G"),
 	_faultPos(this, "FAULT_POS"),
 	_faultAtt(this, "FAULT_ATT"),
 	_positionInitialized(false)
@@ -193,29 +191,21 @@ void KalmanNav::update()
 		       lat, lon, alt);
 	}
 
-	// fast prediciton step
-	// note, using sensors timestamp so we can account
-	// for packet lag
-	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
-	printf("dtFast: %15.10f\n", double(dtFast));
-	_fastTimeStamp = _sensors.timestamp;
-
-	if (dtFast < 1.0f) {
-		predictFast(dtFast);
-		predictSlow(dtFast);
+	// 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));
+	_predictTimeStamp = _sensors.timestamp;
+	// don't predict if time greater than a second
+	if (dt < 1.0f) {
+		predictState(dt);
+		predictStateCovariance(dt);
 		// count fast frames
 		_navFrames += 1;
+	}
 
-	} else _missFast++;
-
-	// slow prediction step
-	//float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
-
-	//if (dtSlow > 1.0f / 100) { // 100 Hz
-	//_slowTimeStamp = _sensors.timestamp;
-	//if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
-	//else _missSlow ++;
-	//}
+	// count times 100 Hz rate isn't met
+	if (dt > 0.01f) _miss++;
 
 	// gps correction step
 	if (gpsUpdate) {
@@ -237,11 +227,10 @@ void KalmanNav::update()
 	// output
 	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
-		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n",
-		       _navFrames / 10, _missFast / 10, _missSlow / 10);
+		printf("nav: %4d Hz, miss #: %4d\n",
+		       _navFrames / 10, _miss/ 10);
 		_navFrames = 0;
-		_missFast = 0;
-		_missSlow = 0;
+		_miss= 0;
 	}
 }
 
@@ -288,7 +277,7 @@ void KalmanNav::updatePublications()
 	SuperBlock::updatePublications();
 }
 
-void KalmanNav::predictFast(float dt)
+void KalmanNav::predictState(float dt)
 {
 	using namespace math;
 	Vector3 w(_sensors.gyro_rad_s);
@@ -336,20 +325,13 @@ void KalmanNav::predictFast(float dt)
 	float lDot = vE / (cosLSing * R);
 	float rotRate = 2 * omega + lDot;
 	float vNDot = fN - vE * rotRate * sinL +
-		      vD * LDot;
+			  vD * LDot;
 	float vDDot = fD - vE * rotRate * cosL -
-		      vN * LDot + _g.get();
+			  vN * LDot + _g.get();
 	float vEDot = fE + vN * rotRate * sinL +
-		      vDDot * rotRate * cosL;
-	printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-	       double(vNDot), double(vEDot), double(vDDot));
-	printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-	       double(LDot), double(lDot), double(rotRate));
+			  vDDot * rotRate * cosL;
 
 	// rectangular integration
-	printf("dt: %8.4f\n", double(dt));
-	printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
-	printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
 	vN += vNDot * dt;
 	vE += vEDot * dt;
 	vD += vDDot * dt;
@@ -358,7 +340,7 @@ void KalmanNav::predictFast(float dt)
 	alt += double(-vD * dt);
 }
 
-void KalmanNav::predictSlow(float dt)
+void KalmanNav::predictStateCovariance(float dt)
 {
 	using namespace math;
 
@@ -473,6 +455,8 @@ void KalmanNav::correctAtt()
 
 	// mag measurement
 	Vector3 zMag(_sensors.magnetometer_ga);
+	//float magNorm = zMag.norm();
+	zMag = zMag.unit();
 
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
@@ -588,9 +572,9 @@ void KalmanNav::correctAtt()
 
 	psi += xCorrect(PSI);
 	// attitude also affects nav velocities
-	//vN += xCorrect(VN);
-	//vE += xCorrect(VE);
-	//vD += xCorrect(VD);
+	vN += xCorrect(VN);
+	vE += xCorrect(VE);
+	vD += xCorrect(VD);
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -602,6 +586,9 @@ void KalmanNav::correctAtt()
 	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();
 	}
 
 	// update quaternions from euler
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index d7068beec..da1a7ee10 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -83,18 +83,23 @@ public:
 	 */
 	void update();
 
+
 	/**
-	 * Fast prediction
-	 * Contains: state integration
+	 * Publication update
 	 */
 	virtual void updatePublications();
-	void predictFast(float dt);
 
 	/**
-	 * Slow prediction
-	 * Contains: covariance integration
+	 * State prediction
+	 * Continuous, non-linear
+	 */
+	void predictState(float dt);
+
+	/**
+	 * State covariance prediction
+	 * Continuous, linear
 	 */
-	void predictSlow(float dt);
+	void predictStateCovariance(float dt);
 
 	/**
 	 * Attitude correction
@@ -133,15 +138,13 @@ protected:
 	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
-	uint64_t _fastTimeStamp;    /**< fast prediction time stamp */
-	uint64_t _slowTimeStamp;    /**< slow prediction time stamp */
+	uint64_t _predictTimeStamp; /**< prediction time stamp */
 	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
 	uint64_t _outTimeStamp;     /**< output time stamp */
 	// frame count
 	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
 	// miss counts
-	uint16_t _missFast;         /**< number of times fast prediction loop missed */
-	uint16_t _missSlow;         /**< number of times slow prediction loop missed */
+	uint16_t _miss;         	/**< number of times fast prediction loop missed */
 	// accelerations
 	float fN, fE, fD;           /**< navigation frame acceleration */
 	// states
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 2cdbc1435..58ebeba3c 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,15 +1,16 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
-PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
-PARAM_DEFINE_FLOAT(KF_G, 9.806f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
+
-- 
cgit v1.2.3


From f8811649cb91fc88cef7b224b29c6c5f235f1d8d Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Wed, 16 Jan 2013 00:24:14 -0500
Subject: Controller/ EKF tuning.

---
 apps/examples/control_demo/params.c     | 12 ++++++------
 apps/examples/kalman_demo/KalmanNav.cpp | 17 +++++++++--------
 apps/examples/kalman_demo/params.c      | 16 ++++++++--------
 3 files changed, 23 insertions(+), 22 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 422e9b90e..3eed83b5c 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -15,16 +15,16 @@ PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
 PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
-PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator
 PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
 
 //  psi -> phi -> p
 PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f);      // heading 2 roll
 PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
-PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f);      // velocity to pitch angle PID, prop gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f);      // velocity to pitch angle PID, prop gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);      // integral gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);      // derivative gain
 PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);   // derivative low-pass
@@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);      // the min commanded pitch angle
 
 
 // theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f);      // pitch angle to pitch-rate PID
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);      // pitch angle to pitch-rate PID
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
@@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
 
 // crosstrack
 PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f);  // cross-track to yaw angle limit 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);     // cross-track to yaw angle gain
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f);     // cross-track to yaw angle gain
 
 // speed command
 PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);       // minimum commanded velocity
@@ -60,4 +60,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);       // maximum commanded velocity
 PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);     // trim aileron, normalized (-1,1)
 PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);   // trim elevator (-1,1)
 PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);     // trim rudder (-1,1)
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);    // trim throttle (0,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index d93a7bdc6..6acf39be8 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -66,7 +66,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	q(),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
@@ -193,9 +193,10 @@ void KalmanNav::update()
 
 	// prediciton step
 	// using sensors timestamp so we can account for packet lag
-	float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
+	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
 	//printf("dt: %15.10f\n", double(dtFast));
 	_predictTimeStamp = _sensors.timestamp;
+
 	// don't predict if time greater than a second
 	if (dt < 1.0f) {
 		predictState(dt);
@@ -213,7 +214,7 @@ void KalmanNav::update()
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -228,9 +229,9 @@ void KalmanNav::update()
 	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
 		printf("nav: %4d Hz, miss #: %4d\n",
-		       _navFrames / 10, _miss/ 10);
+		       _navFrames / 10, _miss / 10);
 		_navFrames = 0;
-		_miss= 0;
+		_miss = 0;
 	}
 }
 
@@ -325,11 +326,11 @@ void KalmanNav::predictState(float dt)
 	float lDot = vE / (cosLSing * R);
 	float rotRate = 2 * omega + lDot;
 	float vNDot = fN - vE * rotRate * sinL +
-			  vD * LDot;
+		      vD * LDot;
 	float vDDot = fD - vE * rotRate * cosL -
-			  vN * LDot + _g.get();
+		      vN * LDot + _g.get();
 	float vEDot = fE + vN * rotRate * sinL +
-			  vDDot * rotRate * cosL;
+		      vDDot * rotRate * cosL;
 
 	// rectangular integration
 	vN += vNDot * dt;
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 58ebeba3c..77225b4c7 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,16 +1,16 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
+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, 0.1f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.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);
 PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
 
-- 
cgit v1.2.3


From 41ac3fdef9e3b7210286b438f3dae50af06b814c Mon Sep 17 00:00:00 2001
From: jgoppert <james.goppert@gmail.com>
Date: Wed, 16 Jan 2013 00:25:53 -0500
Subject: Increased fault threshhold.

---
 apps/examples/kalman_demo/params.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 77225b4c7..bb18d5b92 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, 10.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.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);
-- 
cgit v1.2.3


From ded442fd194442134accf0080be3fe73098481c1 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 16 Jan 2013 13:27:04 -0500
Subject: Added position initialization.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 171 ++++++++++++++++++--------------
 apps/examples/kalman_demo/KalmanNav.hpp |  12 ++-
 apps/examples/kalman_demo/params.c      |   2 +-
 3 files changed, 103 insertions(+), 82 deletions(-)

(limited to 'apps')

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);
-- 
cgit v1.2.3


From 34d70bea4bb2a1719e4d081cf59081b84037d423 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Wed, 16 Jan 2013 13:55:49 -0500
Subject: Control tuning.

---
 apps/examples/control_demo/params.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 3eed83b5c..6525b70f5 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -14,9 +14,9 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
 PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
-PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
-PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator
-PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
 
 //  psi -> phi -> p
 PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f);      // heading 2 roll
-- 
cgit v1.2.3


From c2c0baf843aa376681fcf7bf9dfcac480177b7fb Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Thu, 17 Jan 2013 12:16:21 -0500
Subject: Increased process noise.

---
 apps/examples/kalman_demo/params.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 77225b4c7..3bfe01261 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,8 +1,8 @@
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
-- 
cgit v1.2.3


From 13bb814f2015154cb5cadb814e37db1b6b2e634b Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Thu, 17 Jan 2013 12:18:20 -0500
Subject: Prevented attitude correction from changing velocity when pos not
 init.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 15254f7c7..40166f3a1 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -590,9 +590,11 @@ int KalmanNav::correctAtt()
 
 	psi += xCorrect(PSI);
 	// attitude also affects nav velocities
-	vN += xCorrect(VN);
-	vE += xCorrect(VE);
-	vD += xCorrect(VD);
+	if (_positionInitialized) {
+		vN += xCorrect(VN);
+		vE += xCorrect(VE);
+		vD += xCorrect(VD);
+	}
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-- 
cgit v1.2.3


From dc5ddb937039fd3481009caeb2a472955986e2e4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Jan 2013 08:41:11 +0100
Subject: Defaulting to full auto in auto mode

---
 apps/commander/commander.c | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

(limited to 'apps')

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index f3568ee8d..33f183227 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1845,15 +1845,16 @@ int commander_thread_main(int argc, char *argv[])
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					/* check auto mode switch for correct mode */
-					if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-						/* enable guided mode */
-						update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
+					// /* check auto mode switch for correct mode */
+					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
+					// 	/* enable guided mode */
+					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
 
-					} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
+					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
+						// XXX hardcode to auto for now
 						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
-					}
+					// }
 
 				} else {
 					/* center stick position, set SAS for all vehicle types */
-- 
cgit v1.2.3


From 4b2d1690d33c2ae9248abd9fb025e8a1a30fbe84 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Fri, 18 Jan 2013 10:21:20 -0500
Subject: Set kalman_demo to only publish when it has valid info.

---
 apps/examples/kalman_demo/KalmanNav.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 40166f3a1..a07280515 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -239,7 +239,10 @@ void KalmanNav::update()
 	// publication
 	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
 		_pubTimeStamp = newTimeStamp;
-		updatePublications();
+
+		if (_positionInitialized) _pos.update();
+
+		if (_attitudeInitialized) _att.update();
 	}
 
 	// output
@@ -589,6 +592,7 @@ int KalmanNav::correctAtt()
 	}
 
 	psi += xCorrect(PSI);
+
 	// attitude also affects nav velocities
 	if (_positionInitialized) {
 		vN += xCorrect(VN);
-- 
cgit v1.2.3


From 2542722102ce0ab3520dafe9ae695cf06caae675 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 19 Jan 2013 17:11:12 +0100
Subject: Fixed selective publication update

---
 apps/examples/kalman_demo/KalmanNav.cpp | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

(limited to 'apps')

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index a07280515..7e89dffb2 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -240,9 +240,7 @@ void KalmanNav::update()
 	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
 		_pubTimeStamp = newTimeStamp;
 
-		if (_positionInitialized) _pos.update();
-
-		if (_attitudeInitialized) _att.update();
+		updatePublications();
 	}
 
 	// output
@@ -294,8 +292,13 @@ void KalmanNav::updatePublications()
 	_att.q_valid = true;
 	_att.counter = _navFrames;
 
-	// update publications
-	SuperBlock::updatePublications();
+	// selectively update publications,
+	// do NOT call superblock do-all method
+	if (_positionInitialized)
+		_pos.update();
+
+	if (_attitudeInitialized)
+		_att.update();
 }
 
 int KalmanNav::predictState(float dt)
-- 
cgit v1.2.3