zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF angle constants in degrees for readability (#465) 

* EKF angle constants in degrees for readability

* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S

* EKF controlOpticalFlowFusion() use constants and update comments

* EKF controlMagFusion() use angle in degrees

* EKF move earth spin rate to geo and update usage

* EKF: Fix numerical constant error and clean up comments

Comments do not need to contain numerical values when the code makes these clear.
master
Daniel Agar 7 years ago committed by Paul Riseborough
parent
cebdc3d829
commit
41953ab582
7 changed files with 29 additions and 29 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 17
      EKF/control.cpp
  2. 6
      EKF/covariance.cpp
  3. 8
      EKF/ekf.cpp
  4. 10
      EKF/ekf.h
  5. 10
      EKF/ekf_helper.cpp
  6. 5
      EKF/estimator_interface.h
  7. 2
      geo/geo.h

17
EKF/control.cpp
Unescape View File

@ -58,7 +58,7 @@ void Ekf::controlFusionModes()
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states // Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete // and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) { if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
_control_status.flags.tilt_align = true; _control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
@ -346,11 +346,13 @@ void Ekf::controlOpticalFlowFusion()
// Check if on ground motion is un-suitable for use of optical flow // Check if on ground motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) { if (!_control_status.flags.in_air) {
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
float accel_norm = _accel_vec_filt.norm(); const float accel_norm = _accel_vec_filt.norm();
bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g
|| (accel_norm < 4.9f) // accel less than 0.5g const bool motion_is_excessive = ((accel_norm > (CONSTANTS_ONE_G * 1.5f)) // upper g limit
|| (accel_norm < (CONSTANTS_ONE_G * 0.5f)) // lower g limit
|| (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit || (_ang_rate_mag_filt > _flow_max_rate) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees || (_R_to_earth(2,2) < cosf(math::radians(30.0f)))); // tilted excessively
if (motion_is_excessive) { if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us; _time_bad_motion_us = _imu_sample_delayed.time_us;
@ -516,8 +518,7 @@ void Ekf::controlGpsFusion()
if (_mag_inhibit_yaw_reset_req) { if (_mag_inhibit_yaw_reset_req) {
_mag_inhibit_yaw_reset_req = false; _mag_inhibit_yaw_reset_req = false;
// Zero the yaw bias covariance and set the variance to the initial alignment uncertainty // Zero the yaw bias covariance and set the variance to the initial alignment uncertainty
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * FILTER_UPDATE_PERIOD_S));
setDiag(P, 12, 12, sq(_params.switch_on_gyro_bias * dt));
} }
} }
@ -1307,7 +1308,7 @@ void Ekf::controlMagFusion()
} else if (_mag_bias_observable) { } else if (_mag_bias_observable) {
// monitor yaw rotation in 45 deg sections. // monitor yaw rotation in 45 deg sections.
// a rotation of 45 deg is sufficient to make the mag bias observable // a rotation of 45 deg is sufficient to make the mag bias observable
if (fabsf(_yaw_delta_ef) > 0.7854f) { if (fabsf(_yaw_delta_ef) > math::radians(45.0f)) {
_time_yaw_started = _imu_sample_delayed.time_us; _time_yaw_started = _imu_sample_delayed.time_us;
_yaw_delta_ef = 0.0f; _yaw_delta_ef = 0.0f;
} }

6
EKF/covariance.cpp
Unescape View File

@ -55,7 +55,7 @@ void Ekf::initialiseCovariance()
} }
// calculate average prediction time step in sec // calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; float dt = FILTER_UPDATE_PERIOD_S;
// define the initial angle uncertainty as variances for a rotation vector // define the initial angle uncertainty as variances for a rotation vector
Vector3f rot_vec_var; Vector3f rot_vec_var;
@ -150,7 +150,7 @@ void Ekf::predictCovariance()
float dvy_b = _state.accel_bias(1); float dvy_b = _state.accel_bias(1);
float dvz_b = _state.accel_bias(2); float dvz_b = _state.accel_bias(2);
float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS); float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
float dt_inv = 1.0f / dt; float dt_inv = 1.0f / dt;
// compute noise variance for stationary processes // compute noise variance for stationary processes
@ -907,7 +907,7 @@ void Ekf::resetWindCovariance()
// calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle // calculate the uncertainty in wind speed and direction using the uncertainty in airspeed and sideslip angle
// used to calculate the initial wind speed // used to calculate the initial wind speed
float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f)); float R_spd = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
float R_yaw = sq(0.1745f); float R_yaw = sq(math::radians(10.0f));
// calculate the variance and covariance terms for the wind states // calculate the variance and covariance terms for the wind states
float cos_yaw = cosf(yaw); float cos_yaw = cosf(yaw);

8
EKF/ekf.cpp
Unescape View File

