fix filter

cpp/gtests/main-gtest.cpp

@@ -75,4 +75,48 @@ TEST(squaternion, math_float) {
  qq.normalize();
  q = 2.0f * qq / 2.0f;
  ASSERT_FLOAT_EQ(q.magnitude(), 1.0f);
+}
+
+TEST(squaternion, filter_awm) {
+ // no movement, gravity down
+ vect_t<double> a{0,0,1};
+ // no movement
+ vect_t<double> g{0,0,0};
+ // I "think", compass facing North
+ // x would have a big value
+ // y would be zero
+ // z would be small
+ vect_t<double> m{1,0,.2};
+
+ QCF<double> qcf;
+ QuaternionT<double> q = qcf.update(a,g,m,0.001);
+
+
+ EXPECT_DOUBLE_EQ(q.w, 1.0);
+ EXPECT_DOUBLE_EQ(q.x, 0.0);
+ EXPECT_DOUBLE_EQ(q.y, 0.0);
+ EXPECT_DOUBLE_EQ(q.z, 0.0);
+
+ EXPECT_DOUBLE_EQ(m.x, 1.0);
+ EXPECT_DOUBLE_EQ(m.y, 0.0);
+ EXPECT_DOUBLE_EQ(m.z, 0.2);
+}
+
+
+TEST(squaternion, filter_aw) {
+ vect_t<double> a{0,0,1};
+ vect_t<double> g{0,0,0};
+
+ QCF<double> qcf;
+ QuaternionT<double> q = qcf.update(a,g,0.001);
+
+
+ EXPECT_DOUBLE_EQ(q.w, 1.0);
+ EXPECT_DOUBLE_EQ(q.x, 0.0);
+ EXPECT_DOUBLE_EQ(q.y, 0.0);
+ EXPECT_DOUBLE_EQ(q.z, 0.0);
+
+ EXPECT_DOUBLE_EQ(a.x, 0.0);
+ EXPECT_DOUBLE_EQ(a.y, 0.0);
+ EXPECT_DOUBLE_EQ(a.z, 1.0);
 }

cpp/src/quaternion_filters.hpp

@@ -17,6 +17,22 @@ Quaternion complementary filter:
 using std::sqrt;
 using std::atan2;
 
+template<typename T>
+struct vect_t {
+ T x,y,z;
+
+ bool normalize() {
+ T n = 1.0 / sqrt(x * x + y * y + z * z);
+ if (std::isinf(n)) return false;
+
+ x *= n;
+ y *= n;
+ z *= n;
+
+ return true;
+ }
+};
+
 /*!
 Quaternion complementary filter (QCF) blends the sensor readings from an
 accelerometer and a gyro together to get a stable reading and more reliable
@@ -27,23 +43,75 @@ class QCF {
 public:
  QCF(T a = 0.02) : alpha(a) {}
 
- QuaternionT<T> update(T ax, T ay, T az, T wx, T wy, T wz, T dt) {
- qw = q + 0.5 * dt * QuaternionT<T>(0.0, wx, wy, wz);
- T an = 1.0f / sqrt(ax * ax + ay * ay + az * az);
+ // QuaternionT<T> update(const vect_t<T>& aa, const vect_t<T>& w, const vect_t<T>& mm, T dt) {
+ // qw = q + 0.5 * dt * q * QuaternionT<T>(0.0, w.x, w.y, w.z);
+
+ // vect_t<T> a = aa;
+ // if (!a.normalize()) return q;
+
+ // T roll = atan2(a.y, a.z);
+ // T pitch = atan2(-a.x, sqrt(a.y * a.y + a.z * a.z));
+
+ // vect_t<T> m = mm;
+ // if (!m.normalize()) return q;
+
+ // T cr = cos(roll);
+ // T sr = sin(roll);
+ // T cp = cos(pitch);
+ // T sp = sin(pitch);
+ // T yaw = atan2(
+ // m.z*sr - m.y*cr,
+ // m.x*cp + sp * (m.y * sr + m.z * cr)
+ // );
+
+ // qam = QuaternionT<T>::from_euler(roll, pitch, yaw);
+
+ // q = (1.0 - alpha) * qw + alpha * qam;
+ // return q;
+ // }
+
+ QuaternionT<T> update(const vect_t<T>& aa, const vect_t<T>& w, const vect_t<T>& mm, T dt) {
+ qw = q + 0.5 * dt * q * QuaternionT<T>(0.0, w.x, w.y, w.z);
+
+ vect_t<T> a = aa;
+ if (!a.normalize()) return q;
+
+ T roll = atan2(a.y, a.z);
+ T pitch = atan2(-a.x, sqrt(a.y * a.y + a.z * a.z));
+
+ vect_t<T> m = mm;
+ if (!m.normalize()) return q;
+
+ // WARNING: ahrs.readmedocs switches symbols for roll/pitch compared
+ // to other authors ... below is correct
+ T cr = cos(roll);
+ T sr = sin(roll);
+ T cp = cos(pitch);
+ T sp = sin(pitch);
+ T yaw = atan2(
+ m.z*sp - m.y*cp,
+ m.x*cr + sr * (m.y * sp + m.z * cp)
+ );
+
+ qam = QuaternionT<T>::from_euler(roll, pitch, yaw);
+
+ q = alpha * qw + (1.0 - alpha) * qam;
+ return q;
+ }
 
- if (std::isinf(an)) return q;
+ QuaternionT<T> update(const vect_t<T>& aa, const vect_t<T>& w, T dt) {
+ qw = q + 0.5 * dt * q * QuaternionT<T>(0.0, w.x, w.y, w.z);
 
- ax *= an;
- ay *= an;
- az *= an;
+ vect_t<T> a = aa;
+ a.normalize();
 
- T roll = atan2(ay, az);
- T pitch = atan2(-ax, sqrt(ay * ay + az * az));
+ T roll = atan2(a.y, a.z);
+ T pitch = atan2(-a.x, sqrt(a.y * a.y + a.z * a.z));
  T yaw = 0.0;
 
- qam  = QuaternionT<T>::from_euler(roll, pitch, yaw);
+ qam = QuaternionT<T>::from_euler(roll, pitch, yaw);
 
- q  = (1.0 - alpha) * qw + alpha * qam;
+ q = (1.0 - alpha) * qw + alpha * qam;
  return q;
  }