From 68a6a64213b5af66771a6a302bf06c4c588dc719 Mon Sep 17 00:00:00 2001 From: James Goppert 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 _rAccel; /**< accelerometer measurement noise */ control::BlockParam _magDip; /**< magnetic inclination with level */ control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam _g; /**< gravitational constant */ control::BlockParam _faultPos; /**< fault detection threshold for position */ control::BlockParam _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