forked from Archive/PX4-Autopilot
Fixed yaw estimate, minor comment and code style improvements. Added option for upfront init, prepared process for removal (later) of sensors app and subscription to individual topics. Prototyping rework of params / subscriptions to resolve GCC 4.7 incompatibility of control lib (or rather the fact that we need to work around something which looks like a compiler bug)
This commit is contained in:
Lorenz Meier
parent
582ee13d2a
commit
7d59ee6839
|
|
@ -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
|
||||
|
|
@ -66,6 +67,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
|||
// 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,14 +534,30 @@ 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);
|
||||
// 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 dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
|
||||
|
||||
// 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 magNav = C_nb.transpose() * Vector3(_sensors.magnetometer_ga);
|
||||
float yMag = atan2f(magNav(0),magNav(1)) - psi;
|
||||
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;
|
||||
|
||||
|
|
@ -542,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;
|
||||
|
|
@ -572,8 +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();
|
||||
warnx("fault in attitude: beta = %8.4f", (double)beta);
|
||||
warnx("y:\n"); y.print();
|
||||
}
|
||||
|
||||
// update quaternions from euler
|
||||
|
|
@ -606,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;
|
||||
|
|
@ -639,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))),
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);*/
|
||||
|
|
|
|||
Loading…
Reference in New Issue