Skip to content

Commit

Permalink
[linalg] expand interoperability demonstration samples
Browse files Browse the repository at this point in the history
  • Loading branch information
@FrancoisCarouge
FrancoisCarouge committed Feb 9, 2024
1 parent e7ac901 commit 2504a91
Show file tree
Hide file tree
Showing 5 changed files with 166 additions and 145 deletions.
24 changes: 21 additions & 3 deletions sample/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,28 @@ foreach(SAMPLE "kf_1x1x0_building_height.cpp" "kf_1x1x0_liquid_temperature.cpp"
COMMAND ${TEST_COMMAND} $<TARGET_FILE:kalman_sample_${NAME}_driver>)
endforeach()

foreach(BACKEND IN ITEMS "eigen" "naive")
foreach(SAMPLE "ekf_4x1x0_soaring.cpp" "kf_2x1x1_rocket_altitude.cpp")
get_filename_component(NAME ${SAMPLE} NAME_WE)
add_executable(kalman_sample_${BACKEND}_${NAME}_driver ${SAMPLE})
set_target_properties(
kalman_sample_${BACKEND}_${NAME}_driver
PROPERTIES CXX_STANDARD 23
CXX_EXTENSIONS OFF
INTERPROCEDURAL_OPTIMIZATION TRUE)
target_link_libraries(
kalman_sample_${BACKEND}_${NAME}_driver
PRIVATE kalman kalman_main kalman_linalg_${BACKEND} kalman_options)
separate_arguments(TEST_COMMAND UNIX_COMMAND $ENV{COMMAND})
add_test(NAME kalman_sample_${BACKEND}_${NAME}
COMMAND ${TEST_COMMAND}
$<TARGET_FILE:kalman_sample_${BACKEND}_${NAME}_driver>)
endforeach()
endforeach()

foreach(BACKEND IN ITEMS "eigen")
foreach(SAMPLE
"ekf_4x1x0_soaring.cpp" "kf_2x1x1_rocket_altitude.cpp"
"kf_6x2x0_vehicle_location.cpp" "kf_8x4x0_deep_sort_bounding_box.cpp")
foreach(SAMPLE "kf_6x2x0_vehicle_location.cpp"
"kf_8x4x0_deep_sort_bounding_box.cpp")
get_filename_component(NAME ${SAMPLE} NAME_WE)
add_executable(kalman_sample_${BACKEND}_${NAME}_driver ${SAMPLE})
set_target_properties(
Expand Down
18 changes: 9 additions & 9 deletions sample/ekf_4x1x0_soaring.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,26 +52,26 @@ template <auto Size> using vector = column_vector<float, Size>;
const float strength_covariance{0.0049F};
const float radius_covariance{400};
const float position_covariance{400};
filter.p(kalman::estimate_uncertainty{{strength_covariance, 0, 0, 0},
{0, radius_covariance, 0, 0},
{0, 0, position_covariance, 0},
{0, 0, 0, position_covariance}});
filter.p(kalman::estimate_uncertainty{{strength_covariance, 0.F, 0.F, 0.F},
{0.F, radius_covariance, 0.F, 0.F},
{0.F, 0.F, position_covariance, 0.F},
{0.F, 0.F, 0.F, position_covariance}});

// No process dynamics: F = ∂f/∂X = I4 Default.

filter.transition([](const kalman::state &x, const float &drift_x,
const float &drift_y) -> kalman::state {
//! @todo Could make sure that x[1] stays positive, greater than 40.
const kalman::state drifts{0, 0, drift_x, drift_y};
const kalman::state drifts{0.F, 0.F, drift_x, drift_y};
return x + drifts;
});

const float strength_noise{std::pow(0.001F, 2.F)};
const float distance_noise{std::pow(0.03F, 2.F)};
filter.q(kalman::process_uncertainty{{strength_noise, 0, 0, 0},
{0, distance_noise, 0, 0},
{0, 0, distance_noise, 0},
{0, 0, 0, distance_noise}});
filter.q(kalman::process_uncertainty{{strength_noise, 0.F, 0.F, 0.F},
{0.F, distance_noise, 0.F, 0.F},
{0.F, 0.F, distance_noise, 0.F},
{0.F, 0.F, 0.F, distance_noise}});

const float measure_noise{std::pow(0.45F, 2.F)};
filter.r(kalman::output_uncertainty{measure_noise});
Expand Down
4 changes: 2 additions & 2 deletions sample/kf_2x1x1_rocket_altitude.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ template <auto Size> using vector = column_vector<double, Size>;
// Since our initial state vector is a guess, we will set a very high estimate
// uncertainty. The high estimate uncertainty results in high Kalman gain,
// giving a high weight to the measurement.
filter.p(kalman::estimate_uncertainty{{500, 0}, {0, 500}});
filter.p(kalman::estimate_uncertainty{{500., 0.}, {0., 500.}});

// Prediction
// We will assume a discrete noise model - the noise is different at each time
Expand All @@ -83,7 +83,7 @@ template <auto Size> using vector = column_vector<double, Size>;
[[maybe_unused]] const kalman::input &u,
const std::chrono::milliseconds &delta_time) {
const auto dt{std::chrono::duration<double>(delta_time).count()};
return kalman::state_transition{{1, dt}, {0, 1}};
return kalman::state_transition{{1., dt}, {0., 1.}};
});

