Skip to content

Commit

Permalink
disable velocity divergence check/reset
Browse files Browse the repository at this point in the history
  • Loading branch information
@kd0aij
kd0aij committed Apr 19, 2015
1 parent a7ac528 commit 5672b28
Show file tree
Hide file tree
Showing 3 changed files with 65 additions and 16 deletions.
33 changes: 18 additions & 15 deletions code/estimator_22states.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,11 +244,14 @@ void AttPosEKF::InitialiseParameters()
minFlowRng = 0.3f; //minimum range between ground and flow sensor
moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate

// the following 3 terms are all multiplied together in computation of maxInnov2
gpsGlitchAccel = 150;
gpsGlitchRadius = 10;
gpsPosInnovNSTD = 6;
gpsVelInnovNSTD = 6;
gpsHorizPosNoise = 0.5f;
// maxInnov2 is compared to the square of this parameter; (maxInnov2 > radius) => glitch
gpsGlitchRadius = 5;
// if the vel innov is greater than this number of standard deviations: glitch
gpsVelInnovNSTD = 6;

}

Expand Down Expand Up @@ -360,10 +363,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED()

// transform body delta velocities to delta velocities in the nav frame
// * and + operators have been overloaded
//delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
// delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
// delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
// delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;

// calculate the magnitude of the nav acceleration (required for GPS
// variance estimation)
Expand Down Expand Up @@ -1235,7 +1238,7 @@ void AttPosEKF::FuseVelposNED()
current_ekf_state.posHealth = true;
current_ekf_state.posFailTime = millis();

if (current_ekf_state.posTimeout || (maxPosInnov2 > sq(float(gpsGlitchRadius)))) {
if (current_ekf_state.posTimeout || ((sq(posInnov[0]) + sq(posInnov[1])) > sq(float(gpsGlitchRadius)))) {

gpsPosGlitchOffsetNE.x += posInnov[0];
gpsPosGlitchOffsetNE.y += posInnov[1];
Expand Down Expand Up @@ -3153,14 +3156,14 @@ int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error)
}

// Reset the filter if it diverges too far from GPS
if (VelNEDDiverged()) {

// Reset and fill error report
InitializeDynamic(velNED, magDeclination);

// that's all we can do here, return
ret = 5;
}
// if (VelNEDDiverged()) {
//
// // Reset and fill error report
// InitializeDynamic(velNED, magDeclination);
//
// // that's all we can do here, return
// ret = 5;
// }

// The excessive covariance detection already
// reset the filter. Just need to report here.
Expand Down
6 changes: 5 additions & 1 deletion code/main_closed_loop_float.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -509,6 +509,7 @@ int main(int argc, char *argv[])
_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - msecPosDelay));
// run the fusion step
_ekf->FuseVelposNED();
printf("FuseVelposNED at time = %e \n", IMUtimestamp);
}
else
{
Expand Down Expand Up @@ -560,7 +561,7 @@ int main(int argc, char *argv[])
_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
_ekf->FuseVelposNED();
printf("time = %e \n", IMUtimestamp);
// printf("time = %e \n", IMUtimestamp);
}
else
{
Expand Down Expand Up @@ -1203,6 +1204,9 @@ void readTimingData()
msecMagDelay = timeArray[6];
msecTasDelay = timeArray[7];
_ekf->EAS2TAS = timeArray[8];

printf("msecVelDelay %d\nmsecPosDelay %d\nmsecHgtDelay %d\nmsecMagDelay %d\nmsecTasDelay %d\n",
msecVelDelay, msecPosDelay, msecHgtDelay, msecMagDelay, msecTasDelay);
}

void CloseFiles()
Expand Down
42 changes: 42 additions & 0 deletions code/plot_glitchOffset.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,20 @@
data = np.genfromtxt('GlitchOffsetOut.txt', delimiter=' ', skip_header=1,
skip_footer=1, names=['time', 'velN', 'velE', 'velD', 'posN', 'posE', 'posD', 'posOffN', 'posOffE', 'velOffN', 'velOffE'])

try:
data2 = np.genfromtxt('StateDataOut.txt', delimiter=' ', skip_header=1,
skip_footer=1, names=['time', 'q1', 'q2', 'q3', 'q4', 'Vn', 'Ve', 'Vd', 'Pn', 'Pe', 'Pd',
'Bx', 'By', 'Bz', 'Aoff', 'Wn', 'We', 'Mn', 'Me', 'Md', 'Mbn', 'Mbe', 'Mbd', 'dist'])
except ValueError:
try:
data2 = np.genfromtxt('StateDataOut.txt', delimiter=' ', skip_header=1,
skip_footer=1, names=['time', 'q1', 'q2', 'q3', 'q4', 'Vn', 'Ve', 'Vd', 'Pn', 'Pe', 'Pd',
'Bx', 'By', 'Bz', 'Aoff', 'Wn', 'We', 'Mn', 'Me', 'Md', 'Mbn', 'Mbe', 'Mbd'])
except ValueError:
data2 = np.genfromtxt('StateDataOut.txt', delimiter=' ', skip_header=1,
skip_footer=1, names=['time', 'q1', 'q2', 'q3', 'q4', 'Vn', 'Ve', 'Vd', 'Pn', 'Pe', 'Pd',
'Bx', 'By', 'Bz', 'Wn', 'We', 'Mn', 'Me', 'Md', 'Mbn', 'Mbe', 'Mbd'])

fig = plt.figure(10)

ax1 = fig.add_subplot(211)
Expand Down Expand Up @@ -53,5 +67,33 @@
handles, labels = ax2.get_legend_handles_labels()
ax2.legend(handles, labels, loc=2)

fig = plt.figure(12)

ax1 = fig.add_subplot(311)

ax1.set_title("GPS velNED vs. EKF vel")
ax1.set_ylabel('(m/sec)')
ax1.plot(data['time'], data['velN'], color='r', label='velN')
ax1.plot(data2['time'], data2['Vn'], color='b', label='Vn')
handles, labels = ax1.get_legend_handles_labels()
ax1.legend(handles, labels, loc=2)

ax2 = fig.add_subplot(312)

ax2.set_ylabel('(m/sec)')
ax2.plot(data['time'], data['velE'], color='r', label='velE')
ax2.plot(data2['time'], data2['Ve'], color='b', label='Ve')
handles, labels = ax2.get_legend_handles_labels()
ax2.legend(handles, labels, loc=2)

ax3 = fig.add_subplot(313)

ax3.set_xlabel('time (s)')
ax3.set_ylabel('(m/sec)')
ax3.plot(data['time'], data['velD'], color='r', label='velD')
ax3.plot(data2['time'], data2['Vd'], color='b', label='Vd')
handles, labels = ax3.get_legend_handles_labels()
ax3.legend(handles, labels, loc=2)


plt.show()

0 comments on commit 5672b28

Please sign in to comment.