Function navtk::filtering::first_order_approx_rpy

• Defined in File utils.hpp

Function Documentation

EstimateWithCovariance navtk::filtering::first_order_approx_rpy(const EstimateWithCovariance &ec, std::function<Vector(const Vector&)> &fx)

Transform a joint Gaussian using a numerically derived first-order approximation of a mapping function that produces an Euler angle attitude.

Uses a modified version of the central difference equation to use tilts rather than direct arithmetic of Euler angles. Accuracy will suffer if tilts aren’t small/approximately linear over the perturbation, which is +- 1% of nominal value ec.

Parameters

• ec – First and second moments of an N-state starting distribution.

• fx – A function that takes an N-length observation and maps it to a 3 vector roll, pitch and heading in radians.

Returns

A pair containing estimated Euler angles (typically roll, pitch and heading) and a 3x3 Matrix of reference frame (usually NED) tilt angle covariances.