From a98e11f67eec31dc8b7422f187c4caba05fcdf9b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 30 May 2026 17:58:43 +0900 Subject: [PATCH] =?UTF-8?q?fix(crane=5Fworld=5Fmodel=5Fpublisher):=20?= =?UTF-8?q?=E3=82=B0=E3=83=AD=E3=83=BC=E3=83=90=E3=83=AB=E6=B8=9B=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E6=9C=80=E9=81=A9=E5=8C=96=E3=81=AE=E5=88=9D=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E6=8E=A8=E5=AE=9A=E3=82=92=E5=88=B6=E7=B4=84=E4=BB=98?= =?UTF-8?q?=E3=81=8D=E5=9B=9E=E5=B8=B0=E3=81=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../simple_ball_physics_optimizer.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/crane_world_model_publisher/src/calibration/simple_ball_physics_optimizer.cpp b/crane_world_model_publisher/src/calibration/simple_ball_physics_optimizer.cpp index 6ee692a2c..3675c9f77 100644 --- a/crane_world_model_publisher/src/calibration/simple_ball_physics_optimizer.cpp +++ b/crane_world_model_publisher/src/calibration/simple_ball_physics_optimizer.cpp @@ -328,18 +328,15 @@ auto SimpleBallPhysicsOptimizer::optimizeGlobalDeceleration( if (trajectory.time_points.size() < config_.min_data_points_per_trajectory) continue; // 固定減速度を仮定した初速度推定（v(t) = v0 - decel * t） - // 線形回帰で v0 を求める: velocities = v0 - decel * time_points - std::vector expected_velocities; + // 傾きを -decel に固定したモデルの最適 v0 は残差 (v + decel * t) の平均で与えられる。 + // 自由回帰の切片を使うと傾きが 1 に固定されず v0 が候補 decel と無関係な定数になり、 + // 各 decel 候補の RMSE 評価が歪んでグローバル減速度選定がバイアスするため、 + // 制約付き（固定傾き）推定 v0 = mean(v + decel * t) を用いる。 + double v0_sum = 0.0; for (size_t i = 0; i < trajectory.time_points.size(); ++i) { - expected_velocities.push_back(-decel * trajectory.time_points[i]); // -decel * t の部分 + v0_sum += trajectory.velocities[i] + decel * trajectory.time_points[i]; } - - // velocities - expected_velocities = v0 (定数) を線形回帰で求める - auto [slope, intercept, r_squared] = - performLinearRegression(expected_velocities, trajectory.velocities); - - // slope は 1 に近く、intercept が推定初速度 v0 - double estimated_v0 = intercept; + double estimated_v0 = v0_sum / trajectory.time_points.size(); // 固定減速度モデルでの予測誤差を計算 double rmse = 0.0;