Skip to content

Commit

Permalink
[filter] declarative paradigm
Browse files Browse the repository at this point in the history
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge committed Dec 26, 2023
1 parent 033285c commit 78a2724
Show file tree
Hide file tree
Showing 12 changed files with 713 additions and 376 deletions.
110 changes: 46 additions & 64 deletions benchmark/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,12 @@ FetchContent_MakeAvailable(google_benchmark)

set(PROCESSOR_AFFINITY TRUE)

foreach(BENCHMARK "baseline.cpp" "predict_1x1x0.cpp" "predict_1x1x1.cpp"
"update_1x1x0.cpp" "update_1x1x1.cpp")
foreach(
BENCHMARK
"baseline.cpp"
# "predict_1x1x0.cpp" "predict_1x1x1.cpp" # "update_1x1x0.cpp"
# "update_1x1x1.cpp"
)
get_filename_component(NAME ${BENCHMARK} NAME_WE)
add_executable(kalman_benchmark_${NAME}_driver ${BENCHMARK})
target_include_directories(kalman_benchmark_${NAME}_driver PRIVATE "include")
Expand All @@ -75,66 +79,44 @@ foreach(BENCHMARK "baseline.cpp" "predict_1x1x0.cpp" "predict_1x1x1.cpp"
"--benchmark_out=${NAME}.json")
endforeach()

foreach(BACKEND IN ITEMS "eigen" "naive")
foreach(STATE_SIZE RANGE 1 2)
foreach(INPUT_SIZE RANGE 1 2)
configure_file(predict_linalg_x1x.cpp
predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.cpp)
add_executable(
kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.cpp)
target_include_directories(
kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
PRIVATE "include")
set_target_properties(
kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
PROPERTIES CXX_STANDARD 23
CXX_EXTENSIONS OFF
INTERPROCEDURAL_OPTIMIZATION TRUE)
target_link_libraries(
kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
PRIVATE benchmark::benchmark benchmark::benchmark_main kalman
kalman_linalg_${BACKEND} kalman_options)
separate_arguments(TEST_COMMAND UNIX_COMMAND $ENV{COMMAND})
add_test(
NAME kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}
COMMAND
${TEST_COMMAND}
$<TARGET_FILE:kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver>
"--benchmark_out=predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.json"
)
endforeach()
endforeach()
endforeach()
# foreach(BACKEND IN ITEMS "eigen" "naive") foreach(STATE_SIZE RANGE 1 2)
# foreach(INPUT_SIZE RANGE 1 2) configure_file(predict_linalg_x1x.cpp
# predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.cpp) add_executable(
# kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
# predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.cpp)
# target_include_directories(
# kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
# PRIVATE "include") set_target_properties(
# kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
# PROPERTIES CXX_STANDARD 23 CXX_EXTENSIONS OFF INTERPROCEDURAL_OPTIMIZATION
# TRUE) target_link_libraries(
# kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver
# PRIVATE benchmark::benchmark benchmark::benchmark_main kalman
# kalman_linalg_${BACKEND} kalman_options) separate_arguments(TEST_COMMAND
# UNIX_COMMAND $ENV{COMMAND}) add_test( NAME
# kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE} COMMAND
# ${TEST_COMMAND}
# $<TARGET_FILE:kalman_benchmark_predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}_driver>
# "--benchmark_out=predict_${BACKEND}_${STATE_SIZE}x1x${INPUT_SIZE}.json" )
# endforeach() endforeach() endforeach()

foreach(BACKEND IN ITEMS "eigen")
foreach(STATE_SIZE RANGE 1 2)
foreach(OUTPUT_SIZE RANGE 1 2)
configure_file(update_linalg_xx0.cpp
update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.cpp)
add_executable(
kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.cpp)
target_include_directories(
kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
PRIVATE "include")
set_target_properties(
kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
PROPERTIES CXX_STANDARD 23
CXX_EXTENSIONS OFF
INTERPROCEDURAL_OPTIMIZATION TRUE)
target_link_libraries(
kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
PRIVATE benchmark::benchmark benchmark::benchmark_main kalman
kalman_linalg_${BACKEND} kalman_options)
separate_arguments(TEST_COMMAND UNIX_COMMAND $ENV{COMMAND})
add_test(
NAME kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0
COMMAND
${TEST_COMMAND}
$<TARGET_FILE:kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver>
"--benchmark_out=update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.json"
)
endforeach()
endforeach()
endforeach()
# foreach(BACKEND IN ITEMS "eigen") foreach(STATE_SIZE RANGE 1 2)
# foreach(OUTPUT_SIZE RANGE 1 2) configure_file(update_linalg_xx0.cpp
# update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.cpp) add_executable(
# kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
# update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.cpp)
# target_include_directories(
# kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
# PRIVATE "include") set_target_properties(
# kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
# PROPERTIES CXX_STANDARD 23 CXX_EXTENSIONS OFF INTERPROCEDURAL_OPTIMIZATION
# TRUE) target_link_libraries(
# kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver
# PRIVATE benchmark::benchmark benchmark::benchmark_main kalman
# kalman_linalg_${BACKEND} kalman_options) separate_arguments(TEST_COMMAND
# UNIX_COMMAND $ENV{COMMAND}) add_test( NAME
# kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0 COMMAND
# ${TEST_COMMAND}
# $<TARGET_FILE:kalman_benchmark_update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0_driver>
# "--benchmark_out=update_${BACKEND}_${STATE_SIZE}x${OUTPUT_SIZE}x0.json" )
# endforeach() endforeach() endforeach()
74 changes: 35 additions & 39 deletions include/fcarouge/internal/format.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,73 +45,69 @@ For more information, please refer to <https://unlicense.org> */
#include <type_traits>

namespace fcarouge {
template <typename, typename, typename, typename, typename> class kalman;
template <typename> class kalman;
} // namespace fcarouge

