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

Make flow_innov/-var a matrix Vector2f

Browse Source
This commit is contained in:
kamilritz
2020-06-21 14:24:55 +02:00
committed by Mathieu Bresciani
parent c2801eb488
commit 5356077a32
4 changed files with 25 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.h
+2 -2
View File
@@ -428,8 +428,8 @@ private:
float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2)
// optical flow processing
float _flow_innov[2] {}; ///< flow measurement innovation (rad/sec)
float _flow_innov_var[2] {}; ///< flow innovation variance ((rad/sec)**2)
Vector2f _flow_innov; ///< flow measurement innovation (rad/sec)
Vector2f _flow_innov_var; ///< flow innovation variance ((rad/sec)**2)
Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec)
EKF/ekf_helper.cpp
+6 -6
View File
@@ -727,12 +727,12 @@ void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) const
void Ekf::getFlowInnov(float flow_innov[2]) const
{
memcpy(flow_innov, _flow_innov, sizeof(_flow_innov));
_flow_innov.copyTo(flow_innov);
}
void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
{
memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
_flow_innov_var.copyTo(flow_innov_var);
}
void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const
@@ -983,7 +983,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
if (_control_status.flags.opt_flow) {
float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
}
if (_control_status.flags.gps) {
@@ -1667,9 +1667,9 @@ void Ekf::stopAuxVelFusion()
void Ekf::stopFlowFusion()
{
_control_status.flags.opt_flow = false;
memset(_flow_innov,0.0f,sizeof(_flow_innov));
memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
_flow_innov.setZero();
_flow_innov_var.setZero();
_optflow_test_ratio = 0.0f;
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
EKF/optflow_fusion.cpp
+7 -7
View File
@@ -104,8 +104,8 @@ void Ekf::fuseOptFlow()
const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
if (opt_flow_rate.norm() < _flow_max_rate) {
_flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
_flow_innov(0) = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
} else {
return;
@@ -225,7 +225,7 @@ void Ekf::fuseOptFlow()
// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
_flow_innov_var[0] = t77;
_flow_innov_var(0) = t77;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
@@ -367,7 +367,7 @@ void Ekf::fuseOptFlow()
// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
if (t77 >= R_LOS) {
t78 = 1.0f / t77;
_flow_innov_var[1] = t77;
_flow_innov_var(1) = t77;
} else {
// we need to reinitialise the covariance matrix and abort this fusion step
@@ -407,8 +407,8 @@ void Ekf::fuseOptFlow()
// run the innovation consistency check and record result
bool flow_fail = false;
float test_ratio[2];
test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]);
test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]);
test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
_optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]);
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
@@ -493,7 +493,7 @@ void Ekf::fuseOptFlow()
fixCovarianceErrors(true);
// apply the state corrections
fuse(gain, _flow_innov[obs_index]);
fuse(gain, _flow_innov(obs_index));
_time_last_of_fuse = _time_last_imu;
}
EKF/terrain_estimator.cpp
+10 -10
View File
@@ -231,27 +231,27 @@ void Ekf::fuseFlowForTerrain()
_terrain_var = fmaxf(_terrain_var, 0.0f);
// Cacluate innovation variance
_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;
_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
// calculate the kalman gain for the flow x measurement
const float Kx = _terrain_var * Hx / _flow_innov_var[0];
const float Kx = _terrain_var * Hx / _flow_innov_var(0);
// calculate prediced optical flow about x axis
const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (x axis)
_flow_innov[0] = pred_flow_x - opt_flow_rate(0);
_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
// calculate correction term for terrain variance
const float KxHxP = Kx * Hx * _terrain_var;
// innovation consistency check
const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);
float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
// do not perform measurement update if badly conditioned
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Kx * _flow_innov[0];
_terrain_vpos += Kx * _flow_innov(0);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
_time_last_of_fuse = _time_last_imu;
@@ -261,25 +261,25 @@ void Ekf::fuseFlowForTerrain()
const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
// Calculuate innovation variance
_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;
_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
// calculate the kalman gain for the flow y measurement
const float Ky = _terrain_var * Hy / _flow_innov_var[1];
const float Ky = _terrain_var * Hy / _flow_innov_var(1);
// calculate prediced optical flow about y axis
const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
// calculate flow innovation (y axis)
_flow_innov[1] = pred_flow_y - opt_flow_rate(1);
_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
// calculate correction term for terrain variance
const float KyHyP = Ky * Hy * _terrain_var;
// innovation consistency check
flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);
flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
if (flow_test_ratio <= 1.0f) {
_terrain_vpos += Ky * _flow_innov[1];
_terrain_vpos += Ky * _flow_innov(1);
// guard against negative variance
_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
_time_last_of_fuse = _time_last_imu;