diff --git a/libraries/Filter/ThirdOrderCompFilter.cpp b/libraries/Filter/ThirdOrderCompFilter.cpp deleted file mode 100644 index 051c68dedd..0000000000 --- a/libraries/Filter/ThirdOrderCompFilter.cpp +++ /dev/null @@ -1,148 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#include -#include - -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include -#endif - -// Public Methods ////////////////////////////////////////////////////////////// - -// update_gains - update gains from time constant (given in seconds) -void ThirdOrderCompFilter::update_gains(float time_constant_seconds_xy, float time_constant_seconds_z) -{ - // X & Y axis time constant - if( time_constant_seconds_xy == 0 ) { - _k1_xy = _k2_xy = _k3_xy = 0; - }else{ - _k1_xy = 3 / time_constant_seconds_xy; - _k2_xy = 3 / (time_constant_seconds_xy*time_constant_seconds_xy); - _k3_xy = 1 / (time_constant_seconds_xy*time_constant_seconds_xy*time_constant_seconds_xy); - } - - // Z axis time constant - if( time_constant_seconds_z == 0 ) { - _k1_z = _k2_z = _k3_z = 0; - }else{ - _k1_z = 3 / time_constant_seconds_z; - _k2_z = 3 / (time_constant_seconds_z*time_constant_seconds_z); - _k3_z = 1 / (time_constant_seconds_z*time_constant_seconds_z*time_constant_seconds_z); - } -} - -// set_3rd_order - resets the first order value (i.e. position) -void ThirdOrderCompFilter::set_3rd_order_xy(float x, float y) -{ - _comp_h.x = x; - _comp_h.y = y; - _comp_h_correction.x = 0; - _comp_h_correction.y = 0; - - // clear historic estimates - _hist_3rd_order_estimates_x.clear(); - _hist_3rd_order_estimates_y.clear(); -} - -// set_3rd_order - resets the first order value (i.e. position) -void ThirdOrderCompFilter::set_3rd_order_z(float z ) -{ - _comp_h.z = z; - _comp_h_correction.z = 0; -} - -// set_2nd_order - resets the second order value (i.e. velocity) -void ThirdOrderCompFilter::set_2nd_order_xy(float x, float y) -{ - _comp_v.x = x; - _comp_v.y = y; -} - -// set_2nd_order - resets the second order value (i.e. velocity) -void ThirdOrderCompFilter::set_2nd_order_z(float z ) -{ - _comp_v.z = z; -} - -// correct_3rd_order_z - correct accelerometer offsets using barometer or gps -void ThirdOrderCompFilter::correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat) -{ - float hist_comp_h_x, hist_comp_h_y; - - // 3rd order samples (i.e. position from gps) are delayed by 500ms - // we store historical position at 10hz so 5 iterations ago - if( _hist_3rd_order_estimates_x.num_items() >= 4 ) { - hist_comp_h_x = _hist_3rd_order_estimates_x.peek(3); - hist_comp_h_y = _hist_3rd_order_estimates_y.peek(3); - }else{ - hist_comp_h_x = _comp_h.x; - hist_comp_h_y = _comp_h.y; - } - - // calculate error in position from gps with our historical estimate - float err_x = x - (hist_comp_h_x + _comp_h_correction.x); - float err_y = y - (hist_comp_h_y + _comp_h_correction.y); - - // calculate correction to accelerometers and apply in the body frame - _comp_k1o += dcm_matrix.mul_transpose(Vector3f((err_x*_k3_xy)*deltat,(err_y*_k3_xy)*deltat,0)); - - // correct velocity - _comp_v.x += (err_x*_k2_xy) * deltat; - _comp_v.y += (err_y*_k2_xy) * deltat; - - // correct position - _comp_h_correction.x += err_x*_k1_xy * deltat; - _comp_h_correction.y += err_y*_k1_xy * deltat; -} - -// correct_3rd_order_z - correct accelerometer offsets using barometer or gps -void ThirdOrderCompFilter::correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat) -{ - float hist_comp_h_z; - - // 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz) - // so we should calculate error using historical estimates - if( _hist_3rd_order_estimates_z.num_items() >= 15 ) { - //hist_comp_h_z = _hist_3rd_order_estimates_z.get(); - hist_comp_h_z = _hist_3rd_order_estimates_z.peek(14); - }else{ - hist_comp_h_z = _comp_h.z; - } - - // calculate error in position from baro with our estimate - float err = third_order_sample - (hist_comp_h_z + _comp_h_correction.z); - - // calculate correction to accelerometers and apply in the body frame - _comp_k1o += dcm_matrix.mul_transpose(Vector3f(0,0,(err*_k3_z) * deltat)); - - // correct velocity - _comp_v.z += (err*_k2_z) * deltat; - - // correct position - _comp_h_correction.z += err*_k1_z * deltat; -} - -// recalculate the 2nd and 3rd order estimates -void ThirdOrderCompFilter::calculate(float deltat, Matrix3f& dcm_matrix) -{ - // get earth frame accelerometer correction - comp_k1o_ef = dcm_matrix * _comp_k1o; - - // calculate velocity by adding new acceleration from accelerometers - _comp_v += (-_first_order_sample + comp_k1o_ef) * deltat; - - // calculate new estimate of position - _comp_h += _comp_v * deltat; - - // store 3rd order estimate (i.e. estimated vertical position) for future use - _hist_3rd_order_estimates_z.add(_comp_h.z); - - // store 3rd order estimate (i.e. horizontal position) for future use at 10hz - _historic_xy_counter++; - if( _historic_xy_counter >= THIRD_ORDER_SAVE_POS_10HZ ) { - _historic_xy_counter = 0; - _hist_3rd_order_estimates_x.add(_comp_h.x); - _hist_3rd_order_estimates_y.add(_comp_h.y); - } -} diff --git a/libraries/Filter/ThirdOrderCompFilter.h b/libraries/Filter/ThirdOrderCompFilter.h deleted file mode 100644 index d0b1e0adc8..0000000000 --- a/libraries/Filter/ThirdOrderCompFilter.h +++ /dev/null @@ -1,89 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// -// This is free software; you can redistribute it and/or modify it under -// the terms of the GNU Lesser General Public License as published by the -// Free Software Foundation; either version 2.1 of the License, or (at -// your option) any later version. -// - -/// @file ThirdOrderCompFilter.h -/// @brief A class to implement third order complementary filter (for combining barometer and GPS with accelerometer data) -/// math provided by Jonathan Challenger - -#ifndef __THIRD_ORDER_COMP_FILTER_H__ -#define __THIRD_ORDER_COMP_FILTER_H__ - -#include -#include // Math library for matrix and vector math -#include // ArduPilot general purpose FIFO buffer - -// #defines to control how often historical accel based positions are saved -// so they can later be compared to laggy gps readings -#define THIRD_ORDER_SAVE_POS_10HZ 10 -#define THIRD_ORDER_SAVE_POS_5HZ 20 - -#define THIRD_ORDER_COMP_FILTER_HISTORIC_XY_SAVE_COUNTER_DEFAULT THIRD_ORDER_SAVE_POS_10HZ - -class ThirdOrderCompFilter -{ -public: - // constructor - ThirdOrderCompFilter(float time_constant_seconds_xy, float time_constant_seconds_z) - { - update_gains(time_constant_seconds_xy, time_constant_seconds_z); - }; - - // update_gains - update gains from time constant (given in seconds) - virtual void update_gains(float time_constant_seconds_xy, float time_constant_seconds_z); - - // set_3rd_order - resets the first order value (i.e. position) - virtual void set_3rd_order_xy(float x, float y); - virtual void set_3rd_order_z(float z); - - // set_2nd_order - resets the second order value (i.e. velocity) - virtual void set_2nd_order_xy(float x, float y); - virtual void set_2nd_order_z(float z); - - // correct_3rd_order_z - correct accelerometer offsets using barometer or gps - virtual void correct_3rd_order_xy(float x, float y, Matrix3f& dcm_matrix, float deltat); - virtual void correct_3rd_order_z(float third_order_sample, Matrix3f& dcm_matrix, float deltat); - - // add_1st_order_sample - Add a new 1st order sample (i.e. acceleration) to the filter, but don't recalculate - virtual void add_1st_order_sample(Vector3f& sample) { _first_order_sample = sample; } - - // recalculate the 2nd and 3rd order estimates - virtual void calculate(float deltat, Matrix3f& dcm_matrix); - - // return the new estimate for the 3rd order (i.e. position) - virtual Vector3f& get_3rd_order_estimate() { _comp_h_total = _comp_h + _comp_h_correction; return _comp_h_total; } - - // return the new estimate for the 2nd order (i.e. velocity) - virtual Vector3f& get_2nd_order_estimate() { return _comp_v; } - - // set the 1st order correction vector (i.e. correction to be applied to the accelerometer) - virtual void set_1st_order_correction( const Vector3f &correction_vector) { _comp_k1o = correction_vector; } - - // get the 1st order correction vector (i.e. correction to be applied to the accelerometer) - virtual Vector3f& get_1st_order_correction( void ) { return _comp_k1o; } - -//private: - float _k1_xy; // 1st order error correction gain for horizontal position - float _k2_xy; // 2nd order error correction gain for horizontal position - float _k3_xy; // 3rd order error correction gain for horizontal position - float _k1_z; // 1st order error correction gain for altitude - float _k2_z; // 2nd order error correction gain for altitude - float _k3_z; // 3rd order error correction gain for altitude - Vector3f _comp_k1o; // acceleration estimate - Vector3f _comp_v; // velocity estimate - Vector3f _comp_h; // position estimate - Vector3f _first_order_sample; // acceleration sample - uint8_t _historic_xy_counter; // historic positions saved when this counter reaches 10 - AP_BufferFloat_Size10 _hist_3rd_order_estimates_x; // buffer of historic accel based position to account for lag - AP_BufferFloat_Size10 _hist_3rd_order_estimates_y; // buffer of historic accel based position to account for lag - AP_BufferFloat_Size15 _hist_3rd_order_estimates_z; // buffer of historic accel based altitudes to account for lag - Vector3f _comp_h_correction; // sum of correction to _comp_h from delayed 1st order samples - Vector3f _comp_h_total; // sum of _comp_h + _comp_h_correction - Vector3f comp_k1o_ef; // accelerometer correction in earth frame (only z element is used). here for debug purposes -}; - -#endif // __THIRD_ORDER_COMP_FILTER_H__