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

EKF: update initial angle alignment check

Browse Source
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent bc34b14779
commit 727a43764f
4 changed files with 3 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -7
View File
@@ -51,13 +51,9 @@ void Ekf::controlFusionModes()
// Get the magnetic declination // Get the magnetic declination
calcMagDeclination(); calcMagDeclination();
// Check for tilt convergence during initial alignment // Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
// filter the tilt error vector using a 1 sec time constant LPF float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; if (total_angle_variance < 0.001f && !_control_status.flags.tilt_align) {
_tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt;
// Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states
if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true; _control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
} }
EKF/ekf.cpp
-3
View File
@@ -470,9 +470,6 @@ bool Ekf::initialiseFilter(void)
// update transformation matrix from body to world frame // update transformation matrix from body to world frame
_R_to_earth = quat_to_invrotmat(_state.quat_nominal); _R_to_earth = quat_to_invrotmat(_state.quat_nominal);
// initialise the filtered alignment error estimate to a larger starting value
_tilt_err_length_filt = 1.0f;
// calculate the averaged magnetometer reading // calculate the averaged magnetometer reading
Vector3f mag_init = _mag_filt_state; Vector3f mag_init = _mag_filt_state;
EKF/ekf.h
-3
View File
@@ -180,9 +180,6 @@ private:
float _heading_innov; // heading measurement innovation float _heading_innov; // heading measurement innovation
float _heading_innov_var; // heading measurement innovation variance float _heading_innov_var; // heading measurement innovation variance
Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion
float _tilt_err_length_filt; // filtered length of _tilt_err_vec
// optical flow processing // optical flow processing
float _flow_innov[2]; // flow measurement innovation float _flow_innov[2]; // flow measurement innovation
float _flow_innov_var[2]; // flow innovation variance float _flow_innov_var[2]; // flow innovation variance
EKF/vel_pos_fusion.cpp
-2
View File
@@ -176,13 +176,11 @@ void Ekf::fuseVelPosHeight()
// record the successful velocity fusion time // record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) { if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu; _time_last_vel_fuse = _time_last_imu;
_tilt_err_vec.setZero();
} }
// record the successful position fusion time // record the successful position fusion time
if (pos_check_pass && _fuse_pos) { if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu; _time_last_pos_fuse = _time_last_imu;
_tilt_err_vec.setZero();
} }
// record the successful height fusion time // record the successful height fusion time