dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #321 from dagar/pr-ekf_formatting 

EKF trivial code style cleanup
This commit is contained in:
Paul Riseborough 2017-08-26 18:17:23 +10:00 committed by GitHub
parent 89236ef275 99ba1c3745
commit fac292fef9
12 changed files with 295 additions and 289 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/RingBuffer.h
View File

@ -71,7 +71,7 @@ public:
_size = size;
// 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;
}
_first_write = true;

3
EKF/airspeed_fusion.cpp
View File

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

28
EKF/control.cpp
View File

@ -195,7 +195,7 @@ void Ekf::controlExternalVisionFusion()
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
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
@ -424,7 +424,7 @@ void Ekf::controlGpsFusion()
}
if (_control_status.flags.gps) {
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
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 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;
_gps_sample_delayed.vel -= vel_offset_earth;
@ -577,7 +577,7 @@ void Ekf::controlHeightSensorTimeouts()
reset_height = true;
ECL_WARN("EKF baro hgt timeout - reset to GPS");
} else if (reset_to_baro){
} else if (reset_to_baro) {
// set height sensor health
_baro_hgt_faulty = false;
@ -968,11 +968,13 @@ void Ekf::checkForStuckRange()
_rng_stuck = false;
} 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;
}
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;
}
_range_data_ready = false;
}
@ -984,11 +986,12 @@ void Ekf::checkForStuckRange()
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
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 airspeed_timed_out = _time_last_imu - _time_last_arsp_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)) {
_control_status.flags.wind = false;
@ -1048,15 +1051,12 @@ void Ekf::controlBetaFusion()
}
fuseSideslip();
}
}
}
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.wind) {
// reset the wind states and covariances when starting drag accel fusion

55
EKF/covariance.cpp
View File

@ -95,7 +95,7 @@ void Ekf::initialiseCovariance()
// earth frame and body frame magnetic field
// 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);
}
@ -733,12 +733,14 @@ void Ekf::fixCovarianceErrors()
const float minSafeStateVar = 1e-9f;
float maxStateVar = minSafeStateVar;
bool resetRequired = false;
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
if (P[stateIndex][stateIndex] > maxStateVar) {
maxStateVar = P[stateIndex][stateIndex];
} else if (P[stateIndex][stateIndex] < minSafeStateVar) {
resetRequired = true;
}
for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) {
if (P[stateIndex][stateIndex] > maxStateVar) {
maxStateVar = P[stateIndex][stateIndex];
} 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
@ -746,31 +748,37 @@ void Ekf::fixCovarianceErrors()
// Also limit variance to a maximum equivalent to a 1g uncertainty
const float minStateVarTarget = 1E-8f;
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 (resetRequired) {
float delVelBiasVar[3];
// store all delta velocity bias variances
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
for (uint8_t stateIndex=13; stateIndex<=15; stateIndex++) {
P[stateIndex][stateIndex] = delVelBiasVar[stateIndex-13];
}
float delVelBiasVar[3];
// store all delta velocity bias variances
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
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
// calculate accel bias term aligned with the gravity vector
float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
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
@ -838,7 +846,7 @@ void Ekf::fixCovarianceErrors()
}
void Ekf::resetMagCovariance()
{
{
// set the quaternion covariance terms to zero
zeroRows(P,0,3);
zeroCols(P,0,3);
@ -893,5 +901,4 @@ void Ekf::resetWindCovariance()
P[23][23] = sq(1.0f);
}
}

24
EKF/drag_fusion.cpp
View File

@ -43,7 +43,7 @@
#include "mathlib.h"
void Ekf::fuseDrag()
{
{
float SH_ACC[4] = {}; // Variable used to optimise calculations of measurement jacobian
float H_ACC[24] = {}; // Observation Jacobian
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;
// 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
if (axis_index == 0) {
// 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
// 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[1] = vn - vwn;
@ -149,11 +149,13 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the X body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
drag_sign = 1.0f;
} 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_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
// 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[1] = vn - vwn;
@ -221,11 +223,13 @@ void Ekf::fuseDrag()
// calculate the predicted acceleration and innovation measured along the Y body axis
float drag_sign;
if (rel_wind(axis_index) >= 0.0f) {
drag_sign = 1.0f;
drag_sign = 1.0f;
} 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_test_ratio[axis_index] = sq(_drag_innov[axis_index]) / (25.0f * _drag_innov_var[axis_index]);

26
EKF/ekf.cpp
View File

@ -43,21 +43,6 @@
#include "ekf.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 ret = initialise_interface(timestamp);
@ -617,7 +602,8 @@ void Ekf::calculateOutputStates()
unsigned index = _output_vert_buffer.get_oldest_index();
unsigned index_next;
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;
current_state = _output_vert_buffer.get_from_index(index);
next_state = _output_vert_buffer.get_from_index(index_next);
@ -625,7 +611,7 @@ void Ekf::calculateOutputStates()
// correct the velocity
if (counter == 0) {
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;
@ -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;
// 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
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
outputSample output_states;
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);
// a constant velocity correction is applied
@ -674,7 +660,7 @@ void Ekf::calculateOutputStates()
output_states.pos += pos_correction;
// 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
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;
// 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
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
float courseYawError = gpsCOG - ekfGOG;
@ -444,10 +444,10 @@ bool Ekf::realignYawGPS()
// add the reset amount to the output observer buffered data
outputSample output_states;
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.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
@ -527,10 +527,10 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
// add the reset amount to the output observer buffered data
outputSample output_states;
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.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
@ -664,7 +664,7 @@ void Ekf::calcMagDeclination()
// set source of magnetic declination for internal use
if (_flt_mag_align_complete) {
// 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) {
// use parameter value until GPS is available, then use value returned by geo library
if (_NED_origin_initialised) {

16
EKF/geo.cpp
View File

@ -42,23 +42,15 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifdef POSIX_SHARED
//#include <unistd.h>
//#include <pthread.h>
#include "ecl.h"
#include <stdio.h>
#include <math.h>
#include <stdbool.h>
#include <string.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.
@ -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;
/* 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));
theta = _wrap_pi(theta);

12
EKF/mag_fusion.cpp
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[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 {
for (uint8_t i=0; i<16; i++) {
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 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[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 {
for (uint8_t i=0; i<16; i++) {
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 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[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 {
for (uint8_t i=0; i<16; i++) {
for (uint8_t i = 0; i < 16; i++) {
Kfusion[i] = 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(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 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
@ -812,7 +812,7 @@ void Ekf::fuseDeclination()
Kfusion[23] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
// 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);
// apply covariance correction via P_new = (I -K*H)*P

2
EKF/optflow_fusion.cpp
View File

@ -76,7 +76,7 @@ void Ekf::fuseOptFlow()
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
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
Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

388
EKF/sideslip_fusion.cpp
View File

File diff suppressed because one or more lines are too long

14
ecl.h
View File

@ -56,3 +56,17 @@
#define ECL_ERR printf
#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