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

Missing posNED

Browse Source
This commit is contained in:
kamilritz 2019-09-23 11:40:51 +02:00 committed by Paul Riseborough
parent fd6b364c11
commit 83eb326076
1 changed files with 12 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
EKF/control.cpp
View File

@ -279,9 +279,9 @@ void Ekf::controlExternalVisionFusion()
// correct position and height for offset relative to IMU
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_ev_sample_delayed.posNED(0) -= pos_offset_earth(0);
_ev_sample_delayed.posNED(1) -= pos_offset_earth(1);
_ev_sample_delayed.posNED(2) -= pos_offset_earth(2);
_ev_sample_delayed.pos(0) -= pos_offset_earth(0);
_ev_sample_delayed.pos(1) -= pos_offset_earth(1);
_ev_sample_delayed.pos(2) -= pos_offset_earth(2);
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & MASK_USE_GPS) {
@ -298,7 +298,7 @@ void Ekf::controlExternalVisionFusion()
} else {
// calculate the change in position since the last measurement
Vector3f ev_delta_pos = _ev_sample_delayed.posNED - _pos_meas_prev;
Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
// rotate measurement into body frame is required when fusing with GPS
ev_delta_pos = _ev_rot_mat * ev_delta_pos;
@ -312,13 +312,13 @@ void Ekf::controlExternalVisionFusion()
}
// record observation and estimate for use next time
_pos_meas_prev = _ev_sample_delayed.posNED;
_pos_meas_prev = _ev_sample_delayed.pos;
_hpos_pred_prev(0) = _state.pos(0);
_hpos_pred_prev(1) = _state.pos(1);
} else {
// use the absolute position
Vector3f ev_pos_meas = _ev_sample_delayed.posNED;
Vector3f ev_pos_meas = _ev_sample_delayed.pos;
if (_params.fusion_mode & MASK_ROTATE_EV) {
ev_pos_meas = _ev_rot_mat * ev_pos_meas;
}
@ -350,11 +350,11 @@ void Ekf::controlExternalVisionFusion()
_fuse_hor_vel = true;
_fuse_vert_vel = true;
Vector3f velNED_aligned{_ev_sample_delayed.velNED};
Vector3f vel_aligned{_ev_sample_delayed.vel};
// rotate measurement into correct earth frame if required
if (_params.fusion_mode & MASK_ROTATE_EV) {
velNED_aligned = _ev_rot_mat * _ev_sample_delayed.velNED;
vel_aligned = _ev_rot_mat * _ev_sample_delayed.vel;
}
// correct velocity for offset relative to IMU
@ -362,11 +362,11 @@ void Ekf::controlExternalVisionFusion()
Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = cross_product(ang_rate, pos_offset_body);
Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
velNED_aligned -= vel_offset_earth;
vel_aligned -= vel_offset_earth;
_vel_pos_innov[0] = _state.vel(0) - velNED_aligned(0);
_vel_pos_innov[1] = _state.vel(1) - velNED_aligned(1);
_vel_pos_innov[2] = _state.vel(2) - velNED_aligned(2);
_vel_pos_innov[0] = _state.vel(0) - vel_aligned(0);
_vel_pos_innov[1] = _state.vel(1) - vel_aligned(1);
_vel_pos_innov[2] = _state.vel(2) - vel_aligned(2);
// check if we have been deadreckoning too long
if ((_time_last_imu - _time_last_vel_fuse) > _params.reset_timeout_max) {