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

Reverting EKF change, as it did not really help.

This commit is contained in:
Lorenz Meier 2013-01-06 00:58:35 +01:00
parent 805c08815e
commit aaa9af2293
1 changed files with 2 additions and 25 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,7 +65,6 @@
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/conversions.h>
#include "codegen/attitudeKalmanfilter_initialize.h" #include "codegen/attitudeKalmanfilter_initialize.h"
#include "codegen/attitudeKalmanfilter.h" #include "codegen/attitudeKalmanfilter.h"
@ -255,7 +254,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
bool initialized = false; bool initialized = false;
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
float accel_magnitude = CONSTANTS_ONE_G;
unsigned offset_count = 0; unsigned offset_count = 0;
/* register the perf counter */ /* register the perf counter */
@ -331,34 +329,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1]; z_k[1] = raw.gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; z_k[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
/* /* update accelerometer measurements */
* Check if assumption of zero acceleration roughly holds. if (sensor_last_count[1] != raw.accelerometer_counter) {
* 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; update_vect[1] = 1;
sensor_last_count[1] = raw.accelerometer_counter; sensor_last_count[1] = raw.accelerometer_counter;
sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
sensor_last_timestamp[1] = raw.timestamp; sensor_last_timestamp[1] = raw.timestamp;
} }
z_k[3] = raw.accelerometer_m_s2[0]; z_k[3] = raw.accelerometer_m_s2[0];
z_k[4] = raw.accelerometer_m_s2[1]; z_k[4] = raw.accelerometer_m_s2[1];
z_k[5] = raw.accelerometer_m_s2[2]; z_k[5] = raw.accelerometer_m_s2[2];