diff options
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 196 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 17 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/kalman_main.cpp | 20 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/module.mk | 3 | ||||
-rw-r--r-- | src/modules/att_pos_estimator_ekf/params.c | 33 |
5 files changed, 168 insertions, 101 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index c3836bdfa..197c2e36c 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -40,6 +40,7 @@ #include <poll.h> #include "KalmanNav.hpp" +#include <systemlib/err.h> // constants // Titterton pg. 52 @@ -58,14 +59,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : P0(9, 9), V(6, 6), // attitude measurement ekf matrices - HAtt(6, 9), - RAtt(6, 6), + HAtt(4, 9), + RAtt(4, 4), // position measurement ekf matrices HPos(6, 9), RPos(6, 6), // attitude representations C_nb(), q(), + _accel_sub(-1), + _gyro_sub(-1), + _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz @@ -123,8 +127,19 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // initialize quaternions - q = Quaternion(EulerAngles(phi, theta, psi)); + // gyro, accel and mag subscriptions + _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); + _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + + struct accel_report accel; + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); + + struct mag_report mag; + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); + + // initialize rotation quaternion with a single raw sensor measurement + q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); // initialize dcm C_nb = Dcm(q); @@ -141,6 +156,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : updateParams(); } +math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + return math::Quaternion(q0, q1, q2, q3); + +} + void KalmanNav::update() { using namespace math; @@ -181,8 +235,8 @@ void KalmanNav::update() 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", + warnx("initialized EKF attitude\n"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", double(phi), double(theta), double(psi)); _attitudeInitialized = true; } @@ -202,8 +256,8 @@ void KalmanNav::update() 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", + warnx("initialized EKF state with GPS\n"); + warnx("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); } @@ -233,7 +287,7 @@ void KalmanNav::update() // attitude correction step if (_attitudeInitialized // initialized && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 20 // 20 Hz + && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz ) { _attTimeStamp = _sensors.timestamp; correctAtt(); @@ -480,26 +534,32 @@ int KalmanNav::correctAtt() // trig float cosPhi = cosf(phi); float cosTheta = cosf(theta); - float cosPsi = cosf(psi); + // 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(); + // float sinPsi = sinf(psi); // 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 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(); + + // compensate roll and pitch, but not yaw + // XXX take the vectors out of the C_nb matrix to avoid singularities + math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + + // mag measurement + Vector3 magBody(_sensors.magnetometer_ga); + + // transform to earth frame + Vector3 magNav = C_rp * magBody; + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; + if (yMag > M_PI_F) yMag -= 2*M_PI_F; + if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement Vector3 zAccel(_sensors.accelerometer_m_s2); @@ -512,9 +572,9 @@ int KalmanNav::correctAtt() bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; if (ignoreAccel) { + RAttAdjust(1, 1) = 1.0e10; + RAttAdjust(2, 2) = 1.0e10; RAttAdjust(3, 3) = 1.0e10; - RAttAdjust(4, 4) = 1.0e10; - RAttAdjust(5, 5) = 1.0e10; } else { //printf("correcting attitude with accel\n"); @@ -523,58 +583,25 @@ int KalmanNav::correctAtt() // accel predicted measurement Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); - // combined measurement - Vector zAtt(6); - Vector zAttHat(6); + // calculate residual + Vector y(4); + y(0) = yMag; + y(1) = zAccel(0) - zAccelHat(0); + y(2) = zAccel(1) - zAccelHat(1); + y(3) = zAccel(2) - zAccelHat(2); - 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) = 1; - // 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; + // HAccel + HAtt(1, 1) = cosTheta; + HAtt(2, 0) = -cosPhi * cosTheta; + HAtt(2, 1) = sinPhi * sinTheta; + HAtt(3, 0) = sinPhi * cosTheta; + HAtt(3, 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; @@ -585,7 +612,7 @@ int KalmanNav::correctAtt() if (isnan(val) || isinf(val)) { // abort correction and return - printf("[kalman_demo] numerical failure in att correction\n"); + warnx("numerical failure in att correction\n"); // reset P matrix to P0 P = P0; return ret_error; @@ -615,11 +642,8 @@ int KalmanNav::correctAtt() 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(); + warnx("fault in attitude: beta = %8.4f", (double)beta); + warnx("y:\n"); y.print(); } // update quaternions from euler @@ -652,9 +676,9 @@ int KalmanNav::correctPos() for (size_t i = 0; i < xCorrect.getRows(); i++) { float val = xCorrect(i); - if (isnan(val) || isinf(val)) { + if (!isfinite(val)) { // abort correction and return - printf("[kalman_demo] numerical failure in gps correction\n"); + warnx("numerical failure in gps correction\n"); // fallback to GPS vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; @@ -685,8 +709,8 @@ int KalmanNav::correctPos() static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { - 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", + warnx("fault in gps: beta = %8.4f", (double)beta); + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", double(y(0) / sqrtf(RPos(0, 0))), double(y(1) / sqrtf(RPos(1, 1))), double(y(2) / sqrtf(RPos(2, 2))), @@ -722,8 +746,6 @@ void KalmanNav::updateParams() 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(); @@ -731,9 +753,9 @@ void KalmanNav::updateParams() // bound noise to prevent singularities if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - RAtt(3, 3) = noiseAccelSq; // normalized direction - RAtt(4, 4) = noiseAccelSq; - RAtt(5, 5) = noiseAccelSq; + RAtt(1, 1) = noiseAccelSq; // normalized direction + RAtt(2, 2) = noiseAccelSq; + RAtt(3, 3) = noiseAccelSq; // gps noise float R = R0 + float(alt); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index c2bf18115..49d0d157d 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012, 2013 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 @@ -56,6 +56,10 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/parameter_update.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> + #include <drivers/drv_hrt.h> #include <poll.h> #include <unistd.h> @@ -78,6 +82,9 @@ public: */ virtual ~KalmanNav() {}; + + math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); + /** * The main callback function for the class */ @@ -136,6 +143,11 @@ protected: // publications control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ + + int _accel_sub; /**< Accelerometer subscription */ + int _gyro_sub; /**< Gyroscope subscription */ + int _mag_sub; /**< Magnetometer subscription */ + // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -151,7 +163,8 @@ protected: 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 */ + double lat, lon; /**< lat, lon radians */ + float alt; /**< altitude, meters */ // parameters control::BlockParam<float> _vGyro; /**< gyro process noise */ control::BlockParam<float> _vAccel; /**< accelerometer process noise */ diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp index 890184427..4befdc879 100644 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Example User <mail@example.com> + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: James Goppert * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,8 +33,10 @@ ****************************************************************************/ /** - * @file kalman_demo.cpp - * Demonstration of control library + * @file kalman_main.cpp + * Combined attitude / position estimator. + * + * @author James Goppert */ #include <nuttx/config.h> @@ -51,7 +53,7 @@ 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 */ +static int daemon_task; /**< Handle of deamon task / thread */ /** * Deamon management function. @@ -102,9 +104,9 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; - deamon_task = task_spawn_cmd("att_pos_estimator_ekf", + daemon_task = task_spawn_cmd("att_pos_estimator_ekf", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, + SCHED_PRIORITY_MAX - 30, 4096, kalman_demo_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -134,7 +136,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[]) int kalman_demo_thread_main(int argc, char *argv[]) { - warnx("starting\n"); + warnx("starting"); using namespace math; @@ -146,7 +148,7 @@ int kalman_demo_thread_main(int argc, char *argv[]) nav.update(); } - printf("exiting.\n"); + warnx("exiting."); thread_running = false; diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk index 21b7c9166..8d4a40d95 100644 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ b/src/modules/att_pos_estimator_ekf/module.mk @@ -37,9 +37,6 @@ 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 index 50642f067..4d21b342b 100644 --- a/src/modules/att_pos_estimator_ekf/params.c +++ b/src/modules/att_pos_estimator_ekf/params.c @@ -1,3 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 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. + * + ****************************************************************************/ + #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ |