24 using namespace gtsam;
34 static const Bias kZeroBiasHat, kZeroBias;
37 static const Vector3 kNonZeroOmegaCoriolis(0, 0.1, 0.1);
44 auto radians = [](
double t) {
return t *
M_PI / 180; };
45 static const double kGyroSigma = radians(0.5) / 60;
53 : acc(acc), gyro(gyro),
dt(
dt) {}
58 template <
typename PIM>
62 pim->integrateMeasurement(
m.acc,
m.gyro,
m.dt);
68 const double dt = 0.01, pi100 =
M_PI / 100;
71 for (
int i = 1;
i < 100;
i++) {
72 emplace_back(
Vector3(0.05, 0.09, 0.01),
73 Vector3(pi100, pi100 * 3, 2 * pi100),
dt);