diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 82fc95f67b..260bba6153 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,13 +33,14 @@ /** * @file ekf.cpp - * Definition of ekf attitude position estimator class. + * Core functions for ekf attitude and position estimator. * - * @author Roman Bast + * @author Roman Bast * */ #include "ekf.h" +#include Ekf::Ekf() { @@ -58,10 +59,6 @@ void Ekf::update() _filter_initialised = initialiseFilter(); } - if (!_filter_initialised) { - return; - } - // prediction if (_imu_updated) { predictState(); @@ -70,15 +67,15 @@ void Ekf::update() } // measurement updates - if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _mag_sample_delayed)) { + if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { fuseMag(); } - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _baro_sample_delayed)) { + if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { _fuse_height = true; } - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _gps_sample_delayed)) { + if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { _fuse_pos = true; _fuse_vel = true; } @@ -87,12 +84,94 @@ void Ekf::update() fusePosVel(); } - if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sample_delayed)) { + if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { fuseRange(); } - if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _airspeed_sample_delayed)) { + if (_airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed)) { fuseAirspeed(); } -} \ No newline at end of file +} + +bool Ekf::initialiseFilter(void) +{ + + _state.ang_error.setZero(); + _state.vel.setZero(); + _state.pos.setZero(); + _state.gyro_bias.setZero(); + _state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f; + _state.accel_z_bias = 0.0f; + _state.mag_I.setZero(); + _state.mag_B.setZero(); + _state.wind_vel.setZero(); + + // get initial attitude estimate from accel vector, assuming vehicle is static + Vector3f accel_init = _imu_down_sampled.delta_vel / _imu_down_sampled.delta_vel_dt; + + float pitch = 0.0f; + float roll = 0.0f; + + if (accel_init.norm() > 0.001f) { + accel_init.normalize(); + + pitch = asinf(accel_init(0)); + roll = -asinf(accel_init(1) / cosf(pitch)); + } + + matrix::Euler euler_init(0, pitch, roll); + _state.quat_nominal = Quaternion(euler_init); + + resetVelocity(); + resetPosition(); + + initialiseCovariance(); + + return true; +} + +void Ekf::predictState() +{ + // compute transformation matrix from body to world frame + matrix::Dcm R(_state.quat_nominal); + R.transpose(); + + // attitude error state prediciton + Quaternion dq; + dq.from_axis_angle(_imu_sample_delayed.delta_ang); + _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal.normalize(); + + Vector3f vel_last = _state.vel; + + // predict velocity states + _state.vel += R * _imu_sample_delayed.delta_vel; + _state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt; + + // predict position states via trapezoidal integration of velocity + _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; + + //matrix::Euler euler(_state.quat_nominal); + //printf("roll pitch yaw %.5f %.5f %.5f\n", (double)euler(2), (double)euler(1), (double)euler(0)); +} + + +void Ekf::fusePosVel() +{ + +} + +void Ekf::fuseMag() +{ +} + +void Ekf::fuseAirspeed() +{ + +} + +void Ekf::fuseRange() +{ + +} diff --git a/EKF/ekf.h b/EKF/ekf.h index b3aaffd2e6..f13e5a8be6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,9 +33,9 @@ /** * @file ekf.h - * Definition of ekf attitude position estimator class. + * Class for core functions for ekf attitude and position estimator. * - * @author Roman Bast + * @author Roman Bast * */ @@ -52,14 +52,19 @@ public: private: + static const uint8_t _kNumStates = 24; bool _filter_initialised; bool _fuse_height; // true if there is new baro data bool _fuse_pos; // true if there is new position data from gps bool _fuse_vel; // true if there is new velocity data from gps + float P[_kNumStates][_kNumStates]; // state covariance matrix + bool initialiseFilter(void); + void initialiseCovariance(); + void predictState(); void predictCovariance(); @@ -72,5 +77,9 @@ private: void fuseRange(); + void resetVelocity(); -}; \ No newline at end of file + void resetPosition(); + + +}; diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 88f4883339..d31a510320 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -35,7 +35,7 @@ * @file estimator_base.cpp * Definition of base class for attitude estimators * - * @author Roman Bast + * @author Roman Bast * */ @@ -264,6 +264,21 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _params.airspeed_delay_ms = 0; _params.requiredEph = 10; _params.requiredEpv = 10; + _params.dax_noise = 0.0f; + _params.day_noise = 0.0f; + _params.daz_noise = 0.0f; + _params.dvx_noise = 0.0f; + _params.dvy_noise = 0.0f; + _params.dvz_noise = 0.0f; + _params.delta_ang_sig = 0.0f; + _params.delta_vel_sig = 0.0f; + _params.delta_pos_sig = 0.0f; + _params.delta_gyro_bias_sig = 0.0f; + _params.delta_gyro_scale_sig = 0.0f; + _params.delta_vel_bias_z_sig = 0.0f; + _params.delta_mag_body_sig = 0.0f; + _params.delta_mag_earth_sig = 0.0f; + _params.delta_wind_sig = 0.0f; _dt_imu_avg = 0.0f; _imu_time_last = time_usec; @@ -400,4 +415,4 @@ void EstimatorBase::printStoredGps() for (int i = 0; i < OBS_BUFFER_LENGTH; i++) { printGps(&_gps_buffer[i]); } -} \ No newline at end of file +} diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index ea3c61aebd..55c86d8928 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,7 @@ * @file estimator_base.h * Definition of base class for attitude estimators * - * @author Roman Bast + * @author Roman Bast * */ @@ -64,6 +64,8 @@ public: EstimatorBase(); ~EstimatorBase(); + virtual void update() = 0; + // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -159,8 +161,25 @@ protected: uint32_t airspeed_delay_ms; float requiredEph; float requiredEpv; - } _params; + float dax_noise; + float day_noise; + float daz_noise; + float dvx_noise; + float dvy_noise; + float dvz_noise; + + float delta_ang_sig; + float delta_vel_sig; + float delta_pos_sig; + float delta_gyro_bias_sig; + float delta_gyro_scale_sig; + float delta_vel_bias_z_sig; + float delta_mag_body_sig; + float delta_mag_earth_sig; + float delta_wind_sig; + + } _params; static const uint8_t OBS_BUFFER_LENGTH = 5; static const uint8_t IMU_BUFFER_LENGTH = 25; @@ -228,4 +247,3 @@ public: void printGps(struct gpsSample *data); void printStoredGps(); }; -