diff --git a/.github/workflows/verify_code_style_format.yml b/.github/workflows/verify_code_style_format.yml index 3b5f538fdc..4994c50236 100644 --- a/.github/workflows/verify_code_style_format.yml +++ b/.github/workflows/verify_code_style_format.yml @@ -17,4 +17,4 @@ jobs: - name: Install run: sudo apt install clang-format-14 - name: Format - run: find . -iname *.hpp -o -iname *.cpp | xargs clang-format-14 --Werror --dry-run --verbose -style=file + run: find . -iname *.hpp -o -iname *.cpp -o -iname *.tpp -o -iname support/** | xargs clang-format-14 --Werror --dry-run --verbose -style=file diff --git a/include/fcarouge/internal/kalman.tpp b/include/fcarouge/internal/kalman.tpp new file mode 100644 index 0000000000..b2e438123d --- /dev/null +++ b/include/fcarouge/internal/kalman.tpp @@ -0,0 +1,460 @@ +/*_ __ _ __ __ _ _ + | |/ / /\ | | | \/ | /\ | \ | | + | ' / / \ | | | \ / | / \ | \| | + | < / /\ \ | | | |\/| | / /\ \ | . ` | + | . \ / ____ \| |____| | | |/ ____ \| |\ | + |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| + +Kalman Filter for C++ +Version 0.1.0 +https://github.com/FrancoisCarouge/Kalman + +SPDX-License-Identifier: Unlicense + +This is free and unencumbered software released into the public domain. + +Anyone is free to copy, modify, publish, use, compile, sell, or +distribute this software, either in source code form or as a compiled +binary, for any purpose, commercial or non-commercial, and by any +means. + +In jurisdictions that recognize copyright laws, the author or authors +of this software dedicate any and all copyright interest in the +software to the public domain. We make this dedication for the benefit +of the public at large and to the detriment of our heirs and +successors. We intend this dedication to be an overt act of +relinquishment in perpetuity of all present and future rights to this +software under copyright law. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF +MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. +IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR +OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, +ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR +OTHER DEALINGS IN THE SOFTWARE. + +For more information, please refer to */ + +#ifndef FCAROUGE_INTERNAL_KALMAN_TPP +#define FCAROUGE_INTERNAL_KALMAN_TPP + +namespace fcarouge +{ + +template +[[nodiscard("The returned state estimate column vector X is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::x() const -> state +{ + return filter.x; +} + +template +inline constexpr void +kalman::x(const state &value) +{ + filter.x = value; +} + +template +inline constexpr void +kalman::x(state &&value) +{ + filter.x = std::move(value); +} + +template +inline constexpr void +kalman::x(const value_type &value, + const std::same_as auto + &...values) +{ + filter.x = std::move(state{ value, values... }); +} + +template +inline constexpr void +kalman::x(value_type &&value, + std::same_as auto + &&...values) +{ + filter.x = std::move(state{ std::forward(value), + std::forward(values)... }); +} + +template +[[nodiscard("The returned observation column vector Z is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::z() const -> output +{ + return filter.z; +} + +template +[[nodiscard("The returned control column vector U is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::u() const -> input requires(Input > 0) +{ + return filter.u; +} + +template +[[nodiscard("The returned estimated covariance matrix P is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::p() const -> estimate_uncertainty +{ + return filter.p; +} + +template +inline constexpr void +kalman::p(const estimate_uncertainty &value) +{ + filter.p = value; +} + +template +inline constexpr void +kalman::p(estimate_uncertainty &&value) +{ + filter.p = std::move(value); +} + +template +inline constexpr void +kalman::p(const value_type &value, + const std::same_as auto + &...values) +{ + filter.p = std::move(estimate_uncertainty{ value, values... }); +} + +template +inline constexpr void +kalman::p(value_type &&value, + std::same_as auto + &&...values) +{ + filter.p = std::move( + estimate_uncertainty{ std::forward(value), + std::forward(values)... }); +} + +template +[[nodiscard("The returned process noise covariance matrix Q is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::q() const -> process_uncertainty +{ + return filter.q; +} + +template +inline constexpr void +kalman::q(const process_uncertainty &value) +{ + filter.q = value; +} + +template +inline constexpr void +kalman::q(process_uncertainty &&value) +{ + filter.q = std::move(value); +} + +//! @todo Reset functions or values when the other is set? +template +inline constexpr void +kalman::q(const value_type &value, + const std::same_as auto + &...values) +{ + filter.q = std::move(process_uncertainty{ value, values... }); +} + +template +inline constexpr void +kalman::q(value_type &&value, + std::same_as auto + &&...values) +{ + filter.q = std::move( + process_uncertainty{ std::forward(value), + std::forward(values)... }); +} + +template +inline constexpr void +kalman::q(const noise_process_function &callable) +{ + filter.noise_process_q = callable; +} + +template +inline constexpr void +kalman::q(noise_process_function &&callable) +{ + filter.noise_process_q = std::forward(callable); +} + +template +[[nodiscard("The returned observation noise covariance matrix R is " + "unexpectedly discarded.")]] inline constexpr auto +kalman::r() const -> output_uncertainty +{ + return filter.r; +} + +template +inline constexpr void +kalman::r(const output_uncertainty &value) +{ + filter.r = value; +} + +template +inline constexpr void +kalman::r(output_uncertainty &&value) +{ + filter.r = std::move(value); +} + +template +inline constexpr void +kalman::r(const value_type &value, + const std::same_as auto + &...values) +{ + filter.r = std::move(output_uncertainty{ value, values... }); +} + +template +inline constexpr void +kalman::r(value_type &&value, + std::same_as auto + &&...values) +{ + filter.r = std::move( + output_uncertainty{ std::forward(value), + std::forward(values)... }); +} + +template +inline constexpr void +kalman::r(const noise_observation_function + &callable) +{ + filter.noise_observation_r = callable; +} + +template +inline constexpr void +kalman::r(noise_observation_function &&callable) +{ + filter.noise_observation_r = std::forward(callable); +} + +template +[[nodiscard("The returned state transition matrix F is unexpectedly " + "discarded.")]] inline constexpr auto +kalman::f() const -> state_transition +{ + return filter.f; +} + +// REMOVE NODISCARD? +//////////////////////////////////////////////////////////////////////////////// + +template +inline constexpr void +kalman::transition(const transition_function + &callable) +{ + filter.transition = callable; +} + +template +inline constexpr void +kalman::transition(transition_function &&callable) +{ + filter.transition = std::forward(callable); +} + +template +inline constexpr void +kalman::observation(const observation_function + &callable) +{ + filter.observation = callable; +} + +template +inline constexpr void +kalman::observation(observation_function + &&callable) +{ + filter.observation = std::forward(callable); +} + +template +template +inline constexpr void +kalman::operator()(const auto &...arguments) +{ + filter.template operator()(arguments...); +} + +template +inline constexpr void +kalman::update(const auto &...arguments) +{ + filter.update(arguments...); +} + +template +inline constexpr void +kalman::predict(const auto &...arguments) +{ + filter.predict(arguments...); +} + +} // namespace fcarouge + +#endif // FCAROUGE_KALMAN_HPP diff --git a/include/fcarouge/kalman.hpp b/include/fcarouge/kalman.hpp index cdfa43bd70..bf70fc60e7 100644 --- a/include/fcarouge/kalman.hpp +++ b/include/fcarouge/kalman.hpp @@ -255,6 +255,34 @@ class kalman using innovation_uncertainty = typename implementation::innovation_uncertainty; + //! @brief ... + using observation_state_function = + typename implementation::observation_state_function; + + //! @brief ... + using noise_observation_function = + typename implementation::noise_observation_function; + + //! @brief ... + using transition_state_function = + typename implementation::transition_state_function; + + //! @brief ... + using noise_process_function = + typename implementation::noise_process_function; + + //! @brief ... + //! + //! @todo Conditionally remove this member type when no input is present. + using transition_control_function = + typename implementation::transition_control_function; + + //! @brief ... + using transition_function = typename implementation::transition_function; + + //! @brief ... + using observation_function = typename implementation::observation_function; + //! @} //! @name Public Member Functions @@ -336,30 +364,21 @@ class kalman //! @complexity Constant. [[nodiscard("The returned state estimate column vector X is unexpectedly " "discarded.")]] inline constexpr auto - x() const -> state - { - return filter.x; - } + x() const -> state; //! @brief Sets the state estimate column vector X. //! //! @param value The copied state estimate column vector X. //! //! @complexity Constant. - inline constexpr void x(const state &value) - { - filter.x = value; - } + inline constexpr void x(const state &value); //! @brief Sets the state estimate column vector X. //! //! @param value The moved state estimate column vector X. //! //! @complexity Constant. - inline constexpr void x(state &&value) - { - filter.x = std::move(value); - } + inline constexpr void x(state &&value); //! @brief Sets the state estimate column vector X. //! @@ -369,10 +388,8 @@ class kalman //! estimate column vector X. //! //! @complexity Constant. - inline constexpr void x(const auto &value, const auto &...values) - { - filter.x = std::move(state{ value, values... }); - } + inline constexpr void x(const value_type &value, + const std::same_as auto &...values); //! @brief Sets the state estimate column vector X. //! @@ -382,11 +399,8 @@ class kalman //! estimate column vector X. //! //! @complexity Constant. - inline constexpr void x(auto &&value, auto &&...values) - { - filter.x = std::move(state{ std::forward(value), - std::forward(values)... }); - } + inline constexpr void x(value_type &&value, + std::same_as auto &&...values); //! @brief Returns the last observation column vector Z. //! @@ -395,10 +409,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned observation column vector Z is unexpectedly " "discarded.")]] inline constexpr auto - z() const -> output - { - return filter.z; - } + z() const -> output; //! @brief Returns the last control column vector U. //! @@ -409,10 +420,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned control column vector U is unexpectedly " "discarded.")]] inline constexpr auto - u() const -> input requires(Input > 0) - { - return filter.u; - } + u() const -> input requires(Input > 0); //! @brief Returns the estimated covariance matrix P. //! @@ -421,30 +429,21 @@ class kalman //! @complexity Constant. [[nodiscard("The returned estimated covariance matrix P is unexpectedly " "discarded.")]] inline constexpr auto - p() const -> estimate_uncertainty - { - return filter.p; - } + p() const -> estimate_uncertainty; //! @brief Sets the estimated covariance matrix P. //! //! @param value The copied estimated covariance matrix P. //! //! @complexity Constant. - inline constexpr void p(const estimate_uncertainty &value) - { - filter.p = value; - } + inline constexpr void p(const estimate_uncertainty &value); //! @brief Sets the estimated covariance matrix P. //! //! @param value The moved estimated covariance matrix P. //! //! @complexity Constant. - inline constexpr void p(estimate_uncertainty &&value) - { - filter.p = std::move(value); - } + inline constexpr void p(estimate_uncertainty &&value); //! @brief Sets the estimated covariance matrix P. //! @@ -454,10 +453,8 @@ class kalman //! estimated covariance matrix P. //! //! @complexity Constant. - inline constexpr void p(const auto &value, const auto &...values) - { - filter.p = std::move(estimate_uncertainty{ value, values... }); - } + inline constexpr void p(const value_type &value, + const std::same_as auto &...values); //! @brief Sets the estimated covariance matrix P. //! @@ -467,12 +464,8 @@ class kalman //! covariance matrix P. //! //! @complexity Constant. - inline constexpr void p(auto &&value, auto &&...values) - { - filter.p = std::move( - estimate_uncertainty{ std::forward(value), - std::forward(values)... }); - } + inline constexpr void p(value_type &&value, + std::same_as auto &&...values); //! @brief Returns the process noise covariance matrix Q. //! @@ -481,30 +474,21 @@ class kalman //! @complexity Constant. [[nodiscard("The returned process noise covariance matrix Q is unexpectedly " "discarded.")]] inline constexpr auto - q() const -> process_uncertainty - { - return filter.q; - } + q() const -> process_uncertainty; //! @brief Sets the process noise covariance matrix Q. //! //! @param value The copied process noise covariance matrix Q. //! //! @complexity Constant. - inline constexpr void q(const process_uncertainty &value) - { - filter.q = value; - } + inline constexpr void q(const process_uncertainty &value); //! @brief Sets the process noise covariance matrix Q. //! //! @param value The moved process noise covariance matrix Q. //! //! @complexity Constant. - inline constexpr void q(process_uncertainty &&value) - { - filter.q = std::move(value); - } + inline constexpr void q(process_uncertainty &&value); //! @brief Sets the process noise covariance matrix Q. //! @@ -514,12 +498,8 @@ class kalman //! noise covariance matrix Q. //! //! @complexity Constant. - inline constexpr void q(const auto &value, const auto &...values) requires( - !std::is_assignable_v>) - { - filter.q = std::move(process_uncertainty{ value, values... }); - } + inline constexpr void q(const value_type &value, + const std::same_as auto &...values); //! @brief Sets the process noise covariance matrix Q. //! @@ -529,16 +509,8 @@ class kalman //! process noise covariance matrix Q. //! //! @complexity Constant. - //! - //! @todo Reset functions or values when the other is set? - inline constexpr void q(auto &&value, auto &&...values) requires( - !std::is_assignable_v>) - { - filter.q = std::move( - process_uncertainty{ std::forward(value), - std::forward(values)... }); - } + inline constexpr void q(value_type &&value, + std::same_as auto &&...values); //! @brief Sets the process noise covariance matrix Q function. //! @@ -549,14 +521,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - //! - //! @todo Understand why Clang Tidy doesn't find the out-of-line definition. - inline constexpr void q(const auto &callable) requires( - std::is_assignable_v>) - { - filter.noise_process_q = callable; - } + inline constexpr void q(const noise_process_function &callable); //! @brief Sets the process noise covariance matrix Q function. //! @@ -567,12 +532,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - inline constexpr void q(auto &&callable) requires( - std::is_assignable_v>) - { - filter.noise_process_q = std::forward(callable); - } + inline constexpr void q(noise_process_function &&callable); //! @brief Returns the observation noise covariance //! matrix R. @@ -584,30 +544,21 @@ class kalman //! @complexity Constant. [[nodiscard("The returned observation noise covariance matrix R is " "unexpectedly discarded.")]] inline constexpr auto - r() const -> output_uncertainty - { - return filter.r; - } + r() const -> output_uncertainty; //! @brief Sets the observation noise covariance matrix R. //! //! @param value The copied observation noise covariance matrix R. //! //! @complexity Constant. - inline constexpr void r(const output_uncertainty &value) - { - filter.r = value; - } + inline constexpr void r(const output_uncertainty &value); //! @brief Sets the observation noise covariance matrix R. //! //! @param value The moved observation noise covariance matrix R. //! //! @complexity Constant. - inline constexpr void r(output_uncertainty &&value) - { - filter.r = std::move(value); - } + inline constexpr void r(output_uncertainty &&value); //! @brief Sets the observation noise covariance matrix R. //! @@ -617,12 +568,8 @@ class kalman //! observation noise covariance matrix R. //! //! @complexity Constant. - inline constexpr void r(const auto &value, const auto &...values) requires( - !std::is_assignable_v>) - { - filter.r = std::move(output_uncertainty{ value, values... }); - } + inline constexpr void r(const value_type &value, + const std::same_as auto &...values); //! @brief Sets the observation noise covariance matrix R. //! @@ -632,14 +579,8 @@ class kalman //! observation noise covariance matrix R. //! //! @complexity Constant. - inline constexpr void r(auto &&value, auto &&...values) requires( - !std::is_assignable_v>) - { - filter.r = std::move( - output_uncertainty{ std::forward(value), - std::forward(values)... }); - } + inline constexpr void r(value_type &&value, + std::same_as auto &&...values); //! @brief Sets the observation noise covariance matrix R function. //! @@ -650,14 +591,7 @@ class kalman //! on prediction steps. //! //! @complexity Constant. - //! - //! @todo Understand why Clang Tidy doesn't find the out-of-line definition. - inline constexpr void r(const auto &callable) requires( - std::is_assignable_v>) - { - filter.noise_observation_r = callable; - } + inline constexpr void r(const noise_observation_function &callable); //! @brief Sets the observation noise covariance matrix R function. //! @@ -668,12 +602,7 @@ class kalman //! on prediction steps. //! //! @complexity Constant. - inline constexpr void r(auto &&callable) requires( - std::is_assignable_v>) - { - filter.noise_observation_r = std::forward(callable); - } + inline constexpr void r(noise_observation_function &&callable); //! @brief Returns the state transition matrix F. //! @@ -682,10 +611,7 @@ class kalman //! @complexity Constant. [[nodiscard("The returned state transition matrix F is unexpectedly " "discarded.")]] inline constexpr auto - f() const -> state_transition - { - return filter.f; - } + f() const -> state_transition; //! @brief Sets the state transition matrix F. //! @@ -715,9 +641,8 @@ class kalman //! transition matrix F. //! //! @complexity Constant. - inline constexpr void f(const auto &value, const auto &...values) requires( - !std::is_assignable_v>) + inline constexpr void f(const value_type &value, + const std::same_as auto &...values) { filter.f = std::move(state_transition{ value, values... }); } @@ -730,9 +655,8 @@ class kalman //! transition matrix F. //! //! @complexity Constant. - inline constexpr void f(auto &&value, auto &&...values) requires( - !std::is_assignable_v>) + inline constexpr void f(value_type &&value, + std::same_as auto &&...values) { filter.f = std::move( state_transition{ std::forward(value), @@ -753,9 +677,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - inline constexpr void f(const auto &callable) requires( - std::is_assignable_v>) + inline constexpr void f(const transition_state_function &callable) { filter.transition_state_f = callable; } @@ -774,9 +696,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - inline constexpr void f(auto &&callable) requires( - std::is_assignable_v>) + inline constexpr void f(transition_state_function &&callable) { filter.transition_state_f = std::forward(callable); } @@ -821,9 +741,8 @@ class kalman //! observation, measurement transition matrix H. //! //! @complexity Constant. - inline constexpr void h(const auto &value, const auto &...values) requires( - !std::is_assignable_v>) + inline constexpr void h(const value_type &value, + const std::same_as auto &...values) { filter.h = std::move(output_model{ value, values... }); } @@ -836,9 +755,8 @@ class kalman //! observation, measurement transition matrix H. //! //! @complexity Constant. - inline constexpr void h(auto &&value, auto &&...values) requires( - !std::is_assignable_v>) + inline constexpr void h(value_type &&value, + std::same_as auto &&...values) { filter.h = std::move(output_model{ std::forward(value), @@ -859,9 +777,7 @@ class kalman //! matrix H on update steps. //! //! @complexity Constant. - inline constexpr void h(const auto &callable) requires( - std::is_assignable_v>) + inline constexpr void h(const observation_state_function &callable) { filter.observation_state_h = callable; } @@ -880,9 +796,7 @@ class kalman //! matrix H on update steps. //! //! @complexity Constant. - inline constexpr void h(auto &&callable) requires( - std::is_assignable_v>) + inline constexpr void h(observation_state_function &&callable) { filter.observation_state_h = std::forward(callable); } @@ -927,10 +841,8 @@ class kalman //! transition matrix G. //! //! @complexity Constant. - inline constexpr void g(const auto &value, const auto &...values) requires( - Input > 0 && !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + inline constexpr void g(const value_type &value, + const std::same_as auto &...values) { filter.g = std::move(input_control{ value, values... }); } @@ -943,10 +855,8 @@ class kalman //! transition matrix G. //! //! @complexity Constant. - inline constexpr void g(auto &&value, auto &&...values) requires( - Input > 0 && !std::is_assignable_v< - typename implementation::transition_control_function, - std::decay_t>) + inline constexpr void g(value_type &&value, + std::same_as auto &&...values) { filter.g = std::move(input_control{ std::forward(value), @@ -962,10 +872,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - inline constexpr void g(const auto &callable) requires( - Input > 0 && - std::is_assignable_v>) + inline constexpr void g(const transition_control_function &callable) { filter.transition_control_g = callable; } @@ -979,10 +886,7 @@ class kalman //! prediction steps. //! //! @complexity Constant. - inline constexpr void g(auto &&callable) requires( - Input > 0 && - std::is_assignable_v>) + inline constexpr void g(transition_control_function &&callable) { filter.transition_control_g = std::forward(callable); } @@ -1034,12 +938,7 @@ class kalman //! matrix is the Jacobian of the state transition function. //! //! @complexity Constant. - //! - //! @todo Help the user with callable type definition, visibility. - inline constexpr void transition(const auto &callable) - { - filter.transition = callable; - } + inline constexpr void transition(const transition_function &callable); //! @brief Sets the extended state transition function f(x). //! @@ -1052,12 +951,7 @@ class kalman //! matrix is the Jacobian of the state transition function. //! //! @complexity Constant. - //! - //! @todo Help the user with callable type definition, visibility. - inline constexpr void transition(auto &&callable) - { - filter.transition = std::forward(callable); - } + inline constexpr void transition(transition_function &&callable); //! @brief Sets the extended state observation function h(x). //! @@ -1070,12 +964,7 @@ class kalman //! observation H matrix is the Jacobian of the state observation function. //! //! @complexity Constant. - //! - //! @todo Help the user with callable type definition, visibility. - inline constexpr void observation(const auto &callable) - { - filter.observation = callable; - } + inline constexpr void observation(const observation_function &callable); //! @brief Sets the extended state observation function h(x). //! @@ -1088,12 +977,7 @@ class kalman //! observation H matrix is the Jacobian of the state observation function. //! //! @complexity Constant. - //! - //! @todo Help the user with callable type definition, visibility. - inline constexpr void observation(auto &&callable) - { - filter.observation = std::forward(callable); - } + inline constexpr void observation(observation_function &&callable); //! @} @@ -1134,10 +1018,7 @@ class kalman //! @todo Understand why the implementation cannot be moved out of the class. //! @todo What should be the order of the parameters? Update first? template - inline constexpr void operator()(const auto &...arguments) - { - filter.template operator()(arguments...); - } + inline constexpr void operator()(const auto &...arguments); //! @brief Updates the estimates with the outcome of a measurement. //! @@ -1156,10 +1037,7 @@ class kalman //! sufficient for all clients? //! @todo Consider if returning the state column vector X would be preferable? //! Or fluent interface? Would be compatible with an ES-EKF implementation? - inline constexpr void update(const auto &...arguments) - { - filter.update(arguments...); - } + inline constexpr void update(const auto &...arguments); //! @brief Produces estimates of the state variables and uncertainties. //! @@ -1177,10 +1055,7 @@ class kalman //! sufficient for all clients? //! @todo Consider if returning the state column vector X would be preferable? //! Or fluent interface? Would be compatible with an ES-EKF implementation? - inline constexpr void predict(const auto &...arguments) - { - filter.predict(arguments...); - } + inline constexpr void predict(const auto &...arguments); //! @} @@ -1196,4 +1071,6 @@ class kalman } // namespace fcarouge +#include "internal/kalman.tpp" + #endif // FCAROUGE_KALMAN_HPP diff --git a/sample/rocket_altitude.cpp b/sample/rocket_altitude.cpp index bc3ecc9d07..92b4e7e28f 100644 --- a/sample/rocket_altitude.cpp +++ b/sample/rocket_altitude.cpp @@ -50,7 +50,7 @@ namespace // Initialization // We don't know the rocket location; we will set initial position and // velocity to 0. - k.x(0, 0); + k.x(0., 0.); // 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, @@ -105,7 +105,7 @@ namespace // Measure and Update // The dimension of zn is 1x1 and the dimension of xn is 2x1, so the dimension // of the observation matrix H will be 1x2. - k.h(1, 0); + k.h(1., 0.); // For the sake of the example simplicity, we will assume a constant // measurement uncertainty: R1 = R2...Rn-1 = Rn = R.