diff options
author | James Goppert <james.goppert@gmail.com> | 2013-01-15 12:37:12 -0500 |
---|---|---|
committer | James Goppert <james.goppert@gmail.com> | 2013-01-15 12:37:12 -0500 |
commit | edf0a6bae7fa5ae8c6d4df63489dcf75ec008517 (patch) | |
tree | 428e089cf7f469bc60959bef6db50e3308dadb5b | |
parent | 022c30ea4f8355799b8ef25d8fb31037df527bb9 (diff) | |
download | px4-firmware-edf0a6bae7fa5ae8c6d4df63489dcf75ec008517.tar.gz px4-firmware-edf0a6bae7fa5ae8c6d4df63489dcf75ec008517.tar.bz2 px4-firmware-edf0a6bae7fa5ae8c6d4df63489dcf75ec008517.zip |
Added check for valid attitude data.
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 25 | ||||
-rw-r--r-- | apps/examples/kalman_demo/params.c | 4 |
2 files changed, 19 insertions, 10 deletions
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); |