update attitude_estimator_ekf, includes matlab

This adds the latest c implementation (matlab coder) of
attitude_estimator_ekf, the .m matlab script and the .prj file with the
settings to export the matlab code to c

1 files changed, 34 insertions, 7 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index bc0e3b93a..5b57bfb4d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -50,7 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
 /* gyro offsets process noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
 /* gyro measurement noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
@@ -58,14 +57,38 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
 /* mag measurement noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-/* offset estimation - UNUSED */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
 PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
+/*
+ * Moment of inertia matrix diagonal entry (1, 1)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
+
+/*
+ * Moment of inertia matrix diagonal entry (2, 2)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
+
+
+/*
+ * Moment of inertia matrix diagonal entry (3, 3)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
+
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -73,17 +96,19 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q1 	=	param_find("EKF_ATT_V3_Q1");
 	h->q2 	=	param_find("EKF_ATT_V3_Q2");
 	h->q3 	=	param_find("EKF_ATT_V3_Q3");
-	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
 	h->r0 	=	param_find("EKF_ATT_V4_R0");
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
-	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
+	h->moment_inertia_J[0]   =   param_find("ATT_J11");
+	h->moment_inertia_J[1]   =   param_find("ATT_J22");
+	h->moment_inertia_J[2]   =   param_find("ATT_J33");
+
 	return OK;
 }
 
@@ -93,17 +118,19 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q1, &(p->q[1]));
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));
 
 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));
 
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI_F / 180.0f;
 
 	param_get(h->acc_comp, &(p->acc_comp));
 
+	for (int i = 0; i < 3; i++) {
+		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
+	}
+
 	return OK;
 }