diff options
Diffstat (limited to 'apps/examples/kalman_demo/KalmanNav.cpp')
| -rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 39 |
1 files changed, 21 insertions, 18 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index a735de406..5f71da58e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // miss counts _missFast(0), _missSlow(0), - // state + // accelerations fN(0), fE(0), fD(0), + // state phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), @@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsVel(this, "R_GPS_VEL"), _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), - _rAccel(this, "R_ACCEL") + _rAccel(this, "R_ACCEL"), + _magDip(this, "MAG_DIP"), + _magDec(this, "MAG_DEC") { using namespace math; // initial state covariance matrix P0 = Matrix::identity(9) * 1.0f; - P = P0; + P = P0; // wait for gps lock while (1) { @@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : setLonDegE7(_gps.lon); setAltE3(_gps.alt); - printf("[kalman_demo] initializing EKF state with GPS\n"); - printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", - double(phi), double(theta), double(psi)); - printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", - double(vN), double(vE), double(vD), - lat, lon, alt); + printf("[kalman_demo] initializing EKF state with GPS\n"); + printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n", + double(phi), double(theta), double(psi)); + printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", + double(vN), double(vE), double(vD), + lat, lon, alt); // initialize quaternions q = Quaternion(EulerAngles(phi, theta, psi)); @@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt) vN * LDot + g; 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)); + 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 //printf("dt: %8.4f\n", double(dt)); @@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt) // prepare for matrix float R = R0 + float(alt); - float RSq = R*R; + float RSq = R * R; // F Matrix // Titterton pg. 291 @@ -472,8 +475,8 @@ void KalmanNav::correctAtt() // mag predicted measurement // choosing some typical magnetic field properties, // TODO dip/dec depend on lat/ lon/ time - static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level - static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north + static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level + static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north float bN = cosf(dip) * cosf(dec); float bE = cosf(dip) * sinf(dec); float bD = sinf(dip); @@ -593,7 +596,7 @@ void KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse()*y); + float beta = y.dot(S.inverse() * y); if (beta > 1000.0f) { printf("fault in attitude: beta = %8.4f\n", (double)beta); @@ -671,7 +674,7 @@ void KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse()*y); + float beta = y.dot(S.inverse() * y); if (beta > 1000.0f) { printf("fault in gps: beta = %8.4f\n", (double)beta); |