#include <functional>
#include <tuple>
#include <Eigen/QR>
#include "frc/EigenCore.h"
Go to the source code of this file.
|
template<int CovDim, int States> |
std::tuple< Vectord< CovDim >, Matrixd< CovDim, CovDim > > | frc::SquareRootUnscentedTransform (const Matrixd< CovDim, 2 *States+1 > &sigmas, const Vectord< 2 *States+1 > &Wm, const Vectord< 2 *States+1 > &Wc, std::function< Vectord< CovDim >(const Matrixd< CovDim, 2 *States+1 > &, const Vectord< 2 *States+1 > &)> meanFunc, std::function< Vectord< CovDim >(const Vectord< CovDim > &, const Vectord< CovDim > &)> residualFunc, const Matrixd< CovDim, CovDim > &squareRootR) |
| Computes unscented transform of a set of sigma points and weights. More...
|
|