Skip to content

Commit b22f651

Browse files
FrancoisCarougeFrancoisCarouge
committed
[cpo] support divides customization point object
1 parent d1e105a commit b22f651

File tree

5 files changed

+54
-100
lines changed

5 files changed

+54
-100
lines changed

‎include/fcarouge/internal/kalman.hpp

+7-13
Original file line numberDiff line numberDiff line change
@@ -53,7 +53,7 @@ namespace fcarouge::internal
5353
template <typename State, typename Output = State, typename Input = State,
5454
template <typename> typename Transpose = transpose,
5555
template <typename> typename Symmetrize = symmetrize,
56-
template <typename, typename> typename Divide = divide,
56+
typename Divide = std::divides<void>,
5757
template <typename> typename Identity = identity,
5858
typename... PredictionArguments>
5959
struct kalman {
@@ -74,34 +74,28 @@ struct kalman {
7474
//! @brief Type of the estimated covariance matrix P.
7575
//!
7676
//! @details Also known as Σ.
77-
using estimate_uncertainty =
78-
std::invoke_result_t<Divide<State, State>, State, State>;
77+
using estimate_uncertainty = std::invoke_result_t<Divide, State, State>;
7978

8079
//! @brief Type of the process noise covariance matrix Q.
81-
using process_uncertainty =
82-
std::invoke_result_t<Divide<State, State>, State, State>;
80+
using process_uncertainty = std::invoke_result_t<Divide, State, State>;
8381

8482
//! @brief Type of the observation, measurement noise covariance matrix R.
85-
using output_uncertainty =
86-
std::invoke_result_t<Divide<Output, Output>, Output, Output>;
83+
using output_uncertainty = std::invoke_result_t<Divide, Output, Output>;
8784

8885
//! @brief Type of the state transition matrix F.
8986
//!
9087
//! @details Also known as Φ or A.
91-
using state_transition =
92-
std::invoke_result_t<Divide<State, State>, State, State>;
88+
using state_transition = std::invoke_result_t<Divide, State, State>;
9389

9490
//! @brief Type of the observation transition matrix H.
9591
//!
9692
//! @details Also known as C.
97-
using output_model =
98-
std::invoke_result_t<Divide<Output, State>, Output, State>;
93+
using output_model = std::invoke_result_t<Divide, Output, State>;
9994

10095
//! @brief Type of the control transition matrix G.
10196
//!
10297
//! @details Also known as B.
103-
using input_control =
104-
std::invoke_result_t<Divide<State, Input>, State, Input>;
98+
using input_control = std::invoke_result_t<Divide, State, Input>;
10599

106100
//! @}
107101

‎include/fcarouge/internal/kalman_eigen_operator.hpp

+12-8
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,8 @@ For more information, please refer to <https://unlicense.org> */
4646

4747
#include <Eigen/Eigen>
4848

49+
#include <type_traits>
50+
4951
namespace fcarouge::eigen::internal
5052
{
5153
//! @brief Function object for performing Eigen matrix transposition.
@@ -93,20 +95,22 @@ template <typename Type> struct symmetrize {
9395
//!
9496
//! @details Implemented with the Eigen linear algebra library matrices with
9597
//! sizes fixed at compile-time.
96-
//!
97-
//! @tparam Numerator The type template parameter of the dividend.
98-
//! @tparam Denominator The type template parameter of the divisor.
99-
template <typename Numerator, typename Denominator> struct divide {
98+
struct divide {
10099
//! @brief Returns the quotient of `numerator` and `denominator`.
101100
//!
102101
//! @param numerator The dividend of the division.
103102
//! @param denominator The divisor of the division.
104103
//!
105104
//! @exception May throw implementation-defined exceptions.
106-
[[nodiscard]] inline constexpr Eigen::Matrix<typename Numerator::Scalar,
107-
Numerator::RowsAtCompileTime,
108-
Denominator::RowsAtCompileTime>
109-
operator()(const Numerator &numerator, const Denominator &denominator) const
105+
//!
106+
//! @todo Why compilation fails if we specifcy the return type in the body of
107+
//! the function?
108+
[[nodiscard]] inline constexpr auto operator()(const auto &numerator,
109+
const auto &denominator) const
110+
-> typename Eigen::Matrix<
111+
typename std::decay_t<decltype(numerator)>::Scalar,
112+
std::decay_t<decltype(numerator)>::RowsAtCompileTime,
113+
std::decay_t<decltype(denominator)>::RowsAtCompileTime>
110114
{
111115
return denominator.transpose()
112116
.fullPivHouseholderQr()

‎include/fcarouge/internal/kalman_equation.hpp

+18-29
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@ For more information, please refer to <https://unlicense.org> */
4242
//! @file
4343
//! @brief Kalman filter main project header.
4444

45+
#include <functional>
4546
#include <type_traits>
4647

4748
namespace fcarouge::internal
@@ -50,15 +51,15 @@ namespace fcarouge::internal
5051
extrapolate_state(const auto &x, const auto &ff, const auto &f, const auto &g,
5152
const auto &u)
5253
{
53-
using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
54+
using state = std::decay_t<decltype(x)>;
5455

5556
return state{ ff(x, f) + g * u };
5657
}
5758

5859
[[nodiscard]] inline constexpr auto
5960
extrapolate_state(const auto &x, const auto &ff, const auto &f)
6061
{
61-
using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
62+
using state = std::decay_t<decltype(x)>;
6263

6364
return state{ ff(x, f) };
6465
}
@@ -67,10 +68,8 @@ template <template <typename> class Transpose>
6768
[[nodiscard]] inline constexpr auto
6869
extrapolate_covariance(const auto &p, const auto &f, const auto &q)
6970
{
70-
using estimate_uncertainty =
71-
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
72-
using state_transition =
73-
std::remove_reference_t<std::remove_cv_t<decltype(f)>>;
71+
using estimate_uncertainty = std::decay_t<decltype(p)>;
72+
using state_transition = std::decay_t<decltype(f)>;
7473

7574
Transpose<state_transition> transpose;
7675

@@ -84,8 +83,7 @@ inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f,
8483
{
8584
x = extrapolate_state(x, ff, f);
8685

87-
using estimate_uncertainty =
88-
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
86+
using estimate_uncertainty = std::decay_t<decltype(p)>;
8987

9088
Symmetrize<estimate_uncertainty> symmetrize;
9189

@@ -99,8 +97,7 @@ inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f,
9997
{
10098
x = extrapolate_state(x, ff, f, g, u);
10199

102-
using estimate_uncertainty =
103-
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
100+
using estimate_uncertainty = std::decay_t<decltype(p)>;
104101

105102
Symmetrize<estimate_uncertainty> symmetrize;
106103

@@ -110,7 +107,7 @@ inline constexpr void predict(auto &x, auto &p, const auto &ff, const auto &f,
110107
[[nodiscard]] inline constexpr auto update_state(const auto &x, const auto &k,
111108
const auto &y)
112109
{
113-
using state = std::remove_reference_t<std::remove_cv_t<decltype(x)>>;
110+
using state = std::decay_t<decltype(x)>;
114111

115112
return state{ x + k * y };
116113
}
@@ -120,9 +117,8 @@ template <template <typename> typename Transpose,
120117
[[nodiscard]] inline constexpr auto
121118
update_covariance(const auto &p, const auto &k, const auto &h, const auto &r)
122119
{
123-
using estimate_uncertainty =
124-
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
125-
using gain = std::remove_reference_t<std::remove_cv_t<decltype(k)>>;
120+
using estimate_uncertainty = std::decay_t<decltype(p)>;
121+
using gain = std::decay_t<decltype(k)>;
126122

127123
Transpose<estimate_uncertainty> transpose_p;
128124
Transpose<gain> transpose_k;
@@ -132,39 +128,33 @@ update_covariance(const auto &p, const auto &k, const auto &h, const auto &r)
132128
k * r * transpose_k(k) };
133129
}
134130

135-
template <template <typename> typename Transpose,
136-
template <typename, typename> typename Divide>
131+
template <template <typename> typename Transpose, typename Divide>
137132
[[nodiscard]] inline constexpr auto weight_gain(const auto &p, const auto &h,
138133
const auto &r)
139134
{
140-
using observation = std::remove_reference_t<std::remove_cv_t<decltype(h)>>;
141-
using output_uncertainty =
142-
std::remove_reference_t<std::remove_cv_t<decltype(r)>>;
135+
using observation = std::decay_t<decltype(h)>;
143136
using gain = std::invoke_result_t<Transpose<observation>, observation>;
137+
using innovation_uncertainty = std::decay_t<decltype(r)>;
144138

145139
Transpose<observation> transpose_h;
146-
Divide<gain, output_uncertainty> divide;
147-
148-
using innovation_uncertainty =
149-
std::remove_reference_t<std::remove_cv_t<decltype(r)>>;
140+
Divide divides;
150141

151142
const innovation_uncertainty s{ h * p * transpose_h(h) + r };
152143

153-
return gain{ divide(p * transpose_h(h), s) };
144+
return gain{ divides(p * transpose_h(h), s) };
154145
}
155146

156147
[[nodiscard]] inline constexpr auto innovate(const auto &x, const auto &z,
157148
const auto &h)
158149
{
159-
using innovation = std::remove_reference_t<std::remove_cv_t<decltype(z)>>;
150+
using innovation = std::decay_t<decltype(z)>;
160151

161152
return innovation{ z - h * x };
162153
}
163154

164155
//! @todo Do we want to allow the client to view the gain k? And the residual y?
165156
template <template <typename> typename Transpose,
166-
template <typename> typename Symmetrize,
167-
template <typename, typename> typename Divide,
157+
template <typename> typename Symmetrize, typename Divide,
168158
template <typename> typename Identity>
169159
inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r,
170160
const auto &z)
@@ -175,8 +165,7 @@ inline constexpr void update(auto &x, auto &p, const auto &h, const auto &r,
175165

176166
x = update_state(x, k, y);
177167

178-
using estimate_uncertainty =
179-
std::remove_reference_t<std::remove_cv_t<decltype(p)>>;
168+
using estimate_uncertainty = std::decay_t<decltype(p)>;
180169

181170
Symmetrize<estimate_uncertainty> symmetrize;
182171

‎include/fcarouge/internal/kalman_operator.hpp

-17
Original file line numberDiff line numberDiff line change
@@ -75,23 +75,6 @@ template <typename Type> struct symmetrize {
7575
}
7676
};
7777

78-
//! @brief Function object for performing matrix division.
79-
//!
80-
//! @tparam Numerator The type template parameter of the dividend.
81-
//! @tparam Denominator The type template parameter of the divisor.
82-
template <typename Numerator, typename Denominator> struct divide {
83-
//! @brief Returns the quotient of `numerator` and `denominator`.
84-
//!
85-
//! @param numerator The dividend of the division.
86-
//! @param denominator The divisor of the division.
87-
[[nodiscard]] inline constexpr auto
88-
operator()(const Numerator &numerator,
89-
const Denominator &denominator) const noexcept
90-
{
91-
return numerator / denominator;
92-
}
93-
};
94-
9578
//! @brief Function object for providing an identy matrix.
9679
//!
9780
//! @tparam Type The type template parameter of the matrix.

0 commit comments

Comments
 (0)