@@ -67,6 +67,155 @@ struct kalman {
6767 // ! @todo Support some more specializations, all, or disable others?
6868};
6969
70+ template <typename State, typename Output, typename Transpose,
71+ typename Symmetrize, typename Divide, typename Identity,
72+ typename ... UpdateTypes, typename ... PredictionTypes>
73+ struct kalman <State, Output, void , Transpose, Symmetrize, Divide, Identity,
74+ pack<UpdateTypes...>, pack<PredictionTypes...>> {
75+ struct empty {
76+ };
77+ using state = State;
78+ using output = Output;
79+ using input = empty;
80+ using estimate_uncertainty =
81+ std::
decay_t <std::
invoke_result_t <Divide, State, State>>;
82+ using process_uncertainty =
83+ std::
decay_t <std::
invoke_result_t <Divide, State, State>>;
84+ using output_uncertainty =
85+ std::
decay_t <std::
invoke_result_t <Divide, Output, Output>>;
86+ using state_transition =
87+ std::
decay_t <std::
invoke_result_t <Divide, State, State>>;
88+ using output_model =
89+ std::
decay_t <std::
invoke_result_t <Divide, Output, State>>;
90+ using input_control = empty;
91+ using gain = std::decay_t <std::invoke_result_t <Transpose, output_model>>;
92+ using innovation = output;
93+ using innovation_uncertainty = output_uncertainty;
94+ using observation_state_function =
95+ std::function<output_model(const state &, const UpdateTypes &...)>;
96+ using noise_observation_function = std::function<output_uncertainty(
97+ const state &, const output &, const UpdateTypes &...)>;
98+ using transition_state_function = std::function<state_transition(
99+ const state &, const PredictionTypes &...)>;
100+ using noise_process_function = std::function<process_uncertainty(
101+ const state &, const PredictionTypes &...)>;
102+ using transition_control_function = empty;
103+ using transition_function =
104+ std::function<state(const state &, const PredictionTypes &...)>;
105+ using observation_function =
106+ std::function<output(const state &, const UpdateTypes &...arguments)>;
107+
108+ // ! @todo Is there a simpler way to initialize to the zero matrix?
109+ state x{ 0 * Identity ().template operator ()<state>() };
110+ estimate_uncertainty p{
111+ Identity ().template operator ()<estimate_uncertainty>()
112+ };
113+ process_uncertainty q{
114+ 0 *
Identity ().
template operator ()<process_uncertainty>()
115+ };
116+ output_uncertainty r{ 0 *
117+ Identity ().template operator ()<output_uncertainty>() };
118+ output_model h{ Identity ().template operator ()<output_model>() };
119+ state_transition f{ Identity ().template operator ()<state_transition>() };
120+ gain k{ Identity ().template operator ()<gain>() };
121+ innovation y{ 0 * Identity ().template operator ()<innovation>() };
122+ innovation_uncertainty s{
123+ Identity ().template operator ()<innovation_uncertainty>()
124+ };
125+ output z{ 0 * Identity ().template operator ()<output>() };
126+
127+ // ! @todo Should we pass through the reference to the state x or have the user
128+ // ! access it through k.x() when needed? Where does the practical/performance
129+ // ! tradeoff leans toward? For the general case? For the specialized cases?
130+ // ! Same question applies to other parameters.
131+ // ! @todo Pass the arguments by universal reference?
132+ observation_state_function observation_state_h{
133+ [this ](const state &x, const UpdateTypes &...arguments ) -> output_model {
134+ static_cast <void >(x);
135+ (static_cast <void >(arguments), ...);
136+ return h;
137+ }
138+ };
139+ noise_observation_function noise_observation_r{
140+ [this ](const state &x, const output &z,
141+ const UpdateTypes &...arguments ) -> output_uncertainty {
142+ static_cast <void >(x);
143+ static_cast <void >(z);
144+ (static_cast <void >(arguments), ...);
145+ return r;
146+ }
147+ };
148+ transition_state_function transition_state_f{
149+ [this ](const state &x,
150+ const PredictionTypes &...arguments ) -> state_transition {
151+ static_cast <void >(x);
152+ (static_cast <void >(arguments), ...);
153+ return f;
154+ }
155+ };
156+ noise_process_function noise_process_q{
157+ [this ](const state &x,
158+ const PredictionTypes &...arguments ) -> process_uncertainty {
159+ static_cast <void >(x);
160+ (static_cast <void >(arguments), ...);
161+ return q;
162+ }
163+ };
164+ transition_function transition{
165+ [this ](const state &x, const PredictionTypes &...arguments ) -> state {
166+ (static_cast <void >(arguments), ...);
167+ return f * x;
168+ }
169+ };
170+ observation_function observation{
171+ [this ](const state &x, const UpdateTypes &...arguments ) -> output {
172+ (static_cast <void >(arguments), ...);
173+ return h * x;
174+ }
175+ };
176+
177+ Transpose transpose;
178+ Divide divide;
179+ Symmetrize symmetrize;
180+ Identity identity;
181+
182+ // ! @todo Do we want to store i - k * h in a temporary result for reuse? Or
183+ // ! does the compiler/linker do it for us?
184+ // ! @todo Do we want to support extended custom y = output_difference(z,
185+ // ! observation(x))?
186+ inline constexpr void update (const UpdateTypes &...arguments,
187+ const auto &...output_z)
188+ {
189+ const auto i{ identity.template operator ()<estimate_uncertainty>() };
190+
191+ z = output{ output_z... };
192+ h = observation_state_h (x, arguments...); // x, z, args?
193+ r = noise_observation_r (x, z, arguments...);
194+ s = h * p * transpose (h) + r;
195+ k = divide (p * transpose (h), s);
196+ y = z - observation (x, arguments...);
197+ x = x + k * y;
198+ p = symmetrize (estimate_uncertainty{
199+ (i - k * h) * p * transpose (i - k * h) + k * r * transpose (k) });
200+ }
201+
202+ inline constexpr void predict (const PredictionTypes &...arguments)
203+ {
204+ f = transition_state_f (x, arguments...);
205+ q = noise_process_q (x, arguments...);
206+ x = transition (x, arguments...);
207+ p = symmetrize (estimate_uncertainty{ f * p * transpose (f) + q });
208+ }
209+
210+ inline constexpr void
211+ operator ()(const PredictionTypes &...prediction_arguments,
212+ const UpdateTypes &...update_arguments, const auto &...output_z)
213+ {
214+ update (update_arguments..., output_z...);
215+ predict (prediction_arguments...);
216+ }
217+ };
218+
70219template <typename State, typename Output, typename Input, typename Transpose,
71220 typename Symmetrize, typename Divide, typename Identity,
72221 typename ... UpdateTypes, typename ... PredictionTypes>
0 commit comments