Better attitude filter, not sensitive to sudden accelerations

apps/attitude_estimator_ekf/Makefile

@@ -47,8 +47,6 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/rtGetInf.c \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
-		   codegen/diag.c \
-		   codegen/power.c \
 		   codegen/cross.c 
 
 

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -431,6 +431,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
 					att.yawspeed = x_aposteriori[2];
+					att.rollacc = x_aposteriori[3];
+					att.pitchacc = x_aposteriori[4];
+					att.yawacc = x_aposteriori[5];
+
 					//att.yawspeed =z_k[2] ;
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -50,15 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f);
 /* gyro offsets process noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f);
 PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f);
-/* accelerometer process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f);
-/* magnetometer process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f);
-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, 0.01f);
@@ -66,12 +57,7 @@ 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, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
-/* magnetometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
+
 /* offsets in roll, pitch and yaw of sensor plane and body */
 PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
@@ -85,23 +71,11 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q2 	=	param_find("EKF_ATT_Q2");
 	h->q3 	=	param_find("EKF_ATT_Q3");
 	h->q4 	=	param_find("EKF_ATT_Q4");
-	h->q5 	=	param_find("EKF_ATT_Q5");
-	h->q6 	=	param_find("EKF_ATT_Q6");
-	h->q7 	=	param_find("EKF_ATT_Q7");
-	h->q8 	=	param_find("EKF_ATT_Q8");
-	h->q9 	=	param_find("EKF_ATT_Q9");
-	h->q10 	=	param_find("EKF_ATT_Q10");
-	h->q11 	=	param_find("EKF_ATT_Q11");
 
 	h->r0 	=	param_find("EKF_ATT_R0");
 	h->r1 	=	param_find("EKF_ATT_R1");
 	h->r2 	=	param_find("EKF_ATT_R2");
 	h->r3 	=	param_find("EKF_ATT_R3");
-	h->r4 	=	param_find("EKF_ATT_R4");
-	h->r5 	=	param_find("EKF_ATT_R5");
-	h->r6 	=	param_find("EKF_ATT_R6");
-	h->r7 	=	param_find("EKF_ATT_R7");
-	h->r8 	=	param_find("EKF_ATT_R8");
 
 	h->roll_off  =	param_find("ATT_ROLL_OFFS");
 	h->pitch_off =	param_find("ATT_PITCH_OFFS");
@@ -117,23 +91,11 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
 	param_get(h->q4, &(p->q[4]));
-	param_get(h->q5, &(p->q[5]));
-	param_get(h->q6, &(p->q[6]));
-	param_get(h->q7, &(p->q[7]));
-	param_get(h->q8, &(p->q[8]));
-	param_get(h->q9, &(p->q[9]));
-	param_get(h->q10, &(p->q[10]));
-	param_get(h->q11, &(p->q[11]));
 
 	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->r4, &(p->r[4]));
-	param_get(h->r5, &(p->r[5]));
-	param_get(h->r6, &(p->r[6]));
-	param_get(h->r7, &(p->r[7]));
-	param_get(h->r8, &(p->r[8]));
 
 	param_get(h->roll_off, &(p->roll_off));
 	param_get(h->pitch_off, &(p->pitch_off));

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h

@@ -50,8 +50,8 @@ struct attitude_estimator_ekf_params {
 };
 
 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
-	param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
+	param_t r0, r1, r2, r3;
+	param_t q0, q1, q2, q3, q4;
 	param_t roll_off, pitch_off, yaw_off;
 };
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 
@@ -29,6 +29,6 @@
 /* Variable Definitions */
 
 /* Function Declarations */
-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
 #endif
 /* End of code generation (attitudeKalmanfilter.h) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/cross.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/cross.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/eye.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/eye.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/mrdivide.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/mrdivide.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/norm.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/norm.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rdivide.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'rdivide'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rdivide.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'rdivide'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetInf.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetInf.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetNaN.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetNaN.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_defines.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_nonfinite.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_nonfinite.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtwtypes.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Tue Oct 16 15:27:58 2012
+ * C source code generated on: Sat Jan 19 15:25:29 2013
  *
  */
 
