diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index eb06d9f3e..1d4365766 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -52,10 +52,28 @@ foreach(SAMPLE "kf_1x1x0_building_height.cpp" "kf_1x1x0_liquid_temperature.cpp" COMMAND ${TEST_COMMAND} $) 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} + $) + 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( diff --git a/sample/ekf_4x1x0_soaring.cpp b/sample/ekf_4x1x0_soaring.cpp index b19f4d402..280a98151 100644 --- a/sample/ekf_4x1x0_soaring.cpp +++ b/sample/ekf_4x1x0_soaring.cpp @@ -52,26 +52,26 @@ template using vector = column_vector; 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}); diff --git a/sample/kf_2x1x1_rocket_altitude.cpp b/sample/kf_2x1x1_rocket_altitude.cpp index 04f1c77f1..faee1da0d 100644 --- a/sample/kf_2x1x1_rocket_altitude.cpp +++ b/sample/kf_2x1x1_rocket_altitude.cpp @@ -60,7 +60,7 @@ template using vector = column_vector; // 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 @@ -83,7 +83,7 @@ template using vector = column_vector; [[maybe_unused]] const kalman::input &u, const std::chrono::milliseconds &delta_time) { const auto dt{std::chrono::duration(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: diff --git a/sample/kf_6x2x0_vehicle_location.cpp b/sample/kf_6x2x0_vehicle_location.cpp index cd73fdaf7..519dedc6d 100644 --- a/sample/kf_6x2x0_vehicle_location.cpp +++ b/sample/kf_6x2x0_vehicle_location.cpp @@ -54,28 +54,29 @@ template using vector = column_vector; // 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(); @@ -83,7 +84,8 @@ template using vector = column_vector; // 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 @@ -94,7 +96,7 @@ template using vector = column_vector; // 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); diff --git a/sample/kf_8x4x0_deep_sort_bounding_box.cpp b/sample/kf_8x4x0_deep_sort_bounding_box.cpp index 2f94f28c7..caae1f6fc 100644 --- a/sample/kf_8x4x0_deep_sort_bounding_box.cpp +++ b/sample/kf_8x4x0_deep_sort_bounding_box.cpp @@ -43,106 +43,106 @@ template using vector = column_vector; // A hundred bounding box output measurements `(x, y, a, h)` from Deep SORT's // MOT16 sample, tracker #201. - const kalman::output measured[]{{603.5F, 251.5F, 0.187335092348285F, 379}, - {599, 241, 0.24390243902439F, 328}, - {599, 239.5F, 0.257234726688103F, 311}, - {602.5F, 244, 0.240131578947368F, 304}, - {598, 248.5F, 0.272425249169435F, 301}, - {596.5F, 240.5F, 0.283276450511945F, 293}, - {601, 227, 0.301587301587302F, 252}, - {603.5F, 235.5F, 0.282868525896414F, 251}, - {602, 242.5F, 0.292490118577075F, 253}, - {602.5F, 253, 0.218562874251497F, 334}, - {593, 254, 0.273291925465838F, 322}, - {603, 264, 0.22360248447205F, 322}, - {600.5F, 278.5F, 0.198966408268734F, 387}, - {593, 280, 0.237113402061856F, 388}, - {588.5F, 269, 0.267195767195767F, 378}, - {579, 260, 0.311111111111111F, 360}, - {565.5F, 268.5F, 0.339130434782609F, 345}, - {558.5F, 255.5F, 0.366568914956012F, 341}, - {544, 268, 0.364705882352941F, 340}, - {533, 258.5F, 0.356083086053412F, 337}, - {519, 258, 0.353293413173653F, 334}, - {511.5F, 252.5F, 0.333333333333333F, 333}, - {515.5F, 252.5F, 0.31306990881459F, 329}, - {523.5F, 251, 0.298192771084337F, 332}, - {540, 252.5F, 0.318318318318318F, 333}, - {574, 262, 0.344827586206897F, 348}, - {590.5F, 265, 0.278735632183908F, 348}, - {613, 268, 0.164556962025316F, 316}, - {617, 260.5F, 0.161172161172161F, 273}, - {615.5F, 261.5F, 0.15210355987055F, 309}, - {605.5F, 259, 0.226351351351351F, 296}, - {595.5F, 258.5F, 0.289036544850498F, 301}, - {588, 257.5F, 0.350515463917526F, 291}, - {579.5F, 254, 0.343537414965986F, 294}, - {569.5F, 258.5F, 0.353535353535354F, 297}, - {565.5F, 257, 0.37248322147651F, 298}, - {555, 250, 0.388157894736842F, 304}, - {546.5F, 249, 0.336666666666667F, 300}, - {535, 251, 0.30718954248366F, 306}, - {530, 246, 0.308724832214765F, 298}, - {521, 252, 0.278145695364238F, 302}, - {521.5F, 254.5F, 0.331010452961672F, 287}, - {521, 258.5F, 0.32280701754386F, 285}, - {519.5F, 255, 0.316326530612245F, 294}, - {518.5F, 255, 0.304794520547945F, 292}, - {511, 253, 0.310810810810811F, 296}, - {506, 255, 0.319727891156463F, 294}, - {499, 256, 0.352112676056338F, 284}, - {492.5F, 256.5F, 0.349152542372881F, 295}, - {489.5F, 257, 0.362068965517241F, 290}, - {481, 251.5F, 0.357894736842105F, 285}, - {474, 249, 0.324137931034483F, 290}, - {466, 250, 0.306122448979592F, 294}, - {461.5F, 248, 0.304794520547945F, 292}, - {450.5F, 248.5F, 0.323843416370107F, 281}, - {442, 260.5F, 0.32280701754386F, 285}, - {437, 255.5F, 0.329824561403509F, 285}, - {427, 251.5F, 0.329896907216495F, 291}, - {419, 251, 0.330985915492958F, 284}, - {411, 251, 0.328671328671329F, 286}, - {411, 251.5F, 0.325259515570934F, 289}, - {410, 249, 0.324137931034483F, 290}, - {407, 247.5F, 0.346020761245675F, 289}, - {398.5F, 248.5F, 0.356890459363958F, 283}, - {393, 249, 0.347222222222222F, 288}, - {390.5F, 246.5F, 0.331058020477816F, 293}, - {387, 246, 0.308724832214765F, 298}, - {379.5F, 244.5F, 0.303754266211604F, 293}, - {370, 255.5F, 0.258899676375404F, 309}, - {372, 252.5F, 0.307167235494881F, 293}, - {368, 254.5F, 0.311418685121107F, 289}, - {365.5F, 251, 0.322916666666667F, 288}, - {360.5F, 250.5F, 0.301694915254237F, 295}, - {353, 251.5F, 0.316151202749141F, 291}, - {349.5F, 248.5F, 0.32404181184669F, 287}, - {343.5F, 246, 0.327464788732394F, 284}, - {334.5F, 251.5F, 0.335689045936396F, 283}, - {328.5F, 249.5F, 0.342960288808664F, 277}, - {321.5F, 256.5F, 0.328621908127208F, 283}, - {321.5F, 259.5F, 0.317073170731707F, 287}, - {319.5F, 252, 0.313380281690141F, 284}, - {317.5F, 247.5F, 0.314487632508834F, 283}, - {314.5F, 248, 0.313380281690141F, 284}, - {318.5F, 255, 0.311188811188811F, 286}, - {324.5F, 252, 0.317857142857143F, 280}, - {328.5F, 249, 0.311188811188811F, 286}, - {330, 248, 0.318840579710145F, 276}, - {334.5F, 245, 0.320143884892086F, 278}, - {342.5F, 248, 0.324817518248175F, 274}, - {348, 247.5F, 0.312727272727273F, 275}, - {349.5F, 245.5F, 0.326007326007326F, 273}, - {350, 250, 0.321167883211679F, 274}, - {350.5F, 252.5F, 0.323636363636364F, 275}, - {356.5F, 249, 0.31294964028777F, 278}, - {356.5F, 245, 0.320143884892086F, 278}, - {357, 245, 0.314285714285714F, 280}, - {361, 246, 0.318840579710145F, 276}, - {364, 251.5F, 0.308771929824561F, 285}, - {368, 252.5F, 0.303886925795053F, 283}, - {369, 250.5F, 0.29757785467128F, 289}}; + const kalman::output measured[]{{603.5F, 251.5F, 0.187335092348285F, 379.F}, + {599.F, 241.F, 0.24390243902439F, 328.F}, + {599.F, 239.5F, 0.257234726688103F, 311.F}, + {602.5F, 244.F, 0.240131578947368F, 304.F}, + {598.F, 248.5F, 0.272425249169435F, 301.F}, + {596.5F, 240.5F, 0.283276450511945F, 293.F}, + {601.F, 227.F, 0.301587301587302F, 252.F}, + {603.5F, 235.5F, 0.282868525896414F, 251.F}, + {602.F, 242.5F, 0.292490118577075F, 253.F}, + {602.5F, 253.F, 0.218562874251497F, 334.F}, + {593.F, 254.F, 0.273291925465838F, 322.F}, + {603.F, 264.F, 0.22360248447205F, 322.F}, + {600.5F, 278.5F, 0.198966408268734F, 387.F}, + {593.F, 280.F, 0.237113402061856F, 388.F}, + {588.5F, 269.F, 0.267195767195767F, 378.F}, + {579.F, 260.F, 0.311111111111111F, 360.F}, + {565.5F, 268.5F, 0.339130434782609F, 345.F}, + {558.5F, 255.5F, 0.366568914956012F, 341.F}, + {544.F, 268.F, 0.364705882352941F, 340.F}, + {533.F, 258.5F, 0.356083086053412F, 337.F}, + {519.F, 258.F, 0.353293413173653F, 334.F}, + {511.5F, 252.5F, 0.333333333333333F, 333.F}, + {515.5F, 252.5F, 0.31306990881459F, 329.F}, + {523.5F, 251.F, 0.298192771084337F, 332.F}, + {540.F, 252.5F, 0.318318318318318F, 333.F}, + {574.F, 262.F, 0.344827586206897F, 348.F}, + {590.5F, 265.F, 0.278735632183908F, 348.F}, + {613.F, 268.F, 0.164556962025316F, 316.F}, + {617.F, 260.5F, 0.161172161172161F, 273.F}, + {615.5F, 261.5F, 0.15210355987055F, 309.F}, + {605.5F, 259.F, 0.226351351351351F, 296.F}, + {595.5F, 258.5F, 0.289036544850498F, 301.F}, + {588.F, 257.5F, 0.350515463917526F, 291.F}, + {579.5F, 254.F, 0.343537414965986F, 294.F}, + {569.5F, 258.5F, 0.353535353535354F, 297.F}, + {565.5F, 257.F, 0.37248322147651F, 298.F}, + {555.F, 250.F, 0.388157894736842F, 304.F}, + {546.5F, 249.F, 0.336666666666667F, 300.F}, + {535.F, 251.F, 0.30718954248366F, 306.F}, + {530.F, 246.F, 0.308724832214765F, 298.F}, + {521.F, 252.F, 0.278145695364238F, 302.F}, + {521.5F, 254.5F, 0.331010452961672F, 287.F}, + {521.F, 258.5F, 0.32280701754386F, 285.F}, + {519.5F, 255.F, 0.316326530612245F, 294.F}, + {518.5F, 255.F, 0.304794520547945F, 292.F}, + {511.F, 253.F, 0.310810810810811F, 296.F}, + {506.F, 255.F, 0.319727891156463F, 294.F}, + {499.F, 256.F, 0.352112676056338F, 284.F}, + {492.5F, 256.5F, 0.349152542372881F, 295.F}, + {489.5F, 257.F, 0.362068965517241F, 290.F}, + {481.F, 251.5F, 0.357894736842105F, 285.F}, + {474.F, 249.F, 0.324137931034483F, 290.F}, + {466.F, 250.F, 0.306122448979592F, 294.F}, + {461.5F, 248.F, 0.304794520547945F, 292.F}, + {450.5F, 248.5F, 0.323843416370107F, 281.F}, + {442.F, 260.5F, 0.32280701754386F, 285.F}, + {437.F, 255.5F, 0.329824561403509F, 285.F}, + {427.F, 251.5F, 0.329896907216495F, 291.F}, + {419.F, 251.F, 0.330985915492958F, 284.F}, + {411.F, 251.F, 0.328671328671329F, 286.F}, + {411.F, 251.5F, 0.325259515570934F, 289.F}, + {410.F, 249.F, 0.324137931034483F, 290.F}, + {407.F, 247.5F, 0.346020761245675F, 289.F}, + {398.5F, 248.5F, 0.356890459363958F, 283.F}, + {393.F, 249.F, 0.347222222222222F, 288.F}, + {390.5F, 246.5F, 0.331058020477816F, 293.F}, + {387.F, 246.F, 0.308724832214765F, 298.F}, + {379.5F, 244.5F, 0.303754266211604F, 293.F}, + {370.F, 255.5F, 0.258899676375404F, 309.F}, + {372.F, 252.5F, 0.307167235494881F, 293.F}, + {368.F, 254.5F, 0.311418685121107F, 289.F}, + {365.5F, 251.F, 0.322916666666667F, 288.F}, + {360.5F, 250.5F, 0.301694915254237F, 295.F}, + {353.F, 251.5F, 0.316151202749141F, 291.F}, + {349.5F, 248.5F, 0.32404181184669F, 287.F}, + {343.5F, 246.F, 0.327464788732394F, 284.F}, + {334.5F, 251.5F, 0.335689045936396F, 283.F}, + {328.5F, 249.5F, 0.342960288808664F, 277.F}, + {321.5F, 256.5F, 0.328621908127208F, 283.F}, + {321.5F, 259.5F, 0.317073170731707F, 287.F}, + {319.5F, 252.F, 0.313380281690141F, 284.F}, + {317.5F, 247.5F, 0.314487632508834F, 283.F}, + {314.5F, 248.F, 0.313380281690141F, 284.F}, + {318.5F, 255.F, 0.311188811188811F, 286.F}, + {324.5F, 252.F, 0.317857142857143F, 280.F}, + {328.5F, 249.F, 0.311188811188811F, 286.F}, + {330.F, 248.F, 0.318840579710145F, 276.F}, + {334.5F, 245.F, 0.320143884892086F, 278.F}, + {342.5F, 248.F, 0.324817518248175F, 274.F}, + {348.F, 247.5F, 0.312727272727273F, 275.F}, + {349.5F, 245.5F, 0.326007326007326F, 273.F}, + {350.F, 250.F, 0.321167883211679F, 274.F}, + {350.5F, 252.5F, 0.323636363636364F, 275.F}, + {356.5F, 249.F, 0.31294964028777F, 278.F}, + {356.5F, 245.F, 0.320143884892086F, 278.F}, + {357.F, 245.F, 0.314285714285714F, 280.F}, + {361.F, 246.F, 0.318840579710145F, 276.F}, + {364.F, 251.5F, 0.308771929824561F, 285.F}, + {368.F, 252.5F, 0.303886925795053F, 283.F}, + {369.F, 250.5F, 0.29757785467128F, 289.F}}; // Initialization // The filter is initialized at runtime, on bounding box detection, with the @@ -150,8 +150,8 @@ template using vector = column_vector; // [px, py, pa, ph, vx, vy, va, vh]. const kalman::output initial_box{605.0F, 248.0F, 0.20481927710843373F, 332.0F}; - filter.x(initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0, 0, - 0, 0); + filter.x(initial_box(0), initial_box(1), initial_box(2), initial_box(3), 0.F, + 0.F, 0.F, 0.F); // Experimental position and velocity uncertainty standard deviation weights. const float position_weight{1.F / 20.F}; @@ -171,15 +171,16 @@ template using vector = column_vector; // Constant velocity, linear state transition model. From one image frame to // the other. - const float delta_time{1}; - filter.f(kalman::state_transition{{1, 0, 0, 0, delta_time, 0, 0, 0}, - {0, 1, 0, 0, 0, delta_time, 0, 0}, - {0, 0, 1, 0, 0, 0, delta_time, 0}, - {0, 0, 0, 1, 0, 0, 0, delta_time}, - {0, 0, 0, 0, 1, 0, 0, 0}, - {0, 0, 0, 0, 0, 1, 0, 0}, - {0, 0, 0, 0, 0, 0, 1, 0}, - {0, 0, 0, 0, 0, 0, 0, 1}}); + const float delta_time{1.F}; + filter.f( + kalman::state_transition{{1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F, 0.F}, + {0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F, 0.F}, + {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time, 0.F}, + {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, delta_time}, + {0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F, 0.F}, + {0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 1.F}}); filter.q([position_weight, velocity_weight]( const kalman::state &x) -> kalman::process_uncertainty { @@ -202,10 +203,10 @@ template using vector = column_vector; // Measure and Update // Direct linear observation transition model. - filter.h(kalman::output_model{{1, 0, 0, 0, 0, 0, 0, 0}, - {0, 1, 0, 0, 0, 0, 0, 0}, - {0, 0, 1, 0, 0, 0, 0, 0}, - {0, 0, 0, 1, 0, 0, 0, 0}}); + filter.h(kalman::output_model{{1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + {0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + {0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F, 0.F}, + {0.F, 0.F, 0.F, 1.F, 0.F, 0.F, 0.F, 0.F}}); // Observation, measurement noise covariance. filter.r([position_weight](const kalman::state &x,