Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-03 03:01:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Merge .inc files into headers (#7209)

Browse Source 
Splitting the files didn't help readability or save compilation time and
it confused contributors. Merging them is also in line with how C++
modules will be written.
This commit is contained in:
Tyler Veness
2024-10-14 16:08:10 -07:00
committed by GitHub
parent bedfc09268
commit ee281ea448
55 changed files with 1646 additions and 2531 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
View File

@@ -5,10 +5,17 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include <wpi/array.h>
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
namespace frc {
@@ -68,7 +75,38 @@ class ExtendedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector {
return a - b;
};
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
/**
* Constructs an extended Kalman filter.
@@ -96,7 +134,34 @@ class ExtendedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
/**
* Returns the error covariance matrix P.
@@ -159,7 +224,23 @@ class ExtendedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
// Find continuous A
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -202,7 +283,16 @@ class ExtendedKalmanFilter {
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
std::move(addFuncX));
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -228,7 +318,37 @@ class ExtendedKalmanFilter {
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
addFuncX) {
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
private:
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
@@ -246,5 +366,3 @@ class ExtendedKalmanFilter {
};
} // namespace frc
#include "ExtendedKalmanFilter.inc"

171
wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.inc
View File

@@ -1,171 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/ExtendedKalmanFilter.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
namespace frc {
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector { return a - b; };
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
m_h, m_xHat, InputVector::Zero());
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
} else {
m_initP = StateMatrix::Zero();
}
m_P = m_initP;
}
template <int States, int Inputs, int Outputs>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
// Find continuous A
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, dt, &discA, &discQ);
m_xHat = RK4(m_f, m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(residualFuncY),
std::move(addFuncX));
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void ExtendedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
const Matrixd<Rows, States> C =
NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
const Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Matrixd<Rows, Rows> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Rows> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + Kₖ₊₁(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
} // namespace frc

91
wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
View File

@@ -4,11 +4,21 @@
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include <wpi/array.h>
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -59,7 +69,37 @@ class KalmanFilter {
*/
KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs, units::second_t dt);
const OutputArray& measurementStdDevs, units::second_t dt) {
m_plant = &plant;
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
// Find discrete A and Q
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
Reset();
}
/**
* Returns the error covariance matrix P.
@@ -122,7 +162,19 @@ class KalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -144,7 +196,38 @@ class KalmanFilter {
* @param R Continuous measurement noise covariance matrix.
*/
void Correct(const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R);
const Matrixd<Outputs, Outputs>& R) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -163,5 +246,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
KalmanFilter<2, 1, 1>;
} // namespace frc
#include "KalmanFilter.inc"

110
wpimath/src/main/native/include/frc/estimator/KalmanFilter.inc
View File

@@ -1,110 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/KalmanFilter.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "wpimath/MathShared.h"
namespace frc {
template <int States, int Inputs, int Outputs>
KalmanFilter<States, Inputs, Outputs>::KalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
// Find discrete A and Q
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), m_contQ, dt, &discA, &discQ);
Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(m_contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
m_initP =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
Reset();
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Predict(const InputVector& u,
units::second_t dt) {
// Find discrete A and Q
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(m_plant->A(), m_contQ, dt, &discA, &discQ);
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
// Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
m_P = discA * m_P * discA.transpose() + discQ;
m_dt = dt;
}
template <int States, int Inputs, int Outputs>
void KalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const OutputVector& y,
const Matrixd<Outputs, Outputs>& R) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
const Matrixd<Outputs, Outputs> discR = DiscretizeR<Outputs>(R, m_dt);
Matrixd<Outputs, Outputs> S = C * m_P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
Matrixd<States, Outputs> K =
S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += K * (y - (C * m_xHat + D * u));
// Pₖ₊₁⁺ = (I−Kₖ₊₁C)Pₖ₊₁⁻(I−Kₖ₊₁C)ᵀ + Kₖ₊₁RKₖ₊₁ᵀ
// Use Joseph form for numerical stability
m_P = (StateMatrix::Identity() - K * C) * m_P *
(StateMatrix::Identity() - K * C).transpose() +
K * discR * K.transpose();
}
} // namespace frc

