zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Use norm for vector length everywhere

master
Paul Riseborough 8 years ago committed by Roman
parent
6e5a9aabe1
commit
af26ade1ce
1 changed files with 3 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      EKF/estimator_interface.cpp

6
EKF/estimator_interface.cpp
Unescape View File

@ -121,17 +121,17 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u @@ -121,17 +121,17 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
// calculate a metric which indicates the amount of coning vibration
Vector3f temp = cross_product(imu_sample_new.delta_ang , _delta_ang_prev);
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.length();
_vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
// calculate a metric which indiates the amount of high frequency gyro vibration
temp = imu_sample_new.delta_ang - _delta_ang_prev;
_delta_ang_prev = imu_sample_new.delta_ang;
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.length();
_vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
// calculate a metric which indicates the amount of high fequency accelerometer vibration
temp = imu_sample_new.delta_vel - _delta_vel_prev;
_delta_vel_prev = imu_sample_new.delta_vel;
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.length();
_vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
// accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
if (collect_imu(imu_sample_new)) {

Write Preview
Loading…
Cancel
Save