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

Merge pull request #59 from PX4/pr-ImprovedStartup

Browse Source 
Improvements to EKF startup - partially replaces PR#57
This commit is contained in:
Paul Riseborough 2016-02-24 18:27:12 +11:00
commit e938475acc
3 changed files with 23 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
EKF/covariance.cpp
View File

@ -52,34 +52,37 @@ void Ekf::initialiseCovariance()
}
}
// calculate average prediction time step in sec
float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS;
// XXX use initial guess for the diagonal elements for the covariance matrix
// angle error
P[0][0] = 0.001f;
P[1][1] = 0.001f;
P[2][2] = 0.001f;
P[0][0] = 0.1f;
P[1][1] = 0.1f;
P[2][2] = 0.1f;
// velocity
P[3][3] = 0.1f;
P[4][4] = 0.1f;
P[5][5] = 0.1f;
P[3][3] = sq(fmaxf(_params.gps_vel_noise, 0.01f));
P[4][4] = P[3][3];
P[5][5] = sq(1.5f) * P[3][3];
// position
P[6][6] = 0.1f;
P[7][7] = 0.1f;
P[8][8] = 0.1f;
P[6][6] = sq(fmaxf(_params.gps_pos_noise, 0.01f));
P[7][7] = P[6][6];
P[8][8] = sq(fmaxf(_params.baro_noise, 0.01f));
// gyro bias
P[9][9] = 0.00001f;
P[10][10] = 0.00001f;
P[11][11] = 0.00001f;
P[9][9] = sq(0.035f * dt);
P[10][10] = P[9][9];
P[11][11] = P[9][9];
// gyro scale
P[12][12] = 0.0001f;
P[13][13] = 0.0001f;
P[14][14] = 0.0001f;
P[12][12] = sq(0.001f);
P[13][13] = P[12][12];
P[14][14] = P[12][12];
// accel z bias
P[15][15] = 0.0001f;
P[15][15] = sq(0.2f * dt);
// variances for optional states
// these state variances are set to zero until the states are required, then they must be initialised

2
EKF/ekf.cpp
View File

@ -265,7 +265,7 @@ bool Ekf::initialiseFilter(void)
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = -asinf(_delVel_sum(1) / cosf(pitch));
roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
} else {
return false;

4
EKF/mag_fusion.cpp
View File

@ -610,9 +610,11 @@ void Ekf::fuseHeading()
// TODO - enable use of an off-board heading measurement
// rotate the magnetometer measurement into earth frame using an assumed zero yaw angle
// rotate the magnetometer measurement into earth frame
matrix::Euler<float> euler(_state.quat_nominal);
float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation
// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
euler(2) = 0.0f;
matrix::Dcm<float> R_to_earth(euler);
matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;