206
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -6,7 +6,6 @@
#include <map>
#include <optional>
#include <utility>
#include <vector>
#include <Eigen/Core>
@@ -23,6 +22,7 @@
#include "wpimath/MathShared.h"
namespace frc {
/**
* This class wraps odometry to fuse latency-compensated
* vision measurements with encoder measurements. Robot code should not use this
@@ -59,7 +59,14 @@ class WPILIB_DLLEXPORT PoseEstimator {
PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs);
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
/**
* Sets the pose estimator's trust in vision measurements. This might be used
@@ -72,7 +79,23 @@ class WPILIB_DLLEXPORT PoseEstimator {
* less.
*/
void SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs);
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
/**
* Resets the robot's position on the field.
@@ -85,35 +108,50 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions, const Pose2d& pose);
const WheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
}
/**
* Resets the robot's pose.
*
* @param pose The pose to reset to.
*/
void ResetPose(const Pose2d& pose);
void ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
/**
* Resets the robot's translation.
*
* @param translation The pose to translation to.
*/
void ResetTranslation(const Translation2d& translation);
void ResetTranslation(const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation);
void ResetRotation(const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
m_odometryPoseBuffer.Clear();
}
/**
* Gets the estimated robot pose.
*
* @return The estimated robot pose in meters.
*/
Pose2d GetEstimatedPosition() const;
Pose2d GetEstimatedPosition() const { return m_poseEstimate; }
/**
* Return the pose at a given timestamp, if the buffer is not empty.
@@ -122,7 +160,47 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @return The pose at the given timestamp (or std::nullopt if the buffer is
* empty).
*/
std::optional<Pose2d> SampleAt(units::second_t timestamp) const;
std::optional<Pose2d> SampleAt(units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
}
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only
// information.
if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
return m_odometryPoseBuffer.Sample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to
// sample at.
// First, find the iterator past the sample timestamp, then go back one.
// Note that upper_bound() won't return begin() because we check begin()
// earlier.
auto floorIter = m_visionUpdates.upper_bound(timestamp);
--floorIter;
auto visionUpdate = floorIter->second;
// Step 4: Get the pose measured by odometry at the time of the sample.
auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
// TODO Replace with std::optional::transform() in C++23
if (odometryEstimate) {
return visionUpdate.Compensate(*odometryEstimate);
}
return std::nullopt;
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct
@@ -145,7 +223,63 @@ class WPILIB_DLLEXPORT PoseEstimator {
* frc::Timer::GetFPGATimestamp() as your time source in this case.
*/
void AddVisionMeasurement(const Pose2d& visionRobotPose,
units::second_t timestamp);
units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
m_odometryPoseBuffer.GetInternalBuffer().front().first -
kBufferDuration >
timestamp) {
return;
}
// Step 1: Clean up any old entries
CleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision
// measurement was made.
auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
if (!odometrySample) {
return;
}
// Step 3: Get the vision-compensated pose estimate at the moment the vision
// measurement was made.
auto visionSample = SampleAt(timestamp);
if (!visionSample) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK * Eigen::Vector3d{twist.dx.value(), twist.dy.value(),
twist.dtheta.value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)
auto firstAfter = m_visionUpdates.upper_bound(timestamp);
m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
// Step 9: Update latest pose estimate. Since we cleared all updates after
// this vision update, it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct
@@ -192,7 +326,10 @@ class WPILIB_DLLEXPORT PoseEstimator {
* @return The estimated pose of the robot in meters.
*/
Pose2d Update(const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
/**
* Updates the pose estimator with wheel encoder and gyro information. This
@@ -206,13 +343,53 @@ class WPILIB_DLLEXPORT PoseEstimator {
*/
Pose2d UpdateWithTime(units::second_t currentTime,
const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions);
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
if (m_visionUpdates.empty()) {
m_poseEstimate = odometryEstimate;
} else {
auto visionUpdate = m_visionUpdates.rbegin()->second;
m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
}
return GetEstimatedPosition();
}
private:
/**
* Removes stale vision updates that won't affect sampling.
*/
void CleanUpVisionUpdates();
void CleanUpVisionUpdates() {
// Step 0: If there are no odometry samples, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return;
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
if (m_visionUpdates.empty() ||
oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
return;
}
// Step 3: Find the newest vision update timestamp before or at the oldest
// timestamp.
// First, find the iterator past the oldest odometry timestamp, then go
// back one. Note that upper_bound() won't return begin() because we check
// begin() earlier.
auto newestNeededVisionUpdate =
m_visionUpdates.upper_bound(oldestOdometryTimestamp);
--newestNeededVisionUpdate;
// Step 4: Remove all entries strictly before the newest timestamp we need.
m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
}
struct VisionUpdate {
// The vision-compensated pose estimate
@@ -250,6 +427,5 @@ class WPILIB_DLLEXPORT PoseEstimator {
Pose2d m_poseEstimate;
};
} // namespace frc
#include "frc/estimator/PoseEstimator.inc"
} // namespace frc

