From cd3d5966a1f6e3ce4834c2f70971f5877ae2f663 Mon Sep 17 00:00:00 2001 From: Beluga Xavier Date: Tue, 12 Apr 2022 15:27:10 +0200 Subject: [PATCH] changed covariance to calculated values from matlab --- src/stim300_driver_node.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/stim300_driver_node.cpp b/src/stim300_driver_node.cpp index 0afea57..d6e50fe 100644 --- a/src/stim300_driver_node.cpp +++ b/src/stim300_driver_node.cpp @@ -83,16 +83,17 @@ int main(int argc, char** argv) node.param("sample_rate", sample_rate, 125); node.param("gravity", gravity, 9.80665); + // These values have been estimated by having beluga in a pool for a couple of minutes, and then calculate the variance for each values sensor_msgs::Imu stim300msg{}; - stim300msg.angular_velocity_covariance[0] = variance_gyro; - stim300msg.angular_velocity_covariance[4] = variance_gyro; - stim300msg.angular_velocity_covariance[8] = variance_gyro; - stim300msg.linear_acceleration_covariance[0] = variance_acc; - stim300msg.linear_acceleration_covariance[4] = variance_acc; - stim300msg.linear_acceleration_covariance[8] = variance_acc; - stim300msg.orientation.x = 0; - stim300msg.orientation.y = 0; - stim300msg.orientation.z = 0; + stim300msg.angular_velocity_covariance[0] = 0.0000027474; + stim300msg.angular_velocity_covariance[4] = 0.0000027474; + stim300msg.angular_velocity_covariance[8] = 0.000007312; + stim300msg.linear_acceleration_covariance[0] = 0.00041915; + stim300msg.linear_acceleration_covariance[4] = 0.00041915; + stim300msg.linear_acceleration_covariance[8] = 0.000018995; + stim300msg.orientation.x = 0.00000024358; + stim300msg.orientation.y = 0.00000024358; + stim300msg.orientation.z = 0.00000024358; stim300msg.header.frame_id = "imu_0"; ros::Publisher imuSensorPublisher = node.advertise("imu/data_raw", 1000);