From af26ade1ce758920cb72c027a4930444973f9df4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 28 Nov 2016 15:44:34 +1100 Subject: [PATCH] EKF: Use norm for vector length everywhere --- EKF/estimator_interface.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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)) {