Working on fault detection tolerances.

2 files changed, 24 insertions, 20 deletions

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;