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

Improve on flash memory usage

This commit is contained in:
kamilritz 2019-12-09 08:49:33 +01:00 committed by Paul Riseborough
parent 459b76f9fd
commit 92ba618f57
13 changed files with 75 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
CMakeLists.txt
View File

@ -35,10 +35,15 @@ cmake_minimum_required(VERSION 3.0)
project(ECL CXX)
if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
message(STATUS "set build type to ${CMAKE_BUILD_TYPE}")
if(NOT CMAKE_BUILD_TYPE)
# force debug builds if we test ECL as standalone
if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
set(CMAKE_BUILD_TYPE "Debug" CACHE STRING "Build type" FORCE)
else()
set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Build type" FORCE)
endif()
endif()
message(STATUS "build type is ${CMAKE_BUILD_TYPE}")
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug;Release;RelWithDebInfo;MinSizeRel;Coverage")

8
EKF/airspeed_fusion.cpp
View File

@ -176,7 +176,7 @@ void Ekf::fuseAirspeed()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[5];
for (unsigned row = 0; row < _k_num_states; row++) {
@ -219,11 +219,7 @@ void Ekf::fuseAirspeed()
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
}
}
P = P - KHP;
// correct the covariance matrix for gross errors
fixCovarianceErrors();

40
EKF/control.cpp
View File

