@@ -50,25 +50,9 @@ bool MotionModel::predictState(const rclcpp::Time & time)
50
50
return true ;
51
51
}
52
52
53
- // // if dt is too large, shorten dt and repeat prediction
54
- // {
55
- // const uint32_t repeat = std::floor(dt / dt_max_) + 1;
56
- // const double dt_ = dt / repeat;
57
- // for (uint32_t i = 0; i < repeat; ++i) {
58
- // if (!predictStateStep(dt_, ekf_)) {
59
- // return false;
60
- // }
61
- // // add interval to last_update_time_
62
- // last_update_time_ += rclcpp::Duration::from_seconds(dt_);
63
- // }
64
- // std::cout << "MotionModel::predictState predict dt: " << dt << ", dt_: " << dt_ << ",
65
- // repeat:
66
- // "<< repeat<< std::endl;
67
- // }
68
-
69
53
if (!predictStateStep (dt, ekf_)) return false ;
70
54
71
- // update last_update_time_ to the estimation time
55
+ // update last_update_time_
72
56
last_update_time_ = time ;
73
57
return true ;
74
58
}
@@ -88,20 +72,6 @@ bool MotionModel::getPredictedState(
88
72
dt = 0.0 ;
89
73
}
90
74
91
- // // predict only when dt is small enough
92
- // if (1e-6 /*1usec*/ < dt) {
93
- // // if dt is too large, shorten dt and repeat prediction
94
- // const uint32_t repeat = std::floor(dt / dt_max_) + 1;
95
- // const double dt_ = dt / repeat;
96
- // for (uint32_t i = 0; i < repeat; ++i) {
97
- // if (!predictStateStep(dt_, tmp_ekf_for_no_update)) {
98
- // return false;
99
- // }
100
- // }
101
- // std::cout << "MotionModel::getPredictedStateMatrix predict dt: " << dt << ", dt_: " << dt_ <<
102
- // ", repeat: "<< repeat<< std::endl;
103
- // }
104
-
105
75
if (!predictStateStep (dt, tmp_ekf_for_no_update)) return false ;
106
76
107
77
tmp_ekf_for_no_update.getX (X);
0 commit comments