diff options
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.cpp')
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 393 |
1 files changed, 224 insertions, 169 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 240564b56..7e89dffb2 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -45,9 +45,9 @@ // 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 RSq = 4.0680631591e+13; // earth radius squared -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 +static const int8_t ret_ok = 0; // no error in function +static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), @@ -55,6 +55,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : F(9, 9), G(9, 6), P(9, 9), + P0(9, 9), V(6, 6), // attitude measurement ekf matrices HAtt(6, 9), @@ -66,7 +67,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : C_nb(), q(), // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz + _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications @@ -74,17 +75,16 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), - _fastTimeStamp(hrt_absolute_time()), - _slowTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), _attTimeStamp(hrt_absolute_time()), _outTimeStamp(hrt_absolute_time()), // frame count _navFrames(0), // miss counts - _missFast(0), - _missSlow(0), - // state + _miss(0), + // accelerations fN(0), fE(0), fD(0), + // state phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), @@ -95,42 +95,32 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsVel(this, "R_GPS_VEL"), _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), - _rAccel(this, "R_ACCEL") + _rAccel(this, "R_ACCEL"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), + _attitudeInitialized(false), + _positionInitialized(false), + _attitudeInitCounter(0) { using namespace math; // initial state covariance matrix - P = Matrix::identity(9) * 1.0f; - - // wait for gps lock - while (1) { - struct pollfd fds[1]; - fds[0].fd = _gps.getHandle(); - fds[0].events = POLLIN; - - // poll 10 seconds for new data - int ret = poll(fds, 1, 10000); - - if (ret > 0) { - updateSubscriptions(); - - if (_gps.fix_type > 2) break; - - } else if (ret == 0) { - printf("[kalman_demo] waiting for gps lock\n"); - } - } + P0 = Matrix::identity(9) * 0.01f; + P = P0; // initial state phi = 0.0f; theta = 0.0f; psi = 0.0f; - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); + vN = 0.0f; + vE = 0.0f; + vD = 0.0f; + lat = 0.0f; + lon = 0.0f; + alt = 0.0f; // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -141,8 +131,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // HPos is constant HPos(0, 3) = 1.0f; HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0f; - HPos(3, 7) = 1.0f; + HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; HPos(4, 8) = 1.0f; // initialize all parameters @@ -153,18 +143,13 @@ 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) { // XXX this is seriously bad - should be an emergency return; @@ -186,39 +171,67 @@ void KalmanNav::update() // this clears update flag updateSubscriptions(); - // abort update if no new data - if (!(sensorsUpdate || gpsUpdate)) return; + // initialize attitude when sensors online + if (!_attitudeInitialized && sensorsUpdate && + _sensors.accelerometer_counter > 10 && + _sensors.gyro_counter > 10 && + _sensors.magnetometer_counter > 10) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; + + if (_attitudeInitCounter > 100) { + printf("[kalman_demo] initialized EKF attitude\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + _attitudeInitialized = true; + } + } + + // initialize position when gps received + if (!_positionInitialized && + _attitudeInitialized && // wait for attitude first + gpsUpdate && + _gps.fix_type > 2 && + _gps.counter_pos_valid > 10) { + vN = _gps.vel_n; + vE = _gps.vel_e; + vD = _gps.vel_d; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + _positionInitialized = true; + printf("[kalman_demo] initialized EKF state with GPS\n"); + 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); + } - // fast prediciton step - // note, using sensors timestamp so we can account - // for packet lag - float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; - _fastTimeStamp = _sensors.timestamp; + // prediciton step + // using sensors timestamp so we can account for packet lag + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dt)); + _predictTimeStamp = _sensors.timestamp; - if (dtFast < 1.0f / 50) { - predictFast(dtFast); + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); // count fast frames _navFrames += 1; - - } else _missFast++; - - // slow prediction step - 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 ++; } + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; + // gps correction step - if (gpsUpdate) { + if (_positionInitialized && gpsUpdate) { correctPos(); } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + ) { _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -226,16 +239,17 @@ void KalmanNav::update() // publication if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz _pubTimeStamp = newTimeStamp; + updatePublications(); } // output - if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow); + printf("nav: %4d Hz, miss #: %4d\n", + _navFrames / 10, _miss / 10); _navFrames = 0; - _missFast = 0; - _missSlow = 0; + _miss = 0; } } @@ -278,73 +292,87 @@ void KalmanNav::updatePublications() _att.q_valid = true; _att.counter = _navFrames; - // update publications - SuperBlock::updatePublications(); + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) + _pos.update(); + + if (_attitudeInitialized) + _att.update(); } -void KalmanNav::predictFast(float dt) +int KalmanNav::predictState(float dt) { using namespace math; - Vector3 w(_sensors.gyro_rad_s); - // attitude - q = q + q.derivative(w) * dt; + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSing = cosf(lat); - // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; } - // C_nb update - C_nb = Dcm(q); + // attitude prediction + if (_attitudeInitialized) { + Vector3 w(_sensors.gyro_rad_s); - // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + // attitude + q = q + q.derivative(w) * dt; - // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); + // renormalize quaternion if needed + if (fabsf(q.norm() - 1.0f) > 1e-4f) { + q = q.unit(); + } - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); + // C_nb update + C_nb = Dcm(q); + + // euler update + EulerAngles euler(C_nb); + phi = euler.getPhi(); + theta = euler.getTheta(); + psi = euler.getPsi(); + + // specific acceleration in nav frame + Vector3 accelB(_sensors.accelerometer_m_s2); + Vector3 accelN = C_nb * accelB; + fN = accelN(0); + fE = accelN(1); + fD = accelN(2); + } - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; + // position prediction + if (_positionInitialized) { + // neglects angular deflections in local gravity + // see Titerton pg. 70 + float R = R0 + float(alt); + float LDot = vN / R; + float lDot = vE / (cosLSing * R); + float rotRate = 2 * omega + lDot; + float vNDot = fN - vE * rotRate * sinL + + vD * LDot; + float vDDot = fD - vE * rotRate * cosL - + vN * LDot + _g.get(); + float vEDot = fE + vN * rotRate * sinL + + vDDot * rotRate * cosL; + + // rectangular integration + vN += vNDot * dt; + vE += vEDot * dt; + vD += vDDot * dt; + lat += double(LDot * dt); + lon += double(lDot * dt); + alt += double(-vD * dt); + } - // position update - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + g; - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // 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)); - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); + return ret_ok; } -void KalmanNav::predictSlow(float dt) +int KalmanNav::predictStateCovariance(float dt) { using namespace math; @@ -356,6 +384,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); + float RSq = R * R; // F Matrix // Titterton pg. 291 @@ -436,9 +465,11 @@ void KalmanNav::predictSlow(float dt) // for discrte time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + + return ret_ok; } -void KalmanNav::correctAtt() +int KalmanNav::correctAtt() { using namespace math; @@ -452,12 +483,14 @@ void KalmanNav::correctAtt() // mag measurement Vector3 zMag(_sensors.magnetometer_ga); + //float magNorm = zMag.norm(); + zMag = zMag.unit(); // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); @@ -472,7 +505,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; @@ -483,12 +516,8 @@ void KalmanNav::correctAtt() //printf("correcting attitude with accel\n"); } - // account for banked turn - // this would only work for fixed wing, so try to avoid - //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())).unit(); // combined measurement Vector zAtt(6); @@ -553,9 +582,9 @@ 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 - P = Matrix::identity(9) * 1.0f; - return; + // reset P matrix to P0 + P = P0; + return ret_error; } } @@ -568,9 +597,11 @@ void KalmanNav::correctAtt() psi += xCorrect(PSI); // attitude also affects nav velocities - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -579,38 +610,33 @@ void KalmanNav::correctAtt() // fault detection float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultAtt.get()) { printf("fault in attitude: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + printf("y:\n"); y.print(); + printf("zMagHat:\n"); zMagHat.print(); + printf("zMag:\n"); zMag.print(); + printf("bNav:\n"); bNav.print(); } // update quaternions from euler // angle correction q = Quaternion(EulerAngles(phi, theta, psi)); + + return ret_ok; } -void KalmanNav::correctPos() +int KalmanNav::correctPos() { using namespace math; - Vector y(6); + + // residual + Vector y(5); y(0) = _gps.vel_n - vN; y(1) = _gps.vel_e - vE; - y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat; - y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon; + 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; - // update gps noise - float cosLSing = cosf(lat); - float R = R0 + float(alt); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; - - float noiseLat = _rGpsPos.get() / R; - float noiseLon = _rGpsPos.get() / (R * cosLSing); - RPos(2, 2) = noiseLat * noiseLat; - RPos(3, 3) = noiseLon * noiseLon; - // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance @@ -631,9 +657,9 @@ void KalmanNav::correctPos() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); - // reset P matrix to identity - P = Matrix::identity(9) * 1.0f; - return; + // reset P matrix to P0 + P = P0; + return ret_error; } } @@ -652,10 +678,17 @@ void KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > 1000.0f) { + if (beta > _faultPos.get()) { printf("fault in gps: beta = %8.4f\n", (double)beta); - //printf("y:\n"); y.print(); + 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(4, 4)))); } + + return ret_ok; } void KalmanNav::updateParams() @@ -664,7 +697,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 @@ -676,31 +708,54 @@ 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; // gps noise - float cosLSing = cosf(lat); float R = R0 + float(alt); + float cosLSing = cosf(lat); // prevent singularity - if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f; + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } float noiseVel = _rGpsVel.get(); - float noiseLat = _rGpsPos.get() / R; - float noiseLon = _rGpsPos.get() / (R * cosLSing); + 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) = noiseLat * noiseLat; // lat - RPos(3, 3) = noiseLon * noiseLon; // lon + 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 } |