diff --git a/config/sim_ekf.yaml b/config/sim_ekf.yaml index 3d16bb477..9d2b75e3c 100644 --- a/config/sim_ekf.yaml +++ b/config/sim_ekf.yaml @@ -5,28 +5,28 @@ global_ekf: # This option controls whether to use that DoP covariance matrix, or to # replace it with the configured gps_covariane matrix below use_gps_dop_covariance: false - gps_covariance: [[2, 0, 0], - [0, 2, 0], - [0, 0, 2]] + gps_covariance: [ [ 0.02, 0, 0 ], + [ 0, 0.02, 0 ], + [ 0, 0, 0.02 ] ] - imu_orientation_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_orientation_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_accel_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_accel_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_gyro_covariance: [[0.1, 0, 0], - [0, 0.1, 0], - [0, 0, 0.1]] + imu_gyro_covariance: [ [ 0.01, 0, 0 ], + [ 0, 0.01, 0 ], + [ 0, 0, 0.01 ] ] - imu_mag_pose_covariance: [[0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0], - [0, 0, 0, 0, 0, 0.1]] + imu_mag_pose_covariance: [ [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0 ], + [ 0, 0, 0, 0, 0, 0.1 ] ] # Hz frequency: 30 @@ -69,11 +69,11 @@ global_ekf: # true, true, true, # false, true, true] - pose0_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] + pose0_config: [ true, true, true, + true, true, true, + false, false, false, + false, false, false, + false, false, false ] # pose1_config: [false, false, false, # false, false, true, @@ -95,40 +95,40 @@ global_ekf: use_control: true # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. - control_config: [true, false, false, false, false, true] - acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] - deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] + control_config: [ true, false, false, false, false, true ] + acceleration_limits: [ 1.3, 0.0, 0.0, 0.0, 0.0, 3.4 ] + deceleration_limits: [ 1.3, 0.0, 0.0, 0.0, 0.0, 4.5 ] # x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] - - initial_estimate_covariance: [0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] + process_noise_covariance: [ 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015 ] + + initial_estimate_covariance: [ 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9 ] diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index ea319a0fe..cf512bac9 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,13 +227,13 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.2}, - mAccelDist{0, 0.05}, - mGyroDist{0, 0.02}, + std::normal_distribution<> mGPSDist{0, 0.02}, + mAccelDist{0, 0.01}, + mGyroDist{0, 0.01}, mMagDist{0, 0.1}, - mRollDist{0, 0.05}, - mPitchDist{0, 0.05}, - mYawDist{0, 0.1}; + mRollDist{0, 0.01}, + mPitchDist{0, 0.01}, + mYawDist{0, 0.01}; PeriodicTask mGpsTask; PeriodicTask mImuTask;