diff --git a/numerical/analysis/ConvolutionCorrelation.cpp b/numerical/analysis/ConvolutionCorrelation.cpp index a41f5e7..95d83db 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 5bbab7c..5c3d941 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 6b360ac..1317f34 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 055a0d5..23a43ce 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 e2ecb6a..4eb9510 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 @@ -145,13 +145,10 @@ namespace math } template - constexpr Matrix::Matrix() noexcept - : data{} - {} + constexpr Matrix::Matrix() noexcept = default; template OPTIMIZE_FOR_SPEED constexpr Matrix::Matrix(std::initializer_list> init) - : data{} { size_t row = 0; for (const auto& row_list : init)