diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index a426c3b510..b0f7ed92dd 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -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)) {