Skip to content

fix: resolve issues 254-256 and sonar findings in MPC, IIR, Matrix #286

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
gabrielfrasantos merged 2 commits into from
Aug 14, 2026
Merged

fix: resolve issues 254-256 and sonar findings in MPC, IIR, Matrix #286

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ee7f1ce
fix: resolve issues 254-256 and sonar findings in MPC, IIR, Matrix
gabrielfrasantos Aug 14, 2026
0a228d4
Apply suggestions from code review
gabrielfrasantos Aug 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 22 additions & 25 deletions numerical/analysis/ConvolutionCorrelation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,32 +1,29 @@
#include "numerical/analysis/ConvolutionCorrelation.hpp"

namespace analysis
{
template void LinearConvolution<float, 3, 3>(
const infra::BoundedVector<float>::WithMaxSize<3>&,
const infra::BoundedVector<float>::WithMaxSize<3>&,
infra::BoundedVector<float>::WithMaxSize<5>&);
template void analysis::LinearConvolution<float, 3, 3>(
const infra::BoundedVector<float>::WithMaxSize<3>&,
const infra::BoundedVector<float>::WithMaxSize<3>&,
infra::BoundedVector<float>::WithMaxSize<5>&);

template void CircularConvolution<float, 4>(
const infra::BoundedVector<float>::WithMaxSize<4>&,
const infra::BoundedVector<float>::WithMaxSize<4>&,
infra::BoundedVector<float>::WithMaxSize<4>&);
template void analysis::CircularConvolution<float, 4>(
const infra::BoundedVector<float>::WithMaxSize<4>&,
const infra::BoundedVector<float>::WithMaxSize<4>&,
infra::BoundedVector<float>::WithMaxSize<4>&);

template void CrossCorrelation<float, 5, 5>(
const infra::BoundedVector<float>::WithMaxSize<5>&,
const infra::BoundedVector<float>::WithMaxSize<5>&,
infra::BoundedVector<float>::WithMaxSize<9>&);
template void analysis::CrossCorrelation<float, 5, 5>(
const infra::BoundedVector<float>::WithMaxSize<5>&,
const infra::BoundedVector<float>::WithMaxSize<5>&,
infra::BoundedVector<float>::WithMaxSize<9>&);

template void AutoCorrelation<float, 4>(
const infra::BoundedVector<float>::WithMaxSize<4>&,
infra::BoundedVector<float>::WithMaxSize<7>&);
template void analysis::AutoCorrelation<float, 4>(
const infra::BoundedVector<float>::WithMaxSize<4>&,
infra::BoundedVector<float>::WithMaxSize<7>&);

template std::size_t ArgMaxLag<float, 9>(
const infra::BoundedVector<float>::WithMaxSize<9>&);
template std::size_t analysis::ArgMaxLag<float, 9>(
const infra::BoundedVector<float>::WithMaxSize<9>&);

template void FastConvolution<float, 3, 3, 8>(
const infra::BoundedVector<float>::WithMaxSize<3>&,
const infra::BoundedVector<float>::WithMaxSize<3>&,
infra::BoundedVector<float>::WithMaxSize<5>&,
FastFourierTransform<float>&);
}
template void analysis::FastConvolution<float, 3, 3, 8>(
const infra::BoundedVector<float>::WithMaxSize<3>&,
const infra::BoundedVector<float>::WithMaxSize<3>&,
infra::BoundedVector<float>::WithMaxSize<5>&,
analysis::FastFourierTransform<float>&);
72 changes: 44 additions & 28 deletions numerical/controllers/implementations/Mpc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ namespace controllers

[[nodiscard]] const HessianMatrix& GetHessian() const;
[[nodiscard]] const GradientMatrix& GetGradientMatrix() const;
[[nodiscard]] const GradientMatrix& GetReferenceGainMatrix() const;

private:
using PredictionStateMatrix = math::Matrix<T, TotalStateDim, StateSize>;
Expand All @@ -84,10 +85,11 @@ namespace controllers
PredictionInputMatrix BuildTheta(const StateMatrix& A, const InputMatrix& B) const;
void BuildCostMatrices(const MpcWeights<T, StateSize, InputSize>& weights,
const StateMatrix& A, const InputMatrix& B);
void ApplyConstraints(ControlVector& u) const;
void ApplyConstraints(ControlVector& u, const ControlVector& negG) const;

