forked from Archive/PX4-Autopilot
Rename / move 23 state filter
This commit is contained in:
Lorenz Meier
parent
b5e70927d7
commit
5bf68dad94
|
|
@ -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);
|
||||
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;
|
||||
|
|
@ -1,80 +1,6 @@
|
|||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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 {
|
||||
|
||||
Loading…
Reference in New Issue