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

posNED to pos and velNED to vel and add ev vel covariances to reset

This commit is contained in:
kamilritz 2019-09-23 11:37:19 +02:00 committed by Paul Riseborough
parent 4f6ca3a74c
commit fd6b364c11
4 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
EKF/common.h
View File

@ -80,8 +80,8 @@ struct flow_message {
};
struct ext_vision_message {
Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
@ -153,8 +153,8 @@ struct flowSample {
};
struct extVisionSample {
Vector3f posNED; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f velNED; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)

14
EKF/ekf_helper.cpp
View File

@ -95,14 +95,14 @@ 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_vel) {
Vector3f _ev_vel = _ev_sample_delayed.velNED;
Vector3f _ev_vel = _ev_sample_delayed.vel;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_vel = _ev_rot_mat *_ev_sample_delayed.velNED;
_ev_vel = _ev_rot_mat *_ev_sample_delayed.vel;
}
_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?
setDiag(P, 4, 6, sq(_ev_sample_delayed.velErr));
} else if (_control_status.flags.ev_pos) {
_state.vel.setZero();
zeroOffDiag(P, 4, 6);
@ -156,9 +156,9 @@ 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
Vector3f _ev_pos = _ev_sample_delayed.posNED;
Vector3f _ev_pos = _ev_sample_delayed.pos;
if(_params.fusion_mode & MASK_ROTATE_EV){
_ev_pos = _ev_rot_mat *_ev_sample_delayed.posNED;
_ev_pos = _ev_rot_mat *_ev_sample_delayed.pos;
}
_state.pos(0) = _ev_pos(0);
_state.pos(1) = _ev_pos(1);
@ -308,10 +308,10 @@ void Ekf::resetHeight()
vert_pos_reset = true;
if (std::abs(dt_newest) < std::abs(dt_delayed)) {
_state.pos(2) = ev_newest.posNED(2);
_state.pos(2) = ev_newest.pos(2);
} else {
_state.pos(2) = _ev_sample_delayed.posNED(2);
_state.pos(2) = _ev_sample_delayed.pos(2);
}
}

4
EKF/estimator_interface.cpp
View File

@ -445,8 +445,8 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
ev_sample_new.velErr = evdata->velErr;
ev_sample_new.hgtErr = evdata->hgtErr;
ev_sample_new.quat = evdata->quat;
ev_sample_new.posNED = evdata->posNED;
ev_sample_new.velNED = evdata->velNED;
ev_sample_new.pos = evdata->pos;
ev_sample_new.vel = evdata->vel;
// record time for comparison next measurement
_time_last_ext_vision = time_usec;

2
EKF/vel_pos_fusion.cpp
View File

@ -147,7 +147,7 @@ void Ekf::fuseVelPosHeight()
} else if (_control_status.flags.ev_hgt) {
fuse_map[5] = true;
// calculate the innovation assuming the external vision observation is in local NED frame
innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
innovation[5] = _state.pos(2) - _ev_sample_delayed.pos(2);
// observation variance - defined externally
R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
R[5] = R[5] * R[5];