fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mag and velocity measurement fix. Fault detection working.

Browse Source
This commit is contained in:
jgoppert
2013-01-15 23:36:01 -05:00
parent 68a6a64213
commit ce3f835c63
3 changed files with 62 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+33 -46
View File
@@ -73,15 +73,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_att(&getPublications(), ORB_ID(vehicle_attitude)), _att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps // timestamps
_pubTimeStamp(hrt_absolute_time()), _pubTimeStamp(hrt_absolute_time()),
_fastTimeStamp(hrt_absolute_time()), _predictTimeStamp(hrt_absolute_time()),
_slowTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()), _attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()), _outTimeStamp(hrt_absolute_time()),
// frame count // frame count
_navFrames(0), _navFrames(0),
// miss counts // miss counts
_missFast(0), _miss(0),
_missSlow(0),
// accelerations // accelerations
fN(0), fE(0), fD(0), fN(0), fE(0), fD(0),
// state // state
@@ -96,9 +94,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_rGpsPos(this, "R_GPS_POS"), _rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"), _rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL"), _rAccel(this, "R_ACCEL"),
_magDip(this, "MAG_DIP"), _magDip(this, "ENV_MAG_DIP"),
_magDec(this, "MAG_DEC"), _magDec(this, "ENV_MAG_DEC"),
_g(this, "G"), _g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"), _faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"), _faultAtt(this, "FAULT_ATT"),
_positionInitialized(false) _positionInitialized(false)
@@ -193,29 +191,21 @@ void KalmanNav::update()
lat, lon, alt); lat, lon, alt);
} }
// fast prediciton step // prediciton step
// note, using sensors timestamp so we can account // using sensors timestamp so we can account for packet lag
// for packet lag float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f; //printf("dt: %15.10f\n", double(dtFast));
printf("dtFast: %15.10f\n", double(dtFast)); _predictTimeStamp = _sensors.timestamp;
_fastTimeStamp = _sensors.timestamp; // don't predict if time greater than a second
if (dt < 1.0f) {
if (dtFast < 1.0f) { predictState(dt);
predictFast(dtFast); predictStateCovariance(dt);
predictSlow(dtFast);
// count fast frames // count fast frames
_navFrames += 1; _navFrames += 1;
}
} else _missFast++; // count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// slow prediction step
//float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
//if (dtSlow > 1.0f / 100) { // 100 Hz
//_slowTimeStamp = _sensors.timestamp;
//if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
//else _missSlow ++;
//}
// gps correction step // gps correction step
if (gpsUpdate) { if (gpsUpdate) {
@@ -237,11 +227,10 @@ void KalmanNav::update()
// output // output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp; _outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _missFast / 10, _missSlow / 10); _navFrames / 10, _miss/ 10);
_navFrames = 0; _navFrames = 0;
_missFast = 0; _miss= 0;
_missSlow = 0;
} }
} }
@@ -288,7 +277,7 @@ void KalmanNav::updatePublications()
SuperBlock::updatePublications(); SuperBlock::updatePublications();
} }
void KalmanNav::predictFast(float dt) void KalmanNav::predictState(float dt)
{ {
using namespace math; using namespace math;
Vector3 w(_sensors.gyro_rad_s); Vector3 w(_sensors.gyro_rad_s);
@@ -336,20 +325,13 @@ void KalmanNav::predictFast(float dt)
float lDot = vE / (cosLSing * R); float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot; float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL + float vNDot = fN - vE * rotRate * sinL +
vD * LDot; vD * LDot;
float vDDot = fD - vE * rotRate * cosL - float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get(); vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL + float vEDot = fE + vN * rotRate * sinL +
vDDot * rotRate * cosL; vDDot * rotRate * cosL;
printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
double(vNDot), double(vEDot), double(vDDot));
printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
double(LDot), double(lDot), double(rotRate));
// rectangular integration // rectangular integration
printf("dt: %8.4f\n", double(dt));
printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
vN += vNDot * dt; vN += vNDot * dt;
vE += vEDot * dt; vE += vEDot * dt;
vD += vDDot * dt; vD += vDDot * dt;
@@ -358,7 +340,7 @@ void KalmanNav::predictFast(float dt)
alt += double(-vD * dt); alt += double(-vD * dt);
} }
void KalmanNav::predictSlow(float dt) void KalmanNav::predictStateCovariance(float dt)
{ {
using namespace math; using namespace math;
@@ -473,6 +455,8 @@ void KalmanNav::correctAtt()
// mag measurement // mag measurement
Vector3 zMag(_sensors.magnetometer_ga); Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// mag predicted measurement // mag predicted measurement
// choosing some typical magnetic field properties, // choosing some typical magnetic field properties,
@@ -588,9 +572,9 @@ void KalmanNav::correctAtt()
psi += xCorrect(PSI); psi += xCorrect(PSI);
// attitude also affects nav velocities // attitude also affects nav velocities
//vN += xCorrect(VN); vN += xCorrect(VN);
//vE += xCorrect(VE); vE += xCorrect(VE);
//vD += xCorrect(VD); vD += xCorrect(VD);
// update state covariance // update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter // http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -602,6 +586,9 @@ void KalmanNav::correctAtt()
if (beta > _faultAtt.get()) { if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta); printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print(); printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
} }
// update quaternions from euler // update quaternions from euler
apps/examples/kalman_demo/KalmanNav.hpp
+16 -13
View File
@@ -83,18 +83,23 @@ public:
*/ */
void update(); void update();
/**
* Fast prediction
* Contains: state integration
*/
virtual void updatePublications();
void predictFast(float dt);
/** /**
* Slow prediction * Publication update
* Contains: covariance integration
*/ */
void predictSlow(float dt); virtual void updatePublications();
/**
* State prediction
* Continuous, non-linear
*/
void predictState(float dt);
/**
* State covariance prediction
* Continuous, linear
*/
void predictStateCovariance(float dt);
/** /**
* Attitude correction * Attitude correction
@@ -133,15 +138,13 @@ protected:
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */ control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps // time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _fastTimeStamp; /**< fast prediction time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _slowTimeStamp; /**< slow prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */ uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */ uint64_t _outTimeStamp; /**< output time stamp */
// frame count // frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */ uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts // miss counts
uint16_t _missFast; /**< number of times fast prediction loop missed */ uint16_t _miss; /**< number of times fast prediction loop missed */
uint16_t _missSlow; /**< number of times slow prediction loop missed */
// accelerations // accelerations
float fN, fE, fD; /**< navigation frame acceleration */ float fN, fE, fD; /**< navigation frame acceleration */
// states // states
apps/examples/kalman_demo/params.c
+13 -12
View File
@@ -1,15 +1,16 @@
#include <systemlib/param/param.h> #include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f);
PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f); PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f); PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_G, 9.806f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);