fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 19:23:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+7
View File
@@ -116,6 +116,10 @@ bool Ekf::init(uint64_t timestamp)
_output_new.pos.setZero(); _output_new.pos.setZero();
_output_new.quat_nominal = matrix::Quaternion<float>(); _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_ang.setZero();
_imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f; _imu_down_sampled.delta_ang_dt = 0.0f;
@@ -131,6 +135,9 @@ bool Ekf::init(uint64_t timestamp)
_NED_origin_initialised = false; _NED_origin_initialised = false;
_gps_speed_valid = false; _gps_speed_valid = false;
_mag_healthy = false; _mag_healthy = false;
_filter_initialised = false;
return ret; return ret;
} }