#include <complementary_filter.h>
Public Member Functions | |
ComplementaryFilter () | |
double | getAngularVelocityBiasX () const |
double | getAngularVelocityBiasY () const |
double | getAngularVelocityBiasZ () const |
double | getBiasAlpha () const |
bool | getDoAdaptiveGain () const |
bool | getDoBiasEstimation () const |
double | getGainAcc () const |
double | getGainMag () const |
void | getOrientation (double &q0, double &q1, double &q2, double &q3) const |
bool | getSteadyState () const |
bool | setBiasAlpha (double bias_alpha) |
void | setDoAdaptiveGain (bool do_adaptive_gain) |
void | setDoBiasEstimation (bool do_bias_estimation) |
bool | setGainAcc (double gain) |
bool | setGainMag (double gain) |
void | setOrientation (double q0, double q1, double q2, double q3) |
void | update (double ax, double ay, double az, double wx, double wy, double wz, double dt) |
void | update (double ax, double ay, double az, double wx, double wy, double wz, double mx, double my, double mz, double dt) |
virtual | ~ComplementaryFilter () |
Private Member Functions | |
bool | checkState (double ax, double ay, double az, double wx, double wy, double wz) const |
void | getAccCorrection (double ax, double ay, double az, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3) |
double | getAdaptiveGain (double alpha, double ax, double ay, double az) |
void | getMagCorrection (double mx, double my, double mz, double p0, double p1, double p2, double p3, double &dq0, double &dq1, double &dq2, double &dq3) |
void | getMeasurement (double ax, double ay, double az, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas) |
void | getMeasurement (double ax, double ay, double az, double mx, double my, double mz, double &q0_meas, double &q1_meas, double &q2_meas, double &q3_meas) |
void | getPrediction (double wx, double wy, double wz, double dt, double &q0_pred, double &q1_pred, double &q2_pred, double &q3_pred) const |
void | updateBiases (double ax, double ay, double az, double wx, double wy, double wz) |
Private Attributes | |
double | bias_alpha_ |
bool | do_adaptive_gain_ |
bool | do_bias_estimation_ |
double | gain_acc_ |
double | gain_mag_ |
bool | initialized_ |
double | q0_ |
double | q1_ |
double | q2_ |
double | q3_ |
bool | steady_state_ |
double | wx_bias_ |
double | wx_prev_ |
double | wy_bias_ |
double | wy_prev_ |
double | wz_bias_ |
double | wz_prev_ |
Static Private Attributes | |
static const double | gamma_ = 0.01 |
static const double | kAccelerationThreshold = 0.1 |
static const double | kAngularVelocityThreshold = 0.2 |
static const double | kDeltaAngularVelocityThreshold = 0.01 |
static const double | kGravity = 9.81 |
Definition at line 37 of file complementary_filter.h.
Definition at line 47 of file complementary_filter.cpp.
imu_tools::ComplementaryFilter::~ComplementaryFilter | ( | ) | [virtual] |
Definition at line 59 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::checkState | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | wx, | ||
double | wy, | ||
double | wz | ||
) | const [private] |
Definition at line 265 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getAccCorrection | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | p0, | ||
double | p1, | ||
double | p2, | ||
double | p3, | ||
double & | dq0, | ||
double & | dq1, | ||
double & | dq2, | ||
double & | dq3 | ||
) | [private] |
Definition at line 402 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getAdaptiveGain | ( | double | alpha, |
double | ax, | ||
double | ay, | ||
double | az | ||
) | [private] |
Definition at line 453 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getAngularVelocityBiasX | ( | ) | const |
Definition at line 141 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getAngularVelocityBiasY | ( | ) | const |
Definition at line 146 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getAngularVelocityBiasZ | ( | ) | const |
Definition at line 151 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getBiasAlpha | ( | ) | const |
Definition at line 128 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::getDoAdaptiveGain | ( | ) | const |
Definition at line 76 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::getDoBiasEstimation | ( | ) | const |
Definition at line 66 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getGainAcc | ( | ) | const |
Definition at line 102 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::getGainMag | ( | ) | const |
Definition at line 107 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getMagCorrection | ( | double | mx, |
double | my, | ||
double | mz, | ||
double | p0, | ||
double | p1, | ||
double | p2, | ||
double | p3, | ||
double & | dq0, | ||
double & | dq1, | ||
double & | dq2, | ||
double & | dq3 | ||
) | [private] |
Definition at line 424 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getMeasurement | ( | double | ax, |
double | ay, | ||
double | az, | ||
double & | q0_meas, | ||
double & | q1_meas, | ||
double & | q2_meas, | ||
double & | q3_meas | ||
) | [private] |
Definition at line 374 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getMeasurement | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | mx, | ||
double | my, | ||
double | mz, | ||
double & | q0_meas, | ||
double & | q1_meas, | ||
double & | q2_meas, | ||
double & | q3_meas | ||
) | [private] |
Definition at line 318 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getOrientation | ( | double & | q0, |
double & | q1, | ||
double & | q2, | ||
double & | q3 | ||
) | const |
Definition at line 446 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::getPrediction | ( | double | wx, |
double | wy, | ||
double | wz, | ||
double | dt, | ||
double & | q0_pred, | ||
double & | q1_pred, | ||
double & | q2_pred, | ||
double & | q3_pred | ||
) | const [private] |
Definition at line 302 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::getSteadyState | ( | ) | const |
Definition at line 112 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::setBiasAlpha | ( | double | bias_alpha | ) |
Definition at line 117 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::setDoAdaptiveGain | ( | bool | do_adaptive_gain | ) |
Definition at line 71 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::setDoBiasEstimation | ( | bool | do_bias_estimation | ) |
Definition at line 61 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::setGainAcc | ( | double | gain | ) |
Definition at line 81 of file complementary_filter.cpp.
bool imu_tools::ComplementaryFilter::setGainMag | ( | double | gain | ) |
Definition at line 91 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::setOrientation | ( | double | q0, |
double | q1, | ||
double | q2, | ||
double | q3 | ||
) |
Definition at line 133 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::update | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | wx, | ||
double | wy, | ||
double | wz, | ||
double | dt | ||
) |
Definition at line 156 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::update | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | wx, | ||
double | wy, | ||
double | wz, | ||
double | mx, | ||
double | my, | ||
double | mz, | ||
double | dt | ||
) |
Definition at line 207 of file complementary_filter.cpp.
void imu_tools::ComplementaryFilter::updateBiases | ( | double | ax, |
double | ay, | ||
double | az, | ||
double | wx, | ||
double | wy, | ||
double | wz | ||
) | [private] |
Definition at line 285 of file complementary_filter.cpp.
double imu_tools::ComplementaryFilter::bias_alpha_ [private] |
Definition at line 104 of file complementary_filter.h.
bool imu_tools::ComplementaryFilter::do_adaptive_gain_ [private] |
Definition at line 110 of file complementary_filter.h.
bool imu_tools::ComplementaryFilter::do_bias_estimation_ [private] |
Definition at line 107 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::gain_acc_ [private] |
Definition at line 100 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::gain_mag_ [private] |
Definition at line 101 of file complementary_filter.h.
const double imu_tools::ComplementaryFilter::gamma_ = 0.01 [static, private] |
Definition at line 93 of file complementary_filter.h.
bool imu_tools::ComplementaryFilter::initialized_ [private] |
Definition at line 112 of file complementary_filter.h.
const double imu_tools::ComplementaryFilter::kAccelerationThreshold = 0.1 [static, private] |
Definition at line 96 of file complementary_filter.h.
const double imu_tools::ComplementaryFilter::kAngularVelocityThreshold = 0.2 [static, private] |
Definition at line 95 of file complementary_filter.h.
const double imu_tools::ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01 [static, private] |
Definition at line 97 of file complementary_filter.h.
const double imu_tools::ComplementaryFilter::kGravity = 9.81 [static, private] |
Definition at line 92 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::q0_ [private] |
Definition at line 117 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::q1_ [private] |
Definition at line 117 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::q2_ [private] |
Definition at line 117 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::q3_ [private] |
Definition at line 117 of file complementary_filter.h.
bool imu_tools::ComplementaryFilter::steady_state_ [private] |
Definition at line 113 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wx_bias_ [private] |
Definition at line 123 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wx_prev_ [private] |
Definition at line 120 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wy_bias_ [private] |
Definition at line 123 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wy_prev_ [private] |
Definition at line 120 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wz_bias_ [private] |
Definition at line 123 of file complementary_filter.h.
double imu_tools::ComplementaryFilter::wz_prev_ [private] |
Definition at line 120 of file complementary_filter.h.