@ -85,7 +85,7 @@ bool Ekf::init(uint64_t timestamp)
_control_status.value = 0; _control_status.value = 0;
_control_status_prev.value = 0; _control_status_prev.value = 0;
_dt_ekf_avg = 0.001f * (float)(FILTER_UPDATE_PERIOD_MS); _dt_ekf_avg = FILTER_UPDATE_PERIOD_S;
_fault_status.value = 0; _fault_status.value = 0;
_innov_check_fail_status.value = 0; _innov_check_fail_status.value = 0;
@ -307,7 +307,7 @@ void Ekf::predictState()
{ {
if (!_earth_rate_initialised) { if (!_earth_rate_initialised) {
if (_NED_origin_initialised) { if (_NED_origin_initialised) {
calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); calcEarthRateNED(_earth_rate_NED, (float)_pos_ref.lat_rad);
_earth_rate_initialised = true; _earth_rate_initialised = true;
} }
} }
@ -359,7 +359,7 @@ void Ekf::predictState()
float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt); float input = 0.5f * (_imu_sample_delayed.delta_vel_dt + _imu_sample_delayed.delta_ang_dt);
// filter and limit input between -50% and +100% of nominal value // filter and limit input between -50% and +100% of nominal value
input = math::constrain(input, 0.0005f * (float)(FILTER_UPDATE_PERIOD_MS), 0.002f * (float)(FILTER_UPDATE_PERIOD_MS)); input = math::constrain(input, 0.5f * FILTER_UPDATE_PERIOD_S, 2.0f * FILTER_UPDATE_PERIOD_S);
_dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input; _dt_ekf_avg = 0.99f * _dt_ekf_avg + 0.01f * input;
} }
@ -391,7 +391,7 @@ bool Ekf::collect_imu(const imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded // if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer // write the accumulated IMU data to the ring buffer
const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f; const float target_dt = FILTER_UPDATE_PERIOD_S;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) { if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {

10
EKF/ekf.h
Unescape View File

@ -237,7 +237,6 @@ public:
private: private:
static constexpr uint8_t _k_num_states{24}; ///< number of EKF states static constexpr uint8_t _k_num_states{24}; ///< number of EKF states
static constexpr float _k_earth_rate{0.000072921f}; ///< earth spin rate (rad/sec)
struct { struct {
uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t velNE_counter; ///< number of horizontal position reset events (allow to wrap if count exceeds 255)
@ -252,7 +251,7 @@ private:
Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
} _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information } _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information
float _dt_ekf_avg{0.001f * FILTER_UPDATE_PERIOD_MS}; ///< average update rate of the ekf float _dt_ekf_avg{FILTER_UPDATE_PERIOD_S}; ///< average update rate of the ekf
float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec) float _dt_update{0.01f}; ///< delta time since last ekf update. This time can be used for filters which run at the same rate as the Ekf::update() function. (sec)
stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon stateSample _state{}; ///< state struct of the ekf running at the delayed time horizon
@ -548,7 +547,7 @@ private:
void fuse(float *K, float innovation); void fuse(float *K, float innovation);
// calculate the earth rotation vector from a given latitude // calculate the earth rotation vector from a given latitude
void calcEarthRateNED(Vector3f &omega, double lat_rad) const; void calcEarthRateNED(Vector3f &omega, float lat_rad) const;
// return true id the GPS quality is good enough to set an origin and start aiding // return true id the GPS quality is good enough to set an origin and start aiding
bool gps_is_good(struct gps_message *gps); bool gps_is_good(struct gps_message *gps);
@ -602,10 +601,7 @@ private:
void checkForStuckRange(); void checkForStuckRange();
// return the square of two floating point numbers - used in auto coded sections // return the square of two floating point numbers - used in auto coded sections
inline float sq(float var) static constexpr float sq(float var) { return var * var; }
{
return var * var;
}
// set control flags to use baro height // set control flags to use baro height
void setControlBaroHeight(); void setControlBaroHeight();

10
EKF/ekf_helper.cpp
Unescape View File

@ -793,7 +793,7 @@ void Ekf::constrainStates()
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg); _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
@ -814,11 +814,11 @@ void Ekf::constrainStates()
} }
// calculate the earth rotation vector // calculate the earth rotation vector
void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
{ {
omega(0) = _k_earth_rate * cosf((float)lat_rad); omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
omega(1) = 0.0f; omega(1) = 0.0f;
omega(2) = -_k_earth_rate * sinf((float)lat_rad); omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
} }
// gets the innovations of velocity and position measurements // gets the innovations of velocity and position measurements
@ -1116,7 +1116,7 @@ bool Ekf::reset_imu_bias()
zeroRows(P, 10, 15); zeroRows(P, 10, 15);
// Set the corresponding variances to the values use for initial alignment // Set the corresponding variances to the values use for initial alignment
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS; float dt = FILTER_UPDATE_PERIOD_S;
P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt); P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt); P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us; _last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;

5
EKF/estimator_interface.h
Unescape View File

@ -381,7 +381,8 @@ public:
void print_status(); void print_status();
static const unsigned FILTER_UPDATE_PERIOD_MS = 8; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
protected: protected:
@ -391,7 +392,7 @@ protected:
OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
which sets the maximum frequency at which we can process non-IMU measurements. Measurements that which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
arrive too soon after the previous measurement will not be processed. arrive too soon after the previous measurement will not be processed.
max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS * 0.001) max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
This can be adjusted to match the max sensor data rate plus some margin for jitter. This can be adjusted to match the max sensor data rate plus some margin for jitter.
*/ */
uint8_t _obs_buffer_length{0}; uint8_t _obs_buffer_length{0};

2
geo/geo.h
Unescape View File

@ -61,6 +61,8 @@ static constexpr float CONSTANTS_ABSOLUTE_NULL_CELSIUS = -273.15f; // °C
static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m) static constexpr double CONSTANTS_RADIUS_OF_EARTH = 6371000; // meters (m)
static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m) static constexpr float CONSTANTS_RADIUS_OF_EARTH_F = CONSTANTS_RADIUS_OF_EARTH; // meters (m)
static constexpr float CONSTANTS_EARTH_SPIN_RATE = 7.2921150e-5f; // radians/second (rad/s)
// XXX remove // XXX remove
struct crosstrack_error_s { struct crosstrack_error_s {

Write Preview
Loading…
Cancel
Save