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

EKF: update initial angle alignment check

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-03 20:59:49 +10:00
parent bc34b14779
commit 727a43764f
4 changed files with 3 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
EKF/control.cpp
View File

@ -51,13 +51,9 @@ void Ekf::controlFusionModes()
// Get the magnetic declination
calcMagDeclination();
// Check for tilt convergence during initial alignment
// filter the tilt error vector using a 1 sec time constant LPF
float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt;
_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) {
// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
if (total_angle_variance < 0.001f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}

3
EKF/ekf.cpp
View File

@ -470,9 +470,6 @@ bool Ekf::initialiseFilter(void)
// update transformation matrix from body to world frame
_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
Vector3f mag_init = _mag_filt_state;

3
EKF/ekf.h
View File

@ -180,9 +180,6 @@ private:
float _heading_innov; // heading measurement innovation
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
float _flow_innov[2]; // flow measurement innovation
float _flow_innov_var[2]; // flow innovation variance

2
EKF/vel_pos_fusion.cpp
View File

@ -176,13 +176,11 @@ void Ekf::fuseVelPosHeight()
// record the successful velocity fusion time
if (vel_check_pass && _fuse_hor_vel) {
_time_last_vel_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful position fusion time
if (pos_check_pass && _fuse_pos) {
_time_last_pos_fuse = _time_last_imu;
_tilt_err_vec.setZero();
}
// record the successful height fusion time