rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00
Code Issues Releases Activity

Merged in f5soh/librepilot/LP-589_INS13_Yaw_spin (pull request #504)

Browse Source 
LP-588 INS13 - Scale down Mag variance setting by Be² + Small fixes

Approved-by: Lalanne Laurent <f5soh@free.fr>
Approved-by: Philippe Renon <philippe_renon@yahoo.fr>
This commit is contained in:
Lalanne Laurent 2018-04-29 15:22:28 +00:00 committed by Philippe Renon
commit cb6d86b048
3 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/inc/insgps.h
View File

@ -58,7 +58,7 @@ void INSGPSInit();
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[3],
float BaroAlt, uint16_t SensorsUsed);
const float BaroAlt, uint16_t SensorsUsed);
void INSResetP(const float PDiag[13]);
void INSGetVariance(float PDiag[13]);
void INSSetState(const float pos[3], const float vel[3], const float q[4], const float gyro_bias[3], const float accel_bias[3]);

22
flight/libraries/insgps13state.c
View File

@ -92,7 +92,6 @@ static struct EKFData {
float H[NUMV][NUMX];
// local magnetic unit vector in NED frame
float Be[3];
float BeScaleFactor;
// covariance matrix and state vector
float P[NUMX][NUMX];
float X[NUMX];
@ -281,26 +280,25 @@ void INSSetGyroBiasVar(const float gyro_bias_var[3])
ekf.Q[8] = gyro_bias_var[2];
}
// must be called AFTER SetMagNorth
void INSSetMagVar(const float mag_var[3])
void INSSetMagVar(const float scaled_mag_var[3])
{
ekf.R[6] = mag_var[0] * ekf.BeScaleFactor;
ekf.R[7] = mag_var[1] * ekf.BeScaleFactor;
ekf.R[8] = mag_var[2] * ekf.BeScaleFactor;
ekf.R[6] = scaled_mag_var[0];
ekf.R[7] = scaled_mag_var[1];
ekf.R[8] = scaled_mag_var[2];
}
void INSSetBaroVar(float baro_var)
void INSSetBaroVar(const float baro_var)
{
ekf.R[9] = baro_var;
}
void INSSetMagNorth(const float B[3])
{
ekf.BeScaleFactor = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
float invmag = invsqrtf(B[0] * B[0] + B[1] * B[1] + B[2] * B[2]);
ekf.Be[0] = B[0] * ekf.BeScaleFactor;
ekf.Be[1] = B[1] * ekf.BeScaleFactor;
ekf.Be[2] = B[2] * ekf.BeScaleFactor;
ekf.Be[0] = B[0] * invmag;
ekf.Be[1] = B[1] * invmag;
ekf.Be[2] = B[2] * invmag;
}
void INSStatePrediction(const float gyro_data[3], const float accel_data[3], float dT)
@ -402,8 +400,6 @@ void INSCorrection(const float mag_data[3], const float Pos[3], const float Vel[
Z[3] = Vel[0];
Z[4] = Vel[1];
Z[5] = Vel[2];
// magnetometer data in any units (use unit vector) and in body frame
if (SensorsUsed & MAG_SENSORS) {
// magnetometer data in any units (use unit vector) and in body frame

7
flight/modules/StateEstimation/filterekf.c
View File

@ -232,9 +232,10 @@ static filterResult filter(stateFilter *self, stateEstimation *state)
// Reset the INS algorithm
INSGPSInit();
// variance is measured in mGaus, but internally the EKF works with a normalized vector. Scale down by Be^2
INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX,
this->ekfConfiguration.R.MagY,
this->ekfConfiguration.R.MagZ }
float Be2 = this->homeLocation.Be[0] * this->homeLocation.Be[0] + this->homeLocation.Be[1] * this->homeLocation.Be[1] + this->homeLocation.Be[2] * this->homeLocation.Be[2];
INSSetMagVar((float[3]) { this->ekfConfiguration.R.MagX / Be2,
this->ekfConfiguration.R.MagY / Be2,
this->ekfConfiguration.R.MagZ / Be2 }
);
INSSetAccelVar((float[3]) { this->ekfConfiguration.Q.AccelX,
this->ekfConfiguration.Q.AccelY,