修改所有卡尔曼滤波器中初始化的误差协方差矩阵

cv-rmvl · Mar 29, 2024 · 9c589c2 · 9c589c2
1 parent 97cb924
commit 9c589c2
Show file tree

Hide file tree

Showing 10 changed files with 32 additions and 26 deletions.
diff --git 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::ptr, TrackerState> _tracker_state;

diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);
 }
 
 // 追踪器掉帧处理功能验证