scsl  1.1.1
Shimmering Clarity Standard Library
Madgwick.h
Go to the documentation of this file.
1 
27 #ifndef SCMP_FILTER_MADGWICK_H
28 #define SCMP_FILTER_MADGWICK_H
29 
30 
31 #include <scmp/geom/Vector.h>
32 #include <scmp/geom/Quaternion.h>
33 
34 
36 namespace scmp {
37 
38 namespace estimation {
39 
40 
53 template <typename T>
54 class Madgwick {
55 public:
57  Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame()
58  {};
59 
64  Madgwick(scmp::geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
65  {
66  if (!sf.IsZero()) {
67  sensorFrame = scmp::geom::MakeQuaternion<T>(sf, 0.0);
68  }
69  }
70 
75  deltaT(0.0), previousSensorFrame(), sensorFrame(sf)
76  {};
77 
83  Orientation() const
84  {
85  return this->sensorFrame;
86  }
87 
100  {
101  return (this->sensorFrame * 0.5) * scmp::geom::Quaternion<T>(gyro, 0.0);
102  }
103 
108  void
110  {
111  this->previousSensorFrame = this->sensorFrame;
112  this->sensorFrame = sf;
113  this->deltaT = delta;
114  }
115 
122  void
124  {
125  this->UpdateFrame(sf, this->deltaT);
126  }
127 
139  void
141  {
142  // Ensure the delta isn't zero within a 100 μs
143  // tolerance.
144  if (scmp::WithinTolerance<T>(delta, 0.0, 0.00001)) {
145  return;
146  }
147  scmp::geom::Quaternion<T> q = this->AngularRate(gyro) * delta;
148 
149  this->UpdateFrame(this->sensorFrame + q, delta);
150  }
151 
161  void
163  {
164  this->UpdateAngularOrientation(gyro, this->deltaT);
165  }
166 
173  {
174  return this->sensorFrame.Euler();
175  }
176 
184  void
185  DeltaT(T newDeltaT)
186  {
187  this->deltaT = newDeltaT;
188  }
189 
194  T DeltaT() { return this->deltaT; }
195 
196 private:
197  T deltaT;
198  scmp::geom::Quaternion<T> previousSensorFrame;
199  scmp::geom::Quaternion<T> sensorFrame;
200 };
201 
202 
205 
208 
209 
210 } // namespace estimation
211 } // namespace scmp
212 
213 
214 #endif // SCMP_FILTER_MADGWICK_H
Quaternion implementation suitable for navigation in R3.
Linear algebraic vector class.
Madgwick implements an efficient Orientation estimation for Intertial Measurement Units (IMUs).
Definition: Madgwick.h:54
scmp::geom::Vector< T, 3 > Euler()
Retrieve a vector of the Euler angles in ZYX Orientation.
Definition: Madgwick.h:172
scmp::geom::Quaternion< T > Orientation() const
Return the current orientation as measured by the estimation.
Definition: Madgwick.h:83
void UpdateAngularOrientation(const scmp::geom::Vector< T, 3 > &gyro)
Update the sensor frame with a gyroscope reading.
Definition: Madgwick.h:162
Madgwick(scmp::geom::Quaternion< T > sf)
Initialise the estimation with a sensor frame MakeQuaternion.
Definition: Madgwick.h:74
T DeltaT()
Retrieve the estimation's current ΔT.
Definition: Madgwick.h:194
Madgwick()
The Madgwick estimation is initialised with an identity MakeQuaternion.
Definition: Madgwick.h:57
scmp::geom::Quaternion< T > AngularRate(const scmp::geom::Vector< T, 3 > &gyro) const
Return the estimation's rate of angular change from a sensor frame.
Definition: Madgwick.h:99
void UpdateFrame(const scmp::geom::Quaternion< T > &sf)
Update the sensor frame to a new frame.
Definition: Madgwick.h:123
void DeltaT(T newDeltaT)
Set the default Δt.
Definition: Madgwick.h:185
void UpdateFrame(const scmp::geom::Quaternion< T > &sf, T delta)
Update the sensor frame to a new frame.
Definition: Madgwick.h:109
void UpdateAngularOrientation(const scmp::geom::Vector< T, 3 > &gyro, T delta)
Update the sensor frame with a gyroscope reading.
Definition: Madgwick.h:140
Madgwick(scmp::geom::Vector< T, 3 > sf)
The Madgwick estimation is initialised with a sensor frame.
Definition: Madgwick.h:64
Quaternions provide a representation of Orientation and rotations in three dimensions.
Definition: Quaternion.h:61
bool IsZero() const
Determine whether this is a zero vector.
Definition: Vector.h:158
Shimmering Clarity Math & Physics toolkit.
Definition: estimation.h:31