fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: remove unnecessary const references

Browse Source
This commit is contained in:
Daniel Agar 2022-04-29 14:42:33 -04:00
parent 4a6e958100
commit 6fc857772d
7 changed files with 69 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/modules/ekf2/EKF/EKFGSF_yaw.cpp
View File

@ -261,13 +261,13 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
// predict covariance - equations generated using EKF/python/gsf_ekf_yaw_estimator/main.py
// Local short variable name copies required for readability
const float &P00 = _ekf_gsf[model_index].P(0,0);
const float &P01 = _ekf_gsf[model_index].P(0,1);
const float &P02 = _ekf_gsf[model_index].P(0,2);
const float &P11 = _ekf_gsf[model_index].P(1,1);
const float &P12 = _ekf_gsf[model_index].P(1,2);
const float &P22 = _ekf_gsf[model_index].P(2,2);
const float &psi = _ekf_gsf[model_index].X(2);
const float P00 = _ekf_gsf[model_index].P(0,0);
const float P01 = _ekf_gsf[model_index].P(0,1);
const float P02 = _ekf_gsf[model_index].P(0,2);
const float P11 = _ekf_gsf[model_index].P(1,1);
const float P12 = _ekf_gsf[model_index].P(1,2);
const float P22 = _ekf_gsf[model_index].P(2,2);
const float psi = _ekf_gsf[model_index].X(2);
// Use fixed values for delta velocity and delta angle process noise variances
const float dvxVar = sq(_accel_noise * _delta_vel_dt); // variance of forward delta velocity - (m/s)^2
@ -317,12 +317,12 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
_ekf_gsf[model_index].innov(1) = _ekf_gsf[model_index].X(1) - _vel_NE(1);
// Use temporary variables for covariance elements to reduce verbosity of auto-code expressions
const float &P00 = _ekf_gsf[model_index].P(0,0);
const float &P01 = _ekf_gsf[model_index].P(0,1);
const float &P02 = _ekf_gsf[model_index].P(0,2);
const float &P11 = _ekf_gsf[model_index].P(1,1);
const float &P12 = _ekf_gsf[model_index].P(1,2);
const float &P22 = _ekf_gsf[model_index].P(2,2);
const float P00 = _ekf_gsf[model_index].P(0,0);
const float P01 = _ekf_gsf[model_index].P(0,1);
const float P02 = _ekf_gsf[model_index].P(0,2);
const float P11 = _ekf_gsf[model_index].P(1,1);
const float P12 = _ekf_gsf[model_index].P(1,2);
const float P22 = _ekf_gsf[model_index].P(2,2);
// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
const float t0 = ecl::powf(P01, 2);

10
src/modules/ekf2/EKF/airspeed_fusion.cpp
View File

