forked from Archive/PX4-Autopilot
Merge remote-tracking branch 'px4/mtecs_estimator' into navigator_rewrite_estimator
This commit is contained in:
Julian Oes
commit
11912e0a5e
|
|
@ -83,7 +83,7 @@
|
|||
#include <mathlib/mathlib.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "estimator.h"
|
||||
#include "estimator_23states.h"
|
||||
|
||||
|
||||
|
||||
|
|
@ -458,6 +458,8 @@ FixedwingEstimator::~FixedwingEstimator()
|
|||
} while (_estimator_task != -1);
|
||||
}
|
||||
|
||||
delete _ekf;
|
||||
|
||||
estimator::g_estimator = nullptr;
|
||||
}
|
||||
|
||||
|
|
@ -571,7 +573,7 @@ FixedwingEstimator::task_main()
|
|||
#else
|
||||
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
/* XXX remove this!, BUT increase the data buffer size! */
|
||||
orb_set_interval(_sensor_combined_sub, 4);
|
||||
orb_set_interval(_sensor_combined_sub, 9);
|
||||
#endif
|
||||
|
||||
/* sets also parameters in the EKF object */
|
||||
|
|
@ -816,6 +818,8 @@ FixedwingEstimator::task_main()
|
|||
|
||||
#endif
|
||||
|
||||
//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
|
||||
|
||||
bool airspeed_updated;
|
||||
orb_check(_airspeed_sub, &airspeed_updated);
|
||||
|
||||
|
|
@ -1254,12 +1258,15 @@ FixedwingEstimator::task_main()
|
|||
_ekf->fuseMagData = true;
|
||||
_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
|
||||
|
||||
_ekf->magstate.obsIndex = 0;
|
||||
_ekf->FuseMagnetometer();
|
||||
_ekf->FuseMagnetometer();
|
||||
_ekf->FuseMagnetometer();
|
||||
|
||||
} else {
|
||||
_ekf->fuseMagData = false;
|
||||
}
|
||||
|
||||
if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
|
||||
|
||||
// Fuse Airspeed Measurements
|
||||
if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
|
||||
_ekf->fuseVtasData = true;
|
||||
|
|
@ -1478,6 +1485,7 @@ FixedwingEstimator::print_status()
|
|||
// 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
|
||||
// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
|
||||
|
||||
if (n_states == 23) {
|
||||
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
|
||||
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
|
||||
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
|
||||
|
|
@ -1502,6 +1510,30 @@ FixedwingEstimator::print_status()
|
|||
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
|
||||
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
|
||||
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
|
||||
} else {
|
||||
printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
|
||||
printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
|
||||
printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
|
||||
printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
|
||||
printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
|
||||
printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
|
||||
printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
|
||||
printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
|
||||
printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
|
||||
printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
|
||||
printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
|
||||
printf("states: %s %s %s %s %s %s %s %s %s %s\n",
|
||||
(_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
|
||||
(_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
|
||||
(_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
|
||||
(_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
|
||||
(_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
|
||||
(_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
|
||||
(_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
|
||||
(_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
|
||||
(_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
|
||||
(_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
|
||||
}
|
||||
}
|
||||
|
||||
int FixedwingEstimator::trip_nan() {
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,247 @@
|
|||
#pragma once
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
class AttPosEKF {
|
||||
|
||||
public:
|
||||
|
||||
AttPosEKF();
|
||||
~AttPosEKF();
|
||||
|
||||
/* ##############################################
|
||||
*
|
||||
* M A I N F I L T E R P A R A M E T E R S
|
||||
*
|
||||
* ########################################### */
|
||||
|
||||
/*
|
||||
* parameters are defined here and initialised in
|
||||
* the InitialiseParameters() (which is just 20 lines down)
|
||||
*/
|
||||
|
||||
float covTimeStepMax; // maximum time allowed between covariance predictions
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
float dVelBiasSigma;
|
||||
float magEarthSigma;
|
||||
float magBodySigma;
|
||||
float gndHgtSigma;
|
||||
|
||||
float vneSigma;
|
||||
float vdSigma;
|
||||
float posNeSigma;
|
||||
float posDSigma;
|
||||
float magMeasurementSigma;
|
||||
float airspeedMeasurementSigma;
|
||||
|
||||
float gyroProcessNoise;
|
||||
float accelProcessNoise;
|
||||
|
||||
float EAS2TAS; // ratio f true to equivalent airspeed
|
||||
|
||||
void InitialiseParameters()
|
||||
{
|
||||
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
dAngBiasSigma = 5.0e-7f;
|
||||
dVelBiasSigma = 1e-4f;
|
||||
magEarthSigma = 3.0e-4f;
|
||||
magBodySigma = 3.0e-4f;
|
||||
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
|
||||
|
||||
vneSigma = 0.2f;
|
||||
vdSigma = 0.3f;
|
||||
posNeSigma = 2.0f;
|
||||
posDSigma = 2.0f;
|
||||
|
||||
magMeasurementSigma = 0.05;
|
||||
airspeedMeasurementSigma = 1.4f;
|
||||
gyroProcessNoise = 1.4544411e-2f;
|
||||
accelProcessNoise = 0.5f;
|
||||
}
|
||||
|
||||
// Global variables
|
||||
float KH[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float KHP[n_states][n_states]; // intermediate result used for covariance updates
|
||||
float P[n_states][n_states]; // covariance matrix
|
||||
float Kfusion[n_states]; // Kalman gains
|
||||
float states[n_states]; // state matrix
|
||||
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
|
||||
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
|
||||
|
||||
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
|
||||
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
|
||||
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
|
||||
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f dVelIMU;
|
||||
Vector3f dAngIMU;
|
||||
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
|
||||
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
|
||||
float innovVelPos[6]; // innovation output
|
||||
float varInnovVelPos[6]; // innovation variance output
|
||||
|
||||
float velNED[3]; // North, East, Down velocity obs (m/s)
|
||||
float posNE[2]; // North, East position obs (m)
|
||||
float hgtMea; // measured height (m)
|
||||
float posNED[3]; // North, East Down position (m)
|
||||
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float innovVtas; // innovation output
|
||||
float varInnovVtas; // innovation variance output
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination;
|
||||
float latRef; // WGS-84 latitude of reference point (rad)
|
||||
float lonRef; // WGS-84 longitude of reference point (rad)
|
||||
float hgtRef; // WGS-84 height of reference point (m)
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
|
||||
// GPS input data variables
|
||||
float gpsCourse;
|
||||
float gpsVelD;
|
||||
float gpsLat;
|
||||
float gpsLon;
|
||||
float gpsHgt;
|
||||
uint8_t GPSstatus;
|
||||
|
||||
// Baro input
|
||||
float baroHgt;
|
||||
|
||||
bool statesInitialised;
|
||||
|
||||
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
|
||||
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
|
||||
struct ekf_status_report current_ekf_state;
|
||||
struct ekf_status_report last_ekf_error;
|
||||
|
||||
bool numericalProtection;
|
||||
|
||||
unsigned storeIndex;
|
||||
|
||||
|
||||
void UpdateStrapdownEquationsNED();
|
||||
|
||||
void CovariancePrediction(float dt);
|
||||
|
||||
void FuseVelposNED();
|
||||
|
||||
void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
|
||||
|
||||
// store staes along with system time stamp in msces
|
||||
void StoreStates(uint64_t timestamp_ms);
|
||||
|
||||
/**
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
* @return zero on success, integer indicating the number of invalid states on failure.
|
||||
* Does only copy valid states, if the statesForFusion vector was initialized
|
||||
* correctly by the caller, the result can be safely used, but is a mixture
|
||||
* time-wise where valid states were updated and invalid remained at the old
|
||||
* value.
|
||||
*/
|
||||
int RecallStates(float statesForFusion[n_states], uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
|
||||
|
||||
void calcEarthRateNED(Vector3f &omega, float latitude);
|
||||
|
||||
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
|
||||
|
||||
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
||||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
|
||||
|
||||
static float sq(float valIn);
|
||||
|
||||
void OnGroundCheck();
|
||||
|
||||
void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
void ConstrainStates();
|
||||
|
||||
void ForceSymmetry();
|
||||
|
||||
int CheckAndBound();
|
||||
|
||||
void ResetPosition();
|
||||
|
||||
void ResetVelocity();
|
||||
|
||||
void ZeroVariables();
|
||||
|
||||
void GetFilterState(struct ekf_status_report *state);
|
||||
|
||||
void GetLastErrorState(struct ekf_status_report *last_error);
|
||||
|
||||
bool StatesNaN(struct ekf_status_report *err_report);
|
||||
void FillErrorReport(struct ekf_status_report *err);
|
||||
|
||||
void InitializeDynamic(float (&initvelNED)[3], float declination);
|
||||
|
||||
protected:
|
||||
|
||||
bool FilterHealthy();
|
||||
|
||||
void ResetHeight(void);
|
||||
|
||||
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
|
||||
|
||||
};
|
||||
|
||||
uint32_t millis();
|
||||
|
||||
|
|
@ -1,144 +1,7 @@
|
|||
#include "estimator.h"
|
||||
#include "estimator_23states.h"
|
||||
#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;
|
||||
|
|
@ -1277,7 +1140,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
// data fit is the only assumption we can make
|
||||
// so we might as well take advantage of the computational efficiencies
|
||||
// associated with sequential fusion
|
||||
if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
|
||||
if (useCompass && fuseMagData && (obsIndex < 3))
|
||||
{
|
||||
// Limit range of states modified when on ground
|
||||
if(!onGround)
|
||||
|
|
@ -1293,7 +1156,7 @@ void AttPosEKF::FuseMagnetometer()
|
|||
// three prediction time steps.
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseMagData)
|
||||
if (obsIndex == 0)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtMagMeasTime[0];
|
||||
|
|
@ -1388,11 +1251,6 @@ void AttPosEKF::FuseMagnetometer()
|
|||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
varInnovMag[0] = 1.0f/SK_MX[0];
|
||||
innovMag[0] = MagPred[0] - magData.x;
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseMagData = false;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
|
|
@ -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 {
|
||||
|
||||
|
|
@ -303,7 +229,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]);
|
|||
|
||||
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
|
||||
|
||||
static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
|
||||
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
|
||||
|
||||
static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
|
||||
|
||||
|
|
@ -0,0 +1,139 @@
|
|||
|
||||
#include "estimator_utilities.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");
|
||||
}
|
||||
|
||||
void
|
||||
ekf_debug(const char *fmt, ...)
|
||||
{
|
||||
va_list args;
|
||||
|
||||
va_start(args, fmt);
|
||||
ekf_debug_print(fmt, args);
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#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;
|
||||
};
|
||||
|
||||
void ekf_debug(const char *fmt, ...);
|
||||
|
|
@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator
|
|||
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator.cpp
|
||||
estimator_23states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
|
|
|||
Loading…
Reference in New Issue