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

Rotate external vision info in reset function if necessary

Browse Source
This commit is contained in:
kamilritz 2019-09-17 14:30:34 +02:00 committed by Paul Riseborough
parent 4511b9ff5e
commit 10cbd79db7
1 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
EKF/ekf_helper.cpp
View File

@ -94,11 +94,18 @@ bool Ekf::resetVelocity()
// reset the horizontal velocity variance using the optical flow noise variance
P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
} else if (_control_status.flags.ev_pos) {
} else if (_control_status.flags.ev_vel) {
Vector3f _ev_vel = _ev_sample_delayed.velNED;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _ev_rot_mat *_ev_sample_delayed.velNED;
}
_state.vel(0) = _ev_vel(0);
_state.vel(1) = _ev_vel(1);
_state.vel(2) = _ev_vel(2);
// TODO: to what should we set the covariances?
} else if (_control_status.flags.ev_pos) {
_state.vel.setZero();
zeroOffDiag(P, 4, 6);
} else {
// Used when falling back to non-aiding mode of operation
_state.vel(0) = 0.0f;
@ -149,8 +156,12 @@ bool Ekf::resetPosition()
} else if (_control_status.flags.ev_pos) {
// this reset is only called if we have new ev data at the fusion time horizon
_state.pos(0) = _ev_sample_delayed.posNED(0);
_state.pos(1) = _ev_sample_delayed.posNED(1);
Vector3f _ev_pos = _ev_sample_delayed.posNED;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_pos = _ev_rot_mat *_ev_sample_delayed.posNED;
}
_state.pos(0) = _ev_pos(0);
_state.pos(1) = _ev_pos(1);
// use EV accuracy to reset variances
setDiag(P, 7, 8, sq(_ev_sample_delayed.posErr));