246
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -1,246 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "frc/estimator/PoseEstimator.h"
#include "frc/geometry/Pose2d.h"
#include "frc/geometry/Translation2d.h"
namespace frc {
template <typename WheelSpeeds, typename WheelPositions>
PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
Kinematics<WheelSpeeds, WheelPositions>& kinematics,
Odometry<WheelSpeeds, WheelPositions>& odometry,
const wpi::array<double, 3>& stateStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs)
: m_odometry(odometry), m_poseEstimate(m_odometry.GetPose()) {
for (size_t i = 0; i < 3; ++i) {
m_q[i] = stateStdDevs[i] * stateStdDevs[i];
}
SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
const wpi::array<double, 3>& visionMeasurementStdDevs) {
wpi::array<double, 3> r{wpi::empty_array};
for (size_t i = 0; i < 3; ++i) {
r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i];
}
// Solve for closed form Kalman gain for continuous Kalman filter with A = 0
// and C = I. See wpimath/algorithms.md.
for (size_t row = 0; row < 3; ++row) {
if (m_q[row] == 0.0) {
m_visionK(row, row) = 0.0;
} else {
m_visionK(row, row) =
m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row]));
}
}
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_odometry.ResetPosition(gyroAngle, wheelPositions, pose);
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetTranslation(
const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetRotation(
const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {
return m_poseEstimate;
if (m_visionUpdates.empty()) {
return m_odometry.GetPose();
}
auto visionUpdate = m_visionUpdates.rbegin()->second;
return visionUpdate.Compensate(m_odometry.GetPose());
}
template <typename WheelSpeeds, typename WheelPositions>
std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
units::second_t timestamp) const {
// Step 0: If there are no odometry updates to sample, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return std::nullopt;
}
// Step 1: Make sure timestamp matches the sample from the odometry pose
// buffer. (When sampling, the buffer will always use a timestamp
// between the first and last timestamps)
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
units::second_t newestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().back().first;
timestamp =
std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp);
// Step 2: If there are no applicable vision updates, use the odometry-only
// information.
if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) {
return m_odometryPoseBuffer.Sample(timestamp);
}
// Step 3: Get the latest vision update from before or at the timestamp to
// sample at.
// First, find the iterator past the sample timestamp, then go back one. Note
// that upper_bound() won't return begin() because we check begin() earlier.
auto floorIter = m_visionUpdates.upper_bound(timestamp);
--floorIter;
auto visionUpdate = floorIter->second;
// Step 4: Get the pose measured by odometry at the time of the sample.
auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp);
// Step 5: Apply the vision compensation to the odometry pose.
// TODO Replace with std::optional::transform() in C++23
if (odometryEstimate) {
return visionUpdate.Compensate(*odometryEstimate);
}
return std::nullopt;
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::CleanUpVisionUpdates() {
// Step 0: If there are no odometry samples, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty()) {
return;
}
// Step 1: Find the oldest timestamp that needs a vision update.
units::second_t oldestOdometryTimestamp =
m_odometryPoseBuffer.GetInternalBuffer().front().first;
// Step 2: If there are no vision updates before that timestamp, skip.
if (m_visionUpdates.empty() ||
oldestOdometryTimestamp < m_visionUpdates.begin()->first) {
return;
}
// Step 3: Find the newest vision update timestamp before or at the oldest
// timestamp.
// First, find the iterator past the oldest odometry timestamp, then go
// back one. Note that upper_bound() won't return begin() because we check
// begin() earlier.
auto newestNeededVisionUpdate =
m_visionUpdates.upper_bound(oldestOdometryTimestamp);
--newestNeededVisionUpdate;
// Step 4: Remove all entries strictly before the newest timestamp we need.
m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate);
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
const Pose2d& visionRobotPose, units::second_t timestamp) {
// Step 0: If this measurement is old enough to be outside the pose buffer's
// timespan, skip.
if (m_odometryPoseBuffer.GetInternalBuffer().empty() ||
m_odometryPoseBuffer.GetInternalBuffer().front().first - kBufferDuration >
timestamp) {
return;
}
// Step 1: Clean up any old entries
CleanUpVisionUpdates();
// Step 2: Get the pose measured by odometry at the moment the vision
// measurement was made.
auto odometrySample = m_odometryPoseBuffer.Sample(timestamp);
if (!odometrySample) {
return;
}
// Step 3: Get the vision-compensated pose estimate at the moment the vision
// measurement was made.
auto visionSample = SampleAt(timestamp);
if (!visionSample) {
return;
}
// Step 4: Measure the twist between the old pose estimate and the vision
// pose.
auto twist = visionSample.value().Log(visionRobotPose);
// Step 5: We should not trust the twist entirely, so instead we scale this
// twist by a Kalman gain matrix representing how much we trust vision
// measurements compared to our current pose.
Eigen::Vector3d k_times_twist =
m_visionK *
Eigen::Vector3d{twist.dx.value(), twist.dy.value(), twist.dtheta.value()};
// Step 6: Convert back to Twist2d.
Twist2d scaledTwist{units::meter_t{k_times_twist(0)},
units::meter_t{k_times_twist(1)},
units::radian_t{k_times_twist(2)}};
// Step 7: Calculate and record the vision update.
VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample};
m_visionUpdates[timestamp] = visionUpdate;
// Step 8: Remove later vision measurements. (Matches previous behavior)
auto firstAfter = m_visionUpdates.upper_bound(timestamp);
m_visionUpdates.erase(firstAfter, m_visionUpdates.end());
// Step 9: Update latest pose estimate. Since we cleared all updates after
// this vision update, it's guaranteed to be the latest vision update.
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
wheelPositions);
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const WheelPositions& wheelPositions) {
auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions);
m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate);
if (m_visionUpdates.empty()) {
m_poseEstimate = odometryEstimate;
} else {
auto visionUpdate = m_visionUpdates.rbegin()->second;
m_poseEstimate = visionUpdate.Compensate(odometryEstimate);
}
return GetEstimatedPosition();
}
} // namespace frc