HessianMatrix hessian;
GradientMatrix gradientMatrix;
GradientMatrix referenceGainMatrix;
MpcConstraints<T, InputSize> constraints;
ControlSequence controlSequence;
std::optional<StateVector> reference;
Expand Down Expand Up @@ -182,50 +184,57 @@ namespace controllers
rBar.SetBlock(weights.R, k * InputSize, k * InputSize);

auto thetaT = theta.Transpose();
hessian = thetaT * qBar * theta + rBar;
gradientMatrix = thetaT * qBar * psi;
auto thetaTqBar = thetaT * qBar;
hessian = thetaTqBar * theta + rBar;
gradientMatrix = thetaTqBar * psi;

PredictionStateMatrix onesStack;
for (std::size_t k = 0; k < PredictionHorizon; ++k)
for (std::size_t i = 0; i < StateSize; ++i)
onesStack.at(k * StateSize + i, i) = T(1);
referenceGainMatrix = thetaTqBar * onesStack;
}

template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t PredictionHorizon, std::size_t ControlHorizon>
OPTIMIZE_FOR_SPEED void Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::ApplyConstraints(ControlVector& u) const
OPTIMIZE_FOR_SPEED void Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::ApplyConstraints(ControlVector& u, const ControlVector& negG) const
{
if (!constraints.uMin && !constraints.uMax)
return;

if (constraints.uMin && constraints.uMax)
{
for (std::size_t k = 0; k < ControlHorizon; ++k)
for (std::size_t i = 0; i < InputSize; ++i)
{
auto& val = u.at(k * InputSize + i, 0);
val = std::max(std::min(val, constraints.uMax->at(i, 0)), constraints.uMin->at(i, 0));
}
}
else if (constraints.uMin)
{
for (std::size_t k = 0; k < ControlHorizon; ++k)
for (std::size_t i = 0; i < InputSize; ++i)
u.at(k * InputSize + i, 0) = std::max(u.at(k * InputSize + i, 0), constraints.uMin->at(i, 0));
}
else
{
for (std::size_t k = 0; k < ControlHorizon; ++k)
for (std::size_t i = 0; i < InputSize; ++i)
u.at(k * InputSize + i, 0) = std::min(u.at(k * InputSize + i, 0), constraints.uMax->at(i, 0));
}
constexpr std::size_t MaxIter = 20;

for (std::size_t iter = 0; iter < MaxIter; ++iter)
for (std::size_t j = 0; j < TotalControlDim; ++j)
{
T Hju = T{};
for (std::size_t l = 0; l < TotalControlDim; ++l)
Hju += hessian.at(j, l) * u.at(l, 0);

const T uStar = u.at(j, 0) - (Hju - negG.at(j, 0)) / hessian.at(j, j);
const std::size_t i = j % InputSize;

T bounded = uStar;
if (constraints.uMin)
bounded = std::max(bounded, constraints.uMin->at(i, 0));
if (constraints.uMax)
bounded = std::min(bounded, constraints.uMax->at(i, 0));

u.at(j, 0) = bounded;
}
}

template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t PredictionHorizon, std::size_t ControlHorizon>
OPTIMIZE_FOR_SPEED
typename Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::InputVector
Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::ComputeControl(const StateVector& state)
{
auto effectiveState = reference ? state - *reference : state;
auto g = gradientMatrix * effectiveState;
auto g = gradientMatrix * state;
if (reference)
g = g - referenceGainMatrix * (*reference);
auto negG = g * T(-1.0f);

auto uOptimal = solvers::SolveSystem<T, TotalControlDim, 1>(hessian, negG);
ApplyConstraints(uOptimal);
ApplyConstraints(uOptimal, negG);

for (std::size_t k = 0; k < ControlHorizon; ++k)
controlSequence[k] = uOptimal.template GetBlock<InputSize, 1>(k * InputSize, 0);
Expand Down Expand Up @@ -266,6 +275,13 @@ namespace controllers
return gradientMatrix;
}

