mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
OP-1317 Changed filtering method of IMU airspeed calculation into a Butterworth second order filter
This commit is contained in:
Andres
parent
c5372dd0b4
commit
41dd85b741
@ -47,10 +47,17 @@
|
||||
// Private types
|
||||
// structure with smoothed fuselage orientation, ground speed and their changes in time
|
||||
struct IMUGlobals {
|
||||
float xB[3];
|
||||
float dxB[3];
|
||||
float Vel[3];
|
||||
float dVel[3];
|
||||
// storage variables for Butterworth filter
|
||||
float pn1, pn2;
|
||||
float yn1, yn2;
|
||||
float v1n1, v1n2;
|
||||
float v2n1, v2n2;
|
||||
float v3n1, v3n2;
|
||||
float Vn1,Vn2;
|
||||
|
||||
// storage variables for derivative calculation
|
||||
float pOld, yOld;
|
||||
float v1Old, v2Old, v3Old;
|
||||
};
|
||||
|
||||
// Private variables
|
||||
@ -63,6 +70,63 @@ static inline float Sq(float x)
|
||||
return x * x;
|
||||
}
|
||||
|
||||
// ****** find pitch, yaw from quaternion ********
|
||||
static void Quaternion2PY(const float q0, const float q1, const float q2, const float q3, float *pPtr, float *yPtr, bool principalArg)
|
||||
{
|
||||
float R13, R11, R12;
|
||||
const float q0s = q0 * q0;
|
||||
const float q1s = q1 * q1;
|
||||
const float q2s = q2 * q2;
|
||||
const float q3s = q3 * q3;
|
||||
|
||||
R13 = 2.0f * (q1 * q3 - q0 * q2);
|
||||
R11 = q0s + q1s - q2s - q3s;
|
||||
R12 = 2.0f * (q1 * q2 + q0 * q3);
|
||||
|
||||
*pPtr = asinf(-R13); // pitch always between -pi/2 to pi/2
|
||||
|
||||
const float y_=atan2f(R12, R11);
|
||||
// use old yaw contained in y to add multiples of 2pi to have a continuous yaw if user does not want the principal argument
|
||||
// else simply copy atan2 result into result
|
||||
if(principalArg){
|
||||
*yPtr = y_;
|
||||
}else{
|
||||
const int mod=(int)((y_-*yPtr)/(2.0f*M_PI_F*0.9f));
|
||||
*yPtr = y_- 2.0f*M_PI_F*mod;
|
||||
}
|
||||
}
|
||||
|
||||
static void PY2xB(float p, float y, float x[3])
|
||||
{
|
||||
const float cosp=cosf(p);
|
||||
x[0]=cosp*cosf(y);
|
||||
x[1]=cosp*sinf(y);
|
||||
x[2]=-sinf(p);
|
||||
}
|
||||
|
||||
|
||||
//second order Butterworth filter with cut-off frequency ratio ff
|
||||
// filter is writen in direct from 2, such that only two values wn1=w[n-1] and wn2=w[n-2] need to be stored
|
||||
// function takes care of updating the values wn1 and wn2
|
||||
float FilterButterWorthDF2(const float ff, float xn, float *wn1Ptr, float *wn2Ptr)
|
||||
{
|
||||
// TODO: we need to think about storing the filter instead of calculating it again and again
|
||||
const float ita =1.0f/ tanf(M_PI_F*ff);
|
||||
const float q=sqrtf(2.0f);
|
||||
const float b0 = 1.0f / (1.0f + q*ita + Sq(ita));
|
||||
const float b1= 2.0f*b0;
|
||||
const float b2= b0;
|
||||
const float a1 = 2.0f * (Sq(ita) - 1.0f) * b0;
|
||||
const float a2 = -(1.0f - q*ita + Sq(ita)) * b0;
|
||||
|
||||
const float wn=xn + a1*(*wn1Ptr) + a2*(*wn2Ptr);
|
||||
const float val=b0*wn + b1*(*wn1Ptr) + b2*(*wn2Ptr);
|
||||
*wn2Ptr=*wn1Ptr;
|
||||
*wn1Ptr=wn;
|
||||
return val;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Initialize function loads first data sets, and allocates memory for structure.
|
||||
*/
|
||||
@ -79,20 +143,18 @@ void imu_airspeedInitialize()
|
||||
AttitudeStateData attData;
|
||||
AttitudeStateGet(&attData);
|
||||
|
||||
// for Holt-Winters double exponential smoothing (s smooth variable, b smooth trend)
|
||||
// s1 = x1
|
||||
// b1 = x1 - x0
|
||||
// Calculate x of body frame
|
||||
QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, imu->xB);
|
||||
// get pitch and yaw from quarternion; principal argument for yaw
|
||||
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4, &(imu->pOld),&(imu->yOld),true);
|
||||
|
||||
imu->pn1 = imu->pn2 = imu->pOld;
|
||||
imu->yn1 = imu->yn2 = imu->yOld;
|
||||
|
||||
imu->v1n1 = imu->v1n2 = imu->v1Old = velData.North;
|
||||
imu->v2n1 = imu->v2n2 = imu->v2Old = velData.East;
|
||||
imu->v3n1 = imu->v3n2 = imu->v3Old = velData.Down;
|
||||
|
||||
// ground speed
|
||||
imu->Vel[0] = velData.North;
|
||||
imu->Vel[1] = velData.East;
|
||||
imu->Vel[2] = velData.Down;
|
||||
|
||||
// trend assumed to be zero
|
||||
imu->dxB[0] = imu->dxB[1] = imu->dxB[2] = 0.f;
|
||||
imu->dVel[0] = imu->dVel[1] = imu->dVel[2] = 0.f;
|
||||
// initial guess for airspeed is modulus of groundspeed
|
||||
imu->Vn1 = imu->Vn2 = sqrt(Sq(velData.North) + Sq(velData.East) + Sq(velData.Down));
|
||||
}
|
||||
|
||||
/*
|
||||
@ -118,91 +180,75 @@ void imu_airspeedInitialize()
|
||||
*/
|
||||
void imu_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
|
||||
{
|
||||
const float alpha = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha;
|
||||
const float beta = airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha;
|
||||
|
||||
//pre-filter frequency rate
|
||||
//corresponds to a cut-off frequency of 0.04 Hz or a period of 25 sec
|
||||
const float ff=0.04f * 1000.0f/airspeedSettings->SamplePeriod;
|
||||
// good values for turbulent situation: cut-off 0.01 Hz or a period of 100 sec
|
||||
const float ffV=0.01f * 1000.0f/airspeedSettings->SamplePeriod;
|
||||
// good values for steady situation: cut-off 0.05 Hz or a period of 20 sec
|
||||
// const float ffV=0.05 * 1000.0f/airspeedSettings->SamplePeriod;
|
||||
|
||||
float dxB[3], dVel[3];
|
||||
float normVel2;
|
||||
|
||||
// get values and conduct smoothing of ground speed and orientation independently of the calculation of airspeed
|
||||
{ // Scoping to save memory
|
||||
float xB[3];
|
||||
AttitudeStateData attData;
|
||||
AttitudeStateGet(&attData);
|
||||
VelocityStateData velData;
|
||||
VelocityStateGet(&velData);
|
||||
float p=imu->pOld, y=imu->yOld;
|
||||
float xB[3], xBOld[3];
|
||||
|
||||
// get pitch and roll Euler angles from quaternion
|
||||
// do not calculate the principlal argument of yaw, i.e. use old yaw to add multiples of 2pi to have a continuous yaw
|
||||
Quaternion2PY(attData.q1, attData.q2, attData.q3, attData.q4,&p,&y,false);
|
||||
|
||||
// Calculate rotation matrix
|
||||
QuaternionC2xB(attData.q1, attData.q2, attData.q3, attData.q4, xB);
|
||||
// filter pitch and roll Euler angles instead of fuselage vector to guarantee a unit length at all times
|
||||
p=FilterButterWorthDF2(ff, p, &(imu->pn1), &(imu->pn2));
|
||||
y=FilterButterWorthDF2(ff, y, &(imu->yn1), &(imu->yn2));
|
||||
// transform pitch and yaw into fuselage vector xB
|
||||
PY2xB(imu->pOld,imu->yOld,xBOld);
|
||||
PY2xB(p,y,xB);
|
||||
// calculate change in fuselage vector
|
||||
dxB[0]=xB[0]-xBOld[0];
|
||||
dxB[1]=xB[1]-xBOld[1];
|
||||
dxB[2]=xB[2]-xBOld[2];
|
||||
|
||||
// filter ground speed from VelocityState
|
||||
const float fv1n = FilterButterWorthDF2(ff, velData.North, &(imu->v1n1), &(imu->v1n2));
|
||||
const float fv2n = FilterButterWorthDF2(ff, velData.East, &(imu->v2n1), &(imu->v2n2));
|
||||
const float fv3n = FilterButterWorthDF2(ff, velData.Down, &(imu->v3n1), &(imu->v3n2));
|
||||
// calculate change in ground velocity
|
||||
dVel[0] = fv1n - imu->v1Old;
|
||||
dVel[1] = fv2n - imu->v2Old;
|
||||
dVel[2] = fv3n - imu->v3Old;
|
||||
|
||||
// Holt-Winters double exponential smoothing
|
||||
// Orientation xB
|
||||
float sk = imu->xB[0];
|
||||
imu->xB[0] = alpha * xB[0] + (1.f - alpha) * (sk + imu->dxB[0]);
|
||||
imu->dxB[0] = beta * (imu->xB[0] - sk) + (1.f - beta) * imu->dxB[0];
|
||||
|
||||
sk = imu->xB[1];
|
||||
imu->xB[1] = alpha * xB[1] + (1.f - alpha) * (sk + imu->dxB[1]);
|
||||
imu->dxB[1] = beta * (imu->xB[1] - sk) + (1.f - beta) * imu->dxB[1];
|
||||
|
||||
sk = imu->xB[2];
|
||||
imu->xB[2] = alpha * xB[2] + (1.f - alpha) * (sk + imu->dxB[2]);
|
||||
imu->dxB[2] = beta * (imu->xB[2] - sk) + (1.f - beta) * imu->dxB[2];
|
||||
|
||||
// Ground speed Vel
|
||||
sk = imu->Vel[0];
|
||||
imu->Vel[0] = alpha * velData.North + (1.f - alpha) * (sk + imu->dVel[0]);
|
||||
imu->dVel[0] = beta * (imu->Vel[0] - sk) + (1.f - beta) * imu->dVel[0];
|
||||
|
||||
sk = imu->Vel[1];
|
||||
imu->Vel[1] = alpha * velData.East + (1.f - alpha) * (sk + imu->dVel[1]);
|
||||
imu->dVel[1] = beta * (imu->Vel[1] - sk) + (1.f - beta) * imu->dVel[1];
|
||||
|
||||
sk = imu->Vel[2];
|
||||
imu->Vel[2] = alpha * velData.Down + (1.f - alpha) * (sk + imu->dVel[2]);
|
||||
imu->dVel[2] = beta * (imu->Vel[2] - sk) + (1.f - beta) * imu->dVel[2];
|
||||
|
||||
/////// for debugging purposes only! ////////////
|
||||
airspeedData->f[0] = imu->xB[0];
|
||||
airspeedData->f[1] = imu->xB[1];
|
||||
airspeedData->f[2] = imu->xB[2];
|
||||
|
||||
airspeedData->v[0] = imu->Vel[0];
|
||||
airspeedData->v[1] = imu->Vel[1];
|
||||
airspeedData->v[2] = imu->Vel[2];
|
||||
|
||||
airspeedData->df[0] = imu->dxB[0];
|
||||
airspeedData->df[1] = imu->dxB[1];
|
||||
airspeedData->df[2] = imu->dxB[2];
|
||||
|
||||
airspeedData->dv[0] = imu->dVel[0];
|
||||
airspeedData->dv[1] = imu->dVel[1];
|
||||
airspeedData->dv[2] = imu->dVel[2];
|
||||
airspeedData->absdf = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]);
|
||||
airspeedData->dvdotdf = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2];
|
||||
//////////////////////////////////////////////////
|
||||
// calculate norm of ground speed
|
||||
normVel2 = Sq(fv1n) + Sq(fv2n) + Sq(fv3n);
|
||||
|
||||
// actualise old values
|
||||
imu->pOld = p; imu->yOld = y;
|
||||
imu->v1Old = fv1n; imu->v2Old = fv2n; imu->v3Old = fv3n;
|
||||
}
|
||||
|
||||
// Calculate the norm^2 of the difference between the two fuselage vectors
|
||||
const float normDiffAttitude2 = Sq(imu->dxB[0]) + Sq(imu->dxB[1]) + Sq(imu->dxB[2]);
|
||||
const float normVel2 = Sq(imu->Vel[0]) + Sq(imu->Vel[1]) + Sq(imu->Vel[2]);
|
||||
|
||||
// Some reorientation needed to be able to calculate airspeed and calculate only for sufficient velocity
|
||||
if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY) {
|
||||
// Calculate scalar product of difference vectors
|
||||
const float dvdtDotdfdt = imu->dVel[0] * imu->dxB[0] + imu->dVel[1] * imu->dxB[1] + imu->dVel[2] * imu->dxB[2];
|
||||
const float normDiffAttitude2 = Sq(dxB[0]) + Sq(dxB[1]) + Sq(dxB[2]);
|
||||
// Calculate scalar product of difference vectors
|
||||
const float dvdtDotdfdt = dVel[0] * dxB[0] + dVel[1] * dxB[1] + dVel[2] * dxB[2];
|
||||
|
||||
// Some reorientation needed to be able to calculate airspeed, calculate only for sufficient velocity
|
||||
// a negative scalar product is a clear sign that we are not really able to calculate the airspeed
|
||||
if (normDiffAttitude2 > EPS_REORIENTATION && normVel2 > EPS_VELOCITY && dvdtDotdfdt > 0.f) {
|
||||
// Airspeed modulus: |v| = dv/dt * dxB/dt / |dxB/dt|^2
|
||||
// airspeed is always REAL because normDiffAttitude2 > EPS_REORIENTATION > 0 and REAL dvdtDotdfdt
|
||||
const float airspeed = dvdtDotdfdt / normDiffAttitude2;
|
||||
|
||||
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
|
||||
airspeedData->CalibratedAirspeed = 0;
|
||||
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_ERROR);
|
||||
} else {
|
||||
airspeedData->CalibratedAirspeed = airspeed;
|
||||
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
|
||||
}
|
||||
// filter raw airspeed
|
||||
const float fVn=FilterButterWorthDF2(ffV,airspeed,&(imu->Vn1),&(imu->Vn2));
|
||||
|
||||
airspeedData->CalibratedAirspeed = fVn;
|
||||
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_AIRSPEED, SYSTEMALARMS_ALARM_OK);
|
||||
} else {
|
||||
airspeedData->CalibratedAirspeed = 0;
|
||||
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user