本文整理汇总了C++中KalmanFilter::advance方法的典型用法代码示例。如果您正苦于以下问题：C++ KalmanFilter::advance方法的具体用法？C++ KalmanFilter::advance怎么用？C++ KalmanFilter::advance使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类KalmanFilter的用法示例。
在下文中一共展示了KalmanFilter::advance方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: advance_dt
real_t advance_dt(real_t dt) {
int n_subsims = 8;
KinematicState<real_t> s0 = truth;
KinematicState<real_t> s1;
KinematicState<real_t> b0, b1;
delta_data dat = {rng, dt, base_omega};
// advance the ground truth position/orientation
real_t dt_i = dt / n_subsims;
for (int i = 0; i < n_subsims; i++) {
real_t t_i = t + i / (real_t)n_subsims;
rk4_advance(&s1, 1, t_i, dt_i, &s0, &delta, &macc, &dat, &b0, &b1);
s0 = s1;
}
truth = s0;
index_t n_samples_per_frame = 3;
// only sample velocity occasionally.
if (std::uniform_int_distribution<int>(0,120)(*rng) == 1) n_samples_per_frame++;
Sensor<real_t> *sens;
std::vector< Measurement<real_t> > obs_list(n_samples_per_frame);
// make a few random (fake) observations
// we may not have data from all sensors, but that's OK
for (int i = 0; i < n_samples_per_frame; i++) {
real_t *b = measure_buf + (i * 3);
switch (i) { //(std::uniform_int_distribution<int>(0,3)(*rng)) {
case 0:
sens = &s_mag;
break;
case 1:
sens = &s_acc;
break;
case 2:
sens = &s_gyr;
break;
case 3:
sens = &s_vel;
break;
}
Measurement<real_t> obs;
obs.data = b;
obs.sensor = sens;
sens->simulate(b, truth.begin(), rng);
latest_obs = obs_list[i] = obs;
measure_available = true;
}
// update the filter's estimate, given some jittered sensor readings.
clock_t start = clock();
filter->advance(obs_list.data(), obs_list.size(), t, dt);
clock_t end = clock();
// xxx debug
KinematicState<real_t> &fs = *(KinematicState<real_t>*)filter->x;
fs.orient = fs.orient.unit();
t += dt;
return (end-start) / (real_t)CLOCKS_PER_SEC;
}