Merged new EKF version

1 files changed, 204 insertions, 184 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 1d4df87fe..b507b4c10 100644..100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -77,40 +77,40 @@ 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,
-				  };
+		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[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 */
+		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 */
 
 /* output euler angles */
 static float euler[3] = {0.0f, 0.0f, 0.0f};
 
 static float Rot_matrix[9] = {1.f,  0,  0,
-		      0,  1.f,  0,
-		      0,  0,  1.f
-		     };		/**< init: identity matrix */
+		0,  1.f,  0,
+		0,  0,  1.f
+};		/**< init: identity matrix */
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -159,11 +159,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
-							 SCHED_DEFAULT,
-							 SCHED_PRIORITY_MAX - 5,
-							 20000,
-							 attitude_estimator_ekf_thread_main,
-							 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+				SCHED_DEFAULT,
+				SCHED_PRIORITY_MAX - 5,
+				20000,
+				attitude_estimator_ekf_thread_main,
+				(argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
@@ -250,12 +250,18 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
 
+	uint64_t start_time = hrt_absolute_time();
+	bool initialized = false;
+
+	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
+	unsigned offset_count = 0;
+
 	/* Main loop*/
 	while (!thread_should_exit) {
 
 		struct pollfd fds[2] = {
-			{ .fd = sub_raw,   .events = POLLIN },
-			{ .fd = sub_params, .events = POLLIN }
+				{ .fd = sub_raw,   .events = POLLIN },
+				{ .fd = sub_params, .events = POLLIN }
 		};
 		int ret = poll(fds, 2, 1000);
 
@@ -265,7 +271,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			/* XXX this means no sensor data - should be critical or emergency */
 			printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
 		} else {
-	
+
 			/* only update parameters if they changed */
 			if (fds[1].revents & POLLIN) {
 				/* read from param to clear updated flag */
@@ -282,156 +288,170 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				/* get latest measurements */
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
-				/* Calculate data time difference in seconds */
-				dt = (raw.timestamp - last_measurement) / 1000000.0f;
-				last_measurement = raw.timestamp;
-				uint8_t update_vect[3] = {0, 0, 0};
-
-				/* Fill in gyro measurements */
-				if (sensor_last_count[0] != raw.gyro_counter) {
-					update_vect[0] = 1;
-					sensor_last_count[0] = raw.gyro_counter;
-					sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-					sensor_last_timestamp[0] = raw.timestamp;
-				}
-
-				z_k[0] =  raw.gyro_rad_s[0];
-				z_k[1] =  raw.gyro_rad_s[1];
-				z_k[2] =  raw.gyro_rad_s[2];
-
-				/* update accelerometer measurements */
-				if (sensor_last_count[1] != raw.accelerometer_counter) {
-					update_vect[1] = 1;
-					sensor_last_count[1] = raw.accelerometer_counter;
-					sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-					sensor_last_timestamp[1] = raw.timestamp;
-				}
-				z_k[3] = raw.accelerometer_m_s2[0];
-				z_k[4] = raw.accelerometer_m_s2[1];
-				z_k[5] = raw.accelerometer_m_s2[2];
-
-				/* update magnetometer measurements */
-				if (sensor_last_count[2] != raw.magnetometer_counter) {
-					update_vect[2] = 1;
-					sensor_last_count[2] = raw.magnetometer_counter;
-					sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-					sensor_last_timestamp[2] = raw.timestamp;
-				}
-				z_k[6] = raw.magnetometer_ga[0];
-				z_k[7] = raw.magnetometer_ga[1];
-				z_k[8] = raw.magnetometer_ga[2];
-
-				uint64_t now = hrt_absolute_time();
-				unsigned int time_elapsed = now - last_run;
-				last_run = now;
-
-				if (time_elapsed > loop_interval_alarm) {
-					//TODO: add warning, cpu overload here
-					// if (overloadcounter == 20) {
-					// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-					// 	overloadcounter = 0;
-					// }
-
-					overloadcounter++;
-				}
-
-				int32_t z_k_sizes = 9;
-				// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
-
-				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*/) 
-				{
-					dt = 0.005f;
-					parameters_update(&ekf_param_handles, &ekf_params);
-
-					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] = 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;
-				}
-
-				/* do not execute the filter if not initialized */
-				if (!const_initialized) {
-					continue;
+				if (!initialized) {
+
+					gyro_offsets[0] += raw.gyro_rad_s[0];
+					gyro_offsets[1] += raw.gyro_rad_s[1];
+					gyro_offsets[2] += raw.gyro_rad_s[2];
+
+					if (hrt_absolute_time() - start_time > 3000000LL) {
+						initialized = true;
+						gyro_offsets[0] /= offset_count;
+						gyro_offsets[1] /= offset_count;
+						gyro_offsets[2] /= offset_count;
+					}
+				} else {
+
+					/* Calculate data time difference in seconds */
+					dt = (raw.timestamp - last_measurement) / 1000000.0f;
+					last_measurement = raw.timestamp;
+					uint8_t update_vect[3] = {0, 0, 0};
+
+					/* Fill in gyro measurements */
+					if (sensor_last_count[0] != raw.gyro_counter) {
+						update_vect[0] = 1;
+						sensor_last_count[0] = raw.gyro_counter;
+						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						sensor_last_timestamp[0] = raw.timestamp;
+					}
+
+					z_k[0] =  raw.gyro_rad_s[0];
+					z_k[1] =  raw.gyro_rad_s[1];
+					z_k[2] =  raw.gyro_rad_s[2];
+
+					/* update accelerometer measurements */
+					if (sensor_last_count[1] != raw.accelerometer_counter) {
+						update_vect[1] = 1;
+						sensor_last_count[1] = raw.accelerometer_counter;
+						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						sensor_last_timestamp[1] = raw.timestamp;
+					}
+					z_k[3] = raw.accelerometer_m_s2[0];
+					z_k[4] = raw.accelerometer_m_s2[1];
+					z_k[5] = raw.accelerometer_m_s2[2];
+
+					/* update magnetometer measurements */
+					if (sensor_last_count[2] != raw.magnetometer_counter) {
+						update_vect[2] = 1;
+						sensor_last_count[2] = raw.magnetometer_counter;
+						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						sensor_last_timestamp[2] = raw.timestamp;
+					}
+					z_k[6] = raw.magnetometer_ga[0];
+					z_k[7] = raw.magnetometer_ga[1];
+					z_k[8] = raw.magnetometer_ga[2];
+
+					uint64_t now = hrt_absolute_time();
+					unsigned int time_elapsed = now - last_run;
+					last_run = now;
+
+					if (time_elapsed > loop_interval_alarm) {
+						//TODO: add warning, cpu overload here
+						// if (overloadcounter == 20) {
+						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+						// 	overloadcounter = 0;
+						// }
+
+						overloadcounter++;
+					}
+
+					int32_t z_k_sizes = 9;
+					// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+
+					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*/)
+					{
+						dt = 0.005f;
+						parameters_update(&ekf_param_handles, &ekf_params);
+
+						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] = 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;
+					}
+
+					/* do not execute the filter if not initialized */
+					if (!const_initialized) {
+						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));
+					uint64_t timing_diff = hrt_absolute_time() - timing_start;
+
+					// /* print rotation matrix every 200th time */
+					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],
+						// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
+
+
+						// }
+
+						//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("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[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));
+					}
+
+					// 				int i = printcounter % 9;
+
+					// 	// for (int i = 0; i < 9; i++) {
+					// 		char name[10];
+					// 		sprintf(name, "xapo #%d", i);
+					// 		memcpy(dbg.key, name, sizeof(dbg.key));
+					// 		dbg.value = x_aposteriori[i];
+					// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+
+					printcounter++;
+
+					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+
+					last_data = raw.timestamp;
+
+					/* send out */
+					att.timestamp = raw.timestamp;
+					att.roll = euler[0];
+					att.pitch = euler[1];
+					att.yaw = euler[2];
+
+					att.rollspeed = x_aposteriori[0];
+					att.pitchspeed = x_aposteriori[1];
+					att.yawspeed = x_aposteriori[2];
+					//att.yawspeed =z_k[2] ;
+					/* copy offsets */
+					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
+
+					/* copy rotation matrix */
+					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					att.R_valid = true;
+
+					// Broadcast
+					orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 				}
-
-				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));
-				uint64_t timing_diff = hrt_absolute_time() - timing_start;
-
-				// /* print rotation matrix every 200th time */
-				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],
-					// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
-
-
-					// }
-
-					//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("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[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));
-				}
-
-				// 				int i = printcounter % 9;
-
-				// 	// for (int i = 0; i < 9; i++) {
-				// 		char name[10];
-				// 		sprintf(name, "xapo #%d", i);
-				// 		memcpy(dbg.key, name, sizeof(dbg.key));
-				// 		dbg.value = x_aposteriori[i];
-				// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
-
-				printcounter++;
-
-				if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
-
-				last_data = raw.timestamp;
-
-				/* send out */
-				att.timestamp = raw.timestamp;
-				att.roll = euler[0];
-				att.pitch = euler[1];
-				att.yaw = euler[2];
-
-				// XXX replace with x_apo after fix to filter
-				att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
-				att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
-				att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
-
-				/* copy offsets */
-				memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
-
-				/* copy rotation matrix */
-				memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
-				att.R_valid = true;
-
-				// Broadcast
-				orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 			}
 		}