Updated EKF filter, fixed uploader (reverted to master)

1 files changed, 78 insertions, 59 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index e443f6295..c1f8380e6 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -75,38 +75,45 @@ __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;
+static float dt = 1.0f;
 /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
 /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
-static float z_k[9] = {0};					/**< Measurement vector */
-static float x_aposteriori_k[9] = {0,0,0,0,0,-9.81,1,1,-1};		/**< */
-static float x_aposteriori[9] = {0};
-static float P_aposteriori_k[81] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
-				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,  100.f,  0,   0,   0,   0,
-				   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,
+static float z_k[9];					/**< Measurement vector */
+static float x_aposteriori_k[12];		/**< */
+static float x_aposteriori[12];
+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,
+				   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,
 				  };
-static float P_aposteriori[81] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
-				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,  100.f,  0,   0,   0,   0,
-				   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,
+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 knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
 static float Rot_matrix[9] = {1.f,  0,  0,
 			      0,  1.f,  0,
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
+
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
 static int attitude_estimator_ekf_task;				/**< Handle of deamon task / thread */
@@ -229,6 +236,14 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+
+	/* process noise covariance */
+	float q[12];
+	/* measurement noise covariance */
+	float r[9];
+	/* output euler angles */
+	float euler[3] = {0.0f, 0.0f, 0.0f};
+
 	/* Main loop*/
 	while (!thread_should_exit) {
 
@@ -278,10 +293,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				overloadcounter++;
 			}
 
-			int8_t update_vect[3] = {1, 1, 1};
-			float euler[3];
+			uint8_t update_vect[3] = {1, 1, 1};
 			int32_t z_k_sizes = 9;
-			float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+			// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
 
 			static bool const_initialized = false;
 
@@ -289,38 +303,41 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
 			{
 				dt = 0.005f;
-				q[0] =  1e1;
-				q[1] =  1e1;
-				q[2] =  1e1;
-				q[3] =  1e-6;
-				q[4] =  1e-6;
-				q[5] =  1e-6;
-				q[6] =  1e-1;
-				q[7] =  1e-1;
-				q[8] =  1e-1;
-				q[9] =  1e-1;
-				q[10] = 1e-1;
-				q[11] = 1e-1;
-
-				r[0]= 1e-2;
-				r[1]= 1e-2;
-				r[2]= 1e-2;
-				r[3]= 1e-1;
-				r[4]= 1e-1;
-				r[5]= 1e-1;
-				r[6]= 1e-1;
-				r[7]= 1e-1;
-				r[8]= 1e-1;
+				q[0] =  1e1f;
+				q[1] =  1e1f;
+				q[2] =  1e1f;
+				q[3] =  1e-6f;
+				q[4] =  1e-6f;
+				q[5] =  1e-6f;
+				q[6] =  1e-1f;
+				q[7] =  1e-1f;
+				q[8] =  1e-1f;
+				q[9] =  1e-1f;
+				q[10] = 1e-1f;
+				q[11] = 1e-1f;
+
+				r[0]= 1e-2f;
+				r[1]= 1e-2f;
+				r[2]= 1e-2f;
+				r[3]= 1e-1f;
+				r[4]= 1e-1f;
+				r[5]= 1e-1f;
+				r[6]= 1e-1f;
+				r[7]= 1e-1f;
+				r[8]= 1e-1f;
 
 				x_aposteriori_k[0] = z_k[0];
 				x_aposteriori_k[1] = z_k[1];
 				x_aposteriori_k[2] = z_k[2];
-				x_aposteriori_k[3] = z_k[3];
-				x_aposteriori_k[4] = z_k[4];
-				x_aposteriori_k[5] = z_k[5];
-				x_aposteriori_k[6] = z_k[6];
-				x_aposteriori_k[7] = z_k[7];
-				x_aposteriori_k[8] = z_k[8];
+				x_aposteriori_k[3] = 0.0f;
+				x_aposteriori_k[4] = 0.0f;
+				x_aposteriori_k[5] = 0.0f;
+				x_aposteriori_k[6] = z_k[3];
+				x_aposteriori_k[7] = z_k[4];
+				x_aposteriori_k[8] = z_k[5];
+				x_aposteriori_k[9] = z_k[6];
+				x_aposteriori_k[10] = z_k[7];
+				x_aposteriori_k[11] = z_k[8];
 
 				const_initialized = true;
 			}
@@ -331,15 +348,17 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			}
 
 			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(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, q, 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));
 			uint64_t timing_diff = hrt_absolute_time() - timing_start;
 
 			/* print rotation matrix every 200th time */
-			if (printcounter % 2 == 0) {
+			if (printcounter % 200 == 0) {
 				// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
 				// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
 				// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
@@ -348,8 +367,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 				// }
 
-			// 	printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
-			// 	printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f", (double)euler[0], (double)euler[1], (double)euler[2]);
+				printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
+				printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
 			// 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
 			// 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
 			// 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));