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

Move variable declaration to better place &

Browse Source 
remove airspeed_innov_var_temp variable. setting to global variable even when fusion is aborted is okay
This commit is contained in:
kamilritz 2020-07-03 17:32:48 +02:00 committed by Mathieu Bresciani
parent 05855b7fc1
commit b5765eb3b4
4 changed files with 24 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
EKF/airspeed_fusion.cpp
View File

@ -46,11 +46,6 @@
void Ekf::fuseAirspeed()
{
float SH_TAS[3] = {}; // Variable used to optimise calculations of measurement jacobian
float H_TAS[24] = {}; // Observation Jacobian
float SK_TAS[2] = {}; // Variable used to optimise calculations of the Kalman gain vector
Vector24f Kfusion; // Kalman gain vector
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
@ -69,25 +64,23 @@ void Ekf::fuseAirspeed()
// determine if we need the sideslip fusion to correct states other than wind
const bool update_wind_only = !_is_wind_dead_reckoning;
// Calculate the observation jacobian
// intermediate variable from algebraic optimisation
float SH_TAS[3];
SH_TAS[0] = 1.0f/v_tas_pred;
SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2.0f*vwe))*0.5f;
SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2.0f*vwn))*0.5f;
for (uint8_t i = 0; i < _k_num_states; i++) { H_TAS[i] = 0.0f; }
// Observation Jacobian
float H_TAS[24] = {};
H_TAS[4] = SH_TAS[2];
H_TAS[5] = SH_TAS[1];
H_TAS[6] = vd*SH_TAS[0];
H_TAS[22] = -SH_TAS[2];
H_TAS[23] = -SH_TAS[1];
// We don't want to update the innovation variance if the calculation is ill conditioned
const float _airspeed_innov_var_temp = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
_airspeed_innov_var = (R_TAS + SH_TAS[2]*(P(4,4)*SH_TAS[2] + P(5,4)*SH_TAS[1] - P(22,4)*SH_TAS[2] - P(23,4)*SH_TAS[1] + P(6,4)*vd*SH_TAS[0]) + SH_TAS[1]*(P(4,5)*SH_TAS[2] + P(5,5)*SH_TAS[1] - P(22,5)*SH_TAS[2] - P(23,5)*SH_TAS[1] + P(6,5)*vd*SH_TAS[0]) - SH_TAS[2]*(P(4,22)*SH_TAS[2] + P(5,22)*SH_TAS[1] - P(22,22)*SH_TAS[2] - P(23,22)*SH_TAS[1] + P(6,22)*vd*SH_TAS[0]) - SH_TAS[1]*(P(4,23)*SH_TAS[2] + P(5,23)*SH_TAS[1] - P(22,23)*SH_TAS[2] - P(23,23)*SH_TAS[1] + P(6,23)*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P(4,6)*SH_TAS[2] + P(5,6)*SH_TAS[1] - P(22,6)*SH_TAS[2] - P(23,6)*SH_TAS[1] + P(6,6)*vd*SH_TAS[0]));
if (_airspeed_innov_var_temp >= R_TAS) { // Check for badly conditioned calculation
SK_TAS[0] = 1.0f / _airspeed_innov_var_temp;
if (_airspeed_innov_var >= R_TAS) { // Check for badly conditioned calculation
_fault_status.flags.bad_airspeed = false;
} else { // Reset the estimator covariance matrix
@ -110,8 +103,12 @@ void Ekf::fuseAirspeed()
return;
}
// Variable used to optimise calculations of the Kalman gain
float SK_TAS[2];
SK_TAS[0] = 1.0f / _airspeed_innov_var;
SK_TAS[1] = SH_TAS[1];
Vector24f Kfusion; // Kalman gain
if (!update_wind_only) {
// we have no other source of aiding, so use airspeed measurements to correct states
Kfusion(0) = SK_TAS[0]*(P(0,4)*SH_TAS[2] - P(0,22)*SH_TAS[2] + P(0,5)*SK_TAS[1] - P(0,23)*SK_TAS[1] + P(0,6)*vd*SH_TAS[0]);
@ -144,9 +141,6 @@ void Ekf::fuseAirspeed()
_airspeed_innov = v_tas_pred -
_airspeed_sample_delayed.true_airspeed;
// Calculate the innovation variance
_airspeed_innov_var = 1.0f / SK_TAS[0];
// Compute the ratio of innovation to gate size
_tas_test_ratio = sq(_airspeed_innov) / (sq(fmaxf(_params.tas_innov_gate, 1.0f)) * _airspeed_innov_var);

4
EKF/control.cpp
View File

@ -1279,8 +1279,6 @@ void Ekf::controlFakePosFusion()
// Fuse synthetic position observations every 200msec
if (isTimedOut(_time_last_fake_pos, (uint64_t)2e5)) {
Vector3f fake_pos_obs_var;
// Reset position and velocity states if we re-commence this aiding method
if (isTimedOut(_time_last_fake_pos, (uint64_t)4e5)) {
resetHorizontalPosition();
@ -1294,6 +1292,8 @@ void Ekf::controlFakePosFusion()
}
_time_last_fake_pos = _time_last_imu;
Vector3f fake_pos_obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));

12
EKF/ekf.cpp
View File

@ -251,7 +251,6 @@ void Ekf::predictState()
{
// apply imu bias corrections
Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
// subtract component of angular rate due to earth rotation
corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt;
@ -259,17 +258,12 @@ void Ekf::predictState()
const Quatf dq(AxisAnglef{corrected_delta_ang});
// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
_state.quat_nominal = _state.quat_nominal * dq;
// quaternions must be normalised whenever they are modified
_state.quat_nominal.normalize();
// save the previous value of velocity so we can use trapzoidal integration
const Vector3f vel_last = _state.vel;
_state.quat_nominal = (_state.quat_nominal * dq).normalized();
_R_to_earth = Dcmf(_state.quat_nominal);
// Calculate an earth frame delta velocity
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
// calculate a filtered horizontal acceleration with a 1 sec time constant
@ -277,6 +271,8 @@ void Ekf::predictState()
float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
_accel_lpf_NE(0) = _accel_lpf_NE(0) * alpha + corrected_delta_vel_ef(0);
_accel_lpf_NE(1) = _accel_lpf_NE(1) * alpha + corrected_delta_vel_ef(1);
// save the previous value of velocity so we can use trapzoidal integration
const Vector3f vel_last = _state.vel;
// calculate the increment in velocity using the current orientation
_state.vel += corrected_delta_vel_ef;

17
EKF/sideslip_fusion.cpp
View File

File diff suppressed because one or more lines are too long