diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h index 316405e50afa3ce2a620766e6c3a910615797536..a98aa732950ff8e0168203aaf8beafad95bbbbe0 100644 --- a/include/imu/math/imu2d_tools.h +++ b/include/imu/math/imu2d_tools.h @@ -183,7 +183,6 @@ inline void compose(const MatrixBase<D1>& d1, Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2( 0 ) ); Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2( 3 ) ); Map<Matrix<typename D3::Scalar, 2, 1> > sum_p ( & sum( 0 ) ); - sum(2) = d1(2) + d2(2); Map<Matrix<typename D3::Scalar, 2, 1> > sum_v ( & sum( 3 ) ); compose(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, sum_p, sum(2), sum_v);