From aa0c1160dc5ef2ee5308ce8035cc52e0b8b5f5cb Mon Sep 17 00:00:00 2001 From: Francois Carouge Date: Sat, 3 Sep 2022 19:15:10 -0700 Subject: [PATCH] [filter] return characteristics by reference for performance --- include/fcarouge/kalman.hpp | 160 ++++++++++++++++++++++++++++-------- 1 file changed, 124 insertions(+), 36 deletions(-) diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index 3299be805..7502bb4a4 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -400,7 +400,13 @@ class kalman final { //! @return The state estimate column vector X. //! //! @complexity Constant. - inline constexpr auto x() const -> state; + //! + //! @note Overloading the operator dot would have been nice had it existed. + //! + //! @todo Collapse cv-ref qualifier-aware member functions per C++23 P0847 to + //! avoid duplication: `inline constexpr auto & x(this auto&& self)`. + inline constexpr auto x() const -> const state &; + inline constexpr auto x() -> state &; //! @brief Sets the state estimate column vector X. //! @@ -441,7 +447,7 @@ class kalman final { //! @return The last observation column vector Z. //! //! @complexity Constant. - inline constexpr auto z() const -> output; + inline constexpr auto z() const -> const output &; //! @brief Returns the last control column vector U. //! @@ -450,15 +456,16 @@ class kalman final { //! @return The last control column vector U. //! //! @complexity Constant. - inline constexpr auto u() const -> input - requires(not std::is_same_v); + inline constexpr auto u() const + -> const input &requires(not std::is_same_v); //! @brief Returns the estimated covariance matrix P. //! //! @return The estimated correlated variance matrix P. //! //! @complexity Constant. - inline constexpr auto p() const -> estimate_uncertainty; + inline constexpr auto p() const -> const estimate_uncertainty &; + inline constexpr auto p() -> estimate_uncertainty &; //! @brief Sets the estimated covariance matrix P. //! @@ -479,7 +486,8 @@ class kalman final { //! @return The process noise correlated variance matrix Q. //! //! @complexity Constant. - inline constexpr auto q() const -> process_uncertainty; + inline constexpr auto q() const -> const process_uncertainty &; + inline constexpr auto q() -> process_uncertainty &; //! @brief Sets the process noise covariance matrix Q. //! @@ -525,7 +533,8 @@ class kalman final { //! @return The observation noise correlated variance matrix R. //! //! @complexity Constant. - inline constexpr auto r() const -> output_uncertainty; + inline constexpr auto r() const -> const output_uncertainty &; + inline constexpr auto r() -> output_uncertainty &; //! @brief Sets the observation noise covariance matrix R. //! @@ -568,7 +577,8 @@ class kalman final { //! @return The state transition matrix F. //! //! @complexity Constant. - inline constexpr auto f() const -> state_transition; + inline constexpr auto f() const -> const state_transition &; + inline constexpr auto f() -> state_transition &; //! @brief Sets the state transition matrix F. //! @@ -621,7 +631,8 @@ class kalman final { //! @return The observation, measurement transition matrix H. //! //! @complexity Constant. - inline constexpr auto h() const -> output_model; + inline constexpr auto h() const -> const output_model &; + inline constexpr auto h() -> output_model &; //! @brief Sets the observation transition matrix H. //! @@ -676,8 +687,10 @@ class kalman final { //! @return The control transition matrix G. //! //! @complexity Constant. - inline constexpr auto g() const -> input_control - requires(not std::is_same_v); + inline constexpr auto g() const + -> const input_control &requires(not std::is_same_v); + inline constexpr auto g() + -> input_control &requires(not std::is_same_v); //! @brief Sets the control transition matrix G. //! @@ -726,21 +739,21 @@ class kalman final { //! @return The gain matrix K. //! //! @complexity Constant. - inline constexpr auto k() const -> gain; + inline constexpr auto k() const -> const gain &; //! @brief Returns the innovation column vector Y. //! //! @return The innovation column vector Y. //! //! @complexity Constant. - inline constexpr auto y() const -> innovation; + inline constexpr auto y() const -> const innovation &; //! @brief Returns the innovation uncertainty matrix S. //! //! @return The innovation uncertainty matrix S. //! //! @complexity Constant. - inline constexpr auto s() const -> innovation_uncertainty; + inline constexpr auto s() const -> const innovation_uncertainty &; //! @brief Sets the extended state transition function f(x). //! @@ -916,7 +929,17 @@ template ::x() const -> state { + UpdateTypes, PredictionTypes>::x() const -> const state & { + return filter.x; +} + +template +[[nodiscard("The returned state estimate column vector X is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::x() -> state & { return filter.x; } @@ -964,7 +987,7 @@ template ::z() const -> output { + UpdateTypes, PredictionTypes>::z() const -> const output & { return filter.z; } @@ -974,10 +997,22 @@ template ::u() const -> input - requires(not std::is_same_v) -{ - return filter.u; + UpdateTypes, PredictionTypes>::u() const + -> const input &requires(not std::is_same_v) { + return filter.u; + } + +template +[[nodiscard( + "The returned estimated covariance matrix P is unexpectedly " + "discarded.")]] inline constexpr auto kalman::p() const + -> const estimate_uncertainty & { + return filter.p; } template ::p() const -> estimate_uncertainty { + UpdateTypes, PredictionTypes>::p() -> estimate_uncertainty & { return filter.p; } @@ -1014,7 +1049,17 @@ template ::q() const -> process_uncertainty { + UpdateTypes, PredictionTypes>::q() const -> const process_uncertainty & { + return filter.q; +} + +template +[[nodiscard("The returned process noise covariance matrix Q is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::q() -> process_uncertainty & { return filter.q; } @@ -1061,7 +1106,17 @@ template ::r() const -> output_uncertainty { + UpdateTypes, PredictionTypes>::r() const -> const output_uncertainty & { + return filter.r; +} + +template +[[nodiscard("The returned observation noise covariance matrix R is " + "unexpectedly discarded.")]] inline constexpr auto +kalman::r() -> output_uncertainty & { return filter.r; } @@ -1108,7 +1163,17 @@ template ::f() const -> state_transition { + UpdateTypes, PredictionTypes>::f() const -> const state_transition & { + return filter.f; +} + +template +[[nodiscard("The returned state transition matrix F is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::f() -> state_transition & { return filter.f; } @@ -1155,7 +1220,17 @@ template ::h() const -> output_model { + UpdateTypes, PredictionTypes>::h() const -> const output_model & { + return filter.h; +} + +template +[[nodiscard("The returned observation transition matrix H is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::h() -> output_model & { return filter.h; } @@ -1202,18 +1277,30 @@ template ::g() const -> input_control - requires(not std::is_same_v) -{ - return filter.g; -} + UpdateTypes, PredictionTypes>::g() const + -> const input_control &requires(not std::is_same_v) { + return filter.g; + } template -inline constexpr void -kalman::g(const input_control &value) +[[nodiscard( + "The returned control transition matrix G is unexpectedly " + "discarded.")]] inline constexpr auto kalman::g() + -> input_control &requires(not std::is_same_v) { + return filter.g; + } + +template +inline constexpr void kalman::g(const input_control &value) requires(not std::is_same_v) { filter.g = value; @@ -1256,7 +1343,7 @@ template ::k() const -> gain { + UpdateTypes, PredictionTypes>::k() const -> const gain & { return filter.k; } @@ -1266,7 +1353,7 @@ template ::y() const -> innovation { + UpdateTypes, PredictionTypes>::y() const -> const innovation & { return filter.y; } @@ -1276,7 +1363,8 @@ template ::s() const -> innovation_uncertainty { + UpdateTypes, PredictionTypes>::s() const + -> const innovation_uncertainty & { return filter.s; }