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

Merge pull request #321 from dagar/pr-ekf_formatting 

EKF trivial code style cleanup
master
Paul Riseborough 8 years ago committed by GitHub
parent
89236ef275 99ba1c3745
commit
fac292fef9
12 changed files with 293 additions and 287 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. 2
      EKF/RingBuffer.h
  2. 3
      EKF/airspeed_fusion.cpp
  3. 28
      EKF/control.cpp
  4. 55
      EKF/covariance.cpp
  5. 24
      EKF/drag_fusion.cpp
  6. 26
      EKF/ekf.cpp
  7. 14
      EKF/ekf_helper.cpp
  8. 16
      EKF/geo.cpp
  9. 12
      EKF/mag_fusion.cpp
  10. 2
      EKF/optflow_fusion.cpp
  11. 384
      EKF/sideslip_fusion.cpp
  12. 14
      ecl.h

2
EKF/RingBuffer.h
Unescape View File

@ -71,7 +71,7 @@ public:
_size = size; _size = size;
// set the time elements to zero so that bad data is not retrieved from the buffers // set the time elements to zero so that bad data is not retrieved from the buffers
for (unsigned index=0; index < _size; index++) { for (unsigned index = 0; index < _size; index++) {
_buffer[index].time_us = 0; _buffer[index].time_us = 0;
} }
_first_write = true; _first_write = true;

3
EKF/airspeed_fusion.cpp
Unescape View File

@ -150,8 +150,7 @@ void Ekf::fuseAirspeed()
if (_tas_test_ratio > 1.0f) { if (_tas_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_airspeed = true; _innov_check_fail_status.flags.reject_airspeed = true;
return; return;
} } else {
else {
_innov_check_fail_status.flags.reject_airspeed = false; _innov_check_fail_status.flags.reject_airspeed = false;
} }

28
EKF/control.cpp
Unescape View File

