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

EKF: Prevent badly conditioned covariance calculation when starting or resetting to optical flow

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-08 10:30:40 +10:00
parent c66ed7b662
commit 25682dce91
3 changed files with 50 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
EKF/control.cpp
View File

@ -73,18 +73,16 @@ void Ekf::controlFusionModes()
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
// if we are not using GPS and are in air, then we need to reset the velocity to be consistent with the optical flow reading
// if we are not using GPS then the velocity and position states and covariances need to be set
if (!_control_status.flags.gps) {
// calculate the rotation matrix from body to earth frame
matrix::Dcm<float> body_to_earth(_state.quat_nominal);
// constrain height above ground to be above minimum possible
float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
// calculate absolute distance from focal point to centre of frame assuming a flat earth
float range = heightAboveGndEst / body_to_earth(2, 2);
float range = heightAboveGndEst / _R_to_earth(2, 2);
if (_in_air && (range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) {
// we should ahve reliable OF measurements so
// calculate X and Y body relative velocities from OF measurements
Vector3f vel_optflow_body;
vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt;
@ -93,14 +91,30 @@ void Ekf::controlFusionModes()
// rotate from body to earth frame
Vector3f vel_optflow_earth;
vel_optflow_earth = body_to_earth * vel_optflow_body;
vel_optflow_earth = _R_to_earth * vel_optflow_body;
// take x and Y components
_state.vel(0) = vel_optflow_earth(0);
_state.vel(1) = vel_optflow_earth(1);
} else {
_state.vel.setZero();
_state.vel(0) = 0.0f;
_state.vel(1) = 0.0f;
}
// reset the velocity covariance terms
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);
if (!_in_air) {
// we are likely starting OF for the first time so reset the position and states
_state.pos(0) = 0.0f;
_state.pos(1) = 0.0f;
// align the output observer to the EKF states
alignOutputFilter();
}
}
}

3
EKF/ekf.h
View File

@ -307,6 +307,9 @@ private:
// reset height state of the ekf
void resetHeight();
// modify output filter to match the the EKF state at the fusion time horizon
void alignOutputFilter();
// limit the diagonal of the covariance matrix
void fixCovarianceErrors();

25
EKF/ekf_helper.cpp
View File

@ -225,6 +225,31 @@ void Ekf::resetHeight()
}
// align output filter states to match EKF states at the fusion time horizon
void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quaternion quat_inv = _state.quat_nominal.inversed();
Quaternion q_delta = _output_sample_delayed.quat_nominal * quat_inv;
q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= q_delta;
output_states.quat_nominal.normalize();
output_states.vel += vel_delta;
output_states.pos += pos_delta;
_output_buffer.push_to_index(i,output_states);
}
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{