10 #include "gmock/gmock.h" 11 #include "gtest/gtest.h" 17 simParams.vectornav_imu_quat_noise = 0.f;
18 simParams.vectornav_imu_accelerometer_noise = 0.f;
19 simParams.vectornav_imu_gyro_noise = 0.f;
31 v0 << 0, 0, 0, 0, 0, 0;
61 simParams.vectornav_imu_quat_noise = 0.f;
62 simParams.vectornav_imu_accelerometer_noise = 0.f;
63 simParams.vectornav_imu_gyro_noise = 0.f;
75 velocity << 0, 0, 0, 2, 3, 6;
95 imuLevel << sim_quat[1], sim_quat[2], sim_quat[3], sim_quat[0];
106 simParams.vectornav_imu_quat_noise = 0.f;
107 simParams.vectornav_imu_accelerometer_noise = 0.f;
108 simParams.vectornav_imu_gyro_noise = 0.f;
120 velocity << 1, 2, 3, 0, 0, 0;
138 imuLevel << sim_quat[1], sim_quat[2], sim_quat[3], sim_quat[0];
149 simParams.vectornav_imu_quat_noise = 0.f;
150 simParams.vectornav_imu_accelerometer_noise = 0.f;
151 simParams.vectornav_imu_gyro_noise = 0.f;
163 velocity << 0, 0, -2, 3, 0, 0;
182 imuLevel << sim_quat[1], sim_quat[2], sim_quat[3], sim_quat[0];
197 simParamsNoise.vectornav_imu_quat_noise = 0.005f;
198 simParamsNoise.vectornav_imu_accelerometer_noise = 0.002f;
199 simParamsNoise.vectornav_imu_gyro_noise = 0.005f;
202 simParams.vectornav_imu_quat_noise = 0.f;
203 simParams.vectornav_imu_accelerometer_noise = 0.f;
204 simParams.vectornav_imu_gyro_noise = 0.f;
219 velocity << 2, 1, -2, 3, 3, 2;
234 constexpr
size_t nIterations = 1000;
235 for (
size_t i = 0; i < nIterations; i++) {
240 rpy_avg += rpy / float(nIterations);
Mat3< T > coordinateRotation(CoordinateAxis axis, T theta)
TEST(ImuSimulator, passThrough)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3< T > dBodyPosition
typename Eigen::Matrix< T, 3, 3 > Mat3
typename Eigen::Matrix< T, 4, 1 > Quat
typename Eigen::Matrix< T, 3, 1 > Vec3
typename Eigen::Matrix< T, 6, 1 > SVec
Vec3< float > accelerometer
void updateVectornav(const FBModelState< T > &robotState, const FBModelStateDerivative< T > &robotStateD, VectorNavData *data)
Vec3< typename T::Scalar > quatToRPY(const Eigen::MatrixBase< T > &q)
Quat< typename T::Scalar > rotationMatrixToQuaternion(const Eigen::MatrixBase< T > &r1)
bool almostEqual(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b, T2 tol)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Quat< T > bodyOrientation