@ -195,7 +195,7 @@ void Ekf::controlExternalVisionFusion()
for (unsigned i=0; i < output_length; i++) { for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i); output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change; output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states); _output_buffer.push_to_index(i, output_states);
} }
// apply the change in attitude quaternion to our newest quaternion estimate // apply the change in attitude quaternion to our newest quaternion estimate
@ -424,7 +424,7 @@ void Ekf::controlGpsFusion()
} }
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS fusion"); ECL_INFO("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu; _time_last_gps = _time_last_imu;
} }
} }
@ -470,7 +470,7 @@ void Ekf::controlGpsFusion()
// correct velocity for offset relative to IMU // correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt); Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate,pos_offset_body); Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth; _gps_sample_delayed.vel -= vel_offset_earth;
@ -577,7 +577,7 @@ void Ekf::controlHeightSensorTimeouts()
reset_height = true; reset_height = true;
ECL_WARN("EKF baro hgt timeout - reset to GPS"); ECL_WARN("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro){ } else if (reset_to_baro) {
// set height sensor health // set height sensor health
_baro_hgt_faulty = false; _baro_hgt_faulty = false;
@ -968,11 +968,13 @@ void Ekf::checkForStuckRange()
_rng_stuck = false; _rng_stuck = false;
} else { } else {
if (_range_sample_delayed.rng > _rng_check_max_val) if (_range_sample_delayed.rng > _rng_check_max_val) {
_rng_check_max_val = _range_sample_delayed.rng; _rng_check_max_val = _range_sample_delayed.rng;
}
if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) if (_rng_check_min_val < 0.1f || _range_sample_delayed.rng < _rng_check_min_val) {
_rng_check_min_val = _range_sample_delayed.rng; _rng_check_min_val = _range_sample_delayed.rng;
}
_range_data_ready = false; _range_data_ready = false;
} }
@ -984,11 +986,12 @@ void Ekf::checkForStuckRange()
void Ekf::controlAirDataFusion() void Ekf::controlAirDataFusion()
{ {
// control activation and initialisation/reset of wind states required for airspeed fusion // control activation and initialisation/reset of wind states required for airspeed fusion
// If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates // If both airspeed and sideslip fusion have timed out and we are not using a drag observation model then we no longer have valid wind estimates
bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6;
bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6;
if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { if (_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) {
_control_status.flags.wind = false; _control_status.flags.wind = false;
@ -1048,15 +1051,12 @@ void Ekf::controlBetaFusion()
} }
fuseSideslip(); fuseSideslip();
} }
} }
void Ekf::controlDragFusion() void Ekf::controlDragFusion()
{ {
if (_params.fusion_mode & MASK_USE_DRAG ) { if (_params.fusion_mode & MASK_USE_DRAG) {
if (_control_status.flags.in_air) { if (_control_status.flags.in_air) {
if (!_control_status.flags.wind) { if (!_control_status.flags.wind) {
// reset the wind states and covariances when starting drag accel fusion // reset the wind states and covariances when starting drag accel fusion

55
EKF/covariance.cpp
Unescape View File

@ -95,7 +95,7 @@ void Ekf::initialiseCovariance()
// earth frame and body frame magnetic field // earth frame and body frame magnetic field
// set to observation variance // set to observation variance
for (uint8_t index=16; index <= 21; index ++) { for (uint8_t index = 16; index <= 21; index ++) {
P[index][index] = sq(_params.mag_noise); P[index][index] = sq(_params.mag_noise);
} }
@ -733,12 +733,14 @@ void Ekf::fixCovarianceErrors()
const float minSafeStateVar = 1e-9f; const float minSafeStateVar = 1e-9f;
float maxStateVar = minSafeStateVar; float maxStateVar = minSafeStateVar;
bool resetRequired = false; bool resetRequired = false;
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
if (P[stateIndex][stateIndex] > maxStateVar) { for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
maxStateVar = P[stateIndex][stateIndex]; if (P[stateIndex][stateIndex] > maxStateVar) {
} else if (P[stateIndex][stateIndex] < minSafeStateVar) { maxStateVar = P[stateIndex][stateIndex];
resetRequired = true;
} } else if (P[stateIndex][stateIndex] < minSafeStateVar) {
resetRequired = true;
}
} }
// To ensure stability of the covariance matrix operations, the ratio of a max and min variance must // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must
@ -746,31 +748,37 @@ void Ekf::fixCovarianceErrors()
// Also limit variance to a maximum equivalent to a 1g uncertainty // Also limit variance to a maximum equivalent to a 1g uncertainty
const float minStateVarTarget = 1E-8f; const float minStateVarTarget = 1E-8f;
float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget);
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar, sq(_gravity_mss * _dt_ekf_avg)); for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
P[stateIndex][stateIndex] = math::constrain(P[stateIndex][stateIndex], minAllowedStateVar,
sq(_gravity_mss * _dt_ekf_avg));
} }
// If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero
if (resetRequired) { if (resetRequired) {
float delVelBiasVar[3]; float delVelBiasVar[3];
// store all delta velocity bias variances
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { // store all delta velocity bias variances
delVelBiasVar[stateIndex-13] = P[stateIndex][stateIndex]; for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
} delVelBiasVar[stateIndex - 13] = P[stateIndex][stateIndex];
// reset all delta velocity bias covariances }
zeroCols(P,13,15);
// restore all delta velocity bias variances // reset all delta velocity bias covariances
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) { zeroCols(P, 13, 15);
P[stateIndex][stateIndex] = delVelBiasVar[stateIndex-13];
} // restore all delta velocity bias variances
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
P[stateIndex][stateIndex] = delVelBiasVar[stateIndex - 13];
}
} }
// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
// calculate accel bias term aligned with the gravity vector // calculate accel bias term aligned with the gravity vector
float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
float down_dvel_bias = 0.0f; float down_dvel_bias = 0.0f;
for (uint8_t axis_index=0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2,axis_index); for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.accel_bias(axis_index) * _R_to_earth(2, axis_index);
} }
// check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation // check that the vertical componenent of accel bias is consistent with both the vertical position and velocity innovation
@ -838,7 +846,7 @@ void Ekf::fixCovarianceErrors()
} }
void Ekf::resetMagCovariance() void Ekf::resetMagCovariance()
{ {
// set the quaternion covariance terms to zero // set the quaternion covariance terms to zero
zeroRows(P,0,3); zeroRows(P,0,3);
zeroCols(P,0,3); zeroCols(P,0,3);
@ -893,5 +901,4 @@ void Ekf::resetWindCovariance()
P[23][23] = sq(1.0f); P[23][23] = sq(1.0f);
} }
} }

