From 76a30108d2eab6a65f5fc92e2f9550d25ad5656c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 27 Apr 2013 14:16:34 +0200 Subject: Moved James Gopperts EKF to the new world --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 771 ++++++++++++++++++++++ src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 180 +++++ src/modules/att_pos_estimator_ekf/kalman_main.cpp | 152 +++++ src/modules/att_pos_estimator_ekf/module.mk | 45 ++ src/modules/att_pos_estimator_ekf/params.c | 16 + 5 files changed, 1164 insertions(+) create mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp create mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp create mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp create mode 100644 src/modules/att_pos_estimator_ekf/module.mk create mode 100644 src/modules/att_pos_estimator_ekf/params.c (limited to 'src/modules/att_pos_estimator_ekf') diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp new file mode 100644 index 000000000..4ef150da1 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -0,0 +1,771 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KalmanNav.cpp + * + * kalman filter navigation code + */ + +#include + +#include "KalmanNav.hpp" + +// constants +// 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 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), + // ekf matrices + F(9, 9), + G(9, 6), + P(9, 9), + P0(9, 9), + V(6, 6), + // attitude measurement ekf matrices + HAtt(6, 9), + RAtt(6, 6), + // position measurement ekf matrices + HPos(6, 9), + RPos(6, 6), + // attitude representations + C_nb(), + q(), + // subscriptions + _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 + _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _att(&getPublications(), ORB_ID(vehicle_attitude)), + // timestamps + _pubTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), + _attTimeStamp(hrt_absolute_time()), + _outTimeStamp(hrt_absolute_time()), + // frame count + _navFrames(0), + // miss counts + _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), + // parameters for ground station + _vGyro(this, "V_GYRO"), + _vAccel(this, "V_ACCEL"), + _rMag(this, "R_MAG"), + _rGpsVel(this, "R_GPS_VEL"), + _rGpsPos(this, "R_GPS_POS"), + _rGpsAlt(this, "R_GPS_ALT"), + _rPressAlt(this, "R_PRESS_ALT"), + _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 + P0 = Matrix::identity(9) * 0.01f; + P = P0; + + // initial state + phi = 0.0f; + theta = 0.0f; + psi = 0.0f; + 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)); + + // initialize dcm + C_nb = Dcm(q); + + // HPos is constant + HPos(0, 3) = 1.0f; + HPos(1, 4) = 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; + HPos(5, 8) = 1.0f; + + // initialize all parameters + updateParams(); +} + +void KalmanNav::update() +{ + using namespace math; + + struct pollfd fds[1]; + fds[0].fd = _sensors.getHandle(); + fds[0].events = POLLIN; + + // poll for new data + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + // XXX this is seriously bad - should be an emergency + return; + + } else if (ret == 0) { // timeout + return; + } + + // get new timestamp + uint64_t newTimeStamp = hrt_absolute_time(); + + // check updated subscriptions + if (_param_update.updated()) updateParams(); + + bool gpsUpdate = _gps.updated(); + bool sensorsUpdate = _sensors.updated(); + + // get new information from subscriptions + // this clears update flag + updateSubscriptions(); + + // 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_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + 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); + } + + // 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; + + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); + // count fast frames + _navFrames += 1; + } + + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; + + // gps correction step + if (_positionInitialized && gpsUpdate) { + correctPos(); + } + + // attitude correction step + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + ) { + _attTimeStamp = _sensors.timestamp; + correctAtt(); + } + + // publication + if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz + _pubTimeStamp = newTimeStamp; + + updatePublications(); + } + + // output + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz + _outTimeStamp = newTimeStamp; + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); + _navFrames = 0; + _miss = 0; + } +} + +void KalmanNav::updatePublications() +{ + using namespace math; + + // position publication + _pos.timestamp = _pubTimeStamp; + _pos.time_gps_usec = _gps.timestamp_position; + _pos.valid = true; + _pos.lat = getLatDegE7(); + _pos.lon = getLonDegE7(); + _pos.alt = float(alt); + _pos.relative_alt = float(alt); // TODO, make relative + _pos.vx = vN; + _pos.vy = vE; + _pos.vz = vD; + _pos.hdg = psi; + + // attitude publication + _att.timestamp = _pubTimeStamp; + _att.roll = phi; + _att.pitch = theta; + _att.yaw = psi; + _att.rollspeed = _sensors.gyro_rad_s[0]; + _att.pitchspeed = _sensors.gyro_rad_s[1]; + _att.yawspeed = _sensors.gyro_rad_s[2]; + // TODO, add gyro offsets to filter + _att.rate_offsets[0] = 0.0f; + _att.rate_offsets[1] = 0.0f; + _att.rate_offsets[2] = 0.0f; + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = C_nb(i, j); + + for (int i = 0; i < 4; i++) _att.q[i] = q(i); + + _att.R_valid = true; + _att.q_valid = true; + + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) + _pos.update(); + + if (_attitudeInitialized) + _att.update(); +} + +int KalmanNav::predictState(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + // attitude prediction + if (_attitudeInitialized) { + Vector3 w(_sensors.gyro_rad_s); + + // attitude + q = q + q.derivative(w) * dt; + + // renormalize quaternion if needed + if (fabsf(q.norm() - 1.0f) > 1e-4f) { + q = q.unit(); + } + + // 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); + } + + // 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; + + // XXX position prediction using speed + 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); + } + + return ret_ok; +} + +int KalmanNav::predictStateCovariance(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSq = cosL * cosL; + float tanL = tanf(lat); + + // prepare for matrix + float R = R0 + float(alt); + float RSq = R * R; + + // F Matrix + // Titterton pg. 291 + + F(0, 1) = -(omega * sinL + vE * tanL / R); + F(0, 2) = vN / R; + F(0, 4) = 1.0f / R; + F(0, 6) = -omega * sinL; + F(0, 8) = -vE / RSq; + + F(1, 0) = omega * sinL + vE * tanL / R; + F(1, 2) = omega * cosL + vE / R; + F(1, 3) = -1.0f / R; + F(1, 8) = vN / RSq; + + F(2, 0) = -vN / R; + F(2, 1) = -omega * cosL - vE / R; + F(2, 4) = -tanL / R; + F(2, 6) = -omega * cosL - vE / (R * cosLSq); + F(2, 8) = vE * tanL / RSq; + + F(3, 1) = -fD; + F(3, 2) = fE; + F(3, 3) = vD / R; + F(3, 4) = -2 * (omega * sinL + vE * tanL / R); + F(3, 5) = vN / R; + F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); + F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; + + F(4, 0) = fD; + F(4, 2) = -fN; + F(4, 3) = 2 * omega * sinL + vE * tanL / R; + F(4, 4) = (vN * tanL + vD) / R; + F(4, 5) = 2 * omega * cosL + vE / R; + F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + + vN * vE / (R * cosLSq); + F(4, 8) = -vE * (vN * tanL + vD) / RSq; + + F(5, 0) = -fE; + F(5, 1) = fN; + F(5, 3) = -2 * vN / R; + F(5, 4) = -2 * (omega * cosL + vE / R); + F(5, 6) = 2 * omega * vE * sinL; + F(5, 8) = (vN * vN + vE * vE) / RSq; + + F(6, 3) = 1 / R; + F(6, 8) = -vN / RSq; + + F(7, 4) = 1 / (R * cosL); + F(7, 6) = vE * tanL / (R * cosL); + F(7, 8) = -vE / (cosL * RSq); + + F(8, 5) = -1; + + // G Matrix + // Titterton pg. 291 + G(0, 0) = -C_nb(0, 0); + G(0, 1) = -C_nb(0, 1); + G(0, 2) = -C_nb(0, 2); + G(1, 0) = -C_nb(1, 0); + G(1, 1) = -C_nb(1, 1); + G(1, 2) = -C_nb(1, 2); + G(2, 0) = -C_nb(2, 0); + G(2, 1) = -C_nb(2, 1); + G(2, 2) = -C_nb(2, 2); + + G(3, 3) = C_nb(0, 0); + G(3, 4) = C_nb(0, 1); + G(3, 5) = C_nb(0, 2); + G(4, 3) = C_nb(1, 0); + G(4, 4) = C_nb(1, 1); + G(4, 5) = C_nb(1, 2); + G(5, 3) = C_nb(2, 0); + G(5, 4) = C_nb(2, 1); + G(5, 5) = C_nb(2, 2); + + // continuous predictioon equations + // 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; +} + +int KalmanNav::correctAtt() +{ + using namespace math; + + // trig + float cosPhi = cosf(phi); + float cosTheta = cosf(theta); + float cosPsi = cosf(psi); + float sinPhi = sinf(phi); + float sinTheta = sinf(theta); + float sinPsi = sinf(psi); + + // 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 + 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); + Vector3 bNav(bN, bE, bD); + Vector3 zMagHat = (C_nb.transpose() * bNav).unit(); + + // accel measurement + Vector3 zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.norm(); + zAccel = zAccel.unit(); + + // ignore accel correction when accel mag not close to g + Matrix RAttAdjust = RAtt; + + bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; + + if (ignoreAccel) { + RAttAdjust(3, 3) = 1.0e10; + RAttAdjust(4, 4) = 1.0e10; + RAttAdjust(5, 5) = 1.0e10; + + } else { + //printf("correcting attitude with accel\n"); + } + + // accel predicted measurement + Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + + // combined measurement + Vector zAtt(6); + Vector zAttHat(6); + + for (int i = 0; i < 3; i++) { + zAtt(i) = zMag(i); + zAtt(i + 3) = zAccel(i); + zAttHat(i) = zMagHat(i); + zAttHat(i + 3) = zAccelHat(i); + } + + // HMag , HAtt (0-2,:) + float tmp1 = + cosPsi * cosTheta * bN + + sinPsi * cosTheta * bE - + sinTheta * bD; + HAtt(0, 1) = -( + cosPsi * sinTheta * bN + + sinPsi * sinTheta * bE + + cosTheta * bD + ); + HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE); + HAtt(1, 0) = + (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN + + (cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE + + cosPhi * cosTheta * bD; + HAtt(1, 1) = sinPhi * tmp1; + HAtt(1, 2) = -( + (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN - + (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE + ); + HAtt(2, 0) = -( + (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN + + (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE + + (sinPhi * cosTheta) * bD + ); + HAtt(2, 1) = cosPhi * tmp1; + HAtt(2, 2) = -( + (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN - + (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE + ); + + // HAccel , HAtt (3-5,:) + HAtt(3, 1) = cosTheta; + HAtt(4, 0) = -cosPhi * cosTheta; + HAtt(4, 1) = sinPhi * sinTheta; + HAtt(5, 0) = sinPhi * cosTheta; + HAtt(5, 1) = cosPhi * sinTheta; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Vector y = zAtt - zAttHat; // residual + Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance + Matrix K = P * HAtt.transpose() * S.inverse(); + Vector xCorrect = K * y; + + // 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"); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + if (!ignoreAccel) { + phi += xCorrect(PHI); + theta += xCorrect(THETA); + } + + psi += xCorrect(PSI); + + // attitude also affects nav velocities + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HAtt * P; + + // 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(); + 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; +} + +int KalmanNav::correctPos() +{ + using namespace math; + + // residual + Vector y(6); + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - 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(5) = double(_sensors.baro_alt_meter) - alt; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance + Matrix K = P * HPos.transpose() * S.inverse(); + Vector xCorrect = K * y; + + // 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"); + // fallback to GPS + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + lat += double(xCorrect(LAT)); + lon += double(xCorrect(LON)); + alt += double(xCorrect(ALT)); + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HPos * P; + + // 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(4, 4))), + double(y(5) / sqrtf(RPos(5, 5)))); + } + + return ret_ok; +} + +void KalmanNav::updateParams() +{ + using namespace math; + using namespace control; + SuperBlock::updateParams(); + + // gyro noise + V(0, 0) = _vGyro.get(); // gyro x, rad/s + V(1, 1) = _vGyro.get(); // gyro y + V(2, 2) = _vGyro.get(); // gyro z + + // accel noise + V(3, 3) = _vAccel.get(); // accel x, m/s^2 + V(4, 4) = _vAccel.get(); // accel y + 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 R = R0 + float(alt); + float cosLSing = cosf(lat); + + // prevent singularity + 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; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + float noiseGpsAlt = _rGpsAlt.get(); + float noisePressAlt = _rPressAlt.get(); + + // bound noise to prevent singularities + if (noiseVel < noiseMin) noiseVel = noiseMin; + + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + + if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; + + if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; + + RPos(0, 0) = noiseVel * noiseVel; // vn + RPos(1, 1) = noiseVel * noiseVel; // ve + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon + RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h + RPos(5, 5) = noisePressAlt * noisePressAlt; // h + // XXX, note that RPos depends on lat, so updateParams should + // be called if lat changes significantly +} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp new file mode 100644 index 000000000..c2bf18115 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KalmanNav.hpp + * + * kalman filter navigation code + */ + +#pragma once + +//#define MATRIX_ASSERT +//#define VECTOR_ASSERT + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +/** + * Kalman filter navigation class + * http://en.wikipedia.org/wiki/Extended_Kalman_filter + * Discrete-time extended Kalman filter + */ +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(); + + /** + * 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: + // 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 _sensors; /**< sensors sub. */ + control::UOrbSubscription _gps; /**< gps sub. */ + control::UOrbSubscription _param_update; /**< parameter update sub. */ + // publications + control::UOrbPublication _pos; /**< position pub. */ + control::UOrbPublication _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}; /**< 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 _vGyro; /**< gyro process noise */ + control::BlockParam _vAccel; /**< accelerometer process noise */ + control::BlockParam _rMag; /**< magnetometer measurement noise */ + control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParam _rGpsPos; /**< gps position measurement noise */ + control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam _rPressAlt; /**< press altitude measurement noise */ + 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 + 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); } + void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } + int32_t getAltE3() { return int32_t(alt * 1.0e3); } + void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } +}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp new file mode 100644 index 000000000..aebe3d1fe --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Example User + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file kalman_demo.cpp + * Demonstration of control library + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KalmanNav.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int kalman_demo_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: kalman_demo {start|stop|status} [-p ]\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int att_pos_estimator_ekf_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("kalman_demo already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("kalman_demo", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 4096, + kalman_demo_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tkalman_demo app is running\n"); + + } else { + printf("\tkalman_demo app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int kalman_demo_thread_main(int argc, char *argv[]) +{ + + printf("[kalman_demo] starting\n"); + + using namespace math; + + thread_running = true; + + KalmanNav nav(NULL, "KF"); + + while (!thread_should_exit) { + nav.update(); + } + + printf("[kalman_demo] exiting.\n"); + + thread_running = false; + + return 0; +} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk new file mode 100644 index 000000000..abc664018 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/module.mk @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Basic example application +# + +MODULE_COMMAND = att_pos_estimator_ekf + +# XXX this might be intended for the spawned deamon, validate +MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" + +SRCS = kalman_main.cpp \ + KalmanNav.cpp \ + params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c new file mode 100644 index 000000000..50642f067 --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/params.c @@ -0,0 +1,16 @@ +#include + +/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); +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, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f); +PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); +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); -- cgit v1.2.3