WPILibC++ 2024.3.2
AngleStatistics.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <functional>
8#include <numbers>
9
10#include "frc/EigenCore.h"
11#include "frc/MathUtil.h"
12
13namespace frc {
14
15/**
16 * Subtracts a and b while normalizing the resulting value in the selected row
17 * as if it were an angle.
18 *
19 * @tparam States Number of states.
20 * @param a A vector to subtract from.
21 * @param b A vector to subtract with.
22 * @param angleStateIdx The row containing angles to be normalized.
23 */
24template <int States>
26 const Vectord<States>& b, int angleStateIdx) {
27 Vectord<States> ret = a - b;
28 ret[angleStateIdx] =
30 return ret;
31}
32
33/**
34 * Returns a function that subtracts two vectors while normalizing the resulting
35 * value in the selected row as if it were an angle.
36 *
37 * @tparam States Number of states.
38 * @param angleStateIdx The row containing angles to be normalized.
39 */
40template <int States>
41std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
42AngleResidual(int angleStateIdx) {
43 return [=](auto a, auto b) {
44 return AngleResidual<States>(a, b, angleStateIdx);
45 };
46}
47
48/**
49 * Adds a and b while normalizing the resulting value in the selected row as an
50 * angle.
51 *
52 * @tparam States Number of states.
53 * @param a A vector to add with.
54 * @param b A vector to add with.
55 * @param angleStateIdx The row containing angles to be normalized.
56 */
57template <int States>
59 int angleStateIdx) {
60 Vectord<States> ret = a + b;
61 ret[angleStateIdx] =
62 InputModulus(ret[angleStateIdx], -std::numbers::pi, std::numbers::pi);
63 return ret;
64}
65
66/**
67 * Returns a function that adds two vectors while normalizing the resulting
68 * value in the selected row as an angle.
69 *
70 * @tparam States Number of states.
71 * @param angleStateIdx The row containing angles to be normalized.
72 */
73template <int States>
74std::function<Vectord<States>(const Vectord<States>&, const Vectord<States>&)>
76 return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
77}
78
79/**
80 * Computes the mean of sigmas with the weights Wm while computing a special
81 * angle mean for a select row.
82 *
83 * @tparam CovDim Dimension of covariance of sigma points after passing through
84 * the transform.
85 * @tparam States Number of states.
86 * @param sigmas Sigma points.
87 * @param Wm Weights for the mean.
88 * @param angleStatesIdx The row containing the angles.
89 */
90template <int CovDim, int States>
93 int angleStatesIdx) {
94 double sumSin = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
95 return std::sin(it);
96 }) *
97 Wm)
98 .sum();
99 double sumCos = (sigmas.row(angleStatesIdx).unaryExpr([](auto it) {
100 return std::cos(it);
101 }) *
102 Wm)
103 .sum();
104
105 Vectord<CovDim> ret = sigmas * Wm;
106 ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
107 return ret;
108}
109
110/**
111 * Returns a function that computes the mean of sigmas with the weights Wm while
112 * computing a special angle mean for a select row.
113 *
114 * @tparam CovDim Dimension of covariance of sigma points after passing through
115 * the transform.
116 * @tparam States Number of states.
117 * @param angleStateIdx The row containing the angles.
118 */
119template <int CovDim, int States>
120std::function<Vectord<CovDim>(const Matrixd<CovDim, 2 * States + 1>&,
121 const Vectord<2 * States + 1>&)>
122AngleMean(int angleStateIdx) {
123 return [=](auto sigmas, auto Wm) {
124 return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
125 };
126}
127
128} // namespace frc
angle::radian_t atan2(const Y y, const X x) noexcept
Compute arc tangent with two parameters.
Definition: math.h:179
dimensionless::scalar_t cos(const AngleUnit angle) noexcept
Compute cosine.
Definition: math.h:61
dimensionless::scalar_t sin(const AngleUnit angle) noexcept
Compute sine.
Definition: math.h:81
Definition: AprilTagPoseEstimator.h:15
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
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
Definition: MathUtil.h:166
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:94
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
static constexpr const unit_t< PI > pi(1)
Ratio of a circle's circumference to its diameter.
b
Definition: data.h:44