mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 09:43:34 +08:00
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge
Conflicts: src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp src/modules/uORB/topics/vehicle_attitude.h
This commit is contained in:
Thomas Gubler
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -82,12 +82,11 @@
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/systemlib.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <platforms/px4_defines.h>
|
||||
|
||||
#include "estimator_23states.h"
|
||||
|
||||
|
||||
#include "estimator_22states.h"
|
||||
|
||||
/**
|
||||
* estimator app start / stop handling function
|
||||
@@ -102,7 +101,7 @@ __EXPORT uint64_t getMicros();
|
||||
|
||||
static uint64_t IMUmsec = 0;
|
||||
static uint64_t IMUusec = 0;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
|
||||
static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds
|
||||
|
||||
uint32_t millis()
|
||||
{
|
||||
@@ -223,8 +222,10 @@ private:
|
||||
float _baro_ref; /**< barometer reference altitude */
|
||||
float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
|
||||
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
|
||||
hrt_abstime _last_debug_print = 0;
|
||||
|
||||
perf_counter_t _loop_perf; ///< loop performance counter
|
||||
perf_counter_t _loop_intvl; ///< loop rate counter
|
||||
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
|
||||
perf_counter_t _perf_mag; ///<local performance counter for mag updates
|
||||
perf_counter_t _perf_gps; ///<local performance counter for gps updates
|
||||
@@ -291,10 +292,6 @@ private:
|
||||
|
||||
AttPosEKF *_ekf;
|
||||
|
||||
float _velocity_xy_filtered;
|
||||
float _velocity_z_filtered;
|
||||
float _airspeed_filtered;
|
||||
|
||||
/**
|
||||
* Update our local parameter cache.
|
||||
*/
|
||||
@@ -400,6 +397,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
|
||||
_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
|
||||
_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
|
||||
_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
|
||||
_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
|
||||
@@ -423,10 +421,7 @@ FixedwingEstimator::FixedwingEstimator() :
|
||||
_mavlink_fd(-1),
|
||||
_parameters{},
|
||||
_parameter_handles{},
|
||||
_ekf(nullptr),
|
||||
_velocity_xy_filtered(0.0f),
|
||||
_velocity_z_filtered(0.0f),
|
||||
_airspeed_filtered(0.0f)
|
||||
_ekf(nullptr)
|
||||
{
|
||||
|
||||
_last_run = hrt_absolute_time();
|
||||
@@ -783,6 +778,11 @@ FixedwingEstimator::task_main()
|
||||
_gps.vel_e_m_s = 0.0f;
|
||||
_gps.vel_d_m_s = 0.0f;
|
||||
|
||||
// init lowpass filters for baro and gps altitude
|
||||
float _gps_alt_filt = 0, _baro_alt_filt = 0;
|
||||
float rc = 10.0f; // RC time constant of 1st order LPF in seconds
|
||||
hrt_abstime baro_last = 0;
|
||||
|
||||
_task_running = true;
|
||||
|
||||
while (!_task_should_exit) {
|
||||
@@ -801,6 +801,7 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
perf_count(_loop_intvl);
|
||||
|
||||
/* only update parameters if they changed */
|
||||
if (fds[0].revents & POLLIN) {
|
||||
@@ -1056,6 +1057,11 @@ FixedwingEstimator::task_main()
|
||||
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
|
||||
_ekf->gpsHgt = _gps.alt / 1e3f;
|
||||
|
||||
// update LPF
|
||||
_gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt);
|
||||
|
||||
//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed);
|
||||
|
||||
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
|
||||
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
|
||||
// } else {
|
||||
@@ -1071,7 +1077,6 @@ FixedwingEstimator::task_main()
|
||||
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
|
||||
|
||||
newDataGps = true;
|
||||
|
||||
last_gps = _gps.timestamp_position;
|
||||
|
||||
}
|
||||
@@ -1083,15 +1088,15 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (baro_updated) {
|
||||
|
||||
hrt_abstime baro_last = _baro.timestamp;
|
||||
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
|
||||
float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
|
||||
baro_last = _baro.timestamp;
|
||||
|
||||
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
|
||||
|
||||
if (!_baro_init) {
|
||||
_baro_ref = _baro.altitude;
|
||||
@@ -1181,6 +1186,24 @@ FixedwingEstimator::task_main()
|
||||
|
||||
float initVelNED[3];
|
||||
|
||||
// maintain filtered baro and gps altitudes to calculate weather offset
|
||||
// baro sample rate is ~70Hz and measurement bandwidth is high
|
||||
// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
|
||||
// maintain heavily filtered values for both baro and gps altitude
|
||||
// Assume the filtered output should be identical for both sensors
|
||||
_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
|
||||
// if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
|
||||
// _last_debug_print = hrt_absolute_time();
|
||||
// perf_print_counter(_perf_baro);
|
||||
// perf_reset(_perf_baro);
|
||||
// warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
|
||||
// (double)_baro_gps_offset,
|
||||
// (double)_baro_alt_filt,
|
||||
// (double)_gps_alt_filt,
|
||||
// (double)_global_pos.alt,
|
||||
// (double)_local_pos.z);
|
||||
// }
|
||||
|
||||
/* Initialize the filter first */
|
||||
if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
|
||||
|
||||
@@ -1196,7 +1219,11 @@ FixedwingEstimator::task_main()
|
||||
// Set up height correctly
|
||||
orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
|
||||
_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
|
||||
_baro_gps_offset = _baro.altitude - gps_alt;
|
||||
|
||||
// init filtered gps and baro altitudes
|
||||
_gps_alt_filt = gps_alt;
|
||||
_baro_alt_filt = _baro.altitude;
|
||||
|
||||
_ekf->baroHgt = _baro.altitude;
|
||||
_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
|
||||
|
||||
@@ -1372,10 +1399,15 @@ FixedwingEstimator::task_main()
|
||||
}
|
||||
|
||||
if (newRangeData) {
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->useRangeFinder = true;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->GroundEKF();
|
||||
|
||||
if (_ekf->Tnb.z.z > 0.9f) {
|
||||
// _ekf->rngMea is set in sensor readout already
|
||||
_ekf->fuseRngData = true;
|
||||
_ekf->fuseOptFlowData = false;
|
||||
_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
|
||||
_ekf->OpticalFlowEKF();
|
||||
_ekf->fuseRngData = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1435,22 +1467,6 @@ FixedwingEstimator::task_main()
|
||||
_local_pos.v_z_valid = true;
|
||||
_local_pos.xy_global = true;
|
||||
|
||||
_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
|
||||
_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
|
||||
_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
|
||||
|
||||
|
||||
/* crude land detector for fixedwing only,
|
||||
* TODO: adapt so that it works for both, maybe move to another location
|
||||
*/
|
||||
if (_velocity_xy_filtered < 5
|
||||
&& _velocity_z_filtered < 10
|
||||
&& _airspeed_filtered < 10) {
|
||||
_local_pos.landed = true;
|
||||
} else {
|
||||
_local_pos.landed = false;
|
||||
}
|
||||
|
||||
_local_pos.z_global = false;
|
||||
_local_pos.yaw = _att.yaw;
|
||||
|
||||
@@ -1515,8 +1531,8 @@ FixedwingEstimator::task_main()
|
||||
|
||||
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
|
||||
_wind.timestamp = _global_pos.timestamp;
|
||||
_wind.windspeed_north = _ekf->windSpdFiltNorth;
|
||||
_wind.windspeed_east = _ekf->windSpdFiltEast;
|
||||
_wind.windspeed_north = _ekf->states[14];
|
||||
_wind.windspeed_east = _ekf->states[15];
|
||||
// XXX we need to do something smart about the covariance here
|
||||
// but we default to the estimate covariance for now
|
||||
_wind.covariance_north = _ekf->P[14][14];
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,247 +0,0 @@
|
||||
#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();
|
||||
|
||||
+480
-486
File diff suppressed because one or more lines are too long
+18
-24
@@ -2,7 +2,7 @@
|
||||
|
||||
#include "estimator_utilities.h"
|
||||
|
||||
const unsigned int n_states = 23;
|
||||
const unsigned int n_states = 22;
|
||||
const unsigned int data_buffer_size = 50;
|
||||
|
||||
class AttPosEKF {
|
||||
@@ -29,10 +29,6 @@ public:
|
||||
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 a1; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
@@ -59,9 +55,6 @@ public:
|
||||
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;
|
||||
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
@@ -69,7 +62,6 @@ public:
|
||||
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;
|
||||
@@ -82,12 +74,13 @@ public:
|
||||
accelProcessNoise = 0.5f;
|
||||
|
||||
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
|
||||
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
|
||||
R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
|
||||
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
|
||||
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
|
||||
rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
|
||||
minFlowRng = 0.01f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
|
||||
minFlowRng = 0.3f; //minimum range between ground and flow sensor
|
||||
moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
|
||||
|
||||
}
|
||||
|
||||
struct mag_state_struct {
|
||||
@@ -134,6 +127,7 @@ public:
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow 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)
|
||||
@@ -145,7 +139,7 @@ public:
|
||||
Vector3f lastGyroOffset; // Last gyro offset
|
||||
Vector3f delAngTotal;
|
||||
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinates
|
||||
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
|
||||
Mat3f Tnb; // transformation amtrix from NED to body coordinates
|
||||
|
||||
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
@@ -157,10 +151,6 @@ public:
|
||||
float dtVelPosFilt; // average time between position / velocity fusion steps
|
||||
float dtHgtFilt; // average time between height measurement updates
|
||||
float dtGpsFilt; // average time between gps measurement updates
|
||||
float windSpdFiltNorth; // average wind speed north component
|
||||
float windSpdFiltEast; // average wind speed east component
|
||||
float windSpdFiltAltitude; // the last altitude used to filter wind speed
|
||||
float windSpdFiltClimb; // filtered climb rate
|
||||
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
|
||||
@@ -175,7 +165,8 @@ public:
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float losData[2]; // optical flow LOS rate measurements (rad/sec)
|
||||
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
|
||||
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
|
||||
float innovVtas; // innovation output
|
||||
float innovRng; ///< Range finder innovation
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
@@ -190,6 +181,8 @@ public:
|
||||
bool refSet; ///< flag to indicate if the reference position has been set
|
||||
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
|
||||
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
|
||||
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
|
||||
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
|
||||
|
||||
// GPS input data variables
|
||||
double gpsLat;
|
||||
@@ -217,6 +210,7 @@ public:
|
||||
|
||||
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 useGPS; // boolean true if GPS data is being used
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
@@ -267,11 +261,9 @@ void FuseMagnetometer();
|
||||
|
||||
void FuseAirspeed();
|
||||
|
||||
void FuseRangeFinder();
|
||||
|
||||
void FuseOptFlow();
|
||||
|
||||
void GroundEKF();
|
||||
void OpticalFlowEKF();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
@@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
* Recall the state vector.
|
||||
*
|
||||
* Recalls the vector stored at closest time to the one specified by msec
|
||||
*
|
||||
*FuseOptFlow
|
||||
* @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
|
||||
@@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms);
|
||||
*/
|
||||
int RecallStates(float *statesForFusion, uint64_t msec);
|
||||
|
||||
void RecallOmega(float *omegaForFusion, uint64_t msec);
|
||||
|
||||
void ResetStoredStates();
|
||||
|
||||
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
|
||||
@@ -325,7 +319,7 @@ void CovarianceInit();
|
||||
|
||||
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
|
||||
|
||||
float ConstrainFloat(float val, float min, float max);
|
||||
float ConstrainFloat(float val, float min, float maxf);
|
||||
|
||||
void ConstrainVariances();
|
||||
|
||||
@@ -39,7 +39,7 @@ MODULE_COMMAND = ekf_att_pos_estimator
|
||||
|
||||
SRCS = ekf_att_pos_estimator_main.cpp \
|
||||
ekf_att_pos_estimator_params.c \
|
||||
estimator_23states.cpp \
|
||||
estimator_22states.cpp \
|
||||
estimator_utilities.cpp
|
||||
|
||||
EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100
|
||||
|
||||
Reference in New Issue
Block a user