@ -47,11 +47,11 @@
void Ekf::fuseAirspeed()
{
const float &vn = _state.vel(0); // Velocity in north direction
const float &ve = _state.vel(1); // Velocity in east direction
const float &vd = _state.vel(2); // Velocity in downwards direction
const float &vwn = _state.wind_vel(0); // Wind speed in north direction
const float &vwe = _state.wind_vel(1); // Wind speed in east direction
const float vn = _state.vel(0); // Velocity in north direction
const float ve = _state.vel(1); // Velocity in east direction
const float vd = _state.vel(2); // Velocity in downwards direction
const float vwn = _state.wind_vel(0); // Wind speed in north direction
const float vwe = _state.wind_vel(1); // Wind speed in east direction
// Variance for true airspeed measurement - (m/sec)^2
const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *

32
src/modules/ekf2/EKF/covariance.cpp
View File

@ -100,26 +100,26 @@ void Ekf::initialiseCovariance()
void Ekf::predictCovariance()
{
// assign intermediate state variables
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
const float &dax = _imu_sample_delayed.delta_ang(0);
const float &day = _imu_sample_delayed.delta_ang(1);
const float &daz = _imu_sample_delayed.delta_ang(2);
const float dax = _imu_sample_delayed.delta_ang(0);
const float day = _imu_sample_delayed.delta_ang(1);
const float daz = _imu_sample_delayed.delta_ang(2);
const float &dvx = _imu_sample_delayed.delta_vel(0);
const float &dvy = _imu_sample_delayed.delta_vel(1);
const float &dvz = _imu_sample_delayed.delta_vel(2);
const float dvx = _imu_sample_delayed.delta_vel(0);
const float dvy = _imu_sample_delayed.delta_vel(1);
const float dvz = _imu_sample_delayed.delta_vel(2);
const float &dax_b = _state.delta_ang_bias(0);
const float &day_b = _state.delta_ang_bias(1);
const float &daz_b = _state.delta_ang_bias(2);
const float dax_b = _state.delta_ang_bias(0);
const float day_b = _state.delta_ang_bias(1);
const float daz_b = _state.delta_ang_bias(2);
const float &dvx_b = _state.delta_vel_bias(0);
const float &dvy_b = _state.delta_vel_bias(1);
const float &dvz_b = _state.delta_vel_bias(2);
const float dvx_b = _state.delta_vel_bias(0);
const float dvy_b = _state.delta_vel_bias(1);
const float dvz_b = _state.delta_vel_bias(2);
// Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values
const float dt = _dt_ekf_avg;

18
src/modules/ekf2/EKF/drag_fusion.cpp
View File

@ -66,19 +66,19 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
}
// get latest estimated orientation
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
// get latest velocity in earth frame
const float &vn = _state.vel(0);
const float &ve = _state.vel(1);
const float &vd = _state.vel(2);
const float vn = _state.vel(0);
const float ve = _state.vel(1);
const float vd = _state.vel(2);
// get latest wind velocity in earth frame
const float &vwn = _state.wind_vel(0);
const float &vwe = _state.wind_vel(1);
const float vwn = _state.wind_vel(0);
const float vwe = _state.wind_vel(1);
// predicted specific forces
// calculate relative wind velocity in earth frame and rotate into body frame

8
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -48,10 +48,10 @@
void Ekf::fuseGpsYaw()
{
// assign intermediate state variables
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(_gps_sample_delayed.yaw + _gps_yaw_offset);

26
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -48,14 +48,14 @@
void Ekf::fuseMag(const Vector3f &mag)
{
// assign intermediate variables
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
const float &magN = _state.mag_I(0);
const float &magE = _state.mag_I(1);
const float &magD = _state.mag_I(2);
const float magN = _state.mag_I(0);
const float magE = _state.mag_I(1);
const float magD = _state.mag_I(2);
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
@ -424,10 +424,10 @@ void Ekf::fuseMag(const Vector3f &mag)
bool Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
{
// assign intermediate state variables
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
const float measurement = wrap_pi(yaw);
@ -779,8 +779,8 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
void Ekf::fuseDeclination(float decl_sigma)
{
// assign intermediate state variables
const float &magN = _state.mag_I(0);
const float &magE = _state.mag_I(1);
const float magN = _state.mag_I(0);
const float magE = _state.mag_I(1);
// minimum North field strength before calculation becomes badly conditioned (T)
constexpr float N_field_min = 0.001f;

18
src/modules/ekf2/EKF/sideslip_fusion.cpp
View File

@ -48,19 +48,19 @@
void Ekf::fuseSideslip()
{
// get latest estimated orientation
const float &q0 = _state.quat_nominal(0);
const float &q1 = _state.quat_nominal(1);
const float &q2 = _state.quat_nominal(2);
const float &q3 = _state.quat_nominal(3);
const float q0 = _state.quat_nominal(0);
const float q1 = _state.quat_nominal(1);
const float q2 = _state.quat_nominal(2);
const float q3 = _state.quat_nominal(3);
// get latest velocity in earth frame
const float &vn = _state.vel(0);
const float &ve = _state.vel(1);
const float &vd = _state.vel(2);
const float vn = _state.vel(0);
const float ve = _state.vel(1);
const float vd = _state.vel(2);
// get latest wind velocity in earth frame
const float &vwn = _state.wind_vel(0);
const float &vwe = _state.wind_vel(1);
const float vwn = _state.wind_vel(0);
const float vwe = _state.wind_vel(1);
// calculate relative wind velocity in earth frame and rotate into body frame
const Vector3f rel_wind_earth(vn - vwn, ve - vwe, vd);