Rename / move 23 state filter

src/modules/ekf_att_pos_estimator/estimator_23states.cpp

@@ -2,143 +2,6 @@
 #include <string.h>
 #include <stdarg.h>
 
-// Define EKF_DEBUG here to enable the debug print calls
-// if the macro is not set, these will be completely
-// optimized out by the compiler.
-//#define EKF_DEBUG
-
-#ifdef EKF_DEBUG
-#include <stdio.h>
-
-static void
-ekf_debug_print(const char *fmt, va_list args)
-{
-    fprintf(stderr, "%s: ", "[ekf]");
-    vfprintf(stderr, fmt, args);
-
-    fprintf(stderr, "\n");
-}
-
-static void
-ekf_debug(const char *fmt, ...)
-{
-    va_list args;
-
-    va_start(args, fmt);
-    ekf_debug_print(fmt, args);
-}
-
-#else
-
-static void ekf_debug(const char *fmt, ...) { while(0){} }
-#endif
-
-float Vector3f::length(void) const
-{
-    return sqrt(x*x + y*y + z*z);
-}
-
-void Vector3f::zero(void)
-{
-    x = 0.0f;
-    y = 0.0f;
-    z = 0.0f;
-}
-
-Mat3f::Mat3f() {
-    identity();
-}
-
-void Mat3f::identity() {
-    x.x = 1.0f;
-    x.y = 0.0f;
-    x.z = 0.0f;
-
-    y.x = 0.0f;
-    y.y = 1.0f;
-    y.z = 0.0f;
-
-    z.x = 0.0f;
-    z.y = 0.0f;
-    z.z = 1.0f;
-}
-
-Mat3f Mat3f::transpose(void) const
-{
-    Mat3f ret = *this;
-    swap_var(ret.x.y, ret.y.x);
-    swap_var(ret.x.z, ret.z.x);
-    swap_var(ret.y.z, ret.z.y);
-    return ret;
-}
-
-// overload + operator to provide a vector addition
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x + vecIn2.x;
-    vecOut.y = vecIn1.y + vecIn2.y;
-    vecOut.z = vecIn1.z + vecIn2.z;
-    return vecOut;
-}
-
-// overload - operator to provide a vector subtraction
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x - vecIn2.x;
-    vecOut.y = vecIn1.y - vecIn2.y;
-    vecOut.z = vecIn1.z - vecIn2.z;
-    return vecOut;
-}
-
-// overload * operator to provide a matrix vector product
-Vector3f operator*( Mat3f matIn, Vector3f vecIn)
-{
-    Vector3f vecOut;
-    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
-    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
-    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
-    return vecOut;
-}
-
-// overload % operator to provide a vector cross product
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
-    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
-    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(Vector3f vecIn1, float sclIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-// overload * operator to provide a vector scaler product
-Vector3f operator*(float sclIn1, Vector3f vecIn1)
-{
-    Vector3f vecOut;
-    vecOut.x = vecIn1.x * sclIn1;
-    vecOut.y = vecIn1.y * sclIn1;
-    vecOut.z = vecIn1.z * sclIn1;
-    return vecOut;
-}
-
-void swap_var(float &d1, float &d2)
-{
-    float tmp = d1;
-    d1 = d2;
-    d2 = tmp;
-}
-
 AttPosEKF::AttPosEKF()
 
     /* NOTE: DO NOT initialize class members here. Use ZeroVariables()
@@ -170,7 +33,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     float rotationMag;
     float qUpdated[4];
     float quatMag;
-    float deltaQuat[4];
+    double deltaQuat[4];
     const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
 
 // Remove sensor bias errors
@@ -199,8 +62,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     }
     else
     {
-        deltaQuat[0] = cosf(0.5f*rotationMag);
+        deltaQuat[0] = cos(0.5f*rotationMag);
-        float rotScaler = (sinf(0.5f*rotationMag))/rotationMag;
+        double rotScaler = (sin(0.5f*rotationMag))/rotationMag;
         deltaQuat[1] = correctedDelAng.x*rotScaler;
         deltaQuat[2] = correctedDelAng.y*rotScaler;
         deltaQuat[3] = correctedDelAng.z*rotScaler;

src/modules/ekf_att_pos_estimator/estimator_23states.h

@@ -1,80 +1,6 @@
-#include <math.h>
-#include <stdint.h>
-
 #pragma once
 
-#define GRAVITY_MSS 9.80665f
+#include "estimator_utilities.h"
-#define deg2rad 0.017453292f
-#define rad2deg 57.295780f
-#define pi 3.141592657f
-#define earthRate 0.000072921f
-#define earthRadius 6378145.0f
-#define earthRadiusInv  1.5678540e-7f
-
-class Vector3f
-{
-private:
-public:
-    float x;
-    float y;
-    float z;
-
-    float length(void) const;
-    void zero(void);
-};
-
-class Mat3f
-{
-private:
-public:
-    Vector3f x;
-    Vector3f y;
-    Vector3f z;
-
-    Mat3f();
-
-    void identity();
-    Mat3f transpose(void) const;
-};
-
-Vector3f operator*(float sclIn1, Vector3f vecIn1);
-Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*( Mat3f matIn, Vector3f vecIn);
-Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
-Vector3f operator*(Vector3f vecIn1, float sclIn1);
-
-void swap_var(float &d1, float &d2);
-
-const unsigned int n_states = 23;
-const unsigned int data_buffer_size = 50;
-
-enum GPS_FIX {
-    GPS_FIX_NOFIX = 0,
-    GPS_FIX_2D = 2,
-    GPS_FIX_3D = 3
-};
-
-struct ekf_status_report {
-    bool velHealth;
-    bool posHealth;
-    bool hgtHealth;
-    bool velTimeout;
-    bool posTimeout;
-    bool hgtTimeout;
-    uint32_t velFailTime;
-    uint32_t posFailTime;
-    uint32_t hgtFailTime;
-    float states[n_states];
-    bool angNaN;
-    bool summedDelVelNaN;
-    bool KHNaN;
-    bool KHPNaN;
-    bool PNaN;
-    bool covarianceNaN;
-    bool kalmanGainsNaN;
-    bool statesNaN;
-};
 
 class AttPosEKF {