From ee7f1ce8b67908f9bb421d05833dcae14f904612 Mon Sep 17 00:00:00 2001 From: gfs Date: Fri, 14 Aug 2026 19:46:01 +0000 Subject: [PATCH 1/2] fix: resolve issues 254-256 and sonar findings in MPC, IIR, Matrix MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - #256: IirFilterDesign::HighPassRoots real part now divides by mag2 instead of p.Real(), matching the s→ωc/s transform - #255: MPC reference tracking precomputes referenceGainMatrix (θᵀQ̄·ones_stack) so non-equilibrium references produce the correct unconstrained QP gradient - #254: ApplyConstraints replaced with a coordinate-descent box-QP (20-iter Gauss-Seidel) that re-optimises coupled variables after constraint activation; adds two regression tests covering both fixes - Sonar: ConvolutionCorrelation.cpp drops the namespace wrapper and uses fully-qualified analysis:: names on explicit instantiations - Sonar: Matrix::data gains an in-class = {} initialiser; redundant : data{} dropped from constructors Co-Authored-By: Claude Sonnet 4.6 --- numerical/analysis/ConvolutionCorrelation.cpp | 47 ++++++------ numerical/controllers/implementations/Mpc.hpp | 72 +++++++++++-------- .../implementations/test/TestMpc.cpp | 43 +++++++++++ numerical/filters/passive/IirFilterDesign.hpp | 2 +- numerical/math/Matrix.hpp | 4 +- 5 files changed, 111 insertions(+), 57 deletions(-) diff --git a/numerical/analysis/ConvolutionCorrelation.cpp b/numerical/analysis/ConvolutionCorrelation.cpp index a41f5e79..95d83db6 100644 --- a/numerical/analysis/ConvolutionCorrelation.cpp +++ b/numerical/analysis/ConvolutionCorrelation.cpp @@ -1,32 +1,29 @@ #include "numerical/analysis/ConvolutionCorrelation.hpp" -namespace analysis -{ - template void LinearConvolution( - const infra::BoundedVector::WithMaxSize<3>&, - const infra::BoundedVector::WithMaxSize<3>&, - infra::BoundedVector::WithMaxSize<5>&); +template void analysis::LinearConvolution( + const infra::BoundedVector::WithMaxSize<3>&, + const infra::BoundedVector::WithMaxSize<3>&, + infra::BoundedVector::WithMaxSize<5>&); - template void CircularConvolution( - const infra::BoundedVector::WithMaxSize<4>&, - const infra::BoundedVector::WithMaxSize<4>&, - infra::BoundedVector::WithMaxSize<4>&); +template void analysis::CircularConvolution( + const infra::BoundedVector::WithMaxSize<4>&, + const infra::BoundedVector::WithMaxSize<4>&, + infra::BoundedVector::WithMaxSize<4>&); - template void CrossCorrelation( - const infra::BoundedVector::WithMaxSize<5>&, - const infra::BoundedVector::WithMaxSize<5>&, - infra::BoundedVector::WithMaxSize<9>&); +template void analysis::CrossCorrelation( + const infra::BoundedVector::WithMaxSize<5>&, + const infra::BoundedVector::WithMaxSize<5>&, + infra::BoundedVector::WithMaxSize<9>&); - template void AutoCorrelation( - const infra::BoundedVector::WithMaxSize<4>&, - infra::BoundedVector::WithMaxSize<7>&); +template void analysis::AutoCorrelation( + const infra::BoundedVector::WithMaxSize<4>&, + infra::BoundedVector::WithMaxSize<7>&); - template std::size_t ArgMaxLag( - const infra::BoundedVector::WithMaxSize<9>&); +template std::size_t analysis::ArgMaxLag( + const infra::BoundedVector::WithMaxSize<9>&); - template void FastConvolution( - const infra::BoundedVector::WithMaxSize<3>&, - const infra::BoundedVector::WithMaxSize<3>&, - infra::BoundedVector::WithMaxSize<5>&, - FastFourierTransform&); -} +template void analysis::FastConvolution( + const infra::BoundedVector::WithMaxSize<3>&, + const infra::BoundedVector::WithMaxSize<3>&, + infra::BoundedVector::WithMaxSize<5>&, + analysis::FastFourierTransform&); diff --git a/numerical/controllers/implementations/Mpc.hpp b/numerical/controllers/implementations/Mpc.hpp index 5bbab7c7..5c3d941d 100644 --- a/numerical/controllers/implementations/Mpc.hpp +++ b/numerical/controllers/implementations/Mpc.hpp @@ -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; @@ -84,10 +85,11 @@ namespace controllers PredictionInputMatrix BuildTheta(const StateMatrix& A, const InputMatrix& B) const; void BuildCostMatrices(const MpcWeights& 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 constraints; ControlSequence controlSequence; std::optional reference; @@ -182,37 +184,43 @@ 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 - OPTIMIZE_FOR_SPEED void Mpc::ApplyConstraints(ControlVector& u) const + OPTIMIZE_FOR_SPEED void Mpc::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 @@ -220,12 +228,13 @@ namespace controllers typename Mpc::InputVector Mpc::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(hessian, negG); - ApplyConstraints(uOptimal); + ApplyConstraints(uOptimal, negG); for (std::size_t k = 0; k < ControlHorizon; ++k) controlSequence[k] = uOptimal.template GetBlock(k * InputSize, 0); @@ -266,6 +275,13 @@ namespace controllers return gradientMatrix; } + template + const typename Mpc::GradientMatrix& + Mpc::GetReferenceGainMatrix() const + { + return referenceGainMatrix; + } + #ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD extern template class Mpc; extern template class Mpc; diff --git a/numerical/controllers/implementations/test/TestMpc.cpp b/numerical/controllers/implementations/test/TestMpc.cpp index 6b360acb..1317f34e 100644 --- a/numerical/controllers/implementations/test/TestMpc.cpp +++ b/numerical/controllers/implementations/test/TestMpc.cpp @@ -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 A{ { 0.0f } }; + math::Matrix B{ { 1.0f } }; + + controllers::MpcWeights weights; + weights.Q = math::SquareMatrix{ { 1.0f } }; + weights.R = math::SquareMatrix{ { 1.0f } }; + + controllers::Mpc mpc(A, B, weights); + + math::Vector state{ { 0.0f } }; + mpc.SetReference(math::Vector{ { 1.0f } }); + + auto u = mpc.ComputeControl(state); + EXPECT_NEAR(u.at(0, 0), 0.5f, math::Tolerance()); +} + +TEST_F(TestMpc, coupled_constraints_re_optimize_free_variables) +{ + math::SquareMatrix H{ + { 2.0f, 1.0f }, + { 1.0f, 2.0f } + }; + math::Matrix F{ + { -4.0f }, + { -2.0f } + }; + + controllers::MpcConstraints constraints; + constraints.uMax = math::Vector{ 1.0f }; + + controllers::Mpc mpc(H, F, constraints); + + math::Vector 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); +} diff --git a/numerical/filters/passive/IirFilterDesign.hpp b/numerical/filters/passive/IirFilterDesign.hpp index 055a0d58..23a43cec 100644 --- a/numerical/filters/passive/IirFilterDesign.hpp +++ b/numerical/filters/passive/IirFilterDesign.hpp @@ -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 } }; } diff --git a/numerical/math/Matrix.hpp b/numerical/math/Matrix.hpp index e2ecb6ae..b787b92e 100644 --- a/numerical/math/Matrix.hpp +++ b/numerical/math/Matrix.hpp @@ -125,7 +125,7 @@ namespace math [[nodiscard]] constexpr Matrix GetColumn(size_type col) const; private: - std::array data; + std::array data = {}; }; template @@ -146,12 +146,10 @@ namespace math template constexpr Matrix::Matrix() noexcept - : data{} {} template OPTIMIZE_FOR_SPEED constexpr Matrix::Matrix(std::initializer_list> init) - : data{} { size_t row = 0; for (const auto& row_list : init) From 0a228d4e3b8b8758cbc7c5fe20635333441584db Mon Sep 17 00:00:00 2001 From: gfs Date: Fri, 14 Aug 2026 21:55:21 +0200 Subject: [PATCH 2/2] Apply suggestions from code review --- numerical/math/Matrix.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/numerical/math/Matrix.hpp b/numerical/math/Matrix.hpp index b787b92e..4eb95101 100644 --- a/numerical/math/Matrix.hpp +++ b/numerical/math/Matrix.hpp @@ -145,8 +145,7 @@ namespace math } template - constexpr Matrix::Matrix() noexcept - {} + constexpr Matrix::Matrix() noexcept = default; template OPTIMIZE_FOR_SPEED constexpr Matrix::Matrix(std::initializer_list> init)