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

EKF: correct outputs for IMU offset

Browse Source
This commit is contained in:
Paul Riseborough
2016-04-12 11:14:31 +10:00
parent b46053415f
commit e10093854a
4 changed files with 66 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+2 -2
View File
@@ -583,9 +583,9 @@ void Ekf::calculateOutputStates()
_output_new.quat_nominal = dq * _output_new.quat_nominal;
_output_new.quat_nominal.normalize();
matrix::Dcm<float> R_to_earth(_output_new.quat_nominal);
_R_to_earth_now = quat_to_invrotmat(_output_new.quat_nominal);
Vector3f delta_vel_NED = R_to_earth * delta_vel + _delta_vel_corr;
Vector3f delta_vel_NED = _R_to_earth_now * delta_vel + _delta_vel_corr;
delta_vel_NED(2) += 9.81f * imu_new.delta_vel_dt;
Vector3f vel_last = _output_new.vel;