template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t PredictionHorizon, std::size_t ControlHorizon>
const typename Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::GradientMatrix&
Mpc<T, StateSize, InputSize, PredictionHorizon, ControlHorizon>::GetReferenceGainMatrix() const
{
return referenceGainMatrix;
}

#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
extern template class Mpc<float, 2, 1, 5, 5>;
extern template class Mpc<float, 2, 1, 10, 10>;
Expand Down
43 changes: 43 additions & 0 deletions numerical/controllers/implementations/test/TestMpc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -877,3 +877,46 @@ TEST_F(TestMpc, no_constraints_does_not_clamp_output)

EXPECT_FLOAT_EQ(uUnconstrained.at(0, 0), uDefault.at(0, 0));
}

TEST_F(TestMpc, non_equilibrium_reference_produces_correct_optimal_control)
{
math::SquareMatrix<float, 1> A{ { 0.0f } };
math::Matrix<float, 1, 1> B{ { 1.0f } };

controllers::MpcWeights<float, 1, 1> weights;
weights.Q = math::SquareMatrix<float, 1>{ { 1.0f } };
weights.R = math::SquareMatrix<float, 1>{ { 1.0f } };

controllers::Mpc<float, 1, 1, 1, 1> mpc(A, B, weights);

math::Vector<float, 1> state{ { 0.0f } };
mpc.SetReference(math::Vector<float, 1>{ { 1.0f } });

auto u = mpc.ComputeControl(state);
EXPECT_NEAR(u.at(0, 0), 0.5f, math::Tolerance<float>());
}

TEST_F(TestMpc, coupled_constraints_re_optimize_free_variables)
{
math::SquareMatrix<float, 2> H{
{ 2.0f, 1.0f },
{ 1.0f, 2.0f }
};
math::Matrix<float, 2, 1> F{
{ -4.0f },
{ -2.0f }
};

controllers::MpcConstraints<float, 1> constraints;
constraints.uMax = math::Vector<float, 1>{ 1.0f };

controllers::Mpc<float, 1, 1, 2, 2> mpc(H, F, constraints);

math::Vector<float, 1> state{ { 1.0f } };
auto u = mpc.ComputeControl(state);

EXPECT_NEAR(u.at(0, 0), 1.0f, 1e-4f);

auto seq = mpc.GetControlSequence();
EXPECT_NEAR(seq[1].at(0, 0), 0.5f, 1e-4f);
}
2 changes: 1 addition & 1 deletion numerical/filters/passive/IirFilterDesign.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -309,7 +309,7 @@ namespace filters::passive
for (std::size_t k{ 0 }; k < order; ++k)
{
const T mag2{ protoPoles[k].Real() * protoPoles[k].Real() + protoPoles[k].Imaginary() * protoPoles[k].Imaginary() };
const ComplexT hpPole{ wc / protoPoles[k].Real(), -wc * protoPoles[k].Imaginary() / mag2 };
const ComplexT hpPole{ wc * protoPoles[k].Real() / mag2, -wc * protoPoles[k].Imaginary() / mag2 };
az[k] = BilinearS2Z(hpPole, fs);
bz[k] = { T{ 1 }, T{ 0 } };
}
Expand Down
7 changes: 2 additions & 5 deletions numerical/math/Matrix.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ namespace math
[[nodiscard]] constexpr Matrix<T, Rows, 1> GetColumn(size_type col) const;

private:
std::array<T, Rows * Cols> data;
std::array<T, Rows * Cols> data = {};
};

template<typename T, typename... U>
Expand All @@ -145,13 +145,10 @@ namespace math
}

template<typename T, size_t Rows, size_t Cols>
constexpr Matrix<T, Rows, Cols>::Matrix() noexcept
: data{}
{}
constexpr Matrix<T, Rows, Cols>::Matrix() noexcept = default;

template<typename T, size_t Rows, size_t Cols>
OPTIMIZE_FOR_SPEED constexpr Matrix<T, Rows, Cols>::Matrix(std::initializer_list<std::initializer_list<T>> init)
: data{}
{
size_t row = 0;
for (const auto& row_list : init)
Expand Down
Loading