7 template <
typename Scalar,
size_t N>
16 return point(0)*point(0)
17 + 2.f * point(0) * point(1)
18 + 3.f * point(1) * point(1)
22 template <
typename Scalar>
30 return positionMeasurement - (positionState + bodyOrientation.
conjugate(velocityStateBody) *
dt);
235 floatPoint_plusDX(0) += h;
236 float floatResult_plusDX =
testFunction(floatPoint_plusDX);
239 floatPoint_plusDY(1) += h;
240 float floatResult_plusDY =
testFunction(floatPoint_plusDY);
242 Vector2f numerical_derivative((floatResult_plusDX - floatResult)/h,
243 (floatResult_plusDY - floatResult)/h);
269 for (
size_t i = 0; i < 4; i++)
286 Vector3d4 positionState(D4(5), D4(6), D4(7));
287 Vector3d4 velocityState(D4(-1), D4(0), D4(1));
291 Quaternion<D4> velocityOrientation(D4(0.2
f, 0),D4(-0.1
f, 1),D4(0, 2),D4(1, 3));
293 Vector3d4 positionMeasurement(D4(4.5
f), D4(6.2
f), D4(7.9
f));
306 TEST(
isEqual(numerical_jacobian, auto_jacobian, 1e-3
f));
Dual< Scalar, N > acos(const Dual< Scalar, N > &a)
Dual< Scalar, N > atan(const Dual< Scalar, N > &a)
Dual< Scalar, N > ceil(const Dual< Scalar, N > &a)
Vector3< Type > conjugate(const Vector3< Type > &vec) const
Rotates vector v_1 in frame 1 to vector v_2 in frame 2 using the rotation quaternion q_21 describing ...
Dual< Scalar, N > asin(const Dual< Scalar, N > &a)
const Slice< Type, M, 1, M, N > col(size_t j) const
Vector< Scalar, N > derivative
Dual< Scalar, N > cos(const Dual< Scalar, N > &a)
Matrix< Scalar, M, N > collectReals(const Matrix< Dual< Scalar, D >, M, N > &input)
bool isEqual(const Matrix< Type, M, N > &x, const Matrix< Type, M, N > &y, const Type eps=1e-4f)
T testFunction(const Vector< T, 3 > &point)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Dual< Scalar, N > min(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
Dual< Scalar, N > tan(const Dual< Scalar, N > &a)
Vector< Scalar, 3 > positionError(const Vector< Scalar, 3 > &positionState, const Vector< Scalar, 3 > &velocityStateBody, const Quaternion< Scalar > &bodyOrientation, const Vector< Scalar, 3 > &positionMeasurement, Scalar dt)
Dual< Scalar, N > fmod(const Dual< Scalar, N > &a, Scalar mod)
Dual< Scalar, N > max(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)
bool isEqualAll(Dual< Scalar, N > a, Dual< Scalar, N > b)
bool isEqualF(const Type x, const Type y, const Type eps=1e-4f)
Compare if two floating point numbers are equal.
Dual< Scalar, N > abs(const Dual< Scalar, N > &a)
Dual< Scalar, N > sin(const Dual< Scalar, N > &a)
Dual< Scalar, N > floor(const Dual< Scalar, N > &a)
Matrix< Scalar, M, N > collectDerivatives(const Matrix< Dual< Scalar, N >, M, 1 > &input)
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
Dual< Scalar, N > atan2(const Dual< Scalar, N > &a, const Dual< Scalar, N > &b)