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

Merge pull request #283 from PX4/ekf_matrix_cleanup

Browse Source 
EKF matrix typedef cleanup
This commit is contained in:
Paul Riseborough
2017-07-03 21:02:24 +10:00
committed by GitHub
parent ae118940b1 c44488fdb8
commit eb1e73ec81
11 changed files with 55 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+6 -7
View File
@@ -159,21 +159,20 @@ void Ekf::controlExternalVisionFusion()
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
// get the roll, pitch, yaw estimates from the quaternion states
matrix::Quaternion<float> q_init(_state.quat_nominal(0), _state.quat_nominal(1), _state.quat_nominal(2),
_state.quat_nominal(3));
matrix::Euler<float> euler_init(q_init);
Quatf q_init(_state.quat_nominal);
Eulerf euler_init(q_init);
// get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
matrix::Quaternion<float> q_obs(ev_newest.quat(0), ev_newest.quat(1), ev_newest.quat(2), ev_newest.quat(3));
matrix::Euler<float> euler_obs(q_obs);
Quatf q_obs(ev_newest.quat);
Eulerf euler_obs(q_obs);
euler_init(2) = euler_obs(2);
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quaternion quat_before_reset = _state.quat_nominal;
Quatf quat_before_reset = _state.quat_nominal;
// calculate initial quaternion states for the ekf
_state.quat_nominal = Quaternion(euler_init);
_state.quat_nominal = Quatf(euler_init);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();