#include <ComplementaryFilter.h>
Public Member Functions | |
ComplementaryFilter (const ParametersMap ¶meters=ParametersMap()) | |
double | getAngularVelocityBiasX () const |
double | getAngularVelocityBiasY () const |
double | getAngularVelocityBiasZ () const |
double | getBiasAlpha () const |
bool | getDoAdaptiveGain () const |
bool | getDoBiasEstimation () const |
double | getGainAcc () const |
virtual void | getOrientation (double &qx, double &qy, double &qz, double &qw) const |
bool | getSteadyState () const |
virtual void | parseParameters (const ParametersMap ¶meters) |
virtual void | reset (double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0) |
bool | setBiasAlpha (double bias_alpha) |
void | setDoAdaptiveGain (bool do_adaptive_gain) |
void | setDoBiasEstimation (bool do_bias_estimation) |
bool | setGainAcc (double gain) |
virtual IMUFilter::Type | type () const |
virtual void | updateImpl (double gx, double gy, double gz, double ax, double ay, double az, double dt) |
virtual | ~ComplementaryFilter () |
Public Member Functions inherited from rtabmap::IMUFilter | |
void | update (double gx, double gy, double gz, double ax, double ay, double az, double stamp) |
virtual | ~IMUFilter () |
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 | 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_ |
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 |
Additional Inherited Members | |
Public Types inherited from rtabmap::IMUFilter | |
enum | Type { kMadgwick =0, kComplementaryFilter =1 } |
Static Public Member Functions inherited from rtabmap::IMUFilter | |
static IMUFilter * | create (const ParametersMap ¶meters=ParametersMap()) |
static IMUFilter * | create (IMUFilter::Type type, const ParametersMap ¶meters=ParametersMap()) |
Protected Member Functions inherited from rtabmap::IMUFilter | |
IMUFilter (const ParametersMap ¶meters=ParametersMap()) | |
Definition at line 37 of file ComplementaryFilter.h.
rtabmap::ComplementaryFilter::ComplementaryFilter | ( | const ParametersMap & | parameters = ParametersMap() | ) |
Definition at line 47 of file ComplementaryFilter.cpp.
|
inlinevirtual |
Definition at line 41 of file ComplementaryFilter.h.
|
private |
Definition at line 217 of file ComplementaryFilter.cpp.
|
private |
Definition at line 354 of file ComplementaryFilter.cpp.
|
private |
Definition at line 383 of file ComplementaryFilter.cpp.
double rtabmap::ComplementaryFilter::getAngularVelocityBiasX | ( | ) | const |
Definition at line 144 of file ComplementaryFilter.cpp.
double rtabmap::ComplementaryFilter::getAngularVelocityBiasY | ( | ) | const |
Definition at line 149 of file ComplementaryFilter.cpp.
double rtabmap::ComplementaryFilter::getAngularVelocityBiasZ | ( | ) | const |
Definition at line 154 of file ComplementaryFilter.cpp.
double rtabmap::ComplementaryFilter::getBiasAlpha | ( | ) | const |
Definition at line 122 of file ComplementaryFilter.cpp.
bool rtabmap::ComplementaryFilter::getDoAdaptiveGain | ( | ) | const |
Definition at line 85 of file ComplementaryFilter.cpp.
bool rtabmap::ComplementaryFilter::getDoBiasEstimation | ( | ) | const |
Definition at line 75 of file ComplementaryFilter.cpp.
double rtabmap::ComplementaryFilter::getGainAcc | ( | ) | const |
Definition at line 101 of file ComplementaryFilter.cpp.
|
private |
Definition at line 326 of file ComplementaryFilter.cpp.
|
private |
Definition at line 270 of file ComplementaryFilter.cpp.
|
virtual |
Implements rtabmap::IMUFilter.
Definition at line 376 of file ComplementaryFilter.cpp.
|
private |
Definition at line 254 of file ComplementaryFilter.cpp.
bool rtabmap::ComplementaryFilter::getSteadyState | ( | ) | const |
Definition at line 106 of file ComplementaryFilter.cpp.
|
virtual |
Reimplemented from rtabmap::IMUFilter.
Definition at line 62 of file ComplementaryFilter.cpp.
|
virtual |
Implements rtabmap::IMUFilter.
Definition at line 127 of file ComplementaryFilter.cpp.
bool rtabmap::ComplementaryFilter::setBiasAlpha | ( | double | bias_alpha | ) |
Definition at line 111 of file ComplementaryFilter.cpp.
void rtabmap::ComplementaryFilter::setDoAdaptiveGain | ( | bool | do_adaptive_gain | ) |
Definition at line 80 of file ComplementaryFilter.cpp.
void rtabmap::ComplementaryFilter::setDoBiasEstimation | ( | bool | do_bias_estimation | ) |
Definition at line 70 of file ComplementaryFilter.cpp.
bool rtabmap::ComplementaryFilter::setGainAcc | ( | double | gain | ) |
Definition at line 90 of file ComplementaryFilter.cpp.
|
inlinevirtual |
Implements rtabmap::IMUFilter.
Definition at line 64 of file ComplementaryFilter.h.
|
private |
Definition at line 237 of file ComplementaryFilter.cpp.
|
virtual |
Implements rtabmap::IMUFilter.
Definition at line 159 of file ComplementaryFilter.cpp.
|
private |
Definition at line 89 of file ComplementaryFilter.h.
|
private |
Definition at line 95 of file ComplementaryFilter.h.
|
private |
Definition at line 92 of file ComplementaryFilter.h.
|
private |
Definition at line 86 of file ComplementaryFilter.h.
|
staticprivate |
Definition at line 79 of file ComplementaryFilter.h.
|
private |
Definition at line 97 of file ComplementaryFilter.h.
|
staticprivate |
Definition at line 82 of file ComplementaryFilter.h.
|
staticprivate |
Definition at line 81 of file ComplementaryFilter.h.
|
staticprivate |
Definition at line 83 of file ComplementaryFilter.h.
|
staticprivate |
Definition at line 78 of file ComplementaryFilter.h.
|
private |
Definition at line 102 of file ComplementaryFilter.h.
|
private |
Definition at line 102 of file ComplementaryFilter.h.
|
private |
Definition at line 102 of file ComplementaryFilter.h.
|
private |
Definition at line 102 of file ComplementaryFilter.h.
|
private |
Definition at line 98 of file ComplementaryFilter.h.
|
private |
Definition at line 108 of file ComplementaryFilter.h.
|
private |
Definition at line 105 of file ComplementaryFilter.h.
|
private |
Definition at line 108 of file ComplementaryFilter.h.
|
private |
Definition at line 105 of file ComplementaryFilter.h.
|
private |
Definition at line 108 of file ComplementaryFilter.h.
|
private |
Definition at line 105 of file ComplementaryFilter.h.