Fixed a range of initialization issues in filter, does not any more emit NaN in first iteration

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]);

 }