Function navtk::filtering::second_order_approx

• Defined in File utils.hpp

Function Documentation

EstimateWithCovariance navtk::filtering::second_order_approx(const EstimateWithCovariance &ec, std::function<Vector(const Vector&)> &fx, std::function<Matrix(const Vector&)> jx = 0, std::function<std::vector<Matrix>(const Vector&)> hx = 0)

Transform a joint Gaussian using a second order approximation of an arbitrary mapping function.

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 an M-length result.

• jx – Function that accepts x and returns an MxN Jacobian valid at x. When null, defaults to the calc_numerical_jacobian() function using its default values.

• hx – Function that accepts x and returns an N-length vector of MxN Hessian matrices valid at x. When null, defaults to the calc_numerical_hessians() function using its default values.

Returns

A pair containing an M-length Vector of transformed observations and an MxM Matrix of corresponding covariances.