71
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.h
View File

@@ -4,12 +4,22 @@
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/DARE.h"
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "frc/system/LinearSystem.h"
#include "units/time.h"
#include "wpimath/MathShared.h"
namespace frc {
@@ -63,7 +73,52 @@ class SteadyStateKalmanFilter {
SteadyStateKalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs,
const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Reset();
}
SteadyStateKalmanFilter(SteadyStateKalmanFilter&&) = default;
SteadyStateKalmanFilter& operator=(SteadyStateKalmanFilter&&) = default;
@@ -119,7 +174,9 @@ class SteadyStateKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -127,7 +184,13 @@ class SteadyStateKalmanFilter {
* @param u Same control input used in the last predict step.
* @param y Measurement vector.
*/
void Correct(const InputVector& u, const OutputVector& y);
void Correct(const InputVector& u, const OutputVector& y) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (C * m_xHat + D * u));
}
private:
LinearSystem<States, Inputs, Outputs>* m_plant;
@@ -149,5 +212,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
SteadyStateKalmanFilter<2, 1, 1>;
} // namespace frc
#include "SteadyStateKalmanFilter.inc"

89
wpimath/src/main/native/include/frc/estimator/SteadyStateKalmanFilter.inc
View File

@@ -1,89 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <cmath>
#include <stdexcept>
#include <string>
#include <Eigen/Cholesky>
#include "frc/DARE.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/SteadyStateKalmanFilter.h"
#include "frc/fmt/Eigen.h"
#include "frc/system/Discretization.h"
#include "wpimath/MathShared.h"
namespace frc {
template <int States, int Inputs, int Outputs>
SteadyStateKalmanFilter<States, Inputs, Outputs>::SteadyStateKalmanFilter(
LinearSystem<States, Inputs, Outputs>& plant,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt) {
m_plant = &plant;
auto contQ = MakeCovMatrix(stateStdDevs);
auto contR = MakeCovMatrix(measurementStdDevs);
Matrixd<States, States> discA;
Matrixd<States, States> discQ;
DiscretizeAQ<States>(plant.A(), contQ, dt, &discA, &discQ);
auto discR = DiscretizeR<Outputs>(contR, dt);
const auto& C = plant.C();
if (!IsDetectable<States, Outputs>(discA, C)) {
std::string msg = fmt::format(
"The system passed to the Kalman filter is undetectable!\n\n"
"A =\n{}\nC =\n{}\n",
discA, C);
wpi::math::MathSharedStore::ReportError(msg);
throw std::invalid_argument(msg);
}
Matrixd<States, States> P =
DARE<States, Outputs>(discA.transpose(), C.transpose(), discQ, discR);
// S = CPCᵀ + R
Matrixd<Outputs, Outputs> S = C * P * C.transpose() + discR;
// We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
// efficiently.
//
// K = PCᵀS⁻¹
// KS = PCᵀ
// (KS)ᵀ = (PCᵀ)ᵀ
// SᵀKᵀ = CPᵀ
//
// The solution of Ax = b can be found via x = A.solve(b).
//
// Kᵀ = Sᵀ.solve(CPᵀ)
// K = (Sᵀ.solve(CPᵀ))ᵀ
m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
Reset();
}
template <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
m_xHat = m_plant->CalculateX(m_xHat, u, dt);
}
template <int States, int Inputs, int Outputs>
void SteadyStateKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const OutputVector& y) {
const auto& C = m_plant->C();
const auto& D = m_plant->D();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
m_xHat += m_K * (y - (C * m_xHat + D * u));
}
} // namespace frc

