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

Made fault tolerances adjustable.

Browse Source
This commit is contained in:
James Goppert
2013-01-15 13:12:00 -05:00
parent edf0a6bae7
commit 9cf3d51aec
3 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/examples/kalman_demo/KalmanNav.cpp
+4 -2
View File
@@ -98,6 +98,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_rAccel(this, "R_ACCEL"), _rAccel(this, "R_ACCEL"),
_magDip(this, "MAG_DIP"), _magDip(this, "MAG_DIP"),
_magDec(this, "MAG_DEC"), _magDec(this, "MAG_DEC"),
_faultPos(this, "FAULT_POS"),
_faultAtt(this, "FAULT_ATT"),
_positionInitialized(false) _positionInitialized(false)
{ {
using namespace math; using namespace math;
@@ -601,7 +603,7 @@ void KalmanNav::correctAtt()
// fault detection // fault detection
float beta = y.dot(S.inverse() * y); float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) { 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();
} }
@@ -679,7 +681,7 @@ void KalmanNav::correctPos()
// fault detetcion // fault detetcion
float beta = y.dot(S.inverse() * y); float beta = y.dot(S.inverse() * y);
if (beta > 1000.0f) { if (beta > _faultPos.get()) {
printf("fault in gps: beta = %8.4f\n", (double)beta); printf("fault in gps: beta = %8.4f\n", (double)beta);
printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(y(0) / noiseVel), double(y(0) / noiseVel),
apps/examples/kalman_demo/KalmanNav.hpp
+2
View File
@@ -159,6 +159,8 @@ protected:
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
control::BlockParam<float> _magDip; /**< magnetic inclination with level */ control::BlockParam<float> _magDip; /**< magnetic inclination with level */
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
control::BlockParam<float> _faultPos; /**< fault detection threshold for position */
control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */
// status // status
bool _positionInitialized; /**< status, if position has been init. */ bool _positionInitialized; /**< status, if position has been init. */
// accessors // accessors
apps/examples/kalman_demo/params.c
+2
View File
@@ -10,3 +10,5 @@ PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_MAG_DIP, 60.0f);
PARAM_DEFINE_FLOAT(KF_MAG_DEC, 0.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);