Refactored RPos. Increased global pos output rate for debugging.

1 files changed, 17 insertions, 29 deletions

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
 }