diff options
-rw-r--r-- | apps/examples/control_demo/params.c | 4 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 40 |
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; |