@ -276,8 +276,8 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
Vector3f ev_pos_obs_var{};
Vector2f ev_pos_innov_gates{};
Vector3f ev_pos_obs_var;
Vector2f ev_pos_innov_gates;
// correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@ -350,8 +350,8 @@ void Ekf::controlExternalVisionFusion()
// determine if we should use the velocity observations
if (_control_status.flags.ev_vel) {
Vector3f ev_vel_obs_var{};
Vector2f ev_vel_innov_gates{};
Vector3f ev_vel_obs_var;
Vector2f ev_vel_innov_gates;
Vector3f vel_aligned{_ev_sample_delayed.vel};
@ -691,10 +691,10 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.gps) {
Vector2f gps_vel_innov_gates{}; // [horizontal vertical]
Vector2f gps_pos_innov_gates{}; // [horizontal vertical]
Vector3f gps_vel_obs_var{};
Vector3f gps_pos_obs_var{};
Vector2f gps_vel_innov_gates; // [horizontal vertical]
Vector2f gps_pos_innov_gates; // [horizontal vertical]
Vector3f gps_vel_obs_var;
Vector3f gps_pos_obs_var;
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
@ -1173,8 +1173,8 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.baro_hgt) {
Vector2f baro_hgt_innov_gate{};
Vector3f baro_hgt_obs_var{};
Vector2f baro_hgt_innov_gate;
Vector3f baro_hgt_obs_var;
// vertical position innovation - baro measurement has opposite sign to earth z axis
_baro_hgt_innov(2) = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
@ -1203,8 +1203,8 @@ void Ekf::controlHeightFusion()
baro_hgt_obs_var, _baro_hgt_innov_var,_baro_hgt_test_ratio);
} else if (_control_status.flags.gps_hgt) {
Vector2f gps_hgt_innov_gate{};
Vector3f gps_hgt_obs_var{};
Vector2f gps_hgt_innov_gate;
Vector3f gps_hgt_obs_var;
// vertical position innovation - gps measurement has opposite sign to earth z axis
_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
// observation variance - receiver defined and parameter limited
@ -1220,8 +1220,8 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
// TODO: Tilt check does not belong here, should not set fuse height to true if tilted
Vector2f rng_hgt_innov_gate{};
Vector3f rng_hgt_obs_var{};
Vector2f rng_hgt_innov_gate;
Vector3f rng_hgt_obs_var;
// use range finder with tilt correction
_rng_hgt_innov(2) = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
@ -1234,8 +1234,8 @@ void Ekf::controlHeightFusion()
rng_hgt_obs_var, _rng_hgt_innov_var,_rng_hgt_test_ratio);
} else if (_control_status.flags.ev_hgt) {
Vector2f ev_hgt_innov_gate{};
Vector3f ev_hgt_obs_var{};
Vector2f ev_hgt_innov_gate;
Vector3f ev_hgt_obs_var;
// calculate the innovation assuming the external vision observation is in local NED frame
_ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
@ -1401,8 +1401,8 @@ void Ekf::controlFakePosFusion()
// Fuse synthetic position observations every 200msec
if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) {
Vector3f fake_pos_obs_var{};
Vector2f fake_pos_innov_gate{};
Vector3f fake_pos_obs_var;
Vector2f fake_pos_innov_gate;
// Reset position and velocity states if we re-commence this aiding method
@ -1448,8 +1448,8 @@ void Ekf::controlAuxVelFusion()
if (data_ready && primary_aiding) {
Vector2f aux_vel_innov_gate{};
Vector3f aux_vel_obs_var{};
Vector2f aux_vel_innov_gate;
Vector3f aux_vel_obs_var;
_aux_vel_innov(0) = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_aux_vel_innov(1) = _state.vel(1) - _auxvel_sample_delayed.velNE(1);

18
EKF/covariance.cpp
View File

@ -227,7 +227,7 @@ void Ekf::predictCovariance()
}
// compute noise variance for stationary processes
matrix::Vector<float, _k_num_states> process_noise {};
matrix::Vector<float, _k_num_states> process_noise;
// Construct the process noise variance diagonal for those states with a stationary process model
// These are kinematic states and their error growth is controlled separately by the IMU noise variances
@ -323,7 +323,7 @@ void Ekf::predictCovariance()
SPP[10] = SF[16];
// covariance update
matrix::SquareMatrix<float, _k_num_states> nextP {};
matrix::SquareMatrix<float, _k_num_states> nextP;
// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
nextP(0,0) = P(0,0) + P(1,0)*SF[9] + P(2,0)*SF[11] + P(3,0)*SF[10] + P(10,0)*SF[14] + P(11,0)*SF[15] + P(12,0)*SPP[10] + (daxVar*SQ[10])/4 + SF[9]*(P(0,1) + P(1,1)*SF[9] + P(2,1)*SF[11] + P(3,1)*SF[10] + P(10,1)*SF[14] + P(11,1)*SF[15] + P(12,1)*SPP[10]) + SF[11]*(P(0,2) + P(1,2)*SF[9] + P(2,2)*SF[11] + P(3,2)*SF[10] + P(10,2)*SF[14] + P(11,2)*SF[15] + P(12,2)*SPP[10]) + SF[10]*(P(0,3) + P(1,3)*SF[9] + P(2,3)*SF[11] + P(3,3)*SF[10] + P(10,3)*SF[14] + P(11,3)*SF[15] + P(12,3)*SPP[10]) + SF[14]*(P(0,10) + P(1,10)*SF[9] + P(2,10)*SF[11] + P(3,10)*SF[10] + P(10,10)*SF[14] + P(11,10)*SF[15] + P(12,10)*SPP[10]) + SF[15]*(P(0,11) + P(1,11)*SF[9] + P(2,11)*SF[11] + P(3,11)*SF[10] + P(10,11)*SF[14] + P(11,11)*SF[15] + P(12,11)*SPP[10]) + SPP[10]*(P(0,12) + P(1,12)*SF[9] + P(2,12)*SF[11] + P(3,12)*SF[10] + P(10,12)*SF[14] + P(11,12)*SF[15] + P(12,12)*SPP[10]) + (dayVar*sq(q2))/4 + (dazVar*sq(q3))/4;
@ -755,7 +755,6 @@ void Ekf::fixCovarianceErrors()
// accelerometer bias states
if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || _accel_bias_inhibit) {
P.uncorrelateCovarianceSetVariance<3>(13, 0.0f);
} else {
@ -844,7 +843,9 @@ void Ekf::fixCovarianceErrors()
}
// force symmetry
P.makeRowColSymmetric<6>(16);
P.makeRowColSymmetric<3>(16);
P.makeRowColSymmetric<3>(19);
}
// wind velocity states
@ -865,10 +866,12 @@ void Ekf::fixCovarianceErrors()
void Ekf::resetMagRelatedCovariances()
{
// set the quaternion covariance terms to zero
P.uncorrelateCovarianceSetVariance<4>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
// reset the field state variance to the observation variance
P.uncorrelateCovarianceSetVariance<6>(16, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise));
P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise));
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
saveMagCovData();
@ -882,7 +885,8 @@ void Ekf::clearMagCov()
void Ekf::zeroMagCov()
{
P.uncorrelateCovarianceSetVariance<6>(16, 0.0f);
P.uncorrelateCovarianceSetVariance<3>(16, 0.0f);
P.uncorrelateCovarianceSetVariance<3>(19, 0.0f);
}
void Ekf::resetWindCovariance()

