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

ekf2: init tilt using quaternion from gravity vector

Browse Source
This commit is contained in:
bresch 2023-03-30 15:46:50 +02:00 committed by Matthias Grob
parent 93ba7de0c3
commit ed7c4bc5d7
1 changed files with 2 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/ekf2/EKF/ekf.cpp
View File

@ -243,12 +243,8 @@ bool Ekf::initialiseTilt()
return false;
}
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
const Vector3f gravity_in_body = _accel_lpf.getState().normalized();
const float pitch = asinf(gravity_in_body(0));
const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
_state.quat_nominal = Quatf{Eulerf{roll, pitch, 0.0f}};
// get initial tilt estimate from delta velocity vector, assuming vehicle is static
_state.quat_nominal = Quatf(_accel_lpf.getState(), Vector3f(0.f, 0.f, -1.f));
_R_to_earth = Dcmf(_state.quat_nominal);
return true;