fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:33:34 +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/covariance.cpp
+1 -5
View File
@@ -953,11 +953,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
// calculate accel bias term aligned with the gravity vector
const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
float down_dvel_bias = 0.0f;
for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
down_dvel_bias += _state.delta_vel_bias(axis_index) * _R_to_earth(2, axis_index);
}
const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
bool bad_acc_bias = (fabsf(down_dvel_bias) > dVel_bias_lim