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

Rename IMU biases

Browse Source
This commit is contained in:
kamilritz
2019-12-05 20:09:34 +11:00
committed by Paul Riseborough
parent dae8c2f8dc
commit 07e804676c
7 changed files with 46 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/drag_fusion.cpp
+2 -2
View File
@@ -90,7 +90,7 @@ void Ekf::fuseDrag()
// calculate observation jacobiam and Kalman gain vectors
if (axis_index == 0) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_x * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis
@@ -163,7 +163,7 @@ void Ekf::fuseDrag()
} else if (axis_index == 1) {
// Estimate the airspeed from the measured drag force and ballistic coefficient
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.accel_bias(axis_index) / _dt_ekf_avg;
float mea_acc = _drag_sample_delayed.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg;
float airSpd = sqrtf((2.0f * fabsf(mea_acc)) / (BC_inv_y * rho));
// Estimate the derivative of specific force wrt airspeed along the X axis