145
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
View File

@@ -5,12 +5,19 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include <wpi/SymbolExports.h>
#include <wpi/array.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/MerweScaledSigmaPoints.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
#include "units/time.h"
namespace frc {
@@ -74,7 +81,31 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm) -> StateVector {
return sigmas * Wm;
};
m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a - b; };
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector {
return a - b;
};
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
Reset();
}
/**
* Constructs an unscented Kalman filter with custom mean, residual, and
@@ -120,7 +151,20 @@ class UnscentedKalmanFilter {
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX,
units::second_t dt);
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_meanFuncX(std::move(meanFuncX)),
m_meanFuncY(std::move(meanFuncY)),
m_residualFuncX(std::move(residualFuncX)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
}
/**
* Returns the square-root error covariance matrix S.
@@ -197,7 +241,31 @@ class UnscentedKalmanFilter {
* @param u New control input from controller.
* @param dt Timestep for prediction.
*/
void Predict(const InputVector& u, units::second_t dt);
void Predict(const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -242,7 +310,25 @@ class UnscentedKalmanFilter {
void Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R);
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector {
return a - b;
};
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
std::move(residualFuncY), std::move(residualFuncX),
std::move(addFuncX));
}
/**
* Correct the state estimate x-hat using the measurements in y.
@@ -277,7 +363,54 @@ class UnscentedKalmanFilter {
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX);
addFuncX) {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy +=
m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
(residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
.transpose();
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
private:
std::function<StateVector(const StateVector&, const InputVector&)> m_f;
@@ -309,5 +442,3 @@ extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)
UnscentedKalmanFilter<5, 3, 3>;
} // namespace frc
#include "UnscentedKalmanFilter.inc"

196
wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.inc
View File

@@ -1,196 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
#include "frc/StateSpaceUtil.h"
#include "frc/estimator/UnscentedKalmanFilter.h"
#include "frc/estimator/UnscentedTransform.h"
#include "frc/system/Discretization.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/NumericalJacobian.h"
namespace frc {
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](const Matrixd<States, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wm) -> StateVector {
return sigmas * Wm;
};
m_meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> OutputVector {
return sigmas * Wc;
};
m_residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a - b; };
m_residualFuncY = [](const OutputVector& a,
const OutputVector& b) -> OutputVector { return a - b; };
m_addFuncX = [](const StateVector& a, const StateVector& b) -> StateVector {
return a + b;
};
m_dt = dt;
Reset();
}
template <int States, int Inputs, int Outputs>
UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<StateVector(const StateVector&, const InputVector&)> f,
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
std::function<StateVector(const Matrixd<States, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncX,
std::function<OutputVector(const Matrixd<Outputs, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<OutputVector(const OutputVector&, const OutputVector&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(std::move(f)),
m_h(std::move(h)),
m_meanFuncX(std::move(meanFuncX)),
m_meanFuncY(std::move(meanFuncY)),
m_residualFuncX(std::move(residualFuncX)),
m_residualFuncY(std::move(residualFuncY)),
m_addFuncX(std::move(addFuncX)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_dt = dt;
Reset();
}
template <int States, int Inputs, int Outputs>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Predict(
const InputVector& u, units::second_t dt) {
m_dt = dt;
// Discretize Q before projecting mean and covariance forward
StateMatrix contA =
NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
StateMatrix discA;
StateMatrix discQ;
DiscretizeAQ<States>(contA, m_contQ, m_dt, &discA, &discQ);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discQ);
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
StateVector x = sigmas.template block<States, 1>(0, i);
m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
}
auto [xHat, S] = SquareRootUnscentedTransform<States, States>(
m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX,
discQ.template triangularView<Eigen::Lower>());
m_xHat = xHat;
m_S = S;
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R) {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector {
return a - b;
};
auto residualFuncY = [](const Vectord<Rows>& a,
const Vectord<Rows>& b) -> Vectord<Rows> {
return a - b;
};
auto addFuncX = [](const StateVector& a,
const StateVector& b) -> StateVector { return a + b; };
Correct<Rows>(u, y, std::move(h), R, std::move(meanFuncY),
std::move(residualFuncY), std::move(residualFuncX),
std::move(addFuncX));
}
template <int States, int Inputs, int Outputs>
template <int Rows>
void UnscentedKalmanFilter<States, Inputs, Outputs>::Correct(
const InputVector& u, const Vectord<Rows>& y,
std::function<Vectord<Rows>(const StateVector&, const InputVector&)> h,
const Matrixd<Rows, Rows>& R,
std::function<Vectord<Rows>(const Matrixd<Rows, 2 * States + 1>&,
const Vectord<2 * States + 1>&)>
meanFuncY,
std::function<Vectord<Rows>(const Vectord<Rows>&, const Vectord<Rows>&)>
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)>
residualFuncX,
std::function<StateVector(const StateVector&, const StateVector&)>
addFuncX) {
Matrixd<Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
Eigen::internal::llt_inplace<double, Eigen::Lower>::blocked(discR);
// Transform sigma points into measurement space
Matrixd<Rows, 2 * States + 1> sigmasH;
Matrixd<States, 2 * States + 1> sigmas =
m_pts.SquareRootSigmaPoints(m_xHat, m_S);
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
sigmasH.template block<Rows, 1>(0, i) =
h(sigmas.template block<States, 1>(0, i), u);
}
// Mean and covariance of prediction passed through UT
auto [yHat, Sy] = SquareRootUnscentedTransform<Rows, States>(
sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY,
discR.template triangularView<Eigen::Lower>());
// Compute cross covariance of the state and the measurements
Matrixd<States, Rows> Pxy;
Pxy.setZero();
for (int i = 0; i < m_pts.NumSigmas(); ++i) {
// Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
Pxy += m_pts.Wc(i) *
(residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
(residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
.transpose();
}
// K = (P_{xy} / S_yᵀ) / S_y
// K = (S_y \ P_{xy}ᵀ)ᵀ / S_y
// K = (S_yᵀ \ (S_y \ P_{xy}ᵀ))ᵀ
Matrixd<States, Rows> K =
Sy.transpose()
.fullPivHouseholderQr()
.solve(Sy.fullPivHouseholderQr().solve(Pxy.transpose()))
.transpose();
// x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
Matrixd<States, Rows> U = K * Sy;
for (int i = 0; i < Rows; i++) {
Eigen::internal::llt_inplace<double, Eigen::Upper>::rankUpdate(
m_S, U.template block<States, 1>(0, i), -1);
}
}
} // namespace frc