|
| 1 | +/*_ __ _ __ __ _ _ |
| 2 | + | |/ / /\ | | | \/ | /\ | \ | | |
| 3 | + | ' / / \ | | | \ / | / \ | \| | |
| 4 | + | < / /\ \ | | | |\/| | / /\ \ | . ` | |
| 5 | + | . \ / ____ \| |____| | | |/ ____ \| |\ | |
| 6 | + |_|\_\/_/ \_\______|_| |_/_/ \_\_| \_| |
| 7 | +
|
| 8 | +Kalman Filter for C++ |
| 9 | +Version 0.1.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_KALMAN_HPP |
| 40 | +#define FCAROUGE_INTERNAL_KALMAN_HPP |
| 41 | + |
| 42 | +//! @file |
| 43 | +//! @brief The main Kalman filter class. |
| 44 | + |
| 45 | +#include "kalman_equation.hpp" |
| 46 | +#include "kalman_operator.hpp" |
| 47 | + |
| 48 | +#include <functional> |
| 49 | +#include <type_traits> |
| 50 | + |
| 51 | +namespace fcarouge::internal |
| 52 | +{ |
| 53 | +template <typename State, typename Output = State, typename Input = State, |
| 54 | + template <typename> typename Transpose = transpose, |
| 55 | + template <typename> typename Symmetrize = symmetrize, |
| 56 | + template <typename, typename> typename Divide = divide, |
| 57 | + template <typename> typename Identity = identity, |
| 58 | + typename... PredictionArguments> |
| 59 | +struct kalman { |
| 60 | + //! @name Public Member Types |
| 61 | + //! @{ |
| 62 | + |
| 63 | + //! @brief Type of the state estimate vector X. |
| 64 | + using state = State; |
| 65 | + |
| 66 | + //! @brief Type of the observation vector Z. |
| 67 | + //! |
| 68 | + //! @details Also known as Y. |
| 69 | + using output = Output; |
| 70 | + |
| 71 | + //! @brief Type of the control vector U. |
| 72 | + using input = Input; |
| 73 | + |
| 74 | + //! @brief Type of the estimated covariance matrix P. |
| 75 | + //! |
| 76 | + //! @details Also known as Σ. |
| 77 | + using estimate_uncertainty = |
| 78 | + std:: invoke_result_t<Divide<State, State>, State, State>; |
| 79 | + |
| 80 | + //! @brief Type of the process noise covariance matrix Q. |
| 81 | + using process_uncertainty = |
| 82 | + std:: invoke_result_t<Divide<State, State>, State, State>; |
| 83 | + |
| 84 | + //! @brief Type of the observation, measurement noise covariance matrix R. |
| 85 | + using output_uncertainty = |
| 86 | + std:: invoke_result_t<Divide<Output, Output>, Output, Output>; |
| 87 | + |
| 88 | + //! @brief Type of the state transition matrix F. |
| 89 | + //! |
| 90 | + //! @details Also known as Φ or A. |
| 91 | + using state_transition = |
| 92 | + std:: invoke_result_t<Divide<State, State>, State, State>; |
| 93 | + |
| 94 | + //! @brief Type of the observation transition matrix H. |
| 95 | + //! |
| 96 | + //! @details Also known as C. |
| 97 | + using output_model = |
| 98 | + std:: invoke_result_t<Divide<Output, State>, Output, State>; |
| 99 | + |
| 100 | + //! @brief Type of the control transition matrix G. |
| 101 | + //! |
| 102 | + //! @details Also known as B. |
| 103 | + using input_control = |
| 104 | + std:: invoke_result_t<Divide<State, Input>, State, Input>; |
| 105 | + |
| 106 | + //! @} |
| 107 | + |
| 108 | + //! @name Public Member Variables |
| 109 | + //! @{ |
| 110 | + |
| 111 | + //! @brief The state estimate vector x. |
| 112 | + state x{}; |
| 113 | + |
| 114 | + //! @brief The estimate uncertainty, covariance matrix P. |
| 115 | + //! |
| 116 | + //! @details The estimate uncertainty, covariance is also known as Σ. |
| 117 | + estimate_uncertainty p{ Identity<estimate_uncertainty>()() }; |
| 118 | + |
| 119 | + output_model h{ Identity<output_model>()() }; |
| 120 | + |
| 121 | + output_uncertainty r{ Identity<output_uncertainty>()() }; |
| 122 | + |
| 123 | + state_transition f{ Identity<state_transition>()() }; |
| 124 | + |
| 125 | + process_uncertainty q{}; |
| 126 | + |
| 127 | + input_control g{}; |
| 128 | + |
| 129 | + //! @} |
| 130 | + |
| 131 | + //! @name Public Member Function Objects |
| 132 | + //! @{ |
| 133 | + |
| 134 | + //! @brief Compute observation transition H matrix. |
| 135 | + //! |
| 136 | + //! @details The observation transition H is also known as C. |
| 137 | + std::function<output_model()> transition_observation_h{ [this] { |
| 138 | + return h; |
| 139 | + } }; |
| 140 | + |
| 141 | + //! @brief Compute observation noise R matrix. |
| 142 | + std::function<output_uncertainty()> noise_observation_r{ [this] { |
| 143 | + return r; |
| 144 | + } }; |
| 145 | + |
| 146 | + //! @brief Compute state transition F matrix. |
| 147 | + //! |
| 148 | + //! @details The state transition F matrix is also known as Φ or A. |
| 149 | + //! For non-linear system, or extended filter, F is the Jacobian of the state |
| 150 | + //! transition function. F = ∂fj/∂xi that is each row i contains the the |
| 151 | + //! derivatives of the state transition function for every element j in the |
| 152 | + //! state vector x. |
| 153 | + std::function<state_transition(const PredictionArguments &...)> |
| 154 | + transition_state_f{ [this](const PredictionArguments &...arguments) { |
| 155 | + static_cast<void>((arguments, ...)); |
| 156 | + return f; |
| 157 | + } }; |
| 158 | + |
| 159 | + //! @brief Compute process noise Q matrix. |
| 160 | + std::function<process_uncertainty(const PredictionArguments &...)> |
| 161 | + noise_process_q{ [this](const PredictionArguments &...arguments) { |
| 162 | + static_cast<void>((arguments, ...)); |
| 163 | + return q; |
| 164 | + } }; |
| 165 | + |
| 166 | + //! @brief Compute control transition G matrix. |
| 167 | + std::function<input_control(const PredictionArguments &...)> |
| 168 | + transition_control_g{ [this](const PredictionArguments &...arguments) { |
| 169 | + static_cast<void>((arguments, ...)); |
| 170 | + return g; |
| 171 | + } }; |
| 172 | + |
| 173 | + //! @brief State transition function. |
| 174 | + //! |
| 175 | + //! @details |
| 176 | + // Add prediction arguments? |
| 177 | + std::function<state(const state &, const state_transition &)> predict_state = |
| 178 | + [](const state &x, const state_transition &f) { return state{ f * x }; }; |
| 179 | + |
| 180 | + //! @} |
| 181 | + |
| 182 | + //! @name Public Member Functions |
| 183 | + //! @{ |
| 184 | + |
| 185 | + inline constexpr void observe(const auto &...output_z) |
| 186 | + { |
| 187 | + h = transition_observation_h(); |
| 188 | + r = noise_observation_r(); |
| 189 | + const auto z{ output{ output_z... } }; |
| 190 | + internal::observe<Transpose, Symmetrize, Divide, Identity>(x, p, h, r, z); |
| 191 | + } |
| 192 | + |
| 193 | + inline constexpr void predict(const PredictionArguments &...arguments, |
| 194 | + const auto &...input_u) |
| 195 | + { |
| 196 | + // use member variables |
| 197 | + const auto ff{ predict_state }; |
| 198 | + f = transition_state_f(arguments...); |
| 199 | + q = noise_process_q(arguments...); |
| 200 | + g = transition_control_g(arguments...); |
| 201 | + const auto u{ input{ input_u... } }; |
| 202 | + internal::predict<Transpose, Symmetrize>(x, p, ff, f, q, g, u); |
| 203 | + } |
| 204 | + |
| 205 | + //! @} |
| 206 | +}; |
| 207 | + |
| 208 | +} // namespace fcarouge::internal |
| 209 | + |
| 210 | +#endif // FCAROUGE_INTERNAL_KALMAN_HPP |
0 commit comments