EmrysFilter 0.1.0
State estimation utilities in C++23 using Eigen
Loading...
Searching...
No Matches
RbPointMassFilter.h
Go to the documentation of this file.
1#pragma once
2
9
10#include <expected>
11#include <span>
12#include <type_traits>
13#include <utility>
14
29
30namespace Emrys
31{
32
33namespace internal
34{
35
39template <typename Grid, typename Model, bool = isSwitchingPointModel<Model>>
40struct CellComponentOf
41{
42 using type = Gaussian<typename Grid::RestSpace, typename Grid::Scalar>;
43};
44
46template <typename Grid, typename Model>
47struct CellComponentOf<Grid, Model, true>
48{
50 using type = Hybrid<
51 Gaussian<typename Grid::RestSpace, typename Grid::Scalar>,
52 typename Model::ModeSpaceType,
53 typename Grid::allocator_type>;
54};
55
56} // namespace internal
57
69template <typename Scalar = double, typename Allocator = DefaultAllocator>
71{
72public:
73
74 using ArrayType = Eigen::Map<Eigen::Array<Scalar, Eigen::Dynamic, 1> const>;
75 using allocator_type = Allocator;
76
78 explicit RbPointMassFilterUpdate(Allocator const & allocator = {})
79 : m_cells(internal::ReBound<Allocator, Eigen::Index>(allocator))
80 , m_points{allocator}
81 {}
82
84 [[nodiscard]] std::span<Eigen::Index const>
85 cells() const
86 {
87 return {m_cells.data(), m_cells.size()};
88 }
89
91 [[nodiscard]] ArrayType
93 {
94 return m_points.logLikelihoods();
95 }
96
100 [[nodiscard]] ArrayType
101 nis() const
102 {
103 return m_points.nis();
104 }
105
107 [[nodiscard]] Eigen::Index
109 {
110 return m_points.innovationSize;
111 }
112
114 [[nodiscard]] allocator_type
116 {
117 return allocator_type(m_cells.get_allocator());
118 }
119
121 [[nodiscard]] Scalar
123 {
124 return m_logLikelihood;
125 }
126
127 bool accepted = false;
128
129private:
130
131 friend struct internal::PointLikelihoodsAccess;
132 friend struct internal::UpdateAccess;
133
134 Scalar m_logLikelihood = 0;
135
136 internal::Buffer<Eigen::Index, Allocator> m_cells;
137 internal::PointLikelihoods<Scalar, Allocator> m_points;
138};
139
233template <
234 typename Grid_,
235 typename Model_,
236 typename Covariance_ = internal::PointCovarianceFor<Model_>,
237 typename Method_ = DefaultMethodFor<Model_>,
238 typename Upkeep_ = upkeep::None>
240 : public internal::HeldUpkeep<RbPointMassFilter<Grid_, Model_, Covariance_, Method_, Upkeep_>, Upkeep_>
241{
242 using UpkeepBase = internal::HeldUpkeep<RbPointMassFilter, Upkeep_>;
243
244 static constexpr bool Shared = std::is_same_v<Covariance_, SharedCovariance>;
245
247 static constexpr bool Switching = internal::isSwitchingPointModel<Model_>;
248
250 template <typename P>
251 static constexpr void
252 assertCellsRunUnder()
253 {
254 static_assert(
255 !Switching || internal::isSequential<P>,
256 "a cell's IMM step writes the recursion's scratch, so the cells of a hybrid grid run in order: pass "
257 "Sequential"
258 );
259 }
260
261 static_assert(
262 internal::predictsBy<Model_, Method_>,
263 "a filter with a fixed-step model predicts by the model's method"
264 );
265 static_assert(internal::checkPredictionMethod<Method_>());
266
267public:
268
269 using GridType = Grid_;
270 using Scalar = typename Grid_::Scalar;
271 using SpaceType = typename Grid_::SpaceType;
272 using ModelType = Model_;
273 using MethodType = Method_;
278
287 std::type_identity_t<Model_> model,
288 Grid_ grid,
289 ConditionalType const & conditional,
290 Covariance_ = {},
292 )
293 : RbPointMassFilter{std::move(model), std::move(grid), conditional, Covariance_{}, std::move(method), Upkeep_{}}
294 {}
295
300 std::type_identity_t<Model_> model,
301 Grid_ grid,
302 ConditionalType const & conditional,
303 Covariance_,
305 std::type_identity_t<Upkeep_> upkeep
306 )
307 : UpkeepBase{std::move(upkeep)}
308 , m_steps{std::move(model), std::move(method), grid.get_allocator()}
309 , m_state{std::move(grid), conditional}
310 , m_posterior{m_state.m_conditionals, m_state.get_allocator()}
311 {
312 assertModelPartitions(m_state);
313 reserve();
314 }
315
318 std::type_identity_t<Model_> model,
319 Grid_ grid,
320 ConditionalType const & conditional,
321 std::type_identity_t<Upkeep_> upkeep
322 )
323 requires(!std::same_as<Upkeep_, Emrys::upkeep::None>)
325 std::move(model),
326 std::move(grid),
327 conditional,
328 Covariance_{},
329 MethodFor<Model_, Method_>{},
330 std::move(upkeep)
331 }
332 {}
333
338 std::type_identity_t<Model_> model,
339 Grid_ grid,
340 ConditionalType const & conditional,
341 Covariance_ covariance,
342 std::type_identity_t<Upkeep_> upkeep
343 )
344 requires(!std::same_as<Upkeep_, Emrys::upkeep::None>)
346 std::move(model),
347 std::move(grid),
348 conditional,
349 covariance,
350 MethodFor<Model_, Method_>{},
351 std::move(upkeep)
352 }
353 {}
354
358 explicit RbPointMassFilter(Grid_ grid, Covariance_ = {})
359 requires(std::is_same_v<typename Grid_::RestSpace, Linear<0>> && std::is_same_v<Model_, NoProcessModel>)
360 : UpkeepBase{Upkeep_{}}
361 , m_state{std::move(grid), ConditionalType{}}
362 , m_posterior{m_state.m_conditionals, m_state.get_allocator()}
363 {
364 reserve();
365 }
366
368 RbPointMassFilter(Grid_ grid, Covariance_, std::type_identity_t<Upkeep_> upkeep)
369 requires(!std::same_as<Upkeep_, Emrys::upkeep::None> && std::is_same_v<Model_, NoProcessModel>
370 && std::is_same_v<typename Grid_::RestSpace, Linear<0>>)
371 : UpkeepBase{std::move(upkeep)}
372 , m_state{std::move(grid), ConditionalType{}}
373 , m_posterior{m_state.m_conditionals, m_state.get_allocator()}
374 {
375 reserve();
376 }
377
379 RbPointMassFilter(Grid_ grid, std::type_identity_t<Upkeep_> upkeep)
380 requires(
381 !std::same_as<Upkeep_, Emrys::upkeep::None> && std::is_same_v<Model_, NoProcessModel>
382 && std::is_same_v<typename Grid_::RestSpace, Linear<0>>
383 )
384 : RbPointMassFilter{std::move(grid), Covariance_{}, std::move(upkeep)}
385 {}
386
388 [[nodiscard]] allocator_type
390 {
391 return m_state.get_allocator();
392 }
393
395 [[nodiscard]] StateType const &
396 state() const
397 {
398 return m_state;
399 }
400
405 [[nodiscard]] StateType &
407 {
408 return m_state;
409 }
410
415 void
417 {
418 if constexpr (!std::is_same_v<Model_, NoProcessModel>)
419 {
420 assertModelPartitions(state);
421 }
422 auto const before = m_state.get_allocator();
423 m_state = std::move(state);
424 if (m_state.get_allocator() != before)
425 {
426 // The assignment carried the new state's allocator in, so the scratch follows it.
427 m_posterior = decltype(m_posterior){m_state.m_conditionals, m_state.get_allocator()};
428 if constexpr (Switching)
429 {
430 m_steps.resetScratch(m_state.get_allocator());
431 }
432 }
433 reserve();
434 }
435
437 [[nodiscard]] Model_ const &
438 model() const
439 {
440 return m_steps.model();
441 }
442
444 [[nodiscard]] Model_ &
446 {
447 return m_steps.model();
448 }
449
451 [[nodiscard]] Method_ const &
452 method() const
453 {
454 return m_steps.method();
455 }
456
461 template <typename... Inputs>
462 requires(!isFixedStep<Model_>)
463 void
464 predict(TimeStep<Scalar> auto const & dt, Inputs const &... inputs)
465 {
466 predict(execution::sequential, dt, inputs...);
467 }
468
474 template <ExecutionPolicy P, typename... Inputs>
475 requires(!isFixedStep<Model_>)
476 void
477 predict([[maybe_unused]] P && policy, TimeStep<Scalar> auto const & dt, [[maybe_unused]] Inputs const &... inputs)
478 {
479 [[maybe_unused]] Scalar const step = internal::toScalarTime<Scalar>(dt);
480 if constexpr (Switching)
481 {
482 assertCellsRunUnder<P>();
483 m_state.m_stamp.touchValues();
484 for (Eigen::Index i = 0; i < m_state.size(); ++i)
485 {
486 m_steps.predict(m_state.m_conditionals.conditional(i), step, inputs...);
487 }
488 }
489 else if constexpr (!std::is_same_v<Model_, NoProcessModel>)
490 {
491 moveCells(
492 std::forward<P>(policy),
493 [&](auto const & cell) {
494 return Emrys::predict(m_steps.method(), cell, m_steps.model(), step, inputs...);
495 },
496 [&](auto const & mean) {
497 return Emrys::propagate(m_state.grid().restSpace(), mean, m_steps.model(), step, inputs...);
498 }
499 );
500 }
501 else
502 {
503 static_assert(sizeof...(Inputs) == 0, "a point-mass filter over the whole state takes no input");
504 }
505 }
506
510 template <typename... Inputs>
511 requires(isFixedStep<Model_> && !internal::leadsWithPolicy<Inputs...> && !internal::leadsWithSteps<Inputs...>)
512 void
513 predict(Inputs const &... inputs)
514 {
515 internal::rejectTimeStep<Inputs...>();
516 predict(execution::sequential, inputs...);
517 }
518
520 template <ExecutionPolicy P, typename... Inputs>
521 requires(isFixedStep<Model_> && !internal::leadsWithSteps<Inputs...>)
522 void
523 predict(P && policy, Inputs const &... inputs)
524 {
525 internal::rejectTimeStep<Inputs...>();
526 if constexpr (Switching)
527 {
528 assertCellsRunUnder<P>();
529 m_state.m_stamp.touchValues();
530 for (Eigen::Index i = 0; i < m_state.size(); ++i)
531 {
532 m_steps.predict(m_state.m_conditionals.conditional(i), inputs...);
533 }
534 }
535 else
536 {
537 moveCells(
538 std::forward<P>(policy),
539 [&](auto const & cell) {
540 return Emrys::predict(cell, m_steps.model(), inputs...);
541 },
542 [&](auto const & mean) {
543 return Emrys::propagate(
544 m_state.grid().restSpace(),
545 mean,
546 m_steps.model().model(),
547 m_steps.model().timeStep(),
548 inputs...
549 );
550 }
551 );
552 }
553 }
554
558 template <typename... Inputs>
559 requires isFixedStep<Model_>
560 void
561 predict(Steps steps, Inputs const &... inputs)
562 {
563 predict(execution::sequential, steps, inputs...);
564 }
565
569 template <ExecutionPolicy P, typename... Inputs>
570 requires isFixedStep<Model_>
571 void
572 predict(P && policy, Steps steps, Inputs const &... inputs)
573 {
574 EMRYS_FILTER_ASSERT(steps.count >= 0 && "the number of steps must be nonnegative");
575 for (Eigen::Index i = 0; i < steps.count; ++i)
576 {
577 predict(policy, inputs...);
578 }
579 }
580
583
605 template <internal::WeighsPoints M>
606 [[nodiscard]] std::expected<UpdateType, NumericError>
607 update(M const & measurement)
608 {
609 return update(execution::sequential, measurement);
610 }
611
615 template <ExecutionPolicy P, internal::WeighsPoints M>
616 [[nodiscard]] std::expected<UpdateType, NumericError>
617 update(P && executionPolicy, M const & measurement)
618 {
619 auto result = UpdateType{get_allocator()};
620 auto const updated = update(std::forward<P>(executionPolicy), measurement, result);
621 if (!updated)
622 {
623 return std::unexpected{updated.error()};
624 }
625 return result;
626 }
627
633 template <internal::WeighsPoints M, typename ResultAllocator>
634 [[nodiscard]] std::expected<void, NumericError>
636 {
637 return update(execution::sequential, measurement, result);
638 }
639
643 template <ExecutionPolicy P, internal::WeighsPoints M, typename ResultAllocator>
644 [[nodiscard]] std::expected<void, NumericError>
645 update(P && executionPolicy, M const & measurement, RbPointMassFilterUpdate<Scalar, ResultAllocator> & result)
646 {
647 if constexpr (!internal::hasOwnLikelihood<M> && !Shared)
648 {
649 return update(std::forward<P>(executionPolicy), measurement, reductions::collapse(), result);
650 }
651 else
652 {
653 auto & points = internal::PointLikelihoodsAccess::points(result);
654 if constexpr (internal::hasOwnLikelihood<M>)
655 {
656 internal::updateGivenLikelihood(
657 executionPolicy,
658 m_state.points(),
659 m_state.m_conditionals,
660 measurement,
661 points
662 );
663 finish<false>(std::forward<P>(executionPolicy), result);
664 }
665 else
666 {
667 auto const updated = internal::updateGivenShared(
668 executionPolicy,
669 m_state.points(),
670 m_state.m_conditionals,
671 m_state.mostProbable(),
672 measurement,
673 points,
674 m_posterior
675 );
676 if (!updated)
677 {
678 return std::unexpected{updated.error()};
679 }
680 finish(std::forward<P>(executionPolicy), result);
681 }
682 return {};
683 }
684 }
685
690 template <internal::WeighsPoints M, typename Policy>
691 [[nodiscard]] std::expected<UpdateType, NumericError>
692 update(M const & measurement, Policy const & policy)
693 requires(!Shared)
694 {
695 return update(execution::sequential, measurement, policy);
696 }
697
702 template <ExecutionPolicy P, internal::WeighsPoints M, typename Policy>
703 [[nodiscard]] std::expected<UpdateType, NumericError>
704 update(P && executionPolicy, M const & measurement, Policy const & policy)
705 requires(!Shared)
706 {
707 auto result = UpdateType{get_allocator()};
708 auto const updated = update(std::forward<P>(executionPolicy), measurement, policy, result);
709 if (!updated)
710 {
711 return std::unexpected{updated.error()};
712 }
713 return result;
714 }
715
721 template <internal::WeighsPoints M, typename Policy, typename ResultAllocator>
722 [[nodiscard]] std::expected<void, NumericError>
723 update(M const & measurement, Policy const & policy, RbPointMassFilterUpdate<Scalar, ResultAllocator> & result)
724 requires(!Shared)
725 {
726 return update(execution::sequential, measurement, policy, result);
727 }
728
732 template <ExecutionPolicy P, internal::WeighsPoints M, typename Policy, typename ResultAllocator>
733 [[nodiscard]] std::expected<void, NumericError>
735 P && executionPolicy,
736 M const & measurement,
737 Policy const & policy,
739 )
740 requires(!Shared)
741 {
742 static_assert(
743 !internal::hasOwnLikelihood<M>,
744 "a measurement weighted by its own logLikelihood() has no branches to reduce, so it takes no policy"
745 );
746 if constexpr (Switching && internal::isByMode<M>)
747 {
748 // Each mode's measurement holds its own noise component, so the recursion resolves it.
749 using PathType = typename Grid_::PathType;
750 return updateCells(
751 std::forward<P>(executionPolicy),
752 m_steps.template emptyGivenUpdate<M, SpaceType, PathType>(get_allocator(), modeCount()),
753 [&](auto & cell, auto const & pinned, auto & cellResult) {
754 using Value = std::remove_cvref_t<decltype(pinned)>;
755 return m_steps.update(
756 cell,
757 measurement,
758 GivenBlock<SpaceType, PathType, Value>{m_state.grid().space(), pinned},
759 policy,
760 cellResult
761 );
762 },
763 result
764 );
765 }
766 else if constexpr (Switching)
767 {
768 auto const f = internal::PinnedMeasurement<SpaceType, typename Grid_::PathType, M>{
769 m_state.grid().space(),
770 measurement
771 };
772 return updateCells(
773 std::forward<P>(executionPolicy),
774 m_steps.template emptyGivenUpdate<M, SpaceType, typename Grid_::PathType>(get_allocator(), modeCount()),
775 [&](auto & cell, auto const & pinned, auto & cellResult) {
776 return m_steps.updateGiven(cell, f, pinned, policy, cellResult);
777 },
778 result
779 );
780 }
781 else
782 {
783 auto const updated = internal::updateGivenEach(
784 executionPolicy,
785 m_state.points(),
786 m_state.m_conditionals,
787 measurement,
788 policy,
789 internal::PointLikelihoodsAccess::points(result),
790 m_posterior
791 );
792 if (!updated)
793 {
794 return std::unexpected{updated.error()};
795 }
796 finish(std::forward<P>(executionPolicy), result);
797 return {};
798 }
799 }
800
801private:
802
807 void
808 assertModelPartitions([[maybe_unused]] StateType const & state) const
809 {
810 if constexpr (Switching)
811 {
812 EMRYS_FILTER_ASSERT(
813 m_steps.partitions(state.grid().restSpace())
814 && "every mode's process model must write every coordinate of the rest of the state once"
815 );
816 }
817 else
818 {
819 internal::assertPartitions(
820 m_steps.model(),
821 state.grid().restSpace(),
822 "the process model must write every coordinate of the rest of the state once"
823 );
824 }
825 }
826
828 [[nodiscard]] Eigen::Index
829 modeCount() const
830 requires Switching
831 {
832 return m_steps.modeSpace().cardinality();
833 }
834
845 template <typename P, typename CellResult, typename UpdateCell, typename ResultAllocator>
846 [[nodiscard]] std::expected<void, NumericError>
847 updateCells(
848 P && policy,
849 CellResult cellResult,
850 UpdateCell const & updateCell,
851 RbPointMassFilterUpdate<Scalar, ResultAllocator> & result
852 )
853 {
854 assertCellsRunUnder<P>();
855 // Each cell's hybrid is updated in place below, so a density of the state is stale.
856 m_state.m_stamp.touchValues();
857 auto const updated = internal::updateHybridsGiven(
858 m_state.points(),
859 m_state.m_conditionals,
860 internal::PointLikelihoodsAccess::points(result),
861 cellResult,
862 updateCell
863 );
864 if (!updated)
865 {
866 return updated;
867 }
868 finish<false>(std::forward<P>(policy), result);
869 return {};
870 }
871
876 template <typename P, typename PredictCell, typename Propagate>
877 void
878 moveCells(P && policy, PredictCell const & predictCell, Propagate const & propagate)
879 {
880 m_state.m_stamp.touchValues();
881 auto & conditionals = m_state.m_conditionals;
882 static_assert(!Switching);
883 if constexpr (Shared)
884 {
885 ConditionalType const predicted = predictCell(conditionals.conditional(m_state.mostProbable()));
886 auto means = conditionals.means();
887 internal::forEachIndex(std::forward<P>(policy), means.cols(), [&](Eigen::Index i) {
888 means.col(i) = propagate(means.col(i));
889 });
890 normalizeValues(m_state.grid().restSpace(), means);
891 conditionals.sharedCovariance() = predicted.covarianceLower();
892 }
893 else
894 {
895 internal::forEachIndex(std::forward<P>(policy), conditionals.size(), [&](Eigen::Index i) {
896 auto & cell = conditionals.conditional(i);
897 cell = predictCell(cell);
898 });
899 }
900 }
901
903 void
904 reserve()
905 {
906 m_posterior.reserve(m_state.size());
907 }
908
914 template <bool Posterior = true, typename P, typename ResultAllocator>
915 void
916 finish(P && policy, RbPointMassFilterUpdate<Scalar, ResultAllocator> & result)
917 {
918 auto const cells = m_state.cells();
919 internal::PointLikelihoodsAccess::cells(result).assign(cells.begin(), cells.end());
920 auto & points = internal::PointLikelihoodsAccess::points(result);
921 auto const [logLikelihood, accepted] = [&] {
922 if constexpr (Posterior)
923 {
924 return m_state.reweight(std::forward<P>(policy), points.logLikelihoods(), m_posterior);
925 }
926 else
927 {
928 return m_state.reweight(std::forward<P>(policy), points.logLikelihoods());
929 }
930 }();
931 internal::UpdateAccess::logLikelihood(result) = logLikelihood;
932 result.accepted = accepted;
933 }
934
938 using StepsType = std::conditional_t<
939 Switching,
940 ImmRecursion<Gaussian<typename Grid_::RestSpace, Scalar>, Model_, Method_, allocator_type>,
941 internal::PointSteps<Model_, Method_>>;
942
943 StepsType m_steps;
944 StateType m_state;
945
946 // Scratch, reused across steps.
947 typename StateType::ConditionalsType m_posterior;
948};
949
950namespace internal
951{
952
956template <typename U>
957concept GridUpkeepArgument =
958 UpkeepArgument<U> && !std::same_as<U, SharedCovariance> && !std::same_as<U, PerPointCovariance>;
959
960} // namespace internal
961
963// The method argument's type depends on the model, so it is deduced here; the parameters are in
964// the class's order so MSVC also deduces through the PointMassFilter alias. An upkeep, given last,
965// is told from a covariance form or a method by its type.
966template <
967 typename Grid_,
968 typename Model_,
969 typename Covariance_ = internal::PointCovarianceFor<Model_>,
970 typename Method_ = DefaultMethodFor<Model_>,
971 typename Conditional>
972 requires(!internal::GridUpkeepArgument<Covariance_> && !internal::UpkeepArgument<Method_>)
973RbPointMassFilter(Model_, Grid_, Conditional, Covariance_ = {}, Method_ = {}) -> RbPointMassFilter<
974 Grid_,
975 internal::BoundStep<Model_, typename Grid_::RestSpace, typename Grid_::Scalar>,
976 Covariance_,
977 Method_>;
978
979template <
980 typename Grid_,
981 typename Model_,
982 typename Covariance_ = internal::PointCovarianceFor<Model_>,
983 typename Method_ = DefaultMethodFor<Model_>,
984 typename Upkeep_,
985 typename Conditional>
986 requires internal::GridUpkeepArgument<Upkeep_>
987RbPointMassFilter(Model_, Grid_, Conditional, Upkeep_) -> RbPointMassFilter<
988 Grid_,
989 internal::BoundStep<Model_, typename Grid_::RestSpace, typename Grid_::Scalar>,
990 Covariance_,
991 Method_,
992 Upkeep_>;
993
994template <
995 typename Grid_,
996 typename Model_,
997 typename Covariance_,
998 typename Method_ = DefaultMethodFor<Model_>,
999 typename Upkeep_,
1000 typename Conditional>
1001 requires(!internal::GridUpkeepArgument<Covariance_> && internal::GridUpkeepArgument<Upkeep_>)
1002RbPointMassFilter(Model_, Grid_, Conditional, Covariance_, Upkeep_) -> RbPointMassFilter<
1003 Grid_,
1004 internal::BoundStep<Model_, typename Grid_::RestSpace, typename Grid_::Scalar>,
1005 Covariance_,
1006 Method_,
1007 Upkeep_>;
1008
1009template <
1010 typename Grid_,
1011 typename Model_,
1012 typename Covariance_,
1013 typename Method_,
1014 typename Upkeep_,
1015 typename Conditional>
1016RbPointMassFilter(Model_, Grid_, Conditional, Covariance_, Method_, Upkeep_) -> RbPointMassFilter<
1017 Grid_,
1018 internal::BoundStep<Model_, typename Grid_::RestSpace, typename Grid_::Scalar>,
1019 Covariance_,
1020 Method_,
1021 Upkeep_>;
1022
1023template <
1024 typename Grid_,
1025 typename Model_ = NoProcessModel,
1026 typename Covariance_ = SharedCovariance,
1027 typename Method_ = methods::Linearized,
1028 typename Upkeep_ = upkeep::None>
1029 requires(!internal::GridUpkeepArgument<Covariance_>)
1030RbPointMassFilter(Grid_, Covariance_ = {}) -> RbPointMassFilter<Grid_, Model_, Covariance_, Method_, Upkeep_>;
1031
1032template <
1033 typename Grid_,
1034 typename Model_ = NoProcessModel,
1035 typename Covariance_ = SharedCovariance,
1036 typename Method_ = methods::Linearized,
1037 typename Upkeep_>
1038 requires internal::GridUpkeepArgument<Upkeep_>
1039RbPointMassFilter(Grid_, Upkeep_) -> RbPointMassFilter<Grid_, Model_, Covariance_, Method_, Upkeep_>;
1040
1041template <
1042 typename Grid_,
1043 typename Model_ = NoProcessModel,
1044 typename Covariance_,
1045 typename Method_ = methods::Linearized,
1046 typename Upkeep_>
1047 requires(!internal::GridUpkeepArgument<Covariance_> && internal::GridUpkeepArgument<Upkeep_>)
1048RbPointMassFilter(Grid_, Covariance_, Upkeep_) -> RbPointMassFilter<Grid_, Model_, Covariance_, Method_, Upkeep_>;
1050
1084template <typename Grid_, typename Covariance_ = SharedCovariance, typename Upkeep_ = upkeep::None>
1086
1087} // namespace Emrys
1088
1089#ifndef EMRYS_FILTER_NO_IO
1090
1091namespace Emrys
1092{
1093
1094namespace internal
1095{
1096
1098template <typename Grid_, typename Model_, typename Covariance_, typename Method_, typename Upkeep_>
1099void
1100describe(
1101 std::ostream & stream,
1102 RbPointMassFilter<Grid_, Model_, Covariance_, Method_, Upkeep_> const & filter,
1103 Indent indent
1104)
1105{
1106 describeFilter(stream, "Rao-Blackwellized point-mass filter", filter.state(), indent);
1107}
1108
1109} // namespace internal
1110
1111} // namespace Emrys
1112
1113#endif // EMRYS_FILTER_NO_IO
The allocator conventions of decisions.md [allocation]: DefaultAllocator, Capacity,...
Execution policies (decisions.md [execution-policies]): execution::sequential, the ExecutionPolicy co...
The upkeep a filter holds (decisions.md [upkeep]): internal::HeldUpkeep, the base that gives every fi...
The Gaussian: its parameters GaussianParams, the Gaussian that evaluates the density,...
Grids of points and the state over them (decisions.md [rb-point-mass], [rao-blackwellized-core]): Gri...
The hybrid state of a discrete mode and a component per mode (decisions.md [imm], [component-first]):...
ImmRecursion and ImmFilter (decisions.md [switching-models], [imm], [filters-and-recursions]): a Hybr...
The macros every header includes first.
The update of a Gaussian by a measurement (decisions.md [templated-measurements], [weighted-branches]...
Printing: operator<< and std::formatter for every distribution, every filter (which prints its name a...
Process models as functions with reads and writes (decisions.md [model-policies], [continuous-time-no...
The update that the Rao-Blackwellized filters share (decisions.md [rb-point-mass],...
Reducing an update's branches (decisions.md [weighted-branches], [reduction-policies],...
Propagating a distribution through a function (decisions.md [method-per-measurement],...
The parameters of a Rao-Blackwellized point-mass filter's state: the cells of a grid still alive,...
Definition GridGaussian.h:936
Params< Component_ > ConditionalType
A cell's distribution over the rest: a Gaussian, or a hybrid of Gaussians.
Definition GridGaussian.h:990
typename Grid_::allocator_type allocator_type
The grid's allocator.
Definition GridGaussian.h:999
What a Rao-Blackwellized point-mass filter's update did, per live cell and in total.
Definition RbPointMassFilter.h:71
Allocator allocator_type
where the cells and per-cell arrays come from
Definition RbPointMassFilter.h:75
ArrayType logLikelihoods() const
Definition RbPointMassFilter.h:92
allocator_type get_allocator() const
Definition RbPointMassFilter.h:115
std::span< Eigen::Index const > cells() const
Definition RbPointMassFilter.h:85
Eigen::Map< Eigen::Array< Scalar, Eigen::Dynamic, 1 > const > ArrayType
a per-cell array
Definition RbPointMassFilter.h:74
Eigen::Index innovationSize() const
Definition RbPointMassFilter.h:108
Scalar logLikelihood() const
Definition RbPointMassFilter.h:122
RbPointMassFilterUpdate(Allocator const &allocator={})
An empty result whose cells and arrays come from allocator.
Definition RbPointMassFilter.h:78
ArrayType nis() const
Definition RbPointMassFilter.h:101
bool accepted
Definition RbPointMassFilter.h:127
A Rao-Blackwellized point-mass filter: fixed points for one block of the state, such as a coarse posi...
Definition RbPointMassFilter.h:241
Method_ const & method() const
Definition RbPointMassFilter.h:452
typename Grid_::SpaceType SpaceType
the state space
Definition RbPointMassFilter.h:271
Method_ MethodType
the prediction method
Definition RbPointMassFilter.h:273
void predict(P &&policy, Inputs const &... inputs)
predict(inputs...) under policy, as predict(policy, dt, inputs...) runs.
Definition RbPointMassFilter.h:523
std::expected< UpdateType, NumericError > update(M const &measurement, Policy const &policy)
As update(measurement), with PerPointCovariance, but with each cell's branches reduced by policy (Col...
Definition RbPointMassFilter.h:692
RbPointMassFilter(Grid_ grid, Covariance_, std::type_identity_t< Upkeep_ > upkeep)
As RbPointMassFilter(grid, covariance), with upkeep upkeep, which maintain() runs.
Definition RbPointMassFilter.h:368
RbPointMassFilter(std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_, MethodFor< Model_, Method_ > method, std::type_identity_t< Upkeep_ > upkeep)
As RbPointMassFilter(model, grid, conditional, covariance, method), with upkeep upkeep,...
Definition RbPointMassFilter.h:299
StateType const & state() const
Definition RbPointMassFilter.h:396
RbPointMassFilter(Grid_ grid, Covariance_={})
A filter whose points cover the whole state (see PointMassFilter), with every cell equally probable.
Definition RbPointMassFilter.h:358
StateType & state()
Definition RbPointMassFilter.h:406
std::expected< void, NumericError > update(M const &measurement, Policy const &policy, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
As update(measurement, policy), but into result, whose storage is reused (see update(measurement,...
Definition RbPointMassFilter.h:723
RbPointMassFilter(std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_={}, MethodFor< Model_, Method_ > method={})
A filter with process model model over the rest of the state, points grid, conditional as every cell'...
Definition RbPointMassFilter.h:286
typename StateType::allocator_type allocator_type
the state's allocator
Definition RbPointMassFilter.h:277
std::expected< void, NumericError > update(P &&executionPolicy, M const &measurement, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
update(measurement, result) under executionPolicy, as update(executionPolicy,measurement) runs.
Definition RbPointMassFilter.h:645
NoProcessModel const & model() const
Definition RbPointMassFilter.h:438
void predict(P &&policy, Steps steps, Inputs const &... inputs)
predict(steps, inputs...) under policy, as that many calls of predict(policy, inputs....
Definition RbPointMassFilter.h:572
RbPointMassFilter(std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, Covariance_ covariance, std::type_identity_t< Upkeep_ > upkeep)
As RbPointMassFilter(model, grid, conditional, covariance), with upkeep upkeep, which maintain() runs...
Definition RbPointMassFilter.h:337
void predict(Inputs const &... inputs)
Moves each live cell's Gaussian forward by the fixed-step model's step, with inputs passed as in Emry...
Definition RbPointMassFilter.h:513
std::expected< UpdateType, NumericError > update(P &&executionPolicy, M const &measurement)
update() under executionPolicy: each cell's update is a for-each and the evidence a reduction,...
Definition RbPointMassFilter.h:617
std::expected< UpdateType, NumericError > update(M const &measurement)
Conditions each live cell's Gaussian on measurement given the cell's center, by the measurement's own...
Definition RbPointMassFilter.h:607
void predict(TimeStep< Scalar > auto const &dt, Inputs const &... inputs)
Moves each live cell's Gaussian forward by dt, any TimeStep (see Time steps), through the process mod...
Definition RbPointMassFilter.h:464
RbPointMassFilterUpdate< Scalar, allocator_type > UpdateType
The result of update(): the cells and per-cell arrays from the filter's allocator.
Definition RbPointMassFilter.h:582
std::expected< void, NumericError > update(M const &measurement, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
As update(measurement), but into result, whose storage is reused: an update into a result kept across...
Definition RbPointMassFilter.h:635
GridGaussianParams< Grid_, Covariance_, typename internal::CellComponentOf< Grid_, Model_ >::type > StateType
The state the filter owns: a Gaussian per cell, or a hybrid under a switching model.
Definition RbPointMassFilter.h:275
Model_ & model()
Definition RbPointMassFilter.h:445
RbPointMassFilter(Grid_ grid, std::type_identity_t< Upkeep_ > upkeep)
As RbPointMassFilter(grid), with upkeep upkeep, which maintain() runs.
Definition RbPointMassFilter.h:379
std::expected< void, NumericError > update(P &&executionPolicy, M const &measurement, Policy const &policy, RbPointMassFilterUpdate< Scalar, ResultAllocator > &result)
update(measurement, policy, result) under executionPolicy, as update(executionPolicy,...
Definition RbPointMassFilter.h:734
typename Grid_::Scalar Scalar
the scalar type
Definition RbPointMassFilter.h:270
std::expected< UpdateType, NumericError > update(P &&executionPolicy, M const &measurement, Policy const &policy)
update(measurement, policy) under executionPolicy: each cell's Kalman update and reduction by policy ...
Definition RbPointMassFilter.h:704
Model_ ModelType
the process model
Definition RbPointMassFilter.h:272
RbPointMassFilter(std::type_identity_t< Model_ > model, Grid_ grid, ConditionalType const &conditional, std::type_identity_t< Upkeep_ > upkeep)
As RbPointMassFilter(model, grid, conditional), with upkeep upkeep, which maintain() runs.
Definition RbPointMassFilter.h:317
Grid_ GridType
the fixed points
Definition RbPointMassFilter.h:269
void predict(P &&policy, TimeStep< Scalar > auto const &dt, Inputs const &... inputs)
predict(dt, inputs...) under policy, a for-each over the live cells, so the result is Sequential's (s...
Definition RbPointMassFilter.h:477
void predict(Steps steps, Inputs const &... inputs)
Moves each live cell's Gaussian forward by steps of the fixed-step model, as that many calls of predi...
Definition RbPointMassFilter.h:561
void setState(StateType state)
Replaces the state, as when reinitializing, with one whose rest space the process model's writes part...
Definition RbPointMassFilter.h:416
typename StateType::ConditionalType ConditionalType
a cell's distribution over the rest
Definition RbPointMassFilter.h:276
An execution policy: execution::sequential, a policy of the standard library such as std::execution::...
Definition Execution.h:459
A step of time that converts to Scalar: Scalar itself, or any type with a TimeTraits,...
Definition Time.h:128
CategoricalParams< Space, Scalar > predict(CategoricalParams< Space, Scalar > const &prior, MarkovChain< Space, Scalar > const &chain)
Definition Categorical.h:1049
constexpr Sequential sequential
The default execution policy: plain loops on the calling thread, in index order (Sequential).
Definition Execution.h:128
constexpr Collapse collapse()
Definition Reduction.h:291
RbPointMassFilter< Grid_, NoProcessModel, Covariance_, methods::Linearized, Upkeep_ > PointMassFilter
A Bayesian grid filter: an RbPointMassFilter whose points cover the whole state, so nothing is left G...
Definition RbPointMassFilter.h:1085
Derived::Scalar logLikelihood(M const &measurement, Space const &space, Eigen::MatrixBase< Derived > const &x)
Definition Likelihood.h:433
typename internal::DefaultMethodOf< Model >::type DefaultMethodFor
The prediction method of a filter with process model Model, unless it is given one: Linearized,...
Definition ProcessModel.h:3220
constexpr bool isFixedStep
Whether Model is a FixedStepModel, whose predictions take no time step.
Definition ProcessModel.h:2776
Moments< Space_, typenameDerived::Scalar >::VectorType propagate(Space_ const &space, Eigen::MatrixBase< Derived > const &x, Model const &model, TimeStep< typename Derived::Scalar > auto const &dt, Args const &... args)
Definition ProcessModel.h:2754
std::conditional_t< isFixedStep< Model >, ModelsMethod, Method > MethodFor
The type of a filter constructor's method argument: Method, or ModelsMethod for a fixed-step Model,...
Definition ProcessModel.h:3236
void normalizeValues(S const &space, Eigen::MatrixBase< Derived > const &block)
Brings the values in block (rows are space's coordinates, columns samples) into canonical form,...
Definition Space.h:331
The upkeep operations, each made by its factory (decisions.md [policy-factories], [upkeep]): the acti...
Definition Upkeep.h:380
A block of a state given from outside the Gaussian being updated, as given() makes it,...
Definition Measurement.h:723
Dimension_ plain coordinates, or a resizable run of at most MaxDimension_ of them if Dimension_ is Ei...
Definition Coordinates.h:87
A count of a fixed-step model's steps, for predict(Steps{n}).
Definition Time.h:136
Eigen::Index count
the number of steps, at least zero
Definition Time.h:137
The upkeep a filter holds when it is given none: it takes any state, changes nothing,...
Definition Upkeep.h:889