24
EKF/drag_fusion.cpp
Unescape View File

@ -43,7 +43,7 @@
#include "mathlib.h" #include "mathlib.h"
void Ekf::fuseDrag() void Ekf::fuseDrag()
{ {
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
float H_ACC[24] = {}; // Observation Jacobian float H_ACC[24] = {}; // Observation Jacobian
float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector float SK_ACC[9] = {}; // Variable used to optimise calculations of the Kalman gain vector
@ -85,7 +85,7 @@ void Ekf::fuseDrag()
rel_wind = earth_to_body * rel_wind; rel_wind = earth_to_body * rel_wind;
// perform sequential fusion of XY specific forces // perform sequential fusion of XY specific forces
for (uint8_t axis_index = 0; axis_index <2; axis_index++) { for (uint8_t axis_index = 0; axis_index < 2; axis_index++) {
// calculate observation jacobiam and Kalman gain vectors // calculate observation jacobiam and Kalman gain vectors
if (axis_index == 0) { if (axis_index == 0) {
// Estimate the airspeed from the measured drag force and ballistic coefficient // Estimate the airspeed from the measured drag force and ballistic coefficient
@ -94,7 +94,7 @@ void Ekf::fuseDrag()
// Estimate the derivative of specific force wrt airspeed along the X axis // Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent arithmetic exceptions // Limit lower value to prevent arithmetic exceptions
float Kacc = fmaxf(1e-1f,rho * BC_inv_x * airSpd); float Kacc = fmaxf(1e-1f, rho * BC_inv_x * airSpd);
SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3); SH_ACC[0] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn; SH_ACC[1] = vn - vwn;
@ -149,11 +149,13 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the X body axis // calculate the predicted acceleration and innovation measured along the X body axis
float drag_sign; float drag_sign;
if (rel_wind(axis_index) >= 0.0f) { if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f; drag_sign = 1.0f;
} else { } else {
drag_sign = -1.0f; drag_sign = -1.0f;
} }
float predAccel = -BC_inv_x * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
float predAccel = -BC_inv_x * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
_drag_innov[axis_index] = predAccel - mea_acc; _drag_innov[axis_index] = predAccel - mea_acc;
_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);
@ -164,7 +166,7 @@ void Ekf::fuseDrag()
// Estimate the derivative of specific force wrt airspeed along the X axis // Estimate the derivative of specific force wrt airspeed along the X axis
// Limit lower value to prevent arithmetic exceptions // Limit lower value to prevent arithmetic exceptions
float Kacc = fmaxf(1e-1f,rho * BC_inv_y * airSpd); float Kacc = fmaxf(1e-1f, rho * BC_inv_y * airSpd);
SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); SH_ACC[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
SH_ACC[1] = vn - vwn; SH_ACC[1] = vn - vwn;
@ -221,11 +223,13 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the Y body axis // calculate the predicted acceleration and innovation measured along the Y body axis
float drag_sign; float drag_sign;
if (rel_wind(axis_index) >= 0.0f) { if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f; drag_sign = 1.0f;
} else { } else {
drag_sign = -1.0f; drag_sign = -1.0f;
} }
float predAccel = -BC_inv_y * 0.5f*rho*sq(rel_wind(axis_index)) * drag_sign;
float predAccel = -BC_inv_y * 0.5f * rho * sq(rel_wind(axis_index)) * drag_sign;
_drag_innov[axis_index] = predAccel - mea_acc; _drag_innov[axis_index] = predAccel - mea_acc;
_drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]); _drag_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);

