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

Additional initializations required to reset complimentary filter values if the state estimate ever diverges and requires re-initiailization.

Browse Source
This commit is contained in:
mcsauder 2016-02-25 22:51:14 -07:00
parent 6613335937
commit 5fec0df70d
1 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
EKF/ekf.cpp
View File

@ -115,7 +115,11 @@ bool Ekf::init(uint64_t timestamp)
_output_new.vel.setZero();
_output_new.pos.setZero();
_output_new.quat_nominal = matrix::Quaternion<float>();
_delta_angle_corr.setZero();
_delta_vel_corr.setZero();
_vel_corr.setZero();
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;
@ -131,6 +135,9 @@ bool Ekf::init(uint64_t timestamp)
_NED_origin_initialised = false;
_gps_speed_valid = false;
_mag_healthy = false;
_filter_initialised = false;
return ret;
}