From 23d294453bbdade03a67002f62b898e7aca65a70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Oct 2012 15:08:33 +0200 Subject: Fixed a range of initialization issues in filter, does not any more emit NaN in first iteration --- .../attitude_estimator_ekf_main.c | 59 ++++++++++------------ .../attitude_estimator_ekf_params.c | 6 +-- .../codegen/attitudeKalmanfilter.c | 12 ++--- 3 files changed, 36 insertions(+), 41 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index a291a4914..7bb8490e5 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -71,11 +71,10 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds -static float dt = 1.0f; +static float dt = 0.005f; /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ -static float z_k[9]; /**< Measurement vector */ -static float x_aposteriori_k[12]; /**< */ -static float x_aposteriori[12]; +static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ +static float x_aposteriori_k[12]; /**< states */ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, @@ -88,21 +87,10 @@ static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, -}; +}; /**< init: diagonal matrix with big values */ -static float P_aposteriori[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, -}; /**< init: diagonal matrix with big values */ +static float x_aposteriori[12]; +static float P_aposteriori[144]; /* output euler angles */ static float euler[3] = {0.0f, 0.0f, 0.0f}; @@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* advertise debug value */ struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + orb_advert_t pub_dbg = -1; /* keep track of sensor updates */ uint32_t sensor_last_count[3] = {0, 0, 0}; @@ -293,13 +281,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) gyro_offsets[0] += raw.gyro_rad_s[0]; gyro_offsets[1] += raw.gyro_rad_s[1]; gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count+=1; + offset_count++; + if (hrt_absolute_time() - start_time > 3000000LL) { initialized = true; gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - printf("pipapo %d\n",(int)(gyro_offsets[2]*1000) ); } } else { @@ -316,9 +304,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) sensor_last_timestamp[0] = raw.timestamp; } - z_k[0] = raw.gyro_rad_s[0]-gyro_offsets[0]; - z_k[1] = raw.gyro_rad_s[1]-gyro_offsets[1]; - z_k[2] = raw.gyro_rad_s[2]-gyro_offsets[2]; + z_k[0] = raw.gyro_rad_s[0] - gyro_offsets[0]; + z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; + z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* update accelerometer measurements */ if (sensor_last_count[1] != raw.accelerometer_counter) { @@ -362,7 +350,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) static bool const_initialized = false; /* initialize with good values once we have a reasonable dt estimate */ - if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) + if (!const_initialized && dt < 0.05 && dt > 0.005) { dt = 0.005f; parameters_update(&ekf_param_handles, &ekf_params); @@ -388,16 +376,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) continue; } - dt = 0.004f; - uint64_t timing_start = hrt_absolute_time(); - // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, - // Rot_matrix, x_aposteriori, P_aposteriori); + attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, euler, Rot_matrix, x_aposteriori, P_aposteriori); - /* swap values for next iteration */ - memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); - memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + /* swap values for next iteration, check for fatal inputs */ + if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { + memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); + memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); + } else { + /* due to inputs or numerical failure the output is invalid, skip it */ + continue; + } + uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ @@ -425,7 +416,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) // sprintf(name, "xapo #%d", i); // memcpy(dbg.key, name, sizeof(dbg.key)); // dbg.value = x_aposteriori[i]; + // if (pub_dbg < 0) { + // pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + // } else { // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); + // } printcounter++; diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c index f20d1b204..4fc2e0452 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,9 +61,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f); PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-1f); -PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-1f); +PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f); +PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f); +PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f); /* accelerometer measurement noise */ PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f); PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f); diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 885c01bf3..04f6ea267 100644 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -50,7 +50,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) b_u1 = -1; } - y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1); + y = (real32_T)atan2f((real32_T)b_u0, (real32_T)b_u1); } else if (u1 == 0.0F) { if (u0 > 0.0F) { y = RT_PIF / 2.0F; @@ -60,7 +60,7 @@ static real32_T rt_atan2f_snf(real32_T u0, real32_T u1) y = 0.0F; } } else { - y = (real32_T)atan2(u0, u1); + y = (real32_T)atan2f(u0, u1); } return y; @@ -776,12 +776,12 @@ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const Rot_matrix[6 + i] = z_n_b[i]; } - /* 'attitudeKalmanfilter:193' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ - /* 'attitudeKalmanfilter:194' theta=-asin(Rot_matrix(1,3)); */ - /* 'attitudeKalmanfilter:195' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'attitudeKalmanfilter:193' phi=atan2f(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'attitudeKalmanfilter:194' theta=-asinf(Rot_matrix(1,3)); */ + /* 'attitudeKalmanfilter:195' psi=atan2f(Rot_matrix(1,2),Rot_matrix(1,1)); */ /* 'attitudeKalmanfilter:196' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); - eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); } -- cgit v1.2.3