WPILibC++ 2024.3.2
UnscentedTransform.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 <tuple>
9
10#include <Eigen/QR>
11
12#include "frc/EigenCore.h"
13
14namespace frc {
15
16/**
17 * Computes unscented transform of a set of sigma points and weights. CovDim
18 * returns the mean and square-root covariance of the sigma points in a tuple.
19 *
20 * This works in conjunction with the UnscentedKalmanFilter class. For use with
21 * square-root form UKFs.
22 *
23 * @tparam CovDim Dimension of covariance of sigma points after passing
24 * through the transform.
25 * @tparam States Number of states.
26 * @param sigmas List of sigma points.
27 * @param Wm Weights for the mean.
28 * @param Wc Weights for the covariance.
29 * @param meanFunc A function that computes the mean of 2 * States + 1 state
30 * vectors using a given set of weights.
31 * @param residualFunc A function that computes the residual of two state
32 * vectors (i.e. it subtracts them.)
33 * @param squareRootR Square-root of the noise covaraince of the sigma points.
34 *
35 * @return Tuple of x, mean of sigma points; S, square-root covariance of
36 * sigmas.
37 */
38template <int CovDim, int States>
39std::tuple<Vectord<CovDim>, Matrixd<CovDim, CovDim>>
45 meanFunc,
46 std::function<Vectord<CovDim>(const Vectord<CovDim>&,
47 const Vectord<CovDim>&)>
48 residualFunc,
49 const Matrixd<CovDim, CovDim>& squareRootR) {
50 // New mean is usually just the sum of the sigmas * weight:
51 // n
52 // dot = Σ W[k] Xᵢ[k]
53 // k=1
54 Vectord<CovDim> x = meanFunc(sigmas, Wm);
55
57 for (int i = 0; i < States * 2; i++) {
58 Sbar.template block<CovDim, 1>(0, i) =
59 std::sqrt(Wc[1]) *
60 residualFunc(sigmas.template block<CovDim, 1>(0, 1 + i), x);
61 }
62 Sbar.template block<CovDim, CovDim>(0, States * 2) = squareRootR;
63
64 // Merwe defines the QR decomposition as Aᵀ = QR
65 Matrixd<CovDim, CovDim> S = Sbar.transpose()
66 .householderQr()
67 .matrixQR()
68 .template block<CovDim, CovDim>(0, 0)
69 .template triangularView<Eigen::Upper>();
70
71 Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
72 S, residualFunc(sigmas.template block<CovDim, 1>(0, 0), x), Wc[0]);
73
74 return std::make_tuple(x, S);
75}
76
77} // namespace frc
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
std::tuple< Vectord< CovDim >, Matrixd< CovDim, CovDim > > 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.
Definition: UnscentedTransform.h:40
Eigen::Vector< double, Size > Vectord
Definition: EigenCore.h:12
#define S(label, offset, message)
Definition: Errors.h:119