fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -73,15 +73,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
_fastTimeStamp(hrt_absolute_time()),
_slowTimeStamp(hrt_absolute_time()),
_predictTimeStamp(hrt_absolute_time()),
_attTimeStamp(hrt_absolute_time()),
_outTimeStamp(hrt_absolute_time()),
// frame count
_navFrames(0),
// miss counts
_missFast(0),
_missSlow(0),
_miss(0),
// accelerations
fN(0), fE(0), fD(0),
// state
@ -96,9 +94,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_rGpsPos(this, "R_GPS_POS"),
_rGpsAlt(this, "R_GPS_ALT"),
_rAccel(this, "R_ACCEL"),
_magDip(this, "MAG_DIP"),
_magDec(this, "MAG_DEC"),
_g(this, "G"),
_magDip(this, "ENV_MAG_DIP"),
_magDec(this, "ENV_MAG_DEC"),
_g(this, "ENV_G"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_positionInitialized(false)
@ -193,29 +191,21 @@ void KalmanNav::update()
lat, lon, alt);
}
// fast prediciton step
// note, using sensors timestamp so we can account
// for packet lag
float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
printf("dtFast: %15.10f\n", double(dtFast));
_fastTimeStamp = _sensors.timestamp;
if (dtFast < 1.0f) {
predictFast(dtFast);
predictSlow(dtFast);
// prediciton step
// using sensors timestamp so we can account for packet lag
float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
//printf("dt: %15.10f\n", double(dtFast));
_predictTimeStamp = _sensors.timestamp;
// don't predict if time greater than a second
if (dt < 1.0f) {
predictState(dt);
predictStateCovariance(dt);
// count fast frames
_navFrames += 1;
}
} else _missFast++;
// 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 ++;
//}
// count times 100 Hz rate isn't met
if (dt > 0.01f) _miss++;
// gps correction step
if (gpsUpdate) {
@ -237,11 +227,10 @@ void KalmanNav::update()
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, misses fast: %4d slow: %4d\n",
_navFrames / 10, _missFast / 10, _missSlow / 10);
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss/ 10);
_navFrames = 0;
_missFast = 0;
_missSlow = 0;
_miss= 0;
}
}
@ -288,7 +277,7 @@ void KalmanNav::updatePublications()
SuperBlock::updatePublications();
}
void KalmanNav::predictFast(float dt)
void KalmanNav::predictState(float dt)
{
using namespace math;
Vector3 w(_sensors.gyro_rad_s);
@ -336,20 +325,13 @@ void KalmanNav::predictFast(float dt)
float lDot = vE / (cosLSing * R);
float rotRate = 2 * omega + lDot;
float vNDot = fN - vE * rotRate * sinL +
vD * LDot;
vD * LDot;
float vDDot = fD - vE * rotRate * cosL -
vN * LDot + _g.get();
vN * LDot + _g.get();
float vEDot = fE + vN * rotRate * sinL +
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));
vDDot * rotRate * cosL;
// 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;
vE += vEDot * dt;
vD += vDDot * dt;
@ -358,7 +340,7 @@ void KalmanNav::predictFast(float dt)
alt += double(-vD * dt);
}
void KalmanNav::predictSlow(float dt)
void KalmanNav::predictStateCovariance(float dt)
{
using namespace math;
@ -473,6 +455,8 @@ void KalmanNav::correctAtt()
// mag measurement
Vector3 zMag(_sensors.magnetometer_ga);
//float magNorm = zMag.norm();
zMag = zMag.unit();
// mag predicted measurement
// choosing some typical magnetic field properties,
@ -588,9 +572,9 @@ void KalmanNav::correctAtt()
psi += xCorrect(PSI);
// attitude also affects nav velocities
//vN += xCorrect(VN);
//vE += xCorrect(VE);
//vD += xCorrect(VD);
vN += xCorrect(VN);
vE += xCorrect(VE);
vD += xCorrect(VD);
// update state covariance
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@ -602,6 +586,9 @@ void KalmanNav::correctAtt()
if (beta > _faultAtt.get()) {
printf("fault in attitude: beta = %8.4f\n", (double)beta);
printf("y:\n"); y.print();
printf("zMagHat:\n"); zMagHat.print();
printf("zMag:\n"); zMag.print();
printf("bNav:\n"); bNav.print();
}
// update quaternions from euler

29
apps/examples/kalman_demo/KalmanNav.hpp
View File

@ -83,18 +83,23 @@ public:
*/
void update();
/**
* Fast prediction
* Contains: state integration
*/
virtual void updatePublications();
void predictFast(float dt);
/**
* Slow prediction
* Contains: covariance integration
* Publication update
*/
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
@ -133,15 +138,13 @@ protected:
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _fastTimeStamp; /**< fast prediction time stamp */
uint64_t _slowTimeStamp; /**< slow prediction time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
uint64_t _attTimeStamp; /**< attitude correction time stamp */
uint64_t _outTimeStamp; /**< output time stamp */
// frame count
uint16_t _navFrames; /**< navigation frames completed in output cycle */
// miss counts
uint16_t _missFast; /**< number of times fast prediction loop missed */
uint16_t _missSlow; /**< number of times slow prediction loop missed */
uint16_t _miss; /**< number of times fast prediction loop missed */
// accelerations
float fN, fE, fD; /**< navigation frame acceleration */
// states

25
apps/examples/kalman_demo/params.c
View File

@ -1,15 +1,16 @@
#include <systemlib/param/param.h>
/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1000.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
PARAM_DEFINE_FLOAT(KF_G, 9.806f);
PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f);
PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.1f);
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f);
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f);
PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);