diff --git a/src/processor_imu.cpp b/src/processor_imu.cpp index 0e56a1a4b28e9f16f0779a922e9977bbaaf5b0fd..5264143439bce872834fbe4596161afa695ffa3f 100644 --- a/src/processor_imu.cpp +++ b/src/processor_imu.cpp @@ -120,9 +120,7 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco * and return the motion at the given time_stamp ts_. * * DATA: - * Data receives linear interpolation - * a_ret = (ts - t_ref) / dt * a_sec - * w_ret = (ts - t_ref) / dt * w_sec + * Data receives no change * * DELTA: * The delta's position and velocity receive linear interpolation: @@ -145,8 +143,8 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco { // return null motion. Second stays the same. Motion motion_int ( _motion_ref ); - motion_int.data_ . setZero(); - motion_int.data_cov_ . setZero(); + motion_int.data_ = _motion_second.data_; + motion_int.data_cov_ = _motion_second.data_cov_; motion_int.delta_ = deltaZero(); motion_int.delta_cov_ . setZero(); return motion_int; @@ -155,8 +153,6 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco { // return original second motion. Second motion becomes null motion Motion motion_int ( _motion_second ); - _motion_second.data_ . setZero(); - _motion_second.data_cov_ . setZero(); _motion_second.delta_ = deltaZero(); _motion_second.delta_cov_ . setZero(); return motion_int; @@ -195,34 +191,34 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco MatrixXs J_int(delta_cov_size_, delta_cov_size_); // interpolation factor - Scalar dt = _ts - t_ref; - Scalar tau = dt / (t_sec - t_ref); // interpolation factor (0 to 1) + Scalar dt1 = _ts - t_ref; + Scalar dt2 = t_sec - _ts; + Scalar tau = dt1 / (t_sec - t_ref); // interpolation factor (0 to 1) + Scalar tau2 = tau * tau; - // interpolate data - motion_int.data_ = tau * _motion_second.data_; - motion_int.data_cov_ = tau * _motion_second.data_cov_; + // copy data + motion_int.data_ = _motion_second.data_; + motion_int.data_cov_ = _motion_second.data_cov_; // interpolate delta motion_int.ts_ = _ts; Map<VectorXs> motion_int_dp (motion_int.delta_.data() + 0, 3); Map<Quaternions> motion_int_dq (motion_int.delta_.data() + 3 ); Map<VectorXs> motion_int_dv (motion_int.delta_.data() + 7, 3); - motion_int_dp = tau * motion_sec_dp; // FIXME: delta_p not correctly interpolated - motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq); + motion_int_dp = tau2 * motion_sec_dp; // FIXME: delta_p not correctly interpolated motion_int_dv = tau * motion_sec_dv; + motion_int_dq = Quaternions::Identity().slerp(tau, motion_sec_dq); motion_int.delta_cov_ = tau * _motion_second.delta_cov_; // integrate - deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt, motion_int.delta_integr_, J_ref, J_int); + deltaPlusDelta(_motion_ref.delta_integr_, motion_int.delta_, dt1, motion_int.delta_integr_, J_ref, J_int); motion_int.delta_integr_cov_ = J_ref * _motion_ref.delta_integr_cov_ * J_ref.transpose() + J_int * _motion_second.delta_cov_ * J_int.transpose(); // update second delta ( in place update ) - _motion_second.data_ *= (1 - tau); - _motion_second.data_cov_ *= (1 - tau); - motion_sec_dp = motion_int_dq.conjugate() * ((1 - tau) * motion_sec_dp); - motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq; - motion_sec_dv = motion_int_dq.conjugate() * ((1 - tau) * motion_sec_dv); + motion_sec_dp = motion_int_dq.conjugate() * (motion_sec_dp - motion_int_dp - motion_int_dv * dt2); + motion_sec_dv = motion_int_dq.conjugate() * (motion_sec_dv - motion_int_dv); + motion_sec_dq = motion_int_dq.conjugate() * motion_sec_dq; _motion_second.delta_cov_ *= (1 - tau); // easy interpolation // TODO check for correctness //Dp = Dp; // trivial, just leave the code commented //Dq = Dq; // trivial, just leave the code commented