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

Added accel magnitude check, added conversion functions for various standard cases

This commit is contained in:
Lorenz Meier 2012-12-29 16:21:59 +01:00
parent 1fc0a6546e
commit 4976a3a47d
4 changed files with 139 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -65,6 +65,7 @@
#include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h"
@ -254,6 +255,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
float accel_magnitude = CONSTANTS_ONE_G;
unsigned offset_count = 0;
/* register the perf counter */
@ -329,13 +331,34 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_count[1] != raw.accelerometer_counter) {
/*
* Check if assumption of zero acceleration roughly holds.
* If not, do not update the accelerometer, predict only with gyroscopes.
* The violation of the zero acceleration assumption can only hold shortly,
* and predicting the angle with the gyros is safe for
*/
bool acceleration_stationary = true;
/* lowpass accel magnitude to prevent threshold aliasing in presence of vibrations */
accel_magnitude = accel_magnitude * 0.95f + 0.05f * sqrtf(raw.accelerometer_m_s2[0] * raw.accelerometer_m_s2[0]
+ raw.accelerometer_m_s2[1] * raw.accelerometer_m_s2[1]
+ raw.accelerometer_m_s2[2] * raw.accelerometer_m_s2[2]);
/* check if length of gravity vector differs in more than 15 % from expected result */
if (fabsf(accel_magnitude - CONSTANTS_ONE_G) > (CONSTANTS_ONE_G * 0.15f)) {
/* acceleration violates assumptions, disable updates */
acceleration_stationary = false;
}
/* update accelerometer measurements, reject if vector norm ends up being weird */
if (sensor_last_count[1] != raw.accelerometer_counter &&
acceleration_stationary) {
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];

6
apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
View File

@ -65,9 +65,9 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
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, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
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);

89
apps/systemlib/conversions.c
View File

@ -55,3 +55,92 @@ int16_t_from_bytes(uint8_t bytes[])
return u.w;
}
void rot2quat(const float R[9], float Q[4])
{
float q0_2;
float q1_2;
float q2_2;
float q3_2;
int32_t idx;
/* conversion of rotation matrix to quaternion
* choose the largest component to begin with */
q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
idx = 0;
if (q0_2 < q1_2) {
q0_2 = q1_2;
idx = 1;
}
if (q0_2 < q2_2) {
q0_2 = q2_2;
idx = 2;
}
if (q0_2 < q3_2) {
q0_2 = q3_2;
idx = 3;
}
q0_2 = sqrtf(q0_2);
/* solve for the remaining three components */
if (idx == 0) {
q1_2 = q0_2;
q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
} else if (idx == 1) {
q2_2 = q0_2;
q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
} else if (idx == 2) {
q3_2 = q0_2;
q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
} else {
q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
}
/* return values */
Q[0] = q1_2;
Q[1] = q2_2;
Q[2] = q3_2;
Q[3] = q0_2;
}
void quat2rot(const float Q[4], float R[9])
{
float q0_2;
float q1_2;
float q2_2;
float q3_2;
memset(&R[0], 0, 9U * sizeof(float));
q0_2 = Q[0] * Q[0];
q1_2 = Q[1] * Q[1];
q2_2 = Q[2] * Q[2];
q3_2 = Q[3] * Q[3];
R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
}

22
apps/systemlib/conversions.h
View File

@ -44,6 +44,8 @@
#include <float.h>
#include <stdint.h>
#define CONSTANTS_ONE_G 9.80665f
__BEGIN_DECLS
/**
@ -56,6 +58,26 @@ __BEGIN_DECLS
*/
__EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
/**
* Converts a 3 x 3 rotation matrix to an unit quaternion.
*
* All orientations are expressed in NED frame.
*
* @param R rotation matrix to convert
* @param Q quaternion to write back to
*/
__EXPORT void rot2quat(const float R[9], float Q[4]);
/**
* Converts an unit quaternion to a 3 x 3 rotation matrix.
*
* All orientations are expressed in NED frame.
*
* @param Q quaternion to convert
* @param R rotation matrix to write back to
*/
__EXPORT void quat2rot(const float Q[4], float R[9]);
__END_DECLS
#endif /* CONVERSIONS_H_ */