Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix Eigen maybe-uninitialized warnings (#6636)

Browse Source
This commit is contained in:
Tyler Veness
2024-05-20 16:53:22 -07:00
committed by GitHub
parent 58751338ec
commit 0c822b45ab
3 changed files with 57 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -236,7 +236,7 @@ class ExtendedKalmanFilter {
std::function<OutputVector(const OutputVector&, const OutputVector&)>
m_residualFuncY;
std::function<StateVector(const StateVector&, const StateVector&)> m_addFuncX;
StateVector m_xHat;
StateVector m_xHat = StateVector::Zero();
StateMatrix m_P;
StateMatrix m_contQ;
Matrixd<Outputs, Outputs> m_contR;

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

@@ -5,6 +5,7 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
@@ -23,15 +24,16 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto 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();
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
@@ -61,13 +63,14 @@ ExtendedKalmanFilter<States, Inputs, Outputs>::ExtendedKalmanFilter(
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
: 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;
Reset();
StateMatrix contA = NumericalJacobianX<States, States, Inputs>(
m_f, m_xHat, InputVector::Zero());
Matrixd<Outputs, States> C = NumericalJacobianX<Outputs, States, Inputs>(
@@ -114,9 +117,14 @@ 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 = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
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>

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

@@ -5,6 +5,7 @@
#pragma once
#include <functional>
#include <utility>
#include <Eigen/Cholesky>
@@ -23,16 +24,24 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
std::function<OutputVector(const StateVector&, const InputVector&)> h,
const StateArray& stateStdDevs, const OutputArray& measurementStdDevs,
units::second_t dt)
: m_f(f), m_h(h) {
: m_f(std::move(f)), m_h(std::move(h)) {
m_contQ = MakeCovMatrix(stateStdDevs);
m_contR = MakeCovMatrix(measurementStdDevs);
m_meanFuncX = [](auto sigmas, auto Wm) -> StateVector { return sigmas * Wm; };
m_meanFuncY = [](auto sigmas, auto Wc) -> OutputVector {
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 = [](auto a, auto b) -> StateVector { return a - b; };
m_residualFuncY = [](auto a, auto b) -> OutputVector { return a - b; };
m_addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
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();
@@ -55,13 +64,13 @@ UnscentedKalmanFilter<States, Inputs, Outputs>::UnscentedKalmanFilter(
residualFuncY,
std::function<StateVector(const StateVector&, const StateVector&)> addFuncX,
units::second_t dt)
: m_f(f),
m_h(h),
m_meanFuncX(meanFuncX),
m_meanFuncY(meanFuncY),
m_residualFuncX(residualFuncX),
m_residualFuncY(residualFuncY),
m_addFuncX(addFuncX) {
: 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;
@@ -103,13 +112,23 @@ 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 = [](auto sigmas, auto Wc) -> Vectord<Rows> {
auto meanFuncY = [](const Matrixd<Outputs, 2 * States + 1>& sigmas,
const Vectord<2 * States + 1>& Wc) -> Vectord<Rows> {
return sigmas * Wc;
};
auto residualFuncX = [](auto a, auto b) -> StateVector { return a - b; };
auto residualFuncY = [](auto a, auto b) -> Vectord<Rows> { return a - b; };
auto addFuncX = [](auto a, auto b) -> StateVector { return a + b; };
Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
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>