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

EKF: move calculation of optical flow observation variance into a function 

Allows it to be used when calculating initial state variance
This commit is contained in:
Paul Riseborough 2016-05-08 15:40:06 +10:00
parent 25682dce91
commit 724280fd1f
3 changed files with 31 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -106,8 +106,8 @@ void Ekf::controlFusionModes()
zeroRows(P,4,5);
zeroCols(P,4,5);
// reset the horizontal velocity variance using the optical flow noise
P[5][5] = P[4][4] = sq(range * _params.flow_noise_qual_min);
// reset the horizontal velocity variance using the optical flow noise variance
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
if (!_in_air) {
// we are likely starting OF for the first time so reset the position and states

3
EKF/ekf.h
View File

@ -347,4 +347,7 @@ private:
// zero the specified range of columns in the state covariance matrix
void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar();
};

43
EKF/optflow_fusion.cpp
View File

@ -59,23 +59,8 @@ void Ekf::fuseOptFlow()
float ve = _state.vel(1);
float vd = _state.vel(2);
// calculate the observation noise variance - scaling noise linearly across flow quality range
float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
float weighting = (255.0f - (float)_params.flow_qual_min);
if (weighting >= 1.0f) {
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
1.0f);
} else {
weighting = 0.0f;
}
// take the weighted average of the observation noie for the best and wort flow quality
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
// calculate the optical flow observation variance
float R_LOS = calcOptFlowMeasVar();
float H_LOS[2][24] = {}; // Optical flow observation Jacobians
float Kfusion[24][2] = {}; // Optical flow Kalman gains
@ -548,3 +533,27 @@ void Ekf::calcOptFlowBias()
_delta_time_of = 0.0f;
}
}
// calculate the measurement variance for the optical flow sensor (rad/sec)^2
float Ekf::calcOptFlowMeasVar()
{
// calculate the observation noise variance - scaling noise linearly across flow quality range
float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
float weighting = (255.0f - (float)_params.flow_qual_min);
if (weighting >= 1.0f) {
weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
1.0f);
} else {
weighting = 0.0f;
}
// take the weighted average of the observation noie for the best and wort flow quality
float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
return R_LOS;
}