diff options
Diffstat (limited to 'apps/examples/kalman_demo')
| -rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 17 | ||||
| -rw-r--r-- | apps/examples/kalman_demo/params.c | 16 |
2 files changed, 17 insertions, 16 deletions
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index d93a7bdc6..6acf39be8 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -66,7 +66,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), @@ -193,9 +193,10 @@ void KalmanNav::update() // prediciton step // using sensors timestamp so we can account for packet lag - float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + 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); @@ -213,7 +214,7 @@ void KalmanNav::update() } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz + if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -228,9 +229,9 @@ void KalmanNav::update() if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss/ 10); + _navFrames / 10, _miss / 10); _navFrames = 0; - _miss= 0; + _miss = 0; } } @@ -325,11 +326,11 @@ void KalmanNav::predictState(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; + vDDot * rotRate * cosL; // rectangular integration vN += vNDot * dt; diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 58ebeba3c..77225b4c7 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,16 +1,16 @@ #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -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_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, 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_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); +PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); |