// The control matrix G would be:
Expand Down
34 changes: 18 additions & 16 deletions sample/kf_6x2x0_vehicle_location.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,36 +54,38 @@ template <auto Size> using vector = column_vector<double, Size>;
// Since our initial state vector is a guess, we will set a very high estimate
// uncertainty. The high estimate uncertainty results in a high Kalman Gain,
// giving a high weight to the measurement.
filter.p(kalman::estimate_uncertainty{{500, 0, 0, 0, 0, 0},
{0, 500, 0, 0, 0, 0},
{0, 0, 500, 0, 0, 0},
{0, 0, 0, 500, 0, 0},
{0, 0, 0, 0, 500, 0},
{0, 0, 0, 0, 0, 500}});
filter.p(kalman::estimate_uncertainty{{500., 0., 0., 0., 0., 0.},
{0., 500., 0., 0., 0., 0.},
{0., 0., 500., 0., 0., 0.},
{0., 0., 0., 500., 0., 0.},
{0., 0., 0., 0., 500., 0.},
{0., 0., 0., 0., 0., 500.}});

// Prediction
// The process noise matrix Q would be:
kalman::process_uncertainty q{
{0.25, 0.5, 0.5, 0, 0, 0}, {0.5, 1, 1, 0, 0, 0}, {0.5, 1, 1, 0, 0, 0},
{0, 0, 0, 0.25, 0.5, 0.5}, {0, 0, 0, 0.5, 1, 1}, {0, 0, 0, 0.5, 1, 1}};
{0.25, 0.5, 0.5, 0., 0., 0.}, {0.5, 1., 1., 0., 0., 0.},
{0.5, 1., 1., 0., 0., 0.}, {0., 0., 0., 0.25, 0.5, 0.5},
{0., 0., 0., 0.5, 1., 1.}, {0., 0., 0., 0.5, 1., 1.}};
q *= 0.2 * 0.2;
filter.q(q);

// The state transition matrix F would be:
filter.f(kalman::state_transition{{1, 1, 0.5, 0, 0, 0},
{0, 1, 1, 0, 0, 0},
{0, 0, 1, 0, 0, 0},
{0, 0, 0, 1, 1, 0.5},
{0, 0, 0, 0, 1, 1},
{0, 0, 0, 0, 0, 1}});
filter.f(kalman::state_transition{{1., 1., 0.5, 0., 0., 0.},
{0., 1., 1., 0., 0., 0.},
{0., 0., 1., 0., 0., 0.},
{0., 0., 0., 1., 1., 0.5},
{0., 0., 0., 0., 1., 1.},
{0., 0., 0., 0., 0., 1.}});

// Now we can predict the next state based on the initialization values.
filter.predict();

// Measure and Update
// The dimension of zn is 2x1 and the dimension of xn is 6x1. Therefore the
// dimension of the observation matrix H shall be 2x6.
filter.h(kalman::output_model{{1, 0, 0, 0, 0, 0}, {0, 0, 0, 1, 0, 0}});
filter.h(
kalman::output_model{{1., 0., 0., 0., 0., 0.}, {0., 0., 0., 1., 0., 0.}});

// Assume that the x and y measurements are uncorrelated, i.e. error in the x
// coordinate measurement doesn't depend on the error in the y coordinate
Expand All @@ -94,7 +96,7 @@ template <auto Size> using vector = column_vector<double, Size>;
// For the sake of the example simplicity, we will assume a constant
// measurement uncertainty: R1 = R2...Rn-1 = Rn = R The measurement error
// standard deviation: σxm = σym = 3m. The variance 9.
filter.r(kalman::output_uncertainty{{9, 0}, {0, 9}});
filter.r(kalman::output_uncertainty{{9., 0.}, {0., 9.}});

// The measurement values: z1 = [-393.66, 300.4]
filter.update(-393.66, 300.4);
Expand Down
Loading

0 comments on commit 2504a91

Please sign in to comment.