fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 12:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset

Browse Source
This commit is contained in:
Lorenz Meier
2014-04-21 10:47:15 +02:00
parent c08544721a
commit aa3aafb1e5
2 changed files with 123 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/estimator.cpp
+105 -34
View File
@@ -1,7 +1,37 @@
#include "estimator.h"
//#include <stdio.h>
#include <string.h>
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
// optimized out by the compiler.
//#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
static void
ekf_debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[ekf]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
static void
ekf_debug(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
ekf_debug_print(fmt, args);
}
#else
static void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
@@ -15,6 +45,10 @@ void Vector3f::zero(void)
}
Mat3f::Mat3f() {
identity();
}
void Mat3f::identity() {
x.x = 1.0f;
x.y = 0.0f;
x.z = 0.0f;
@@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() :
storeIndex(0)
{
InitialiseParameters();
ZeroVariables();
}
AttPosEKF::~AttPosEKF()
@@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED()
void AttPosEKF::FuseMagnetometer()
{
static uint8_t obsIndex;
static float MagPred[3];
static float SH_MAG[9];
static float q0 = 0.0f;
static float q1 = 0.0f;
static float q2 = 0.0f;
static float q3 = 1.0f;
static float magN = 0.4f;
static float magE = 0.0f;
static float magD = 0.3f;
static float magXbias = 0.0f;
static float magYbias = 0.0f;
static float magZbias = 0.0f;
float R_MAG = sq(magMeasurementSigma);
float DCM[3][3] =
{
{1.0f,0.0f,0.0f} ,
{0.0f,1.0f,0.0f} ,
{0.0f,0.0f,1.0f}
};
float &q0 = magstate.q0;
float &q1 = magstate.q1;
float &q2 = magstate.q2;
float &q3 = magstate.q3;
float &magN = magstate.magN;
float &magE = magstate.magE;
float &magD = magstate.magD;
float &magXbias = magstate.magXbias;
float &magYbias = magstate.magYbias;
float &magZbias = magstate.magZbias;
unsigned &obsIndex = magstate.obsIndex;
Mat3f &DCM = magstate.DCM;
float *MagPred = &magstate.MagPred[0];
float &R_MAG = magstate.R_MAG;
float *SH_MAG = &magstate.SH_MAG[0];
float SK_MX[6];
float SK_MY[5];
float SK_MZ[6];
@@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer()
// rotate predicted earth components into body axes and calculate
// predicted measurments
DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
DCM[0][1] = 2*(q1*q2 + q0*q3);
DCM[0][2] = 2*(q1*q3-q0*q2);
DCM[1][0] = 2*(q1*q2 - q0*q3);
DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
DCM[1][2] = 2*(q2*q3 + q0*q1);
DCM[2][0] = 2*(q1*q3 + q0*q2);
DCM[2][1] = 2*(q2*q3 - q0*q1);
DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias;
MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias;
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;
DCM.x.y = 2*(q1*q2 + q0*q3);
DCM.x.z = 2*(q1*q3-q0*q2);
DCM.y.x = 2*(q1*q2 - q0*q3);
DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;
DCM.y.z = 2*(q2*q3 + q0*q1);
DCM.z.x = 2*(q1*q3 + q0*q2);
DCM.z.y = 2*(q2*q3 - q0*q1);
DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias;
MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias;
MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias;
// scale magnetometer observation error with total angular rate
R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU);
@@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
bool err = false;
// check all integrators
if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
err_report->statesNaN = true;
ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z);
err = true;
} // delta angles
if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
err_report->statesNaN = true;
ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z);
err = true;
} // delta angles
if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
err_report->statesNaN = true;
ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z);
err = true;
} // delta velocities
// check all states and covariance matrices
for (unsigned i = 0; i < n_states; i++) {
for (unsigned j = 0; j < n_states; j++) {
if (!isfinite(KH[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("KH NaN");
err = true;
} // intermediate result used for covariance updates
if (!isfinite(KHP[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("KHP NaN");
err = true;
} // intermediate result used for covariance updates
if (!isfinite(P[i][j])) {
err_report->covarianceNaN = true;
ekf_debug("P NaN");
err = true;
} // covariance matrix
}
@@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
if (!isfinite(Kfusion[i])) {
err_report->kalmanGainsNaN = true;
ekf_debug("Kfusion NaN");
err = true;
} // Kalman gains
if (!isfinite(states[i])) {
err_report->statesNaN = true;
ekf_debug("states NaN: i: %u val: %f", i, states[i]);
err = true;
} // state matrix
}
@@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound()
// Reset the filter if the states went NaN
if (StatesNaN(&last_ekf_error)) {
ekf_debug("re-initializing dynamic");
InitializeDynamic(velNED);
@@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
magstate.q3 = 1.0f;
magstate.magN = 0.4f;
magstate.magE = 0.0f;
magstate.magD = 0.3f;
magstate.magXbias = 0.0f;
magstate.magYbias = 0.0f;
magstate.magZbias = 0.0f;
magstate.R_MAG = sq(magMeasurementSigma);
magstate.DCM.identity();
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
@@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables()
}
correctedDelAng.zero();
summedDelAng.zero();
summedDelVel.zero();
for (unsigned i = 0; i < data_buffer_size; i++) {
@@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables()
statetimeStamp[i] = 0;
}
memset(&magstate, 0, sizeof(magstate));
magstate.q0 = 1.0f;
magstate.DCM.identity();
memset(&current_ekf_state, 0, sizeof(current_ekf_state));
}
src/modules/ekf_att_pos_estimator/estimator.h
+18 -1
View File
@@ -33,6 +33,7 @@ public:
Mat3f();
void identity();
Mat3f transpose(void) const;
};
@@ -140,7 +141,23 @@ public:
accelProcessNoise = 0.5f;
}
struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
} magstate;
// Global variables