2
EKF/drag_fusion.cpp
View File

@ -243,7 +243,7 @@ void Ekf::fuseDrag()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[9];
for (unsigned row = 0; row < _k_num_states; row++) {

34
EKF/ekf.h
View File

@ -351,7 +351,7 @@ private:
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec)
Vector2f _last_known_posNE{}; ///< last known local NE position vector (m)
Vector2f _last_known_posNE; ///< last known local NE position vector (m)
float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)
uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec)
@ -380,32 +380,32 @@ private:
bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event.
bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements
matrix::SquareMatrix<float, _k_num_states> P {}; ///< state covariance matrix
matrix::SquareMatrix<float, _k_num_states> P; ///< state covariance matrix
Vector3f _delta_vel_bias_var_accum; ///< kahan summation algorithm accumulator for delta velocity bias variance
Vector3f _delta_angle_bias_var_accum; ///< kahan summation algorithm accumulator for delta angle bias variance
Vector3f _gps_vel_innov {}; ///< GPS velocity innovations (m/sec)
Vector3f _gps_vel_innov_var {}; ///< GPS velocity innovation variances ((m/sec)**2)
Vector3f _gps_vel_innov; ///< GPS velocity innovations (m/sec)
Vector3f _gps_vel_innov_var; ///< GPS velocity innovation variances ((m/sec)**2)
Vector3f _gps_pos_innov {}; ///< GPS position innovations (m)
Vector3f _gps_pos_innov_var {}; ///< GPS position innovation variances (m**2)
Vector3f _gps_pos_innov; ///< GPS position innovations (m)
Vector3f _gps_pos_innov_var; ///< GPS position innovation variances (m**2)
Vector3f _ev_vel_innov {}; ///< external vision velocity innovations (m/sec)
Vector3f _ev_vel_innov_var {}; ///< external vision velocity innovation variances ((m/sec)**2)
Vector3f _ev_vel_innov; ///< external vision velocity innovations (m/sec)
Vector3f _ev_vel_innov_var; ///< external vision velocity innovation variances ((m/sec)**2)
Vector3f _ev_pos_innov {}; ///< external vision position innovations (m)
Vector3f _ev_pos_innov_var {}; ///< external vision position innovation variances (m**2)
Vector3f _ev_pos_innov; ///< external vision position innovations (m)
Vector3f _ev_pos_innov_var; ///< external vision position innovation variances (m**2)
Vector3f _baro_hgt_innov {}; ///< baro hgt innovations (m)
Vector3f _baro_hgt_innov_var {}; ///< baro hgt innovation variances (m**2)
Vector3f _baro_hgt_innov; ///< baro hgt innovations (m)
Vector3f _baro_hgt_innov_var; ///< baro hgt innovation variances (m**2)
Vector3f _rng_hgt_innov {}; ///< range hgt innovations (m)
Vector3f _rng_hgt_innov_var {}; ///< range hgt innovation variances (m**2)
Vector3f _rng_hgt_innov; ///< range hgt innovations (m)
Vector3f _rng_hgt_innov_var; ///< range hgt innovation variances (m**2)
Vector3f _aux_vel_innov {}; ///< horizontal auxiliary velocity innovations: (m/sec)
Vector3f _aux_vel_innov_var {}; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
Vector3f _aux_vel_innov; ///< horizontal auxiliary velocity innovations: (m/sec)
Vector3f _aux_vel_innov_var; ///< horizontal auxiliary velocity innovation variances: ((m/sec)**2)
float _heading_innov{0.0f}; ///< heading measurement innovation (rad)
float _heading_innov_var{0.0f}; ///< heading measurement innovation variance (rad**2)
@ -484,7 +484,7 @@ private:
// variables used to inhibit accel bias learning
bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited
Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
Vector3f _accel_vec_filt; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2