26
EKF/ekf.cpp
Unescape View File

@ -43,21 +43,6 @@
#include "ekf.h" #include "ekf.h"
#include "mathlib.h" #include "mathlib.h"
#ifndef __PX4_QURT
#if defined(__cplusplus) && !defined(__PX4_NUTTX)
#include <cmath>
#define ISFINITE(x) std::isfinite(x)
#else
#define ISFINITE(x) isfinite(x)
#endif
#endif
#if defined(__PX4_QURT)
// Missing math.h defines
#define ISFINITE(x) __builtin_isfinite(x)
#endif
bool Ekf::init(uint64_t timestamp) bool Ekf::init(uint64_t timestamp)
{ {
bool ret = initialise_interface(timestamp); bool ret = initialise_interface(timestamp);
@ -617,7 +602,8 @@ void Ekf::calculateOutputStates()
unsigned index = _output_vert_buffer.get_oldest_index(); unsigned index = _output_vert_buffer.get_oldest_index();
unsigned index_next; unsigned index_next;
unsigned size = _output_vert_buffer.get_length(); unsigned size = _output_vert_buffer.get_length();
for (unsigned counter=0; counter < (size - 1); counter++) {
for (unsigned counter = 0; counter < (size - 1); counter++) {
index_next = (index + 1) % size; index_next = (index + 1) % size;
current_state = _output_vert_buffer.get_from_index(index); current_state = _output_vert_buffer.get_from_index(index);
next_state = _output_vert_buffer.get_from_index(index_next); next_state = _output_vert_buffer.get_from_index(index_next);
@ -625,7 +611,7 @@ void Ekf::calculateOutputStates()
// correct the velocity // correct the velocity
if (counter == 0) { if (counter == 0) {
current_state.vel_d += vel_d_correction; current_state.vel_d += vel_d_correction;
_output_vert_buffer.push_to_index(index,current_state); _output_vert_buffer.push_to_index(index, current_state);
} }
next_state.vel_d += vel_d_correction; next_state.vel_d += vel_d_correction;
@ -633,7 +619,7 @@ void Ekf::calculateOutputStates()
next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt; next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
// push the updated data to the buffer // push the updated data to the buffer
_output_vert_buffer.push_to_index(index_next,next_state); _output_vert_buffer.push_to_index(index_next, next_state);
// advance the index // advance the index
index = (index + 1) % size; index = (index + 1) % size;
@ -664,7 +650,7 @@ void Ekf::calculateOutputStates()
// loop through the output filter state history and apply the corrections to the velocity and position states // loop through the output filter state history and apply the corrections to the velocity and position states
outputSample output_states; outputSample output_states;
unsigned max_index = _output_buffer.get_length() - 1; unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index=0; index <= max_index; index++) { for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index); output_states = _output_buffer.get_from_index(index);
// a constant velocity correction is applied // a constant velocity correction is applied
@ -674,7 +660,7 @@ void Ekf::calculateOutputStates()
output_states.pos += pos_correction; output_states.pos += pos_correction;
// push the updated data to the buffer // push the updated data to the buffer
_output_buffer.push_to_index(index,output_states); _output_buffer.push_to_index(index, output_states);
} }

14
EKF/ekf_helper.cpp
Unescape View File

@ -351,10 +351,10 @@ bool Ekf::realignYawGPS()
bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps; bool badVelInnov = ((_vel_pos_test_ratio[0] > 1.0f) || (_vel_pos_test_ratio[1] > 1.0f)) && _control_status.flags.gps;
// calculate GPS course over ground angle // calculate GPS course over ground angle
float gpsCOG = atan2f(_gps_sample_delayed.vel(1),_gps_sample_delayed.vel(0)); float gpsCOG = atan2f(_gps_sample_delayed.vel(1), _gps_sample_delayed.vel(0));
// calculate course yaw angle // calculate course yaw angle
float ekfGOG = atan2f(_state.vel(1),_state.vel(0)); float ekfGOG = atan2f(_state.vel(1), _state.vel(0));
// Check the EKF and GPS course over ground for consistency // Check the EKF and GPS course over ground for consistency
float courseYawError = gpsCOG - ekfGOG; float courseYawError = gpsCOG - ekfGOG;
@ -444,10 +444,10 @@ bool Ekf::realignYawGPS()
// add the reset amount to the output observer buffered data // add the reset amount to the output observer buffered data
outputSample output_states; outputSample output_states;
unsigned output_length = _output_buffer.get_length(); unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) { for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i); output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change; output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states); _output_buffer.push_to_index(i, output_states);
} }
// apply the change in attitude quaternion to our newest quaternion estimate // apply the change in attitude quaternion to our newest quaternion estimate
@ -527,10 +527,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// add the reset amount to the output observer buffered data // add the reset amount to the output observer buffered data
outputSample output_states; outputSample output_states;
unsigned output_length = _output_buffer.get_length(); unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) { for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i); output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change; output_states.quat_nominal *= _state_reset_status.quat_change;
_output_buffer.push_to_index(i,output_states); _output_buffer.push_to_index(i, output_states);
} }
// apply the change in attitude quaternion to our newest quaternion estimate // apply the change in attitude quaternion to our newest quaternion estimate
@ -664,7 +664,7 @@ void Ekf::calcMagDeclination()
// set source of magnetic declination for internal use // set source of magnetic declination for internal use
if (_flt_mag_align_complete) { if (_flt_mag_align_complete) {
// Use value consistent with earth field state // Use value consistent with earth field state
_mag_declination = atan2f(_state.mag_I(1),_state.mag_I(0)); _mag_declination = atan2f(_state.mag_I(1), _state.mag_I(0));
} else if (_params.mag_declination_source & MASK_USE_GEO_DECL) { } else if (_params.mag_declination_source & MASK_USE_GEO_DECL) {
// use parameter value until GPS is available, then use value returned by geo library // use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) { if (_NED_origin_initialised) {

16
EKF/geo.cpp
Unescape View File

@ -42,23 +42,15 @@
* @author Anton Babushkin <anton.babushkin@me.com> * @author Anton Babushkin <anton.babushkin@me.com>
*/ */
#ifdef POSIX_SHARED #ifdef POSIX_SHARED
//#include <unistd.h>
//#include <pthread.h> #include "ecl.h"
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include <string.h> #include <string.h>
#include <float.h> #include <float.h>
#ifndef __PX4_QURT
#if defined(__cplusplus) && !defined(__PX4_NUTTX)
#include <cmath>
#define ISFINITE(x) std::isfinite(x)
#else
#define ISFINITE(x) isfinite(x)
#endif
#endif
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved.
@ -473,7 +465,7 @@ float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_ne
double d_lon = lon_next_rad - lon_now_rad; double d_lon = lon_next_rad - lon_now_rad;
/* conscious mix of double and float trig function to maximize speed and efficiency */ /* conscious mix of double and float trig function to maximize speed and efficiency */
float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , float theta = atan2f(sin(d_lon) * cos(lat_next_rad),
cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
theta = _wrap_pi(theta); theta = _wrap_pi(theta);

12
EKF/mag_fusion.cpp
Unescape View File

@ -207,7 +207,7 @@ void Ekf::fuseMag()
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]); Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] - P[22][2]*SH_MAG[1] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[4] - P[22][18]*SK_MX[3]);
Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]); Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][1]*SH_MAG[0] - P[23][2]*SH_MAG[1] + P[23][3]*SH_MAG[2] + P[23][0]*SK_MX[2] - P[23][16]*SK_MX[1] + P[23][17]*SK_MX[4] - P[23][18]*SK_MX[3]);
} else { } else {
for (uint8_t i=0; i<16; i++) { for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f; Kfusion[i] = 0.0f;
} }
Kfusion[22] = 0.0f; Kfusion[22] = 0.0f;
@ -261,7 +261,7 @@ void Ekf::fuseMag()
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]); Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]); Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][0]*SH_MAG[2] + P[23][1]*SH_MAG[1] + P[23][2]*SH_MAG[0] - P[23][3]*SK_MY[2] - P[23][17]*SK_MY[1] - P[23][16]*SK_MY[3] + P[23][18]*SK_MY[4]);
} else { } else {
for (uint8_t i=0; i<16; i++) { for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f; Kfusion[i] = 0.0f;
} }
Kfusion[22] = 0.0f; Kfusion[22] = 0.0f;
@ -315,7 +315,7 @@ void Ekf::fuseMag()
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]); Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] - P[22][1]*SH_MAG[2] + P[22][3]*SH_MAG[0] + P[22][2]*SK_MZ[2] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[4] - P[22][17]*SK_MZ[3]);
Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]); Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][0]*SH_MAG[1] - P[23][1]*SH_MAG[2] + P[23][3]*SH_MAG[0] + P[23][2]*SK_MZ[2] + P[23][18]*SK_MZ[1] + P[23][16]*SK_MZ[4] - P[23][17]*SK_MZ[3]);
} else { } else {
for (uint8_t i=0; i<16; i++) { for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 0.0f; Kfusion[i] = 0.0f;
} }
Kfusion[22] = 0.0f; Kfusion[22] = 0.0f;
@ -523,9 +523,9 @@ void Ekf::fuseHeading()
[ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)] [ cos(pitch)*sin(yaw) + cos(yaw)*sin(pitch)*sin(roll), cos(roll)*cos(yaw), sin(pitch)*sin(yaw) - cos(pitch)*cos(yaw)*sin(roll)]
[ -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll)] [ -cos(roll)*sin(pitch), sin(roll), cos(pitch)*cos(roll)]
*/ */
float yaw = atan2f(-_R_to_earth(0, 1) , _R_to_earth(1, 1)); // first rotation (yaw) float yaw = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll) float roll = asinf(_R_to_earth(2, 1)); // second rotation (roll)
float pitch = atan2f(-_R_to_earth(2, 0) , _R_to_earth(2, 2)); // third rotation (pitch) float pitch = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation predicted_hdg = yaw; // we will need the predicted heading to calculate the innovation
@ -812,7 +812,7 @@ void Ekf::fuseDeclination()
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN); Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
// calculate innovation and constrain // calculate innovation and constrain
float innovation = atan2f(magE , magN) - _mag_declination; float innovation = atan2f(magE, magN) - _mag_declination;
innovation = math::constrain(innovation, -0.5f, 0.5f); innovation = math::constrain(innovation, -0.5f, 0.5f);
// apply covariance correction via P_new = (I -K*H)*P // apply covariance correction via P_new = (I -K*H)*P

2
EKF/optflow_fusion.cpp
Unescape View File

@ -76,7 +76,7 @@ void Ekf::fuseOptFlow()
Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
// calculate the velocity of the sensor reelative to the imu in body frame // calculate the velocity of the sensor reelative to the imu in body frame
Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ , pos_offset_body); Vector3f vel_rel_imu_body = cross_product(_flow_sample_delayed.gyroXYZ, pos_offset_body);
// calculate the velocity of the sensor in the earth frame // calculate the velocity of the sensor in the earth frame
Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

384
EKF/sideslip_fusion.cpp
View File

File diff suppressed because one or more lines are too long

14
ecl.h
Unescape View File

@ -56,3 +56,17 @@
#define ECL_ERR printf #define ECL_ERR printf
#endif #endif
#ifndef __PX4_QURT
#if defined(__cplusplus) && !defined(__PX4_NUTTX)
#include <cmath>
#define ISFINITE(x) std::isfinite(x)
#else
#define ISFINITE(x) isfinite(x)
#endif
#endif
#if defined(__PX4_QURT)
// Missing math.h defines
#define ISFINITE(x) __builtin_isfinite(x)
#endif

Write Preview
Loading…
Cancel
Save