diff options
Diffstat (limited to 'apps')
-rw-r--r-- | apps/commander/commander.c | 13 | ||||
-rw-r--r-- | apps/examples/control_demo/params.c | 44 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 393 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 134 | ||||
-rw-r--r-- | apps/examples/kalman_demo/params.c | 14 | ||||
-rw-r--r-- | apps/mavlink/mavlink_receiver.c | 19 | ||||
-rw-r--r-- | apps/mavlink/orb_listener.c | 2 |
7 files changed, 371 insertions, 248 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 103c53dc6..7654b3426 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1850,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[]) update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - /* check auto mode switch for correct mode */ - if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - /* enable guided mode */ - update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); + // /* check auto mode switch for correct mode */ + // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { + // /* enable guided mode */ + // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { + // XXX hardcode to auto for now update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - } + // } } else { /* center stick position, set SAS for all vehicle types */ diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 8f923f5a1..6525b70f5 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -14,50 +14,50 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass // stabilization mode -PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator -PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder +PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // roll rate 2 aileron +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder // psi -> phi -> p PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f); -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); -PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); -PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); -PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain +PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass +PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f); // integrator wind up guard +PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f); // the max commanded pitch angle +PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID 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); PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f); // h -> thr -PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); +PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f); // altitude to throttle PID PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f); PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); // crosstrack -PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f); +PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain // speed command -PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); -PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); -PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); +PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f); // commanded velocity +PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity // trim -PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); -PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f); +PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) +PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1) 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 } diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index 1ea46d40f..7709ac697 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -68,52 +68,108 @@ class KalmanNav : public control::SuperBlock { public: + /** + * Constructor + */ KalmanNav(SuperBlock *parent, const char *name); + + /** + * Deconstuctor + */ + virtual ~KalmanNav() {}; + /** + * The main callback function for the class + */ void update(); + + + /** + * Publication update + */ virtual void updatePublications(); - void predictFast(float dt); - void predictSlow(float dt); - void correctAtt(); - void correctPos(); + + /** + * State prediction + * Continuous, non-linear + */ + int predictState(float dt); + + /** + * State covariance prediction + * Continuous, linear + */ + int predictStateCovariance(float dt); + + /** + * Attitude correction + */ + int correctAtt(); + + /** + * Position correction + */ + int correctPos(); + + /** + * Overloaded update parameters + */ virtual void updateParams(); protected: - math::Matrix F; - math::Matrix G; - math::Matrix P; - math::Matrix V; - math::Matrix HAtt; - math::Matrix RAtt; - math::Matrix HPos; - math::Matrix RPos; - math::Dcm C_nb; - math::Quaternion q; - control::UOrbSubscription<sensor_combined_s> _sensors; - control::UOrbSubscription<vehicle_gps_position_s> _gps; - control::UOrbSubscription<parameter_update_s> _param_update; - control::UOrbPublication<vehicle_global_position_s> _pos; - control::UOrbPublication<vehicle_attitude_s> _att; - uint64_t _pubTimeStamp; - uint64_t _fastTimeStamp; - uint64_t _slowTimeStamp; - uint64_t _attTimeStamp; - uint64_t _outTimeStamp; - uint16_t _navFrames; - uint16_t _missFast; - uint16_t _missSlow; - float fN, fE, fD; + // kalman filter + math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix G; /**< noise shaping matrix for gyro/accel */ + math::Matrix P; /**< state covariance matrix */ + math::Matrix P0; /**< initial state covariance matrix */ + math::Matrix V; /**< gyro/ accel noise matrix */ + math::Matrix HAtt; /**< attitude measurement matrix */ + math::Matrix RAtt; /**< attitude measurement noise matrix */ + math::Matrix HPos; /**< position measurement jacobian matrix */ + math::Matrix RPos; /**< position measurement noise matrix */ + // attitude + math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Quaternion q; /**< quaternion from body to nav frame */ + // subscriptions + control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */ + control::UOrbSubscription<vehicle_gps_position_s> _gps; /**< gps sub. */ + control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ + control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + // time stamps + uint64_t _pubTimeStamp; /**< output data publication time stamp */ + uint64_t _predictTimeStamp; /**< prediction time stamp */ + uint64_t _attTimeStamp; /**< attitude correction time stamp */ + uint64_t _outTimeStamp; /**< output time stamp */ + // frame count + uint16_t _navFrames; /**< navigation frames completed in output cycle */ + // miss counts + uint16_t _miss; /**< number of times fast prediction loop missed */ + // accelerations + float fN, fE, fD; /**< navigation frame acceleration */ // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; - float phi, theta, psi; - float vN, vE, vD; - double lat, lon, alt; - control::BlockParam<float> _vGyro; - control::BlockParam<float> _vAccel; - control::BlockParam<float> _rMag; - control::BlockParam<float> _rGpsVel; - control::BlockParam<float> _rGpsPos; - control::BlockParam<float> _rGpsAlt; - control::BlockParam<float> _rAccel; + enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ + float phi, theta, psi; /**< 3-2-1 euler angles */ + float vN, vE, vD; /**< navigation velocity, m/s */ + double lat, lon, alt; /**< lat, lon, alt, radians */ + // parameters + control::BlockParam<float> _vGyro; /**< gyro process noise */ + control::BlockParam<float> _vAccel; /**< accelerometer process noise */ + control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ + control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ + control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ + control::BlockParam<float> _magDip; /**< magnetic inclination with level */ + control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParam<float> _g; /**< gravitational constant */ + control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ + control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + // status + bool _attitudeInitialized; + bool _positionInitialized; + uint16_t _attitudeInitCounter; + // accessors int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 03cdca5ed..3bfe01261 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,10 +1,16 @@ #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); 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); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); +PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); + diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index fa63c419f..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; @@ -367,8 +369,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil imu at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil imu at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -412,8 +414,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil gps at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil gps at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } @@ -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 @@ -454,8 +459,8 @@ handle_message(mavlink_message_t *msg) hil_frames += 1 ; // output - if ((timestamp - old_timestamp) > 1000000) { - printf("receiving hil pressure at %d hz\n", hil_frames); + if ((timestamp - old_timestamp) > 10000000) { + printf("receiving hil pressure at %d hz\n", hil_frames/10); old_timestamp = timestamp; hil_frames = 0; } diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 8920714ef..ec5149745 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -718,7 +718,7 @@ uorb_receive_start(void) /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ + orb_set_interval(mavlink_subs.global_pos_sub, 100); /* 10 Hz active updates */ /* --- LOCAL POS VALUE --- */ mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); |