4
EKF/ekf_helper.cpp
View File

@ -1531,7 +1531,9 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
float t44 = t17-t36;
// zero all the quaternion covariances
P.uncorrelateCovarianceSetVariance<4>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(0, 0.0f);
P.uncorrelateCovarianceSetVariance<2>(2, 0.0f);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
P(0,0) = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;

14
EKF/estimator_interface.h
View File

@ -481,13 +481,13 @@ protected:
// innovation consistency check monitoring ratios
float _yaw_test_ratio{}; // yaw innovation consistency check ratio
float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios
Vector2f _gps_vel_test_ratio {}; // GPS velocity innovation consistency check ratios
Vector2f _gps_pos_test_ratio {}; // GPS position innovation consistency check ratios
Vector2f _ev_vel_test_ratio {}; // EV velocity innovation consistency check ratios
Vector2f _ev_pos_test_ratio {}; // EV position innovation consistency check ratios
Vector2f _aux_vel_test_ratio{}; // Auxiliray horizontal velocity innovation consistency check ratio
Vector2f _baro_hgt_test_ratio {}; // baro height innovation consistency check ratios
Vector2f _rng_hgt_test_ratio {}; // range finder height innovation consistency check ratios
Vector2f _gps_vel_test_ratio; // GPS velocity innovation consistency check ratios
Vector2f _gps_pos_test_ratio; // GPS position innovation consistency check ratios
Vector2f _ev_vel_test_ratio; // EV velocity innovation consistency check ratios
Vector2f _ev_pos_test_ratio ; // EV position innovation consistency check ratios
Vector2f _aux_vel_test_ratio; // Auxiliray horizontal velocity innovation consistency check ratio
Vector2f _baro_hgt_test_ratio; // baro height innovation consistency check ratios
Vector2f _rng_hgt_test_ratio; // range finder height innovation consistency check ratios
float _optflow_test_ratio{}; // Optical flow innovation consistency check ratio
float _tas_test_ratio{}; // tas innovation consistency check ratio
float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio

2
EKF/gps_yaw_fusion.cpp
View File

@ -233,7 +233,7 @@ void Ekf::fuseGpsAntYaw()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) {

4
EKF/mag_fusion.cpp
View File

@ -754,7 +754,7 @@ void Ekf::fuseHeading()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[4];
for (unsigned row = 0; row < _k_num_states; row++) {
@ -897,7 +897,7 @@ void Ekf::fuseDeclination(float decl_sigma)
// first calculate expression for KHP
// then calculate P - KHP
// take advantage of the empty columns in KH to reduce the number of operations
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[2];
for (unsigned row = 0; row < _k_num_states; row++) {

2
EKF/optflow_fusion.cpp
View File

@ -442,7 +442,7 @@ void Ekf::fuseOptFlow()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[7];
for (unsigned row = 0; row < _k_num_states; row++) {

2
EKF/sideslip_fusion.cpp
View File

@ -218,7 +218,7 @@ void Ekf::fuseSideslip()
// apply covariance correction via P_new = (I -K*H)*P
// first calculate expression for KHP
// then calculate P - KHP
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
float KH[9];
for (unsigned row = 0; row < _k_num_states; row++) {

2
EKF/vel_pos_fusion.cpp
View File

@ -149,7 +149,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
Kfusion[row] = P(row,state_index) / innov_var;
}
matrix::SquareMatrix<float, _k_num_states> KHP {};
matrix::SquareMatrix<float, _k_num_states> KHP;
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {