Skip to content

Commit 751ae99

Browse files
FrancoisCarougeFrancoisCarouge
authored
[filter] constexpr support (#314)
1 parent a2eb315 commit 751ae99

File tree

5 files changed

+194
-18
lines changed

5 files changed

+194
-18
lines changed

‎include/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -72,6 +72,7 @@ target_sources(
7272
FILES
7373
"fcarouge/algorithm.hpp"
7474
"fcarouge/internal/format.hpp"
75+
"fcarouge/internal/function.hpp"
7576
"fcarouge/internal/kalman.hpp"
7677
"fcarouge/internal/kalman.tpp"
7778
"fcarouge/internal/utility.hpp"

‎include/fcarouge/internal/function.hpp

+106
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
/* __ _ __ __ _ _
2+
| |/ / /\ | | | \/ | /\ | \ | |
3+
| ' / / \ | | | \ / | / \ | \| |
4+
| < / /\ \ | | | |\/| | / /\ \ | . ` |
5+
| . \ / ____ \| |____| | | |/ ____ \| |\ |
6+
|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_|
7+
8+
Kalman Filter
9+
Version 0.2.0
10+
https://github.com/FrancoisCarouge/Kalman
11+
12+
SPDX-License-Identifier: Unlicense
13+
14+
This is free and unencumbered software released into the public domain.
15+
16+
Anyone is free to copy, modify, publish, use, compile, sell, or
17+
distribute this software, either in source code form or as a compiled
18+
binary, for any purpose, commercial or non-commercial, and by any
19+
means.
20+
21+
In jurisdictions that recognize copyright laws, the author or authors
22+
of this software dedicate any and all copyright interest in the
23+
software to the public domain. We make this dedication for the benefit
24+
of the public at large and to the detriment of our heirs and
25+
successors. We intend this dedication to be an overt act of
26+
relinquishment in perpetuity of all present and future rights to this
27+
software under copyright law.
28+
29+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
30+
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
31+
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
32+
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
33+
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
34+
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
35+
OTHER DEALINGS IN THE SOFTWARE.
36+
37+
For more information, please refer to <https://unlicense.org> */
38+
39+
#ifndef FCAROUGE_INTERNAL_FUNCTION_HPP
40+
#define FCAROUGE_INTERNAL_FUNCTION_HPP
41+
42+
#include <memory>
43+
44+
namespace fcarouge::internal {
45+
46+
// Compile-time `std::function` partial drop-in.
47+
template <typename Undefined> class function;
48+
49+
template <typename Result, typename... Arguments>
50+
class function<Result(Arguments...)> {
51+
public:
52+
template <typename Callable>
53+
constexpr explicit function(Callable callee)
54+
: storage{std::make_unique<implementation<Callable>>(callee)} {}
55+
56+
template <typename Callable> function &operator=(Callable &&callee) {
57+
storage = std::make_unique<implementation<Callable>>(callee);
58+
return *this;
59+
}
60+
61+
constexpr auto operator()(Arguments... arguments) const -> Result {
62+
return (*storage)(arguments...);
63+
}
64+
65+
private:
66+
struct interface {
67+
constexpr virtual auto operator()(Arguments...) -> Result = 0;
68+
constexpr virtual ~interface() = default;
69+
};
70+
71+
template <typename Callable> struct implementation final : interface {
72+
public:
73+
constexpr explicit implementation(Callable callee) : memory{callee} {}
74+
constexpr auto operator()(Arguments... arguments) -> Result override {
75+
return memory(arguments...);
76+
}
77+
constexpr ~implementation() override = default;
78+
79+
private:
80+
Callable memory{};
81+
};
82+
83+
//! @todo Support optimized small storage alternatives?
84+
std::unique_ptr<interface> storage{};
85+
};
86+
87+
template <typename Callable> struct function_traits {};
88+
89+
template <typename Result, typename Type, typename... Arguments>
90+
struct function_traits<Result (Type::*)(Arguments...) const> {
91+
using type = Result(Arguments...);
92+
};
93+
94+
template <typename Callable>
95+
using function_traits_t =
96+
typename function_traits<decltype(&Callable::operator())>::type;
97+
98+
template <typename Callable>
99+
function(Callable) -> function<function_traits_t<Callable>>;
100+
101+
template <typename Result, typename... Arguments>
102+
function(Result(Arguments...)) -> function<Result(Arguments...)>;
103+
104+
} // namespace fcarouge::internal
105+
106+
#endif // FCAROUGE_INTERNAL_FUNCTION_HPP

‎include/fcarouge/internal/kalman.hpp

+18-18
Original file line numberDiff line numberDiff line change
@@ -39,9 +39,9 @@ For more information, please refer to <https://unlicense.org> */
3939
#ifndef FCAROUGE_INTERNAL_KALMAN_HPP
4040
#define FCAROUGE_INTERNAL_KALMAN_HPP
4141

42+
#include "function.hpp"
4243
#include "utility.hpp"
4344

44-
#include <functional>
4545
#include <tuple>
4646

4747
namespace fcarouge::internal {
@@ -69,18 +69,18 @@ struct kalman<State, Output, void, pack<UpdateTypes...>,
6969
using innovation = output;
7070
using innovation_uncertainty = output_uncertainty;
7171
using observation_state_function =
72-
std::function<output_model(const state &, const UpdateTypes &...)>;
73-
using noise_observation_function = std::function<output_uncertainty(
72+
function<output_model(const state &, const UpdateTypes &...)>;
73+
using noise_observation_function = function<output_uncertainty(
7474
const state &, const output &, const UpdateTypes &...)>;
75-
using transition_state_function = std::function<state_transition(
76-
const state &, const PredictionTypes &...)>;
77-
using noise_process_function = std::function<process_uncertainty(
78-
const state &, const PredictionTypes &...)>;
75+
using transition_state_function =
76+
function<state_transition(const state &, const PredictionTypes &...)>;
77+
using noise_process_function =
78+
function<process_uncertainty(const state &, const PredictionTypes &...)>;
7979
using transition_control_function = empty;
8080
using transition_function =
81-
std::function<state(const state &, const PredictionTypes &...)>;
81+
function<state(const state &, const PredictionTypes &...)>;
8282
using observation_function =
83-
std::function<output(const state &, const UpdateTypes &...)>;
83+
function<output(const state &, const UpdateTypes &...)>;
8484
using update_types = std::tuple<UpdateTypes...>;
8585
using prediction_types = std::tuple<PredictionTypes...>;
8686

@@ -181,19 +181,19 @@ struct kalman<State, Output, Input, pack<UpdateTypes...>,
181181
using innovation = output;
182182
using innovation_uncertainty = output_uncertainty;
183183
using observation_state_function =
184-
std::function<output_model(const state &, const UpdateTypes &...)>;
185-
using noise_observation_function = std::function<output_uncertainty(
184+
function<output_model(const state &, const UpdateTypes &...)>;
185+
using noise_observation_function = function<output_uncertainty(
186186
const state &, const output &, const UpdateTypes &...)>;
187-
using transition_state_function = std::function<state_transition(
187+
using transition_state_function = function<state_transition(
188188
const state &, const input &, const PredictionTypes &...)>;
189-
using noise_process_function = std::function<process_uncertainty(
190-
const state &, const PredictionTypes &...)>;
189+
using noise_process_function =
190+
function<process_uncertainty(const state &, const PredictionTypes &...)>;
191191
using transition_control_function =
192-
std::function<input_control(const PredictionTypes &...)>;
193-
using transition_function = std::function<state(const state &, const input &,
194-
const PredictionTypes &...)>;
192+
function<input_control(const PredictionTypes &...)>;
193+
using transition_function =
194+
function<state(const state &, const input &, const PredictionTypes &...)>;
195195
using observation_function =
196-
std::function<output(const state &, const UpdateTypes &...)>;
196+
function<output(const state &, const UpdateTypes &...)>;
197197
using update_types = std::tuple<UpdateTypes...>;
198198
using prediction_types = std::tuple<PredictionTypes...>;
199199

‎test/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ FetchContent_MakeAvailable(eigen)
4646

4747
foreach(
4848
TEST
49+
"kalman_constructor_default_constexpr.cpp"
4950
"kalman_constructor_default_float_1x1x1.cpp"
5051
"kalman_constructor_default.cpp"
5152
"kalman_f.cpp"

‎test/kalman_constructor_default_constexpr.cpp

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
/* __ _ __ __ _ _
2+
| |/ / /\ | | | \/ | /\ | \ | |
3+
| ' / / \ | | | \ / | / \ | \| |
4+
| < / /\ \ | | | |\/| | / /\ \ | . ` |
5+
| . \ / ____ \| |____| | | |/ ____ \| |\ |
6+
|_|\_\/_/ \_\______|_| |_/_/ \_\_| \_|
7+
8+
Kalman Filter
9+
Version 0.2.0
10+
https://github.com/FrancoisCarouge/Kalman
11+
12+
SPDX-License-Identifier: Unlicense
13+
14+
This is free and unencumbered software released into the public domain.
15+
16+
Anyone is free to copy, modify, publish, use, compile, sell, or
17+
distribute this software, either in source code form or as a compiled
18+
binary, for any purpose, commercial or non-commercial, and by any
19+
means.
20+
21+
In jurisdictions that recognize copyright laws, the author or authors
22+
of this software dedicate any and all copyright interest in the
23+
software to the public domain. We make this dedication for the benefit
24+
of the public at large and to the detriment of our heirs and
25+
successors. We intend this dedication to be an overt act of
26+
relinquishment in perpetuity of all present and future rights to this
27+
software under copyright law.
28+
29+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
30+
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
31+
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
32+
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
33+
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
34+
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
35+
OTHER DEALINGS IN THE SOFTWARE.
36+
37+
For more information, please refer to <https://unlicense.org> */
38+
39+
#include "fcarouge/kalman.hpp"
40+
41+
#include <cassert>
42+
43+
namespace fcarouge::test {
44+
namespace {
45+
46+
//! @test Verifies default values are initialized for single-dimension filters
47+
//! without input control within constant expression.
48+
//!
49+
//! @todo Make the lambda `consteval` when MSVC supports it.
50+
[[maybe_unused]] constexpr auto test{[] {
51+
kalman filter;
52+
53+
assert(filter.f() == 1);
54+
assert(filter.h() == 1);
55+
assert(filter.k() == 1);
56+
assert(filter.p() == 1);
57+
assert(filter.q() == 0 && "No process noise by default.");
58+
assert(filter.r() == 0 && "No observation noise by default.");
59+
assert(filter.s() == 1);
60+
assert(filter.x() == 0 && "Origin state.");
61+
assert(filter.y() == 0);
62+
assert(filter.z() == 0);
63+
64+
return 0;
65+
}()};
66+
67+
} // namespace
68+
} // namespace fcarouge::test

0 commit comments

Comments
 (0)