fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 13:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use new added matrix functions

Browse Source
This commit is contained in:
Kamil Ritz
2020-08-24 19:24:27 +10:00
committed by Paul Riseborough
parent 1f637af7de
commit 114516b784
3 changed files with 3 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+1 -3
View File
@@ -324,9 +324,7 @@ void Ekf::calculateOutputStates()
const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = _R_to_earth_now(2, 0) * delta_angle(0) +
_R_to_earth_now(2, 1) * delta_angle(1) +
_R_to_earth_now(2, 2) * delta_angle(2);
const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic