29 AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
43 return [=](
auto a,
auto b) {
62 InputModulus(ret[angleStateIdx], -std::numbers::pi, std::numbers::pi);
76 return [=](
auto a,
auto b) {
return AngleAdd<States>(a, b, angleStateIdx); };
90template <
int CovDim,
int States>
94 double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](
auto it) {
99 double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](
auto it) {
106 ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
119template <
int CovDim,
int States>
123 return [=](
auto sigmas,
auto Wm) {
Vectord< States > AngleAdd(const Vectord< States > &a, const Vectord< States > &b, int angleStateIdx)
Adds a and b while normalizing the resulting value in the selected row as an angle.
Definition AngleStatistics.h:58
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition EigenCore.h:21
WPILIB_DLLEXPORT constexpr units::radian_t AngleModulus(units::radian_t angle)
Wraps an angle to the range -π to π radians (-180 to 180 degrees).
Definition MathUtil.h:167
Eigen::Vector< double, Size > Vectord
Definition EigenCore.h:12
Vectord< States > AngleResidual(const Vectord< States > &a, const Vectord< States > &b, int angleStateIdx)
Subtracts a and b while normalizing the resulting value in the selected row as if it were an angle.
Definition AngleStatistics.h:25
constexpr T InputModulus(T input, T minimumInput, T maximumInput)
Returns modulus of input.
Definition MathUtil.h:95
Vectord< CovDim > AngleMean(const Matrixd< CovDim, 2 *States+1 > &sigmas, const Vectord< 2 *States+1 > &Wm, int angleStatesIdx)
Computes the mean of sigmas with the weights Wm while computing a special angle mean for a select row...
Definition AngleStatistics.h:91