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

Refactoring velPos fusion

Browse Source
This commit is contained in:
kamilritz
2019-12-05 20:09:34 +11:00
committed by Paul Riseborough
parent 6b5f011bc2
commit 86b9079bdc
6 changed files with 423 additions and 418 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+186 -100
View File
@@ -157,16 +157,15 @@ void Ekf::controlFusionModes()
controlDragFusion();
controlHeightFusion();
// For efficiency, fusion of direct state observations for position and velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, baro, range finder, etc)
controlVelPosFusion();
// Additional data from an external vision pose estimator can be fused.
// Additional data odoemtery data from an external estimator can be fused.
controlExternalVisionFusion();
// Additional NE velocity data from an auxiliary sensor can be fused
// Additional horizontal velocity data from an auxiliary sensor can be fused
controlAuxVelFusion();
// Fake position measurement for constraining drift when no other velocity or position measurements
controlFakePosFusion();
// check if we are no longer fusing measurements that directly constrain velocity drift
update_deadreckoning_status();
}
@@ -266,23 +265,13 @@ void Ekf::controlExternalVisionFusion()
}
}
// determine if we should start using the height observations
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
// don't start using EV data unless data is arriving frequently
if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
setControlEVHeight();
resetHeight();
}
}
// determine if we should use the vertical position observation
if (_control_status.flags.ev_hgt) {
_fuse_height = true;
}
bool ev_fuse_mask[4]{};
float ev_obs_var[6]{};
float ev_innov_gate[4]{};
// determine if we should use the horizontal position observations
if (_control_status.flags.ev_pos) {
_fuse_pos = true;
ev_fuse_mask[HPOS] = true;
// correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@@ -312,11 +301,11 @@ void Ekf::controlExternalVisionFusion()
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
// use the change in position since the last measurement
_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
_ev_vel_pos_innov[3] = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_ev_vel_pos_innov[4] = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.5f);
ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.5f);
}
// record observation and estimate for use next time
@@ -330,10 +319,10 @@ void Ekf::controlExternalVisionFusion()
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
}
_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
_ev_vel_pos_innov[3] = _state.pos(0) - ev_pos_meas(0);
_ev_vel_pos_innov[4] = _state.pos(1) - ev_pos_meas(1);
// observation 1-STD error
_posObsNoiseNE = fmaxf(_ev_sample_delayed.posErr, 0.01f);
ev_obs_var[3] = ev_obs_var[4] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_pos_fuse) > _params.reset_timeout_max) {
@@ -347,16 +336,12 @@ void Ekf::controlExternalVisionFusion()
}
// innovation gate size
_posInnovGateNE = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}else{
_vel_pos_innov[3] = 0.0f;
_vel_pos_innov[4] = 0.0f;
ev_innov_gate[HPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}
// determine if we should use the velocity observations
if (_control_status.flags.ev_vel) {
_fuse_hor_vel = true;
_fuse_vert_vel = true;
ev_fuse_mask[HVEL] = ev_fuse_mask[VVEL] = true;
Vector3f vel_aligned{_ev_sample_delayed.vel};
@@ -372,9 +357,9 @@ void Ekf::controlExternalVisionFusion()
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
vel_aligned -= vel_offset_earth;
_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
_ev_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
_ev_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
_ev_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {
@@ -385,19 +370,15 @@ void Ekf::controlExternalVisionFusion()
}
// observation 1-STD error
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = fmaxf(_ev_sample_delayed.velErr, 0.01f);
ev_obs_var[0] = ev_obs_var[1] = ev_obs_var[2] = fmaxf(_ev_sample_delayed.velErr, 0.01f);
// innovation gate size
_vvelInnovGate = _hvelInnovGate = fmaxf(_params.ev_vel_innov_gate, 1.0f);
ev_innov_gate[HVEL] = ev_innov_gate[VVEL] = fmaxf(_params.ev_vel_innov_gate, 1.0f);
}
// Fuse available NED position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
_fuse_vert_vel = _fuse_hor_vel = false;
_fuse_pos = _fuse_height = false;
_fuse_hpos_as_odom = false;
if (ev_fuse_mask[HVEL] || ev_fuse_mask[VVEL] || ev_fuse_mask[HPOS]) {
fuseVelPosHeightSeq(_ev_vel_pos_innov, ev_innov_gate, ev_obs_var , ev_fuse_mask, _ev_vel_pos_innov_var, _ev_vel_pos_test_ratio);
}
// determine if we should use the yaw observation
@@ -702,9 +683,14 @@ void Ekf::controlGpsFusion()
// Only use GPS data for position and velocity aiding if enabled
if (_control_status.flags.gps) {
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
bool gps_fuse_mask[4]{};
float gps_obs_var[6]{};
float gps_innov_gate[4]{};
// Enable full velocity and horizontal position fusion
gps_fuse_mask[HVEL] = gps_fuse_mask[VVEL] = true;
gps_fuse_mask[HPOS] = true;
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f / _imu_sample_delayed.delta_ang_dt);
@@ -725,27 +711,32 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.opt_flow || _control_status.flags.ev_pos || _control_status.flags.ev_vel) {
// if we are using other sources of aiding, then relax the upper observation
// noise limit which prevents bad GPS perturbing the position estimate
_posObsNoiseNE = fmaxf(_gps_sample_delayed.hacc, lower_limit);
gps_obs_var[3] = gps_obs_var[4] = fmaxf(_gps_sample_delayed.hacc, lower_limit);
} else {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
_posObsNoiseNE = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
gps_obs_var[3] = gps_obs_var[4] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit);
}
_velObsVarNED(2) = _velObsVarNED(1) = _velObsVarNED(0) = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
gps_obs_var[0] = gps_obs_var[1] = gps_obs_var[2] = sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise));
// calculate innovations
_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
_gps_vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0);
_gps_vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1);
_gps_vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2);
_gps_vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0);
_gps_vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1);
// set innovation gate size
_posInnovGateNE = fmaxf(_params.gps_pos_innov_gate, 1.0f);
_hvelInnovGate = _vvelInnovGate = fmaxf(_params.gps_vel_innov_gate, 1.0f);
gps_innov_gate[HPOS] = fmaxf(_params.gps_pos_innov_gate, 1.0f);
gps_innov_gate[HVEL] = gps_innov_gate[VVEL] = fmaxf(_params.gps_vel_innov_gate, 1.0f);
// fuse GPS measurement
fuseVelPosHeightSeq(_gps_vel_pos_innov,gps_innov_gate,
gps_obs_var,gps_fuse_mask,
_gps_vel_pos_innov_var,_gps_vel_pos_test_ratio);
}
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
@@ -995,7 +986,12 @@ void Ekf::controlHeightSensorTimeouts()
void Ekf::controlHeightFusion()
{
// set control flags for the desired primary height source
bool height_fuse_mask[4]{};
float height_innov_gate[4]{};
float height_test_ratio[4]{};
float height_obs_var[6]{};
float height_innov_var[6]{};
float height_innov[6]{};
checkRangeAidSuitability();
_range_aid_mode_selected = (_params.range_aid == 1) && isRangeAidSuitable();
@@ -1004,7 +1000,7 @@ void Ekf::controlHeightFusion()
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
@@ -1019,7 +1015,7 @@ void Ekf::controlHeightFusion()
} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
@@ -1037,7 +1033,7 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_intermittent) {
// switch to gps if there was a reset to gps
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using gps height, calculate height sensor offset such that current
// measurement matches our current height estimate
@@ -1050,7 +1046,7 @@ void Ekf::controlHeightFusion()
// set the height data source to range if requested
if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = _range_data_ready;
height_fuse_mask[VPOS] = _range_data_ready;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
@@ -1072,7 +1068,7 @@ void Ekf::controlHeightFusion()
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
@@ -1086,7 +1082,7 @@ void Ekf::controlHeightFusion()
if (_range_aid_mode_selected && _range_data_ready && _rng_hgt_valid) {
setControlRangeHeight();
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using range finder, calculate height sensor offset such that current
// measurement matches our current height estimate
@@ -1101,7 +1097,7 @@ void Ekf::controlHeightFusion()
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_intermittent && _gps_checks_passed) {
setControlGPSHeight();
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using gps height, calculate height sensor offset such that current
// measurement matches our current height estimate
@@ -1111,7 +1107,7 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
@@ -1123,9 +1119,17 @@ void Ekf::controlHeightFusion()
// Determine if we rely on EV height but switched to baro
if (_params.vdist_sensor_type == VDIST_SENSOR_EV) {
// don't start using EV data unless data is arriving frequently
if (!_control_status.flags.ev_hgt && ((_time_last_imu - _time_last_ext_vision) < (2 * EV_MAX_INTERVAL))) {
height_fuse_mask[VPOS] = true;
setControlEVHeight();
resetHeight();
}
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
height_fuse_mask[VPOS] = true;
// we have just switched to using baro height, we don't need to set a height sensor offset
// since we track a separate _baro_hgt_offset
@@ -1133,6 +1137,11 @@ void Ekf::controlHeightFusion()
_hgt_sensor_offset = 0.0f;
}
}
// TODO: Add EV normal case here
// determine if we should use the vertical position observation
if (_control_status.flags.ev_hgt) {
height_fuse_mask[VPOS] = true;
}
}
// calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference
@@ -1157,9 +1166,73 @@ void Ekf::controlHeightFusion()
}
_fuse_height = true;
height_fuse_mask[VPOS] = true;
}
if (height_fuse_mask[VPOS]) {
if (_control_status.flags.baro_hgt) {
// vertical position innovation - baro measurement has opposite sign to earth z axis
height_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
// observation variance - user parameter defined
height_obs_var[5] = sq(fmaxf(_params.baro_noise, 0.01f));
// innovation gate size
height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f);
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
float deadzone_start = 0.0f;
float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (_control_status.flags.gnd_effect) {
if (height_innov[5] < -deadzone_start) {
if (height_innov[5] <= -deadzone_end) {
height_innov[5] += deadzone_end;
} else {
height_innov[5] = -deadzone_start;
}
}
}
} else if (_control_status.flags.gps_hgt) {
// vertical position innovation - gps measurement has opposite sign to earth z axis
height_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
// observation variance - receiver defined and parameter limited
// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
height_obs_var[5] = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
// innovation gate size
height_innov_gate[VPOS] = fmaxf(_params.baro_innov_gate, 1.0f);
} else if (_control_status.flags.rng_hgt && (_R_rng_to_earth_2_2 > _params.range_cos_max_tilt)) {
// use range finder with tilt correction
height_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2,
_params.rng_gnd_clearance)) - _hgt_sensor_offset;
// observation variance - user parameter defined
height_obs_var[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f);
// innovation gate size
height_innov_gate[VPOS] = fmaxf(_params.range_innov_gate, 1.0f);
} else if (_control_status.flags.ev_hgt) {
// calculate the innovation assuming the external vision observation is in local NED frame
height_innov[5] = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
height_obs_var[5] = sq(fmaxf(_ev_sample_delayed.hgtErr, 0.01f));
// innovation gate size
height_innov_gate[VPOS] = fmaxf(_params.ev_pos_innov_gate, 1.0f);
}
// fuse height inforamtion
fuseVelPosHeightSeq(height_innov,height_innov_gate,
height_obs_var,height_fuse_mask,
height_innov_var,height_test_ratio);
// This is a temporary hack until we do proper height sensor fusion
_gps_vel_pos_innov[5] = height_innov[5];
_gps_vel_pos_innov_var[5] = height_innov_var[5];
_gps_vel_pos_test_ratio[VPOS] = height_test_ratio[VPOS];
}
}
@@ -1296,13 +1369,13 @@ void Ekf::controlDragFusion()
}
}
void Ekf::controlVelPosFusion()
void Ekf::controlFakePosFusion()
{
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
// if we aren't doing any aiding, fake position measurements at the last known position to constrain drift
// Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz
if (!(_params.fusion_mode & MASK_USE_GPS)) {
_control_status.flags.gps = false;
}
bool fake_gps_fuse_mask[4]{};
float fake_gps_obs_var[6]{};
float fake_gps_innov_gate[4]{};
if (!_control_status.flags.gps &&
!_control_status.flags.opt_flow &&
@@ -1314,51 +1387,45 @@ void Ekf::controlVelPosFusion()
_using_synthetic_position = true;
// Fuse synthetic position observations every 200msec
if (((_time_last_imu - _time_last_fake_gps) > (uint64_t)2e5) || _fuse_height) {
if (((_time_last_imu - _time_last_fake_pos) > (uint64_t)2e5) || _fuse_height) {
// Reset position and velocity states if we re-commence this aiding method
if ((_time_last_imu - _time_last_fake_gps) > (uint64_t)4e5) {
if ((_time_last_imu - _time_last_fake_pos) > (uint64_t)4e5) {
resetPosition();
resetVelocity();
_fuse_hpos_as_odom = false;
if (_time_last_fake_gps != 0) {
if (_time_last_fake_pos != 0) {
ECL_WARN_TIMESTAMPED("EKF stopping navigation");
}
}
// Fuse horizontal position
fake_gps_fuse_mask[HPOS] = true;
_fuse_pos = true;
_fuse_hor_vel = false;
_fuse_vert_vel = false;
_time_last_fake_gps = _time_last_imu;
_time_last_fake_pos = _time_last_imu;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
_posObsNoiseNE = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
fake_gps_obs_var[3] = fake_gps_obs_var[4] = fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise);
} else {
_posObsNoiseNE = 0.5f;
fake_gps_obs_var[3] = fake_gps_obs_var[4] = 0.5f;
}
_vel_pos_innov[0] = 0.0f;
_vel_pos_innov[1] = 0.0f;
_vel_pos_innov[2] = 0.0f;
_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
_gps_vel_pos_innov[3] = _state.pos(0) - _last_known_posNE(0);
_gps_vel_pos_innov[4] = _state.pos(1) - _last_known_posNE(1);
// glitch protection is not required so set gate to a large value
_posInnovGateNE = 100.0f;
fake_gps_innov_gate[HPOS] = 100.0f;
fuseVelPosHeightSeq(_gps_vel_pos_innov,fake_gps_innov_gate,
fake_gps_obs_var,fake_gps_fuse_mask,
_gps_vel_pos_innov_var,_gps_vel_pos_test_ratio);
}
} else {
_using_synthetic_position = false;
}
// Fuse available NED velocity and position data into the main filter
if (_fuse_height || _fuse_pos || _fuse_hor_vel || _fuse_vert_vel) {
fuseVelPosHeight();
}
}
void Ekf::controlAuxVelFusion()
@@ -1367,13 +1434,32 @@ void Ekf::controlAuxVelFusion()
bool primary_aiding = _control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.opt_flow;
if (data_ready && primary_aiding) {
_fuse_hor_vel = _fuse_vert_vel = _fuse_pos = _fuse_height = false;
_fuse_hor_vel_aux = true;
_aux_vel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
_aux_vel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
_velObsVarNED(0) = _auxvel_sample_delayed.velVarNE(0);
_velObsVarNED(1) = _auxvel_sample_delayed.velVarNE(1);
_hvelInnovGate = _params.auxvel_gate;
fuseVelPosHeight();
bool auxvel_fuse_mask[4]{};
float auxvel_innov[6]{};
float auxvel_innov_gate[4]{};
float auxvel_obs_var[4]{};
float auxvel_innov_var[6]{};
float auxvel_test_ratio[4]{};
auxvel_fuse_mask[HVEL] = true;
auxvel_innov[0] = _state.vel(0) - _auxvel_sample_delayed.velNE(0);
auxvel_innov[1] = _state.vel(1) - _auxvel_sample_delayed.velNE(1);
auxvel_innov_gate[HVEL] = _params.auxvel_gate;
auxvel_obs_var[0] = _auxvel_sample_delayed.velVarNE(0);
auxvel_obs_var[1] = _auxvel_sample_delayed.velVarNE(1);
fuseVelPosHeightSeq(auxvel_innov,auxvel_innov_gate,
auxvel_obs_var,auxvel_fuse_mask,
auxvel_innov_var,auxvel_test_ratio);
_aux_vel_innov[0] = auxvel_innov[0];
_aux_vel_innov[1] = auxvel_innov[1];
_aux_vel_innov_var[0] = auxvel_innov_var[0];
_aux_vel_innov_var[1] = auxvel_innov_var[1];
_aux_vel_test_ratio = auxvel_test_ratio[HVEL];
}
}