template <typename State, typename Output, typename Input, typename UpdateTypes,
typename PredictionTypes, typename Char>
template <typename Filter, typename Char>
// It is allowed to add template specializations for any standard library class
// template to the namespace std only if the declaration depends on at least one
// program-defined type and the specialization satisfies all requirements for
// the original template, except where such specializations are prohibited.
// NOLINTNEXTLINE(cert-dcl58-cpp)
struct std::formatter<
fcarouge::kalman<State, Output, Input, UpdateTypes, PredictionTypes>,
Char> {
struct std::formatter<fcarouge::kalman<Filter>, Char> {
constexpr auto parse(std::basic_format_parse_context<Char> &parse_context) {
return parse_context.begin();
}

//! @todo P2585 may be useful in simplifying and standardizing the support.
template <typename OutputIt>
auto format(const fcarouge::kalman<State, Output, Input, UpdateTypes,
PredictionTypes> &filter,
auto format(const fcarouge::kalman<Filter> &filter,
std::basic_format_context<OutputIt, Char> &format_context) const
-> OutputIt {
format_context.advance_to(
format_to(format_context.out(), R"({{"f": {}, )", filter.f()));

if constexpr (not std::is_same_v<Input, void>) {
format_context.advance_to(
format_to(format_context.out(), R"("g": {}, )", filter.g()));
}
// if constexpr (not std::is_same_v<Input, void>) {
// format_context.advance_to(
// format_to(format_context.out(), R"("g": {}, )", filter.g()));
// }

format_context.advance_to(format_to(format_context.out(),
R"("h": {}, "k": {}, "p": {}, )",
filter.h(), filter.k(), filter.p()));

{
constexpr auto end{fcarouge::internal::repack_s<PredictionTypes>};
constexpr decltype(end) begin{0};
constexpr decltype(end) next{1};
fcarouge::internal::for_constexpr<begin, end, next>(
[&format_context, &filter](auto position) {
format_context.advance_to(
format_to(format_context.out(), R"("prediction_{}": {}, )",
position(), filter.template predict<position>()));
});
}
// {
// constexpr auto end{fcarouge::internal::repack_s<PredictionTypes>};
// constexpr decltype(end) begin{0};
// constexpr decltype(end) next{1};
// fcarouge::internal::for_constexpr<begin, end, next>(
// [&format_context, &filter](auto position) {
// format_context.advance_to(
// format_to(format_context.out(), R"("prediction_{}": {}, )",
// position(), filter.template predict<position>()));
// });
// }

format_context.advance_to(format_to(format_context.out(),
R"("q": {}, "r": {}, "s": {}, )",
filter.q(), filter.r(), filter.s()));

if constexpr (not std::is_same_v<Input, void>) {
format_context.advance_to(
format_to(format_context.out(), R"("u": {}, )", filter.u()));
}

{
constexpr auto end{fcarouge::internal::repack_s<UpdateTypes>};
constexpr decltype(end) begin{0};
constexpr decltype(end) next{1};
fcarouge::internal::for_constexpr<begin, end, next>(
[&format_context, &filter](auto position) {
format_context.advance_to(
format_to(format_context.out(), R"("update_{}": {}, )",
position(), filter.template update<position>()));
});
}
// if constexpr (not std::is_same_v<Input, void>) {
// format_context.advance_to(
// format_to(format_context.out(), R"("u": {}, )", filter.u()));
// }

// {
// constexpr auto end{fcarouge::internal::repack_s<UpdateTypes>};
// constexpr decltype(end) begin{0};
// constexpr decltype(end) next{1};
// fcarouge::internal::for_constexpr<begin, end, next>(
// [&format_context, &filter](auto position) {
// format_context.advance_to(
// format_to(format_context.out(), R"("update_{}": {}, )",
// position(), filter.template update<position>()));
// });
// }

format_context.advance_to(format_to(format_context.out(),
R"("x": {}, "y": {}, "z": {}}})",
Expand Down
88 changes: 87 additions & 1 deletion include/fcarouge/internal/kalman.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@ For more information, please refer to <https://unlicense.org> */
#include "utility.hpp"

#include <tuple>
#include <type_traits>

namespace fcarouge::internal {
template <typename, typename, typename, typename, typename>
Expand Down Expand Up @@ -202,14 +203,14 @@ struct kalman<State, Output, Input, pack<UpdateTypes...>,
estimate_uncertainty p{identity_v<estimate_uncertainty>};
process_uncertainty q{zero_v<process_uncertainty>};
output_uncertainty r{zero_v<output_uncertainty>};
input u{zero_v<input>};
output_model h{identity_v<output_model>};
state_transition f{identity_v<state_transition>};
input_control g{identity_v<input_control>};
gain k{identity_v<gain>};
innovation y{zero_v<innovation>};
innovation_uncertainty s{identity_v<innovation_uncertainty>};
output z{zero_v<output>};
input u{zero_v<input>};
update_types update_arguments{};
prediction_types prediction_arguments{};
transpose t{};
Expand Down Expand Up @@ -293,6 +294,91 @@ struct kalman<State, Output, Input, pack<UpdateTypes...>,
p = estimate_uncertainty{f * p * t(f) + q};
}
};

struct make_filter {
inline constexpr auto operator()() const {
return kalman<double, double, void, empty_pack, empty_pack>{};
}
template <typename... Arguments>
inline constexpr auto operator()(Arguments &&...arguments) const {
if constexpr (sizeof...(Arguments) == 3) {
return kalman<get_t<"state", Arguments...>, get_t<"state", Arguments...>,
void, empty_pack, empty_pack>{
get<"state">(arguments...),
get<"estimate_uncertainty">(arguments...),
{},
get<"output_uncertainty">(arguments...)};
}
if constexpr (sizeof...(Arguments) == 4) {
return kalman<get_t<"state", Arguments...>, get_t<"state", Arguments...>,
void, empty_pack, empty_pack>{
get<"state">(arguments...), get<"estimate_uncertainty">(arguments...),
get<"process_uncertainty">(arguments...),
get<"output_uncertainty">(arguments...)};
}
if constexpr (sizeof...(Arguments) == 5) {
return kalman<get_t<"state", Arguments...>, get_t<"state", Arguments...>,
get_t<"input", Arguments...>, empty_pack, empty_pack>{
get<"state">(arguments...), get<"estimate_uncertainty">(arguments...),
get<"process_uncertainty">(arguments...),
get<"output_uncertainty">(arguments...)};
}
}
};

inline constexpr make_filter filter{};

template <typename... Arguments>
using filter_t = std::invoke_result_t<make_filter, Arguments...>;

template <typename Type> struct state {
Type value;
using type = Type;
};

//! @todo Rename to `state_uncertainty`? For consistency? Keep alias for
//! literature convention?
template <typename Type> struct estimate_uncertainty {
Type value;
using type = Type;
};

template <typename Type> struct output_uncertainty {
Type value;
using type = Type;
};

template <typename Type> struct process_uncertainty {
Type value;
using type = Type;
};

template <typename Type> struct input {
Type value;
using type = Type;
};

template <typename Type> struct output {
Type value;
using type = Type;
};

template <typename Type> struct output_model {
Type value;
using type = Type;
};

//! @todo Better name not ending by *_types?
template <typename Type> struct update_types {
Type value;
using type = Type;
};

template <typename Type> struct prediction_types {
Type value;
using type = Type;
};

} // namespace fcarouge::internal

#endif // FCAROUGE_INTERNAL_KALMAN_HPP
Loading

0 comments on commit 78a2724

Please sign in to comment.