Exposed measurement noise covariance and process noise covariance as MAVLink parameters for attitude EKF

1 files changed, 185 insertions, 189 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 10405758f..41f469ae4 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -58,25 +58,20 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/parameter_update.h>
 #include <arch/board/up_hrt.h>
 
 #include <systemlib/systemlib.h>
 
 #include "codegen/attitudeKalmanfilter_initialize.h"
 #include "codegen/attitudeKalmanfilter.h"
+#include "attitude_estimator_ekf_params.h"
 
 __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
-
-// #define N_STATES 6
-
-// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
-// #define REPROJECTION_COUNTER_LIMIT 125
-
 static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 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];					/**< Measurement vector */
 static float x_aposteriori_k[12];		/**< */
@@ -94,6 +89,7 @@ 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,   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,
@@ -107,11 +103,14 @@ static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   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] */
+
+/* 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 */
@@ -223,6 +222,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* rate-limit raw data updates to 200Hz */
 	orb_set_interval(sub_raw, 4);
 
+	/* subscribe to param changes */
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -241,18 +243,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 
-	/* process noise covariance */
-	float q[12];
-	/* measurement noise covariance */
-	float r[9];
-	/* output euler angles */
-	float euler[3] = {0.0f, 0.0f, 0.0f};
+	struct attitude_estimator_ekf_params ekf_params;
+
+	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+
+	/* initialize parameter handles */
+	parameters_init(&ekf_param_handles);
 
 	/* Main loop*/
 	while (!thread_should_exit) {
 
-		struct pollfd fds[1] = {
+		struct pollfd fds[2] = {
 			{ .fd = sub_raw,   .events = POLLIN },
+			{ .fd = sub_params, .events = POLLIN }
 		};
 		int ret = poll(fds, 1, 1000);
 
@@ -262,180 +265,173 @@ 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 {
-
-			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;
+	
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), sub_params, &update);
+
+				/* update parameters */
+				parameters_update(&ekf_param_handles, &ekf_params);
 			}
 
-			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;
+			/* only run filter if sensor values changed */
+			if (fds[0].revents & POLLIN) {
+
+				/* 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;
+				}
+
+				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];
+
+				/* 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);
 			}
-			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;
-				q[0] =  1e1f;
-				q[1] =  1e1f;
-				q[2] =  1e1f;
-				/* process noise gyro offset covariance */
-				q[3] =  1e-4f;
-				q[4] =  1e-4f;
-				q[5] =  1e-4f;
-				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] = 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, 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 % 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];
-
-			/* 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);
 		}
 
 		loopcounter++;