From 5bf68dad940b2c2641796badd928faea9fb963a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 7 Jun 2014 12:52:06 +0200 Subject: [PATCH] Rename / move 23 state filter --- .../{estimator.cpp => estimator_23states.cpp} | 143 +----------------- .../{estimator.h => estimator_23states.h} | 76 +--------- 2 files changed, 4 insertions(+), 215 deletions(-) rename src/modules/ekf_att_pos_estimator/{estimator.cpp => estimator_23states.cpp} (98%) rename src/modules/ekf_att_pos_estimator/{estimator.h => estimator_23states.h} (87%) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp similarity index 98% rename from src/modules/ekf_att_pos_estimator/estimator.cpp rename to src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 89e137adcf..ca9db96855 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -2,143 +2,6 @@ #include #include -// 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 - -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); - float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; + deltaQuat[0] = cos(0.5f*rotationMag); + double rotScaler = (sin(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h similarity index 87% rename from src/modules/ekf_att_pos_estimator/estimator.h rename to src/modules/ekf_att_pos_estimator/estimator_23states.h index 4014629237..6d3b891081 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -1,80 +1,6 @@ -#include -#include - #pragma once -#define GRAVITY_MSS 9.80665f -#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; -}; +#include "estimator_utilities.h" class AttPosEKF {