dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Better attitude filter, not sensitive to sudden accelerations

This commit is contained in:
Lorenz Meier 2013-01-21 17:30:53 +01:00
parent 48e497e406
commit ac215185a9
32 changed files with 679 additions and 360 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
apps/attitude_estimator_ekf/Makefile Normal file → Executable file
View File

@ -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

4
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c Normal file → Executable file
View File

@ -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));

40
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c Normal file → Executable file
View File

@ -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));

4
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h Normal file → Executable file
View File

@ -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;
};

898
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c Normal file → Executable file
View File

File diff suppressed because it is too large Load Diff

4
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h Normal file → Executable file
View File

@ -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) */

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/eye.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/eye.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rdivide.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rdivide.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_defines.h Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c Normal file → Executable file
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h Normal file → Executable file
View File

@ -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
*
*/

6
apps/attitude_estimator_ekf/codegen/rtwtypes.h Normal file → Executable file
View File

@ -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
*=======================================================================*/
/*=======================================================================*

1
apps/examples/kalman_demo/KalmanNav.cpp
View File

@ -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

1
apps/mavlink/mavlink_receiver.c
View File

@ -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);
}

35
apps/uORB/topics/vehicle_attitude.h Normal file → Executable file
View File

@ -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 */
};