From 9c589c2431a732613a087d6c4c9b90f9b06600e0 Mon Sep 17 00:00:00 2001 From: zhaoxi <535394140@qq.com> Date: Fri, 29 Mar 2024 14:28:58 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E6=89=80=E6=9C=89=E5=8D=A1?= =?UTF-8?q?=E5=B0=94=E6=9B=BC=E6=BB=A4=E6=B3=A2=E5=99=A8=E4=B8=AD=E5=88=9D?= =?UTF-8?q?=E5=A7=8B=E5=8C=96=E7=9A=84=E8=AF=AF=E5=B7=AE=E5=8D=8F=E6=96=B9?= =?UTF-8?q?=E5=B7=AE=E7=9F=A9=E9=98=B5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- extra/group/include/rmvl/group/gyro_group.h | 6 +++--- extra/group/src/gyro_group/init.cpp | 2 +- extra/tracker/param/gyro_tracker.para | 12 ++++++------ extra/tracker/param/planar_tracker.para | 8 ++++---- extra/tracker/param/rune_tracker.para | 6 ++---- extra/tracker/src/gyro_tracker/filter_update.cpp | 6 +++--- extra/tracker/src/planar_tracker/filter_update.cpp | 4 ++-- extra/tracker/src/rune_tracker/filter_update.cpp | 2 +- extra/tracker/test/gyro_tracker_test.cpp | 6 +++++- extra/tracker/test/planar_tracker_test.cpp | 6 +++++- 10 files changed, 32 insertions(+), 26 deletions(-) diff --git a/extra/group/include/rmvl/group/gyro_group.h b/extra/group/include/rmvl/group/gyro_group.h index cc1166aa..5a5c23c3 100644 --- a/extra/group/include/rmvl/group/gyro_group.h +++ b/extra/group/include/rmvl/group/gyro_group.h @@ -28,9 +28,9 @@ namespace rm //! 整车状态序列组 class GyroGroup final : public group { - double _tick; //!< 时间点 - GyroData _gyro_data; //!< 当前陀螺仪数据 - bool _is_tracked = false; //!< 是否为目标序列组 + double _tick; //!< 时间点 + GyroData _gyro_data; //!< 当前陀螺仪数据 + bool _is_tracked{}; //!< 是否为目标序列组 //! 追踪器状态哈希表 [追踪器 : 追踪器状态] std::unordered_map _tracker_state; diff --git a/extra/group/src/gyro_group/init.cpp b/extra/group/src/gyro_group/init.cpp index 3a85c821..b23e9b88 100644 --- a/extra/group/src/gyro_group/init.cpp +++ b/extra/group/src/gyro_group/init.cpp @@ -75,7 +75,7 @@ void GyroGroup::initFilter() // 初始化旋转中心点位置滤波器 _center3d_filter.setQ(para::gyro_group_param.CENTER3D_Q); _center3d_filter.setR(para::gyro_group_param.CENTER3D_R); - _center3d_filter.init(cv::Matx61f(_center3d(0), _center3d(1), _center3d(2), 0, 0, 0), 1e-2); + _center3d_filter.init({_center3d(0), _center3d(1), _center3d(2), 0, 0, 0}, 1e5f); } } // namespace rm diff --git a/extra/tracker/param/gyro_tracker.para b/extra/tracker/param/gyro_tracker.para index 18e882b9..6e2e2643 100644 --- a/extra/tracker/param/gyro_tracker.para +++ b/extra/tracker/param/gyro_tracker.para @@ -2,9 +2,9 @@ double SAMPLE_INTERVAL = 15 # 采样时间（单位: ms） float MAX_ROTSPEED = 20 # 模型旋转速度上限（单位: rad/s） float MIN_ROTSPEED = 0 # 模型旋转速度下限（单位: rad/s） -Matx44f MOTION_Q = Matx44f::eye() # 目标转角过程噪声协方差矩阵 -Matx22f MOTION_R = Matx22f::diag({0.1, 0.1}) # 目标转角测量噪声协方差矩阵 -Matx66f POSITION_Q = Matx66f::eye() # 位置过程噪声协方差矩阵 -Matx33f POSITION_R = Matx33f::diag({2, 2, 2}) # 位置测量噪声协方差矩阵 -Matx44f POSE_Q = Matx44f::eye() # 姿态过程噪声协方差矩阵 -Matx22f POSE_R = Matx22f::diag({5, 5 20}) # 姿态测量噪声协方差矩阵 +Matx44f MOTION_Q = Matx44f::eye() # 目标转角过程噪声协方差矩阵 +Matx22f MOTION_R = Matx22f::diag({0.01, 0.01}) # 目标转角测量噪声协方差矩阵 +Matx66f POSITION_Q = Matx66f::eye() # 位置过程噪声协方差矩阵 +Matx33f POSITION_R = Matx33f::diag({0.1, 0.1, 0.5}) # 位置测量噪声协方差矩阵 +Matx44f POSE_Q = Matx44f::eye() # 姿态过程噪声协方差矩阵 +Matx22f POSE_R = Matx22f::diag({2, 2}) # 姿态测量噪声协方差矩阵 diff --git a/extra/tracker/param/planar_tracker.para b/extra/tracker/param/planar_tracker.para index fd775a17..b8341773 100644 --- a/extra/tracker/param/planar_tracker.para +++ b/extra/tracker/param/planar_tracker.para @@ -1,8 +1,8 @@ uint32_t TRACK_FRAMES = 4 # 追踪帧数 double SAMPLE_INTERVAL = 15 # 采样时间 -Matx22f DIS_Q = Matx22f::diag({0.1, 0.5}) # 距离状态空间方程，过程噪声协方差 -float DIS_R = 0.1 # 距离状态空间方程，测量噪声协方差 +Matx22f DIS_Q = Matx22f::eye() # 距离状态空间方程，过程噪声协方差 +float DIS_R = 0.1 # 距离状态空间方程，测量噪声协方差 -Matx44f MOTION_Q = Matx44f::eye() # 运动状态空间方程，过程噪声协方差矩阵 -Matx22f MOTION_R = Matx22f::diag({0.1, 0.1}) # 运动状态空间方程，测量噪声协方差矩阵 +Matx44f MOTION_Q = Matx44f::eye() # 运动状态空间方程，过程噪声协方差矩阵 +Matx22f MOTION_R = Matx22f::diag({0.01, 0.01}) # 运动状态空间方程，测量噪声协方差矩阵 diff --git a/extra/tracker/param/rune_tracker.para b/extra/tracker/param/rune_tracker.para index 667ce18b..6c976232 100644 --- a/extra/tracker/param/rune_tracker.para +++ b/extra/tracker/param/rune_tracker.para @@ -1,7 +1,5 @@ uint32_t TRACK_FRAMES = 4 # 追踪帧数 double SAMPLE_INTERVAL = 10 # 采样时间（单位: ms） -Matx22f ROTATE_Q = Matx22f::eye() # 角度滤波器过程噪声协方差矩阵 -float ROTATE_R = 1 # 角度滤波器测量噪声协方差矩阵 -Matx44f MOTION_Q = Matx44f::eye() # 运动滤波器过程噪声协方差矩阵 -Matx44f MOTION_R = Matx44f::diag({1, 1, 4, 4}) # 运动滤波器测量噪声协方差矩阵 +Matx22f ROTATE_Q = Matx22f::eye() # 角度滤波器过程噪声协方差矩阵 +float ROTATE_R = 0.01 # 角度滤波器测量噪声协方差矩阵 diff --git a/extra/tracker/src/gyro_tracker/filter_update.cpp b/extra/tracker/src/gyro_tracker/filter_update.cpp index 28cdf7fc..b55ad32e 100644 --- a/extra/tracker/src/gyro_tracker/filter_update.cpp +++ b/extra/tracker/src/gyro_tracker/filter_update.cpp @@ -25,19 +25,19 @@ void GyroTracker::initFilter() _motion_filter.setQ(para::gyro_tracker_param.MOTION_Q); const auto &relative_angle = first_combo->getRelativeAngle(); cv::Matx41f init_move_vec = {relative_angle.x, relative_angle.y, 0, 0}; - _motion_filter.init(init_move_vec, 1e-2); + _motion_filter.init(init_move_vec, 1e5f); // 初始化位置滤波器 _center3d_filter.setR(para::gyro_tracker_param.POSITION_R); _center3d_filter.setQ(para::gyro_tracker_param.POSITION_Q); const auto &tvec = first_combo->getExtrinsics().tvec(); cv::Matx61f init_position_vec = {tvec(0), tvec(1), tvec(2), 0, 0, 0}; - _center3d_filter.init(init_position_vec, 1e-2); + _center3d_filter.init(init_position_vec, 1e5f); // 初始化姿态滤波器 _pose_filter.setR(para::gyro_tracker_param.POSE_R); _pose_filter.setQ(para::gyro_tracker_param.POSE_Q); const auto &pose = Armor::cast(first_combo)->getPose(); cv::Vec4f init_pose_vec = {pose(0), pose(1), 0, 0}; - _pose_filter.init(init_pose_vec, 1e-2); + _pose_filter.init(init_pose_vec, 1e5f); } void GyroTracker::updateMotionFilter() diff --git a/extra/tracker/src/planar_tracker/filter_update.cpp b/extra/tracker/src/planar_tracker/filter_update.cpp index e1b3c494..02bf9ff6 100644 --- a/extra/tracker/src/planar_tracker/filter_update.cpp +++ b/extra/tracker/src/planar_tracker/filter_update.cpp @@ -23,14 +23,14 @@ void PlanarTracker::initFilter() _distance_filter.setR({para::planar_tracker_param.DIS_R}); _distance_filter.setQ(para::planar_tracker_param.DIS_Q); cv::Matx21f init_dis_vec = {first_combo->getExtrinsics().distance(), 0}; - _distance_filter.init(init_dis_vec, 1e-2); + _distance_filter.init(init_dis_vec, 1e5f); // 初始化运动滤波器 _motion_filter.setR(para::planar_tracker_param.MOTION_R); _motion_filter.setQ(para::planar_tracker_param.MOTION_Q); cv::Matx41f init_move_vec = {first_combo->getRelativeAngle().x, first_combo->getRelativeAngle().y, 0, 0}; - _motion_filter.init(init_move_vec, 1e-2); + _motion_filter.init(init_move_vec, 1e5f); } void PlanarTracker::updateDistanceFilter() diff --git a/extra/tracker/src/rune_tracker/filter_update.cpp b/extra/tracker/src/rune_tracker/filter_update.cpp index 2d54d32c..c550e935 100644 --- a/extra/tracker/src/rune_tracker/filter_update.cpp +++ b/extra/tracker/src/rune_tracker/filter_update.cpp @@ -24,7 +24,7 @@ void RuneTracker::initFilter(float init_angle, float init_speed) // 初始化旋转滤波器 _filter.setR({para::rune_tracker_param.ROTATE_R}); _filter.setQ(para::rune_tracker_param.ROTATE_Q); - _filter.init({init_angle, init_speed}, 1e-2); + _filter.init({init_angle, init_speed}, 1e5f); } void RuneTracker::updateRotateFilter(float t) diff --git a/extra/tracker/test/gyro_tracker_test.cpp b/extra/tracker/test/gyro_tracker_test.cpp index e5663917..56a42587 100644 --- a/extra/tracker/test/gyro_tracker_test.cpp +++ b/extra/tracker/test/gyro_tracker_test.cpp @@ -96,7 +96,11 @@ TEST_F(GyroTrackerTest, tracker_update_with_1_armor) Armor::ptr armor2 = buildArmor(cv::Point(505, 300), 8); p_tracker->update(armor2); EXPECT_EQ(p_tracker->size(), 2); - EXPECT_NE(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); + // Kalman Filter 初始化时，观测值为 armor2，初速度为 0，因此先验估计值为 armor + // 最优估计应该更倾向于 armor2 + float da1 = getDistance(p_tracker->getRelativeAngle(), armor->getRelativeAngle()); + float da2 = getDistance(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); + EXPECT_GE(da1, da2); } } // namespace rm_test diff --git a/extra/tracker/test/planar_tracker_test.cpp b/extra/tracker/test/planar_tracker_test.cpp index 410ce3b0..dc47b408 100644 --- a/extra/tracker/test/planar_tracker_test.cpp +++ b/extra/tracker/test/planar_tracker_test.cpp @@ -94,7 +94,11 @@ TEST_F(PlanarTrackerTest, tracker_update_with_1_armor) rm::Armor::ptr armor2 = buildArmor(cv::Point(505, 300), 8); p_tracker->update(armor2); EXPECT_EQ(p_tracker->size(), 2); - EXPECT_NE(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); + // Kalman Filter 初始化时，观测值为 armor2，初速度为 0，因此先验估计值为 armor + // 最优估计应该更倾向于 armor2 + float da1 = rm::getDistance(p_tracker->getRelativeAngle(), armor->getRelativeAngle()); + float da2 = rm::getDistance(p_tracker->getRelativeAngle(), armor2->getRelativeAngle()); + EXPECT_GE(da1, da2); } // 追踪器掉帧处理功能验证