From d233ca39900b0bf8dccd5d0b6e56437f732c5c26 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 10 Dec 2015 16:36:10 +0100 Subject: [PATCH] added complementary filter for real time state estimation --- EKF/ekf.cpp | 119 +++++++++++++++++++++++++++++++++++++---- EKF/ekf.h | 10 +++- EKF/estimator_base.cpp | 8 ++- EKF/estimator_base.h | 10 ++-- 4 files changed, 132 insertions(+), 15 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 72aae18086..db605e6494 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -52,6 +52,9 @@ Ekf::Ekf(): { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); + _delta_angle_corr.setZero(); + _delta_vel_corr.setZero(); + _vel_corr.setZero(); } @@ -65,8 +68,12 @@ bool Ekf::update() bool ret = false; // indicates if there has been an update if (!_filter_initialised) { _filter_initialised = initialiseFilter(); + if (!_filter_initialised) { + return false; + } } printStates(); + //printStatesFast(); // prediction if (_imu_updated) { ret = true; @@ -105,14 +112,7 @@ bool Ekf::update() fuseAirspeed(); } - // write to output if this has been a prediction step - if (_imu_updated) { - _output_delayed.vel = _state.vel; - _output_delayed.pos = _state.pos; - _output_delayed.quat_nominal = _state.quat_nominal; - _output_delayed.time_us = _imu_time_last; - _imu_updated = false; - } + calculateOutputStates(); return ret; } @@ -142,9 +142,18 @@ bool Ekf::initialiseFilter(void) roll = -asinf(accel_init(1) / cosf(pitch)); } - matrix::Euler euler_init(0, pitch, roll); + matrix::Euler euler_init(roll, pitch, 0.0f); _state.quat_nominal = Quaternion(euler_init); + magSample mag_init = _mag_buffer.get_newest(); + if (mag_init.time_us == 0) { + return false; + } + + matrix::Dcm R_to_earth(euler_init); + _state.mag_I = R_to_earth * mag_init.mag; + + resetVelocity(); resetPosition(); @@ -184,6 +193,73 @@ void Ekf::predictState() constrainStates(); } +void Ekf::calculateOutputStates() +{ + imuSample imu_new = _imu_sample_new; + Vector3f delta_angle; + delta_angle(0) = imu_new.delta_ang(0) * _state.gyro_scale(0); + delta_angle(1) = imu_new.delta_ang(1) * _state.gyro_scale(1); + delta_angle(2) = imu_new.delta_ang(2) * _state.gyro_scale(2); + + delta_angle -= _state.gyro_bias; + + Vector3f delta_vel = imu_new.delta_vel; + delta_vel(2) -= _state.accel_z_bias; + + delta_angle += _delta_angle_corr; + Quaternion dq; + dq.from_axis_angle(delta_angle); + + _output_new.time_us = imu_new.time_us; + _output_new.quat_nominal = dq * _output_new.quat_nominal; + _output_new.quat_nominal.normalize(); + + matrix::Dcm R_to_earth(_output_new.quat_nominal); + + Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr; + delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt; + + Vector3f vel_last = _output_new.vel; + + _output_new.vel += delta_vel_NED; + + _output_new.pos += (_output_new.vel + vel_last) * (imu_new.delta_vel_dt * 0.5f) + _vel_corr * imu_new.delta_vel_dt; + + if (_imu_updated) { + _output_buffer.push(_output_new); + _imu_updated = false; + } + + if (!_output_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_output_sample_delayed)) + { + return; + } + + Quaternion quat_inv = _state.quat_nominal.inversed(); + Quaternion q_error = _output_sample_delayed.quat_nominal * quat_inv; + q_error.normalize(); + Vector3f delta_ang_error; + + float scalar; + + if (q_error(0) >= 0.0f) { + scalar = -2.0f; + } else { + scalar = 2.0f; + } + + delta_ang_error(0) = scalar * q_error(1); + delta_ang_error(1) = scalar * q_error(2); + delta_ang_error(2) = scalar * q_error(3); + + _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt; + + _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; + + _vel_corr = (_state.pos - _output_sample_delayed.pos); + +} + void Ekf::fuseAirspeed() { @@ -237,3 +313,28 @@ void Ekf::printStates() counter++; } + +void Ekf::printStatesFast() +{ + static int counter_fast = 0; + + if (counter_fast % 50 == 0) { + printf("quaternion\n"); + for(int i = 0; i < 4; i++) { + printf("quat %i %.5f\n", i, (double)_output_new.quat_nominal(i)); + } + + printf("vel\n"); + for(int i = 0; i < 3; i++) { + printf("v %i %.5f\n", i, (double)_output_new.vel(i)); + } + + printf("pos\n"); + for(int i = 0; i < 3; i++) { + printf("p %i %.5f\n", i, (double)_output_new.pos(i)); + } + + counter_fast = 0; + } + counter_fast++; +} diff --git a/EKF/ekf.h b/EKF/ekf.h index 481db13d27..561010de57 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -72,6 +72,13 @@ private: float P[_k_num_states][_k_num_states]; // state covariance matrix + // complementary filter states + Vector3f _delta_angle_corr; + Vector3f _delta_vel_corr; + Vector3f _vel_corr; + + void calculateOutputStates(); + bool initialiseFilter(void); void initialiseCovariance(); @@ -110,7 +117,8 @@ private: void printStates(); - void calcEarthRateNED(Vector3f &omega, double lat_rad) const; + void printStatesFast(); + void calcEarthRateNED(Vector3f &omega, double lat_rad) const; }; diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index 866f10b90e..5f9665831b 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -83,6 +83,8 @@ void EstimatorBase::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64 imu_sample_new.time_us = time_usec; + _imu_sample_new = imu_sample_new; + imu_sample_new.delta_ang(0) = imu_sample_new.delta_ang(0) * _state.gyro_scale(0); imu_sample_new.delta_ang(1) = imu_sample_new.delta_ang(1) * _state.gyro_scale(1); imu_sample_new.delta_ang(2) = imu_sample_new.delta_ang(2) * _state.gyro_scale(2); @@ -273,7 +275,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _params.gps_pos_noise = 1.0f; _params.baro_noise = 0.1f; _params.mag_heading_noise = 3e-2f; - _params.mag_declination_deg = 10.0f; + _params.mag_declination_deg = 0.0f; _params.heading_innov_gate = 0.5f; _dt_imu_avg = 0.0f; @@ -285,6 +287,10 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _imu_sample_delayed.delta_vel_dt = 0.0f; _imu_sample_delayed.time_us = time_usec; + _output_new.vel.setZero(); + _output_new.pos.setZero(); + _output_new.quat_nominal = matrix::Quaternion(); + _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 20693f6d1d..4b3a88979e 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -202,7 +202,9 @@ protected: airspeedSample _airspeed_sample_delayed; flowSample _flow_sample_delayed; - outputSample _output_delayed; + outputSample _output_sample_delayed; + outputSample _output_new; + imuSample _imu_sample_new; struct map_projection_reference_s _posRef; float _gps_alt_ref; @@ -266,17 +268,17 @@ public: void copy_quaternion(float *quat) { for (unsigned i = 0; i < 4; i++) { - quat[i] = _state.quat_nominal(i); + quat[i] = _output_new.quat_nominal(i); } } void copy_velocity(float *vel) { for (unsigned i = 0; i < 3; i++) { - vel[i] = _state.vel(i); + vel[i] = _output_new.vel(i); } } void copy_position(float *pos) { for (unsigned i = 0; i < 3; i++) { - pos[i] = _state.pos(i); + pos[i] = _output_new.pos(i); } } void copy_timestamp(uint64_t *time_us) {