17 #ifndef STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_
18 #define STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_
32 namespace state_estimation
40 template<
typename StateT>
43 static_assert(
sizeof(StateT) == 0,
"This class must be specialized to a specific state type.");
53 template<
typename StateT>
56 using AccelerationArray = std::array<
73 explicit WienerNoise(
const AccelerationArray & acceleration_variances)
74 : m_acceleration_variances{acceleration_variances} {}
85 typename State::Matrix crtp_covariance(
const std::chrono::nanoseconds &)
const;
88 AccelerationArray m_acceleration_variances{};
91 template<
typename StateT,
typename OtherScalarT>
94 std::array<typename StateT::Scalar, number_of_acceleration_components<StateT>::value> variances;
95 if (acceleration_variances.size() != variances.size()) {
96 throw std::runtime_error(
97 "There must be " + std::to_string(variances.size()) +
" acceleration variances");
99 std::copy(acceleration_variances.begin(), acceleration_variances.end(), variances.begin());
107 template<
typename ScalarT>
109 :
public std::integral_constant<std::size_t, 2UL> {};
115 template<
typename ScalarT>
117 :
public std::integral_constant<std::size_t, 3UL> {};
123 template<
typename ScalarT>
125 :
public std::integral_constant<std::size_t, 3UL> {};
131 template<
typename ScalarT>
133 :
public std::integral_constant<std::size_t, 4UL> {};
139 template<
typename ScalarT>
141 :
public std::integral_constant<std::size_t, 6UL> {};
148 #endif // STATE_ESTIMATION__NOISE_MODEL__WIENER_NOISE_HPP_