WPILibC++ 2024.3.2
MerweScaledSigmaPoints.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 <cmath>
8
9#include "frc/EigenCore.h"
10
11namespace frc {
12
13/**
14 * Generates sigma points and weights according to Van der Merwe's 2004
15 * dissertation[1] for the UnscentedKalmanFilter class.
16 *
17 * It parametrizes the sigma points using alpha, beta, kappa terms, and is the
18 * version seen in most publications. Unless you know better, this should be
19 * your default choice.
20 *
21 * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
22 * Inference in Dynamic State-Space Models" (Doctoral dissertation)
23 *
24 * @tparam States The dimensionality of the state. 2 * States + 1 weights will
25 * be generated.
26 */
27template <int States>
29 public:
30 /**
31 * Constructs a generator for Van der Merwe scaled sigma points.
32 *
33 * @param alpha Determines the spread of the sigma points around the mean.
34 * Usually a small positive value (1e-3).
35 * @param beta Incorporates prior knowledge of the distribution of the mean.
36 * For Gaussian distributions, beta = 2 is optimal.
37 * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
38 */
39 explicit MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
40 int kappa = 3 - States) {
41 m_alpha = alpha;
42 m_kappa = kappa;
43
44 ComputeWeights(beta);
45 }
46
47 /**
48 * Returns number of sigma points for each variable in the state x.
49 */
50 int NumSigmas() { return 2 * States + 1; }
51
52 /**
53 * Computes the sigma points for an unscented Kalman filter given the mean
54 * (x) and square-root covariance(S) of the filter.
55 *
56 * @param x An array of the means.
57 * @param S Square-root covariance of the filter.
58 *
59 * @return Two dimensional array of sigma points. Each column contains all of
60 * the sigmas for one dimension in the problem space. Ordered by
61 * Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
62 *
63 */
65 const Vectord<States>& x, const Matrixd<States, States>& S) {
66 double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
67 double eta = std::sqrt(lambda + States);
68 Matrixd<States, States> U = eta * S;
69
71 sigmas.template block<States, 1>(0, 0) = x;
72 for (int k = 0; k < States; ++k) {
73 sigmas.template block<States, 1>(0, k + 1) =
74 x + U.template block<States, 1>(0, k);
75 sigmas.template block<States, 1>(0, States + k + 1) =
76 x - U.template block<States, 1>(0, k);
77 }
78
79 return sigmas;
80 }
81
82 /**
83 * Returns the weight for each sigma point for the mean.
84 */
85 const Vectord<2 * States + 1>& Wm() const { return m_Wm; }
86
87 /**
88 * Returns an element of the weight for each sigma point for the mean.
89 *
90 * @param i Element of vector to return.
91 */
92 double Wm(int i) const { return m_Wm(i, 0); }
93
94 /**
95 * Returns the weight for each sigma point for the covariance.
96 */
97 const Vectord<2 * States + 1>& Wc() const { return m_Wc; }
98
99 /**
100 * Returns an element of the weight for each sigma point for the covariance.
101 *
102 * @param i Element of vector to return.
103 */
104 double Wc(int i) const { return m_Wc(i, 0); }
105
106 private:
109 double m_alpha;
110 int m_kappa;
111
112 /**
113 * Computes the weights for the scaled unscented Kalman filter.
114 *
115 * @param beta Incorporates prior knowledge of the distribution of the mean.
116 */
117 void ComputeWeights(double beta) {
118 double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
119
120 double c = 0.5 / (States + lambda);
123
124 m_Wm(0) = lambda / (States + lambda);
125 m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
126 }
127};
128
129} // namespace frc
Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the Unscente...
Definition: MerweScaledSigmaPoints.h:28
int NumSigmas()
Returns number of sigma points for each variable in the state x.
Definition: MerweScaledSigmaPoints.h:50
MerweScaledSigmaPoints(double alpha=1e-3, double beta=2, int kappa=3 - States)
Constructs a generator for Van der Merwe scaled sigma points.
Definition: MerweScaledSigmaPoints.h:39
double Wc(int i) const
Returns an element of the weight for each sigma point for the covariance.
Definition: MerweScaledSigmaPoints.h:104
Matrixd< States, 2 *States+1 > SquareRootSigmaPoints(const Vectord< States > &x, const Matrixd< States, States > &S)
Computes the sigma points for an unscented Kalman filter given the mean (x) and square-root covarianc...
Definition: MerweScaledSigmaPoints.h:64
double Wm(int i) const
Returns an element of the weight for each sigma point for the mean.
Definition: MerweScaledSigmaPoints.h:92
const Vectord< 2 *States+1 > & Wm() const
Returns the weight for each sigma point for the mean.
Definition: MerweScaledSigmaPoints.h:85
const Vectord< 2 *States+1 > & Wc() const
Returns the weight for each sigma point for the covariance.
Definition: MerweScaledSigmaPoints.h:97
auto sqrt(const UnitType &value) noexcept -> unit_t< square_root< typename units::traits::unit_t_traits< UnitType >::unit_type >, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the square root of value
Definition: math.h:483
Definition: AprilTagPoseEstimator.h:15
Eigen::Matrix< double, Rows, Cols, Options, MaxRows, MaxCols > Matrixd
Definition: EigenCore.h:21
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
constexpr common_return_t< T1, T2 > beta(const T1 a, const T2 b) noexcept
Compile-time beta function.
Definition: beta.hpp:42
static constexpr const charge::coulomb_t e(1.6021766208e-19)
elementary charge.
static constexpr const velocity::meters_per_second_t c(299792458.0)
Speed of light in vacuum.
auto pow(const UnitType &value) noexcept -> unit_t< typename units::detail::power_of_unit< power, typename units::traits::unit_t_traits< UnitType >::unit_type >::type, typename units::traits::unit_t_traits< UnitType >::underlying_type, linear_scale >
computes the value of value raised to the power
Definition: base.h:2806
#define S(label, offset, message)
Definition: Errors.h:119