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

EKF: allow initialising without mag depending on configuration

Browse Source 
- reduce initial number of required mag and baro samples before init
This commit is contained in:
Daniel Agar
2020-12-17 19:55:51 +11:00
committed by Paul Riseborough
parent 1237087d70
commit 398fe159ce
6 changed files with 762 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -2
View File
@@ -347,9 +347,10 @@ void Ekf::resetHeight()
// align output filter states to match EKF states at the fusion time horizon
void Ekf::alignOutputFilter()
{
const outputSample output_delayed = _output_buffer.get_oldest();
const outputSample &output_delayed = _output_buffer.get_oldest();
// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
Quatf q_delta = _state.quat_nominal * output_delayed.quat_nominal.inversed();
Quatf q_delta{_state.quat_nominal * output_delayed.quat_nominal.inversed()};
q_delta.normalize();
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon