Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : Changes to test harness to support accel bias states

This commit is contained in:
Paul Riseborough 2014-01-31 09:27:56 +11:00 committed by Andrew Tridgell
parent 5219869389
commit 401e3aee43
2 changed files with 150 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/AP_NavEKF/examples/AP_NavEKF/AP_NavEKF.pde
View File

@ -67,6 +67,10 @@ static LogReader LogReader(ins, barometer, compass, g_gps);
static FILE *plotf;
static FILE *plotf2;
static FILE *ekf1f;
static FILE *ekf2f;
static FILE *ekf3f;
static FILE *ekf4f;
static bool done_baro_init;
static bool done_home_init;
@ -95,8 +99,17 @@ void setup()
plotf = fopen("plot.dat", "w");
plotf2 = fopen("plot2.dat", "w");
ekf1f = fopen("EKF1.dat", "w");
ekf2f = fopen("EKF2.dat", "w");
ekf3f = fopen("EKF3.dat", "w");
ekf4f = fopen("EKF4.dat", "w");
fprintf(plotf, "time SIM.Roll SIM.Pitch SIM.Yaw BAR.Alt FLIGHT.Roll FLIGHT.Pitch FLIGHT.Yaw FLIGHT.dN FLIGHT.dE FLIGHT.Alt DCM.Roll DCM.Pitch DCM.Yaw EKF.Roll EKF.Pitch EKF.Yaw INAV.dN INAV.dE INAV.Alt EKF.dN EKF.dE EKF.Alt\n");
fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ AX AY AZ MN ME MD MX MY MZ E1ref E2ref E3ref\n");
fprintf(plotf2, "time E1 E2 E3 VN VE VD PN PE PD GX GY GZ WN WE MN ME MD MX MY MZ E1ref E2ref E3ref\n");
fprintf(ekf1f, "timestamp TimeMS Roll Pitch Yaw VN VE VD PN PE PD GX GY GZ\n");
fprintf(ekf2f, "timestamp TimeMS AX AY AZ VWN VWE MN ME MD MX MY MZ\n");
fprintf(ekf3f, "timestamp TimeMS IVN IVE IVD IPN IPE IPD IMX IMY IMZ IVT\n");
fprintf(ekf4f, "timestamp TimeMS SVN SVE SVD SPN SPE SPD SMX SMY SMZ SVT\n");
ahrs.set_ekf_use(true);
@ -164,26 +177,41 @@ void loop()
if ((type == LOG_PLANE_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_PLANE) ||
(type == LOG_COPTER_ATTITUDE_MSG && LogReader.vehicle == LogReader::VEHICLE_COPTER)) {
Vector3f ekf_euler;
Vector3f velNED;
Vector3f posNED;
Vector3f gyroBias;
Vector3f accelBias;
Vector3f windVel;
Vector3f magNED;
Vector3f magXYZ;
Vector3f DCM_attitude;
Vector3f ekf_relpos;
Vector3f velInnov;
Vector3f posInnov;
Vector3f magInnov;
float tasInnov;
Vector3f velVar;
Vector3f posVar;
Vector3f magVar;
float tasVar;
ahrs.get_secondary_attitude(DCM_attitude);
NavEKF.getEulerAngles(ekf_euler);
NavEKF.getVelNED(velNED);
NavEKF.getPosNED(posNED);
NavEKF.getGyroBias(gyroBias);
NavEKF.getAccelBias(accelBias);
NavEKF.getWind(windVel);
NavEKF.getMagNED(magNED);
NavEKF.getMagXYZ(magXYZ);
NavEKF.getInnovations(velInnov, posInnov, magInnov, tasInnov);
NavEKF.getVariances(velVar, posVar, magVar, tasVar);
ekf_relpos = ahrs.get_relative_position_NED();
Vector3f inav_pos = inertial_nav.get_position() * 0.01f;
float temp = degrees(ekf_euler.z);
if (temp < 0.0f) temp = temp + 360.0f;
fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n",
hal.scheduler->millis() * 0.001f,
@ -209,7 +237,7 @@ void loop()
ekf_relpos.x,
ekf_relpos.y,
-ekf_relpos.z);
fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.3f %.3f %.3f %.4f %.4f %.4f %.4f %.4f %.4f %.1f %.1f %.1f\n",
fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n",
hal.scheduler->millis() * 0.001f,
degrees(ekf_euler.x),
degrees(ekf_euler.y),
@ -220,12 +248,11 @@ void loop()
posNED.x,
posNED.y,
posNED.z,
gyroBias.x,
gyroBias.y,
gyroBias.z,
accelBias.x,
accelBias.y,
accelBias.z,
60*degrees(gyroBias.x),
60*degrees(gyroBias.y),
60*degrees(gyroBias.z),
windVel.x,
windVel.y,
magNED.x,
magNED.y,
magNED.z,
@ -235,6 +262,120 @@ void loop()
LogReader.get_attitude().x,
LogReader.get_attitude().y,
LogReader.get_attitude().z);
// define messages for EKF1 data packet
int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg)
int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg)
uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg)
float velN = (float)(velNED.x); // velocity North (m/s)
float velE = (float)(velNED.y); // velocity East (m/s)
float velD = (float)(velNED.z); // velocity Down (m/s)
float posN = (float)(posNED.x); // metres North
float posE = (float)(posNED.y); // metres East
float posD = (float)(posNED.z); // metres Down
int16_t gyrX = (int16_t)(6000*degrees(gyroBias.x)); // centi-deg/min
int16_t gyrY = (int16_t)(6000*degrees(gyroBias.y)); // centi-deg/min
int16_t gyrZ = (int16_t)(6000*degrees(gyroBias.z)); // centi-deg/min
// print EKF1 data packet
fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %d %d %d\n",
hal.scheduler->millis() * 0.001f,
hal.scheduler->millis(),
roll,
pitch,
yaw,
velN,
velE,
velD,
posN,
posE,
posD,
gyrX,
gyrY,
gyrZ);
// define messages for EKF2 data packet
int8_t accX = (int8_t)(100*accelBias.x);
int8_t accY = (int8_t)(100*accelBias.y);
int8_t accZ = (int8_t)(100*accelBias.z);
int16_t windN = (int16_t)(100*windVel.x);
int16_t windE = (int16_t)(100*windVel.y);
int16_t magN = (int16_t)(magNED.x);
int16_t magE = (int16_t)(magNED.y);
int16_t magD = (int16_t)(magNED.z);
int16_t magX = (int16_t)(magXYZ.x);
int16_t magY = (int16_t)(magXYZ.y);
int16_t magZ = (int16_t)(magXYZ.z);
// print EKF2 data packet
fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n",
hal.scheduler->millis() * 0.001f,
hal.scheduler->millis(),
accX,
accY,
accZ,
windN,
windE,
magN,
magE,
magD,
magX,
magY,
magZ);
// define messages for EKF3 data packet
int16_t innovVN = (int16_t)(100*velInnov.x);
int16_t innovVE = (int16_t)(100*velInnov.y);
int16_t innovVD = (int16_t)(100*velInnov.z);
int16_t innovPN = (int16_t)(100*posInnov.x);
int16_t innovPE = (int16_t)(100*posInnov.y);
int16_t innovPD = (int16_t)(100*posInnov.z);
int16_t innovMX = (int16_t)(magInnov.x);
int16_t innovMY = (int16_t)(magInnov.y);
int16_t innovMZ = (int16_t)(magInnov.z);
int16_t innovVT = (int16_t)(100*tasInnov);
// print EKF3 data packet
fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n",
hal.scheduler->millis() * 0.001f,
hal.scheduler->millis(),
innovVN,
innovVE,
innovVD,
innovPN,
innovPE,
innovPD,
innovMX,
innovMY,
innovMZ,
innovVT);
// define messages for EKF4 data packet
int16_t sqrtvarVN = (int16_t)(100*sqrtf(velVar.x));
int16_t sqrtvarVE = (int16_t)(100*sqrtf(velVar.y));
int16_t sqrtvarVD = (int16_t)(100*sqrtf(velVar.z));
int16_t sqrtvarPN = (int16_t)(100*sqrtf(posVar.x));
int16_t sqrtvarPE = (int16_t)(100*sqrtf(posVar.y));
int16_t sqrtvarPD = (int16_t)(100*sqrtf(posVar.z));
int16_t sqrtvarMX = (int16_t)(sqrtf(magVar.x));
int16_t sqrtvarMY = (int16_t)(sqrtf(magVar.y));
int16_t sqrtvarMZ = (int16_t)(sqrtf(magVar.z));
int16_t sqrtvarVT = (int16_t)(100*sqrtf(tasVar));
// print EKF4 data packet
fprintf(ekf4f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n",
hal.scheduler->millis() * 0.001f,
hal.scheduler->millis(),
sqrtvarVN,
sqrtvarVE,
sqrtvarVD,
sqrtvarPN,
sqrtvarPE,
sqrtvarPD,
sqrtvarMX,
sqrtvarMY,
sqrtvarMZ,
sqrtvarVT);
}
}
}

2
libraries/AP_NavEKF/examples/AP_NavEKF/plotit.sh
View File

@ -20,7 +20,7 @@ cat <<EOF > _plot.gnu
set style data lines
set xlabel "time(s)"
$cmd
pause -1 "hit return to exit"
EOF
gnuplot _plot.gnu