@@ -26,8 +26,8 @@
  *   Number of bits:     char:   8    short:   16    int:  32
  *                       long:  32      native word size:  32
  *   Byte ordering: LittleEndian
- *   Signed integer division rounds to: Undefined
- *   Shift right on a signed integer as arithmetic shift: off
+ *   Signed integer division rounds to: Zero
+ *   Shift right on a signed integer as arithmetic shift: on
  *=======================================================================*/
 
 /*=======================================================================* 

apps/examples/kalman_demo/KalmanNav.cpp

@@ -290,7 +290,6 @@ void KalmanNav::updatePublications()
 
 	_att.R_valid = true;
 	_att.q_valid = true;
-	_att.counter = _navFrames;
 
 	// selectively update publications,
 	// do NOT call superblock do-all method

apps/mavlink/mavlink_receiver.c

@@ -511,7 +511,6 @@ handle_message(mavlink_message_t *msg)
 			hil_attitude.yawspeed = hil_state.yawspeed;
 
 			/* set timestamp and notify processes (broadcast) */
-			hil_attitude.counter++;
 			hil_attitude.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
 		}

apps/uORB/topics/vehicle_attitude.h

@@ -57,29 +57,26 @@
  */
 struct vehicle_attitude_s {
 
-	/*
-	 * Actual data, this is specific to the type of data which is stored in this struct
-	 * A line containing L0GME will be added by the Python logging code generator to the
-	 * logged dataset.
-	 */
-	uint64_t timestamp;   /**< in microseconds since system start          */
+	uint64_t timestamp;	/**< in microseconds since system start          */
 
 	/* This is similar to the mavlink message ATTITUDE, but for onboard use */
 
-	/* @warning Roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
+	/** @warning roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
 
-	float roll;           /**< Roll angle (rad, Tait-Bryan, NED)             LOGME    */
-	float pitch;          /**< Pitch angle (rad, Tait-Bryan, NED)            LOGME    */
-	float yaw;            /**< Yaw angle (rad, Tait-Bryan, NED)              LOGME    */
-	float rollspeed;      /**< Roll angular speed (rad/s, Tait-Bryan, NED)   LOGME    */
-	float pitchspeed;     /**< Pitch angular speed (rad/s, Tait-Bryan, NED)  LOGME    */
-	float yawspeed;       /**< Yaw angular speed (rad/s, Tait-Bryan, NED)    LOGME    */
-	float rate_offsets[3];/**< Offsets of the body angular rates from zero */
-	float R[3][3];        /**< Rotation matrix body to world, (Tait-Bryan, NED)       */
-	float q[4];           /**< Quaternion (NED) */
-	bool R_valid;         /**< Rotation matrix valid */
-	bool q_valid;         /**< Quaternion valid */
-	uint16_t counter;     /**< Counter of all attitude messages (wraps)    */
+	float roll;		/**< Roll angle (rad, Tait-Bryan, NED)				*/
+	float pitch;		/**< Pitch angle (rad, Tait-Bryan, NED)				*/
+	float yaw;		/**< Yaw angle (rad, Tait-Bryan, NED)				*/
+	float rollspeed;	/**< Roll angular speed (rad/s, Tait-Bryan, NED)		*/
+	float pitchspeed;	/**< Pitch angular speed (rad/s, Tait-Bryan, NED)		*/
+	float yawspeed;		/**< Yaw angular speed (rad/s, Tait-Bryan, NED)			*/
+	float rollacc;		/**< Roll angular accelration (rad/s, Tait-Bryan, NED)		*/
+	float pitchacc;		/**< Pitch angular acceleration (rad/s, Tait-Bryan, NED)	*/
+	float yawacc;		/**< Yaw angular acceleration (rad/s, Tait-Bryan, NED)		*/
+	float rate_offsets[3];	/**< Offsets of the body angular rates from zero		*/
+	float R[3][3];		/**< Rotation matrix body to world, (Tait-Bryan, NED)		*/
+	float q[4];		/**< Quaternion (NED)						*/
+	bool R_valid;		/**< Rotation matrix valid					*/
+	bool q_valid;		/**< Quaternion valid						*/
 
 };