EmrysFilter 0.1.0
State estimation utilities in C++23 using Eigen
Loading...
Searching...
No Matches
Transform.h
Go to the documentation of this file.
1#pragma once
2
14
15#include <algorithm>
16#include <array>
17#include <cmath>
18#include <concepts>
19#include <cstddef>
20#include <expected>
21#include <limits>
22#include <optional>
23#include <tuple>
24#include <type_traits>
25#include <utility>
26
27#include <Eigen/Cholesky>
28#include <Eigen/Eigenvalues>
29#include <unsupported/Eigen/AutoDiff>
30
52
53namespace Emrys
54{
55
150
151namespace internal
152{
153
157template <typename Problem>
158struct LinearizationAt
159{
160 typename Problem::template OutputVector<typename Problem::Scalar> mean;
161 typename Problem::JacobianMatrix jacobian;
162};
163
168template <typename Problem>
169void
170normalizeLinearization(Problem const & problem, LinearizationAt<Problem> & result)
171{
172 normalizeValues(problem.outputSpace(), result.mean);
173}
174
176template <typename Problem>
177inline constexpr bool returnsJacobian = isWithJacobian<
178 std::remove_cvref_t<decltype(std::declval<Problem const &>().template returned<typename Problem::Scalar>(
179 std::declval<typename Problem::template InputVector<typename Problem::Scalar> const &>()
180 ))>>;
181
186template <typename Problem>
187using DualOf = Eigen::AutoDiffScalar<Eigen::Matrix<
188 typename Problem::Scalar,
189 Problem::InputDifferenceSize,
190 1,
191 Eigen::ColMajor,
192 Problem::InputDifferenceMaxSize,
193 1>>;
194
199template <typename Problem, typename Dual = DualOf<Problem>>
200inline constexpr bool returnsDuals = requires {
201 requires std::same_as<typename std::remove_cvref_t<typename Problem::template Returned<Dual>>::Scalar, Dual>;
202};
203
208template <typename Problem>
209[[nodiscard]] LinearizationAt<Problem>
210linearizeReturned(Problem const & problem)
211{
212 using Scalar = typename Problem::Scalar;
213 using JacobianMatrix = typename Problem::JacobianMatrix;
214 auto returned = problem.template returned<Scalar>(problem.mean());
215 using Blocks = decltype(returned.jacobian);
216 auto result = LinearizationAt<Problem>{
217 std::move(returned.value),
218 JacobianMatrix(problem.outputDifferenceSize(), problem.inputDifferenceSize())
219 };
220 EMRYS_FILTER_ASSERT(
221 result.mean.size() == valueDimension(problem.outputSpace()) && "the output does not fit the output space"
222 );
223 problem.forEachUncertain([&]<std::size_t I>(auto const & input) {
224 auto const & block = [&]() -> auto const & {
225 if constexpr (isArgumentJacobians<Blocks>)
226 {
227 return std::get<I>(returned.jacobian.blocks);
228 }
229 else
230 {
231 return jacobianBlock<Problem::ArgInfo::ordinal[I]>(returned.jacobian);
232 }
233 }();
234 EMRYS_FILTER_ASSERT(block.rows() == problem.outputDifferenceSize() && block.cols() == input.differenceDimension() &&
235 "a Jacobian block must have a row per output difference coordinate and a column per input "
236 "difference coordinate");
237 result.jacobian.middleCols(problem.differenceOffset(I), input.differenceDimension()) = block;
238 });
239 normalizeLinearization(problem, result);
240 return result;
241}
242
247template <typename Problem>
248[[nodiscard]] LinearizationAt<Problem>
249linearizeByAutoDiff(Problem const & problem)
250{
251 using Scalar = typename Problem::Scalar;
252 using Dual = DualOf<Problem>;
253 using OutputSpace = typename Problem::OutputSpaceType;
254 Eigen::Index const n = problem.inputDifferenceSize();
255 typename Problem::template InputVector<Scalar> const mu{problem.mean()};
256 typename Problem::template InputDifferenceVector<Dual> delta;
257 delta.resize(n);
258 for (Eigen::Index i = 0; i < n; ++i)
259 {
260 delta(i) = Dual{Scalar(0), int(n), int(i)};
261 }
262 typename Problem::template InputVector<Dual> const x{problem.stepInputs(mu.template cast<Dual>(), delta)};
263 typename Problem::template OutputVector<Dual> const y{problem.template evaluate<Dual>(x)};
264
265 auto result = LinearizationAt<Problem>{
266 Problem::template OutputVector<Scalar>::Zero(y.size()),
267 Problem::JacobianMatrix::Zero(problem.outputDifferenceSize(), n)
268 };
269 for (Eigen::Index r = 0; r < y.size(); ++r)
270 {
271 result.mean(r) = y(r).value();
272 }
273 auto const deviation = [&] {
274 if constexpr (HasMinus<OutputSpace>)
275 {
276 return typename Problem::template OutputDifferenceVector<Dual>{
277 Emrys::minus(problem.outputSpace(), y, result.mean.template cast<Dual>())
278 };
279 }
280 else
281 {
282 return y;
283 }
284 }();
285 EMRYS_FILTER_ASSERT(deviation.size() == result.jacobian.rows());
286 for (Eigen::Index r = 0; r < deviation.size(); ++r)
287 {
288 // An output that does not depend on the inputs may carry no derivatives at all.
289 if (deviation(r).derivatives().size() > 0)
290 {
291 result.jacobian.row(r) = deviation(r).derivatives().transpose();
292 }
293 }
294 normalizeLinearization(problem, result);
295 return result;
296}
297
299template <typename Problem>
300[[nodiscard]] LinearizationAt<Problem>
301linearizeAt(methods::Linearized const &, Problem const & problem)
302{
303 if constexpr (returnsJacobian<Problem>)
304 {
305 return linearizeReturned(problem);
306 }
307 else
308 {
309 static_assert(
310 returnsDuals<Problem>,
311 "methods::linearized() differentiates f automatically, so f must take its uncertain inputs as const auto& "
312 "and build its result with their scalar, which it may take as its template parameter, "
313 "[]<typename T>(auto const & x), or return its Jacobian with withJacobian()"
314 );
315 if constexpr (returnsDuals<Problem>)
316 {
317 return linearizeByAutoDiff(problem);
318 }
319 else
320 {
321 std::unreachable();
322 }
323 }
324}
325
329template <typename Method, typename Problem>
330 requires isLinearMethod<Method>
331[[nodiscard]] LinearizationAt<Problem>
332transformResult(Method const & method, Problem const & problem)
333{
334 return linearizeAt(method, problem);
335}
336
340template <typename Method, typename Scalar, typename Problem>
341[[nodiscard]] typename Problem::RootResult
342transformResult(methods::Iterated<Method, Scalar> const & /*method*/, Problem const & /*problem*/)
343{
344 static_assert(
345 sizeof(Method) == 0,
346 "Iterated is a measurement update's method: a transform or a prediction has no posterior to relinearize at, "
347 "and an update given a block does not support it yet; use its inner method there"
348 );
349 return {};
350}
351
353template <typename Scalar>
354struct SigmaWeights
355{
356 Scalar scale;
357 Scalar center;
358 Scalar centerCov;
359 Scalar point;
360};
361
366template <typename Scalar, UnscentedRule Rule, int N>
367[[nodiscard]] consteval SigmaWeights<Scalar>
368unscentedWeights()
369{
370 if constexpr (N == 0)
371 {
372 return SigmaWeights<Scalar>{Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
373 }
374 else
375 {
376 constexpr double n = N;
377 constexpr UnscentedParams<double> params = Rule::template params<double>(N);
378 constexpr double alphaSquared = params.alpha * params.alpha;
379 constexpr double lambda = alphaSquared * (n + params.kappa) - n;
380 static_assert(n + lambda > 0, "the sigma-point spread must be positive");
381 constexpr double center = lambda / (n + lambda);
382 return SigmaWeights<Scalar>{
383 Scalar(compileTimeSqrt(n + lambda)),
384 Scalar(center),
385 Scalar(center + 1 - alphaSquared + params.beta),
386 Scalar(1 / (2 * (n + lambda)))
387 };
388 }
389}
390
400template <typename Scalar, int N, UnscentedRule Rule>
401[[nodiscard]] SigmaWeights<Scalar>
402sigmaWeights(Rule const & /*method*/, Eigen::Index n)
403{
404 if constexpr (N != Eigen::Dynamic && takesDoubleConstants<Scalar>)
405 {
406 EMRYS_FILTER_ASSERT(n == N);
407 return unscentedWeights<Scalar, Rule, N>();
408 }
409 else
410 {
411 if (n == 0)
412 {
413 return {Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
414 }
415 UnscentedParams<Scalar> const params = Rule::template params<Scalar>(n);
416 Scalar const alphaSquared = params.alpha * params.alpha;
417 Scalar const lambda = alphaSquared * (Scalar(n) + params.kappa) - Scalar(n);
418 EMRYS_FILTER_ASSERT(Scalar(n) + lambda > Scalar(0) && "the sigma-point spread must be positive");
419 Scalar const center = lambda / (Scalar(n) + lambda);
420 return {
421 Eigen::numext::sqrt(Scalar(n) + lambda),
422 center,
423 center + Scalar(1) - alphaSquared + params.beta,
424 Scalar(1) / (Scalar(2) * (Scalar(n) + lambda))
425 };
426 }
427}
428
432template <typename Problem, int Rows, int MaxRows>
433using WhitenedOf =
434 MatrixOf<typename Problem::Scalar, Rows, Problem::OutputDifferenceSize, MaxRows, Problem::OutputDifferenceMaxSize>;
435
440template <typename Problem, typename Deviations>
441struct SigmaMoments
442{
443 typename Problem::template OutputVector<typename Problem::Scalar> mean;
444
446 WhitenedOf<Problem, Deviations::ColsAtCompileTime, Deviations::MaxColsAtCompileTime> whitened;
447
448 typename Problem::OutputMatrix unexplained;
449 Deviations plus;
450 Deviations minus;
451 typename Problem::template OutputDifferenceVector<typename Problem::Scalar> center;
452};
453
463template <typename Problem, typename Samples>
464[[nodiscard]] auto
465sigmaMoments(
466 Problem const & problem,
467 SigmaWeights<typename Problem::Scalar> const & weights,
468 typename Problem::template OutputVector<typename Problem::Scalar> const & center,
469 Samples const & plusPoints,
470 Samples const & minusPoints
471)
472{
473 using Scalar = typename Problem::Scalar;
474 using OutputVector = typename Problem::template OutputVector<Scalar>;
475 using OutputDeviations =
476 typename Problem::template OutputDeviations<Samples::ColsAtCompileTime, Samples::MaxColsAtCompileTime>;
477 auto const & space = problem.outputSpace();
478 OutputDeviations plusDeviations = Emrys::minus(space, plusPoints, center);
479 OutputDeviations minusDeviations = Emrys::minus(space, minusPoints, center);
480 OutputVector mean =
481 Emrys::plus(space, center, weights.point * (plusDeviations.rowwise().sum() + minusDeviations.rowwise().sum()));
482
483 typename Problem::template OutputDifferenceVector<Scalar> const centerDeviation = Emrys::minus(space, center, mean);
484 plusDeviations = Emrys::minus(space, plusPoints, mean);
485 minusDeviations = Emrys::minus(space, minusPoints, mean);
486
487 using Result = SigmaMoments<Problem, OutputDeviations>;
488 OutputDeviations const residual = Scalar(0.5) * (plusDeviations + minusDeviations);
489 decltype(Result::whitened) whitened =
490 (weights.point * weights.scale) * (plusDeviations - minusDeviations).transpose();
491 typename Problem::OutputMatrix unexplained = weights.centerCov * centerDeviation * centerDeviation.transpose();
492 if constexpr (Samples::ColsAtCompileTime != 0)
493 {
494 // A product over no points has an inner size of zero, which MSVC's Eigen refuses to block.
495 unexplained.noalias() += (Scalar(2) * weights.point) * multiply(residual, residual.transpose());
496 }
497 unexplained = (Scalar(0.5) * (unexplained + unexplained.transpose())).eval();
498 return Result{
499 std::move(mean),
500 std::move(whitened),
501 std::move(unexplained),
502 std::move(plusDeviations),
503 std::move(minusDeviations),
504 centerDeviation
505 };
506}
507
515template <
516 typename Problem,
517 int Points_ =
518 Problem::InputDifferenceSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * Problem::InputDifferenceSize + 1,
519 int MaxPoints_ =
520 Problem::InputDifferenceMaxSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * Problem::InputDifferenceMaxSize + 1>
521struct SigmaRoot
522{
524 static constexpr int Points = Points_;
525
527 static constexpr int MaxPoints = MaxPoints_;
528
530 using RootMatrix = typename Problem::template OutputDeviations<Points, MaxPoints>;
531
532 typename Problem::template OutputVector<typename Problem::Scalar> mean;
533 RootMatrix root;
534 bool centerNegative = false;
535};
536
540template <typename Problem>
541struct SigmaOutputs
542{
543 typename Problem::template OutputVector<typename Problem::Scalar> center;
544 typename Problem::template OutputSamples<> plus;
545 typename Problem::template OutputSamples<> minus;
546};
547
553template <typename Problem, typename Spread>
554[[nodiscard]] SigmaOutputs<Problem>
555evaluateSigmaPoints(
556 Problem const & problem,
557 typename Problem::template InputVector<typename Problem::Scalar> const & mu,
558 Spread const & spread
559)
560{
561 using Scalar = typename Problem::Scalar;
562 using InputVector = typename Problem::template InputVector<Scalar>;
563 constexpr int N = Problem::InputDifferenceSize;
564 constexpr int NMax = Problem::InputDifferenceMaxSize;
565 using Outputs = typename Problem::template OutputSamples<
566 N == Eigen::Dynamic ? Eigen::Dynamic : 2 * N + 1,
567 NMax == Eigen::Dynamic ? Eigen::Dynamic : 2 * NMax + 1>;
568 Eigen::Index const n = spread.cols();
569 auto outputs = Outputs{};
570 problem.evaluateEach(
571 2 * n + 1,
572 [&](Eigen::Index k) -> InputVector {
573 if (k == 0)
574 {
575 return mu;
576 }
577 Eigen::Index const j = (k - 1) / 2;
578 return k % 2 == 1 ? problem.plusInputs(mu, spread.col(j)) : problem.plusInputs(mu, -spread.col(j));
579 },
580 outputs
581 );
582 auto result = SigmaOutputs<Problem>{};
583 result.center = outputs.col(0);
584 // Sized by resize(), since braces would set the coefficients of a fixed-size vector.
585 result.plus.resize(outputs.rows(), n);
586 result.minus.resize(outputs.rows(), n);
587 // Over no coordinates there are no plus and minus points, at compile time.
588 if constexpr (N != 0)
589 {
590 for (Eigen::Index j = 0; j < n; ++j)
591 {
592 result.plus.col(j) = outputs.col(1 + 2 * j);
593 result.minus.col(j) = outputs.col(2 + 2 * j);
594 }
595 }
596 return result;
597}
598
604template <UnscentedRule Rule, typename Problem, typename A = roots::DefaultAxes>
605[[nodiscard]] SigmaRoot<Problem>
606transformRoot(Rule const & method, Problem const & problem, A const & axes = {})
607{
608 using Scalar = typename Problem::Scalar;
609 using Result = SigmaRoot<Problem>;
610 Eigen::Index const n = problem.inputDifferenceSize();
611 SigmaWeights<Scalar> const weights = sigmaWeights<Scalar, Problem::InputDifferenceSize>(method, n);
612
613 typename Problem::template InputVector<Scalar> const mu{problem.mean()};
614 typename Problem::InputMatrix const spread = weights.scale * problem.root(axes);
615 auto const outputs = evaluateSigmaPoints(problem, mu, spread);
616
617 auto moments = sigmaMoments(problem, weights, outputs.center, outputs.plus, outputs.minus);
618 Eigen::Index const m = problem.outputDifferenceSize();
619 Scalar const pointRoot = Eigen::numext::sqrt(weights.point);
620 Scalar const centerRoot = Eigen::numext::sqrt(Eigen::numext::abs(weights.centerCov));
621 typename Result::RootMatrix root(m, 2 * n + 1);
622 root.leftCols(n) = pointRoot * moments.plus;
623 root.middleCols(n, n) = pointRoot * moments.minus;
624 root.col(2 * n) = centerRoot * moments.center;
625 return Result{std::move(moments.mean), std::move(root), weights.centerCov < Scalar(0)};
626}
627
632template <UnscentedRule Rule, typename Problem, typename A = roots::DefaultAxes>
633[[nodiscard]] typename Problem::RootResult
634transformResult(Rule const & method, Problem const & problem, A const & axes = {})
635{
636 using Scalar = typename Problem::Scalar;
637 Eigen::Index const n = problem.inputDifferenceSize();
638 SigmaWeights<Scalar> const weights = sigmaWeights<Scalar, Problem::InputDifferenceSize>(method, n);
639
640 typename Problem::template InputVector<Scalar> const mu{problem.mean()};
641 typename Problem::InputMatrix root = problem.root(axes);
642 typename Problem::InputMatrix const spread = weights.scale * root;
643 auto const outputs = evaluateSigmaPoints(problem, mu, spread);
644
645 auto moments = sigmaMoments(problem, weights, outputs.center, outputs.plus, outputs.minus);
646 return {
647 std::move(moments.mean),
648 std::move(root),
649 typename Problem::CrossMatrix{std::move(moments.whitened)},
650 std::move(moments.unexplained)
651 };
652}
653
658template <PointSet Points>
659[[nodiscard]] consteval int
660pointCount(int n)
661{
662 if (n == Eigen::Dynamic)
663 {
664 return Eigen::Dynamic;
665 }
666 if (n == 0)
667 {
668 return 1;
669 }
670 return int(std::min<Eigen::Index>(Points::pointCount(n), MaxPointCount + 1));
671}
672
674template <PointSet Points>
675[[nodiscard]] Eigen::Index
676pointCountAt(Eigen::Index n)
677{
678 return n == 0 ? Eigen::Index(1) : Points::pointCount(n);
679}
680
685template <typename Scalar, typename Points, int N, int NMax>
686struct UnitPoints
687{
688 static constexpr int Count = pointCount<Points>(N);
689 static constexpr int MaxCount = pointCount<Points>(NMax);
690
691 static_assert(
692 NMax == Eigen::Dynamic || MaxCount <= std::max(2 * NMax + 2, MaxPointCount),
693 "a point set over this many coordinates has more than MaxPointCount (4096) points; declare the "
694 "coordinates the model is affine in with affineIn(), or use a set with fewer points, such as a Gauss-Hermite "
695 "rule of a lower order"
696 );
697
698 MatrixOf<Scalar, N, Count, NMax, MaxCount> points;
699 MatrixOf<Scalar, Count, 1, MaxCount, 1> weights;
700 Eigen::Index reference;
701};
702
707template <typename Scalar, int N, int NMax, PointSet Points>
708[[nodiscard]] UnitPoints<Scalar, Points, N, NMax>
709unitPoints(Points const & /*set*/, Eigen::Index n)
710{
711 using Result = UnitPoints<Scalar, Points, N, NMax>;
712 Eigen::Index const count = pointCountAt<Points>(n);
713 auto result = Result{};
714 result.points.resize(n, count);
715 result.weights.resize(count);
716 result.reference = 0;
717 if (n == 0)
718 {
719 result.weights(0) = Scalar(1);
720 return result;
721 }
722 result.reference = Points::unitPoints(n, result.points, result.weights);
723 EMRYS_FILTER_ASSERT(
724 result.reference >= 0 && result.reference < count
725 && "the point the mean is taken about must be one of the points"
726 );
727 return result;
728}
729
733template <typename Problem, typename Deviations, typename Whitened>
734struct PointMoments
735{
736 typename Problem::template OutputVector<typename Problem::Scalar> mean;
737 Whitened whitened;
738 typename Problem::OutputMatrix unexplained;
739 Deviations deviations;
740};
741
755template <typename Problem, typename Samples, typename Points, typename Weights, typename Root>
756[[nodiscard]] auto
757pointMoments(
758 Problem const & problem,
759 Points const & points,
760 Weights const & weights,
761 Eigen::Index reference,
762 Samples const & outputs,
763 Root const & root
764)
765{
766 using Scalar = typename Problem::Scalar;
767 using OutputVector = typename Problem::template OutputVector<Scalar>;
768 using OutputDeviations =
769 typename Problem::template OutputDeviations<Samples::ColsAtCompileTime, Samples::MaxColsAtCompileTime>;
770 auto const & space = problem.outputSpace();
771 OutputVector const center = outputs.col(reference);
772 OutputDeviations deviations = Emrys::minus(space, outputs, center);
773 typename Problem::template OutputDifferenceVector<Scalar> step;
774 step.noalias() = multiply(deviations, weights);
775 OutputVector mean = Emrys::plus(space, center, step);
776 deviations = Emrys::minus(space, outputs, mean);
777 using Whitened = WhitenedOf<Problem, Points::RowsAtCompileTime, Points::MaxRowsAtCompileTime>;
778 Whitened whitened = multiply(points * weights.asDiagonal(), deviations.transpose());
779 EMRYS_FILTER_ASSERT(root.cols() == whitened.rows() && "the root must have a column per coordinate of the points");
780 for (Eigen::Index j = 0; j < root.cols(); ++j)
781 {
782 if ((root.col(j).array() == Scalar(0)).all())
783 {
784 whitened.row(j).setZero();
785 }
786 }
787 OutputDeviations residuals = deviations;
788 residuals.noalias() -= multiply(whitened.transpose(), points);
789 typename Problem::OutputMatrix unexplained = multiply(residuals * weights.asDiagonal(), residuals.transpose());
790 unexplained = (Scalar(0.5) * (unexplained + unexplained.transpose())).eval();
791 return PointMoments<Problem, OutputDeviations, Whitened>{
792 std::move(mean),
793 std::move(whitened),
794 std::move(unexplained),
795 std::move(deviations)
796 };
797}
798
803template <typename Problem, typename Unit, typename Spread>
804[[nodiscard]] auto
805evaluatePoints(Problem const & problem, Unit const & unit, Spread const & spread)
806{
807 using Scalar = typename Problem::Scalar;
808 using OutputSamples = typename Problem::template OutputSamples<Unit::Count, Unit::MaxCount>;
809 typename Problem::template InputVector<Scalar> const mu{problem.mean()};
810 auto outputs = OutputSamples{};
811 problem.evaluateEach(
812 unit.weights.size(),
813 [&](Eigen::Index j) {
814 return problem.plusInputs(mu, spread.col(j));
815 },
816 outputs
817 );
818 return outputs;
819}
820
825template <typename Points, typename Problem, typename A = roots::DefaultAxes>
826 requires isPointSet<Points>
827[[nodiscard]] auto
828transformRoot(Points const & set, Problem const & problem, A const & axes = {})
829{
830 using Scalar = typename Problem::Scalar;
831 using Unit = UnitPoints<Scalar, Points, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>;
832 using Result = SigmaRoot<Problem, Unit::Count, Unit::MaxCount>;
833 using Spread =
834 MatrixOf<Scalar, Problem::InputDifferenceSize, Unit::Count, Problem::InputDifferenceMaxSize, Unit::MaxCount>;
835 Unit const unit = unitPoints<Scalar, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>(
836 set,
837 problem.inputDifferenceSize()
838 );
839 typename Problem::InputMatrix const inputRoot = problem.root(axes);
840 auto spread = Spread{};
841 spread.resize(unit.points.rows(), unit.points.cols());
842 spread.noalias() = multiply(inputRoot, unit.points);
843 auto moments = pointMoments(
844 problem,
845 unit.points,
846 unit.weights,
847 unit.reference,
848 evaluatePoints(problem, unit, spread),
849 inputRoot
850 );
851 typename Result::RootMatrix root = moments.deviations * unit.weights.cwiseSqrt().asDiagonal();
852 return Result{std::move(moments.mean), std::move(root), false};
853}
854
860template <typename Points, typename Problem, typename A = roots::DefaultAxes>
861 requires isPointSet<Points>
862[[nodiscard]] typename Problem::RootResult
863transformResult(Points const & set, Problem const & problem, A const & axes = {})
864{
865 using Scalar = typename Problem::Scalar;
866 using Unit = UnitPoints<Scalar, Points, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>;
867 using Spread =
868 MatrixOf<Scalar, Problem::InputDifferenceSize, Unit::Count, Problem::InputDifferenceMaxSize, Unit::MaxCount>;
869 Unit const unit = unitPoints<Scalar, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>(
870 set,
871 problem.inputDifferenceSize()
872 );
873 typename Problem::InputMatrix root = problem.root(axes);
874 auto spread = Spread{};
875 spread.resize(unit.points.rows(), unit.points.cols());
876 spread.noalias() = multiply(root, unit.points);
877 auto moments =
878 pointMoments(problem, unit.points, unit.weights, unit.reference, evaluatePoints(problem, unit, spread), root);
879 return {
880 std::move(moments.mean),
881 std::move(root),
882 typename Problem::CrossMatrix{std::move(moments.whitened)},
883 std::move(moments.unexplained)
884 };
885}
886
888template <typename Rule, typename A, typename Problem>
889[[nodiscard]] typename Problem::RootResult
890transformResult(methods::Spread<Rule, A> const & method, Problem const & problem)
891{
892 return transformResult(method.rule, problem, method.axes);
893}
894
896template <typename Rule, typename A, typename Problem>
897[[nodiscard]] auto
898transformRoot(methods::Spread<Rule, A> const & method, Problem const & problem)
899{
900 return transformRoot(method.rule, problem, method.axes);
901}
902
908template <typename Problem, typename X, typename Affine, typename Columns>
909[[nodiscard]] typename Problem::template OutputVector<typename Problem::Scalar>
910valueWithAffineJacobian(Problem const & problem, X const & x, Affine const & affine, Columns && columns)
911{
912 using Scalar = typename Problem::Scalar;
913 using OutputVector = typename Problem::template OutputVector<Scalar>;
914 using Index = Eigen::Index;
915 Index const l = affine.size();
916 if constexpr (returnsJacobian<Problem>)
917 {
918 auto returned = problem.template returned<Scalar>(x);
919 using Blocks = decltype(returned.jacobian);
920 problem.forEachUncertain([&]<std::size_t I>(auto const & input) {
921 auto const & block = [&]() -> auto const & {
922 if constexpr (isArgumentJacobians<Blocks>)
923 {
924 return std::get<I>(returned.jacobian.blocks);
925 }
926 else
927 {
928 return jacobianBlock<Problem::ArgInfo::ordinal[I]>(returned.jacobian);
929 }
930 }();
931 Index const start = problem.differenceOffset(I);
932 Index const size = input.differenceDimension();
933 for (Index j = 0; j < l; ++j)
934 {
935 if (affine(j) >= start && affine(j) < start + size)
936 {
937 columns.col(j) = block.col(affine(j) - start);
938 }
939 }
940 });
941 OutputVector y = std::move(returned.value);
942 EMRYS_FILTER_ASSERT(
943 y.size() == valueDimension(problem.outputSpace()) && "the output does not fit the output space"
944 );
945 return y;
946 }
947 else
948 {
949 using Derivatives =
950 Eigen::Matrix<Scalar, Affine::SizeAtCompileTime, 1, Eigen::ColMajor, Affine::MaxSizeAtCompileTime, 1>;
951 using Dual = Eigen::AutoDiffScalar<Derivatives>;
952 static_assert(
953 returnsDuals<Problem, Dual>,
954 "a function that a sigma-point method factors affine coordinates out of returns its Jacobian, or is "
955 "generic "
956 "in its scalar, to be differentiated in those coordinates"
957 );
958 using OutputSpace = typename Problem::OutputSpaceType;
959 Index const total = problem.inputDifferenceSize();
960 typename Problem::template InputDifferenceVector<Dual> delta;
961 delta.resize(total);
962 for (Index i = 0; i < total; ++i)
963 {
964 delta(i) = Dual{Scalar(0), Derivatives::Zero(l)};
965 }
966 for (Index j = 0; j < l; ++j)
967 {
968 delta(affine(j)) = Dual{Scalar(0), int(l), int(j)};
969 }
970 typename Problem::template InputVector<Dual> const dual{problem.stepInputs(x.template cast<Dual>(), delta)};
971 typename Problem::template OutputVector<Dual> const y{problem.template evaluate<Dual>(dual)};
972 OutputVector value = OutputVector::Zero(y.size());
973 for (Index r = 0; r < y.size(); ++r)
974 {
975 value(r) = y(r).value();
976 }
977 auto const deviation = [&] {
978 if constexpr (HasMinus<OutputSpace>)
979 {
980 return typename Problem::template OutputDifferenceVector<Dual>{
981 Emrys::minus(problem.outputSpace(), y, value.template cast<Dual>())
982 };
983 }
984 else
985 {
986 return y;
987 }
988 }();
989 for (Index r = 0; r < deviation.size(); ++r)
990 {
991 // An output that does not depend on the affine coordinates may carry no derivatives.
992 if (deviation(r).derivatives().size() > 0)
993 {
994 columns.row(r) = deviation(r).derivatives().transpose();
995 }
996 else
997 {
998 columns.row(r).setZero();
999 }
1000 }
1001 return value;
1002 }
1003}
1004
1014template <typename Problem, typename Affine, typename ShiftAt, typename Outputs, typename Jacobians>
1015void
1016evaluateWithAffineJacobians(
1017 Problem const & problem,
1018 typename Problem::template InputVector<typename Problem::Scalar> const & mu,
1019 Affine const & affine,
1020 Eigen::Index count,
1021 ShiftAt const & shiftAt,
1022 Outputs & outputs,
1023 Jacobians & jacobians
1024)
1025{
1026 using Index = Eigen::Index;
1027 Index const l = affine.size();
1028 if constexpr (Problem::EvaluatesBatches)
1029 {
1030 EMRYS_FILTER_ASSERT(jacobians.cols() == l * count && "a Jacobian over the affine coordinates per point");
1031 problem.evaluateEachWithAffineJacobians(
1032 count,
1033 [&](Index k) {
1034 return problem.plusInputs(mu, shiftAt(k));
1035 },
1036 outputs,
1037 jacobians
1038 );
1039 }
1040 else
1041 {
1042 for (Index k = 0; k < count; ++k)
1043 {
1044 auto const y = valueWithAffineJacobian(
1045 problem,
1046 problem.plusInputs(mu, shiftAt(k)),
1047 affine,
1048 jacobians.middleCols(k * l, l)
1049 );
1050 if (k == 0)
1051 {
1052 outputs.resize(y.size(), count);
1053 }
1054 outputs.col(k) = y;
1055 }
1056 }
1057}
1058
1067template <typename Problem, int AffineSize, int AffineMaxSize, typename Points>
1068[[nodiscard]] typename Problem::RootResult
1069transformResult(FactoredAt<AffineSize, AffineMaxSize, Points> const & method, Problem const & problem)
1070{
1071 using Scalar = typename Problem::Scalar;
1072 using Index = Eigen::Index;
1073 using InputVector = typename Problem::template InputVector<Scalar>;
1074 using InputDifferenceVector = typename Problem::template InputDifferenceVector<Scalar>;
1075 using OutputVector = typename Problem::template OutputVector<Scalar>;
1076 using OutputMatrix = typename Problem::OutputMatrix;
1077 constexpr int OutputSize = Problem::OutputDifferenceSize;
1078 constexpr int OutputMaxSize = Problem::OutputDifferenceMaxSize;
1079 // The rest of the joint input's differences: the coordinates that get sigma points.
1080 constexpr int RestSize = Problem::InputDifferenceSize == Eigen::Dynamic || AffineSize == Eigen::Dynamic
1081 ? Eigen::Dynamic
1082 : Problem::InputDifferenceSize - AffineSize;
1083 constexpr int RestMaxSize = RestSize == Eigen::Dynamic ? Problem::InputDifferenceMaxSize : RestSize;
1084 using RestIndices = IndexVector<RestSize, RestMaxSize>;
1085 using OutputSamples = typename Problem::template OutputSamples<RestSize, RestMaxSize>;
1086 using RestMatrix = MatrixOf<Scalar, RestSize, RestSize, RestMaxSize, RestMaxSize>;
1087 using RestAffine = MatrixOf<Scalar, RestSize, AffineSize, RestMaxSize, AffineMaxSize>;
1088 using RestCross = MatrixOf<Scalar, RestSize, OutputSize, RestMaxSize, OutputMaxSize>;
1089 using AffineMatrix = MatrixOf<Scalar, AffineSize, AffineSize, AffineMaxSize, AffineMaxSize>;
1090 using AffineRest = MatrixOf<Scalar, AffineSize, RestSize, AffineMaxSize, RestMaxSize>;
1091 using AffineCross = MatrixOf<Scalar, AffineSize, OutputSize, AffineMaxSize, OutputMaxSize>;
1092 using AffineJacobian = MatrixOf<Scalar, OutputSize, AffineSize, OutputMaxSize, AffineMaxSize>;
1093
1094 auto const & affine = method.affine;
1095 Index const total = problem.inputDifferenceSize();
1096 Index const l = affine.size();
1097 Index const n = total - l;
1098 EMRYS_FILTER_ASSERT(
1099 distinctIndices(affine) && (l == 0 || (affine.minCoeff() >= 0 && affine.maxCoeff() < total))
1100 && "affine coordinates must be distinct coordinates of the joint input"
1101 );
1102 auto rest = RestIndices{};
1103 rest.resize(n);
1104 for (Index i = 0, k = 0; i < total; ++i)
1105 {
1106 if ((affine.array() != i).all())
1107 {
1108 rest(k++) = i;
1109 }
1110 }
1111
1112 // x_a given x_n: mean mu_a + gain (x_n - mu_n), covariance conditional; points from a root of P_nn.
1113 InputVector const mu = problem.mean();
1114 typename Problem::InputMatrix const cov = problem.covarianceLower();
1115 AffineMatrix conditional = lowerSymmetricBlock(cov, affine, affine);
1116 RestMatrix restRoot = RestMatrix::Zero(n, n);
1117 AffineRest gain = AffineRest::Zero(l, n);
1118 if constexpr (RestSize != 0)
1119 {
1120 if (n > 0)
1121 {
1122 auto const ldlt = Eigen::LDLT<RestMatrix>{lowerSymmetricBlock(cov, rest, rest)};
1123 RestAffine const restAffine = lowerSymmetricBlock(cov, rest, affine);
1124 gain = transposedSolve<AffineRest>(ldlt, restAffine);
1125 conditional.noalias() -= multiply(gain, restAffine);
1126 refuseFailingAxes<AxesOf<Points>>();
1127 restRoot = rootValue(axesOf(method.points).root(lowerSymmetricBlock(cov, rest, rest)));
1128 }
1129 }
1130 conditional = (Scalar(0.5) * (conditional + conditional.transpose())).eval();
1131 // L_c, the root of P_a|n, which spreads x_a given x_n as the rest's root spreads x_n.
1132 AffineMatrix conditionalRoot = AffineMatrix::Zero(l, l);
1133 if constexpr (AffineSize != 0)
1134 {
1135 if (l > 0)
1136 {
1137 conditionalRoot = defaultRoot(conditional);
1138 }
1139 }
1140
1141 // f at each point, and its Jacobian in x_a there kept as column block k of jacobians, in one
1142 // evaluation of the points (evaluateWithAffineJacobians()).
1143 Index const m = problem.outputDifferenceSize();
1144 // The affine coordinates' parts (decisions.md [linearization-forms]): with the mean Jacobian H_a = sum_i w_i h_i,
1145 // Psi_a = L_c^T H_a^T, and Omega_a = sum_i w_i (h_i - H_a) P_a|n (h_i - H_a)^T in a second pass
1146 // over the Jacobians kept, so that neither is recovered by subtraction.
1147 auto const affineParts = [&](auto const & jacobians, auto const & pointWeights) {
1148 Index const count = pointWeights.size();
1149 AffineJacobian meanJacobian = AffineJacobian::Zero(m, l);
1150 for (Index k = 0; k < count; ++k)
1151 {
1152 meanJacobian += pointWeights(k) * jacobians.middleCols(k * l, l);
1153 }
1154 OutputMatrix omega = OutputMatrix::Zero(m, m);
1155 for (Index k = 0; k < count; ++k)
1156 {
1157 AffineJacobian const deviation = jacobians.middleCols(k * l, l) - meanJacobian;
1158 AffineCross const spread = multiply(conditional, deviation.transpose());
1159 omega.noalias() += pointWeights(k) * multiply(deviation, spread);
1160 }
1161 AffineCross whitened = multiply(conditionalRoot.transpose(), meanJacobian.transpose());
1162 return std::pair{std::move(whitened), std::move(omega)};
1163 };
1164
1165 // The output mean, the rows of x_n in Psi and the points' Omega, and x_a's parts.
1166 // With every coordinate affine there is nothing to spread: the center alone, by any rule.
1167 auto [mean, restWhitened, pointOmega, affineWhitened, affineOmega] = [&] {
1168 if constexpr (isUnscented<RuleOf<Points>> || RestSize == 0)
1169 {
1170 constexpr int Count = RestSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * RestSize + 1;
1171 constexpr int MaxCount = RestMaxSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * RestMaxSize + 1;
1172 using Jacobians = MatrixOf<
1173 Scalar,
1174 OutputSize,
1175 sizeProduct<AffineSize, Count>,
1176 OutputMaxSize,
1177 sizeProduct<AffineMaxSize, MaxCount>>;
1178 using PointWeights = MatrixOf<Scalar, Count, 1, MaxCount, 1>;
1179 SigmaWeights<Scalar> const weights = [&] {
1180 if constexpr (RestSize == 0)
1181 {
1182 return SigmaWeights<Scalar>{Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
1183 }
1184 else
1185 {
1186 return sigmaWeights<Scalar, RestSize>(ruleOf(method.points), n);
1187 }
1188 }();
1189 RestMatrix const spread = weights.scale * restRoot;
1190 AffineRest const affineSpread = [&] {
1191 if constexpr (RestSize != 0)
1192 {
1193 return AffineRest{multiply(gain, spread)};
1194 }
1195 else
1196 {
1197 return AffineRest{};
1198 }
1199 }();
1200 // The center, then the plus and minus points of each column of the spread in turn.
1201 auto const shiftAt = [&]([[maybe_unused]] Index k) {
1202 InputDifferenceVector shift = InputDifferenceVector::Zero(total);
1203 if constexpr (RestSize != 0)
1204 {
1205 if (k > 0)
1206 {
1207 Index const j = (k - 1) / 2;
1208 shift(rest) = spread.col(j);
1209 shift(affine) = affineSpread.col(j);
1210 if (k % 2 == 0)
1211 {
1212 shift = -shift;
1213 }
1214 }
1215 }
1216 return shift;
1217 };
1218 auto jacobians = Jacobians{};
1219 jacobians.resize(m, l * (2 * n + 1));
1220 auto outputs = typename Problem::template OutputSamples<Count, MaxCount>{};
1221 evaluateWithAffineJacobians(problem, mu, affine, 2 * n + 1, shiftAt, outputs, jacobians);
1222 auto pointWeights = PointWeights{};
1223 pointWeights.resize(2 * n + 1);
1224 pointWeights(0) = weights.center;
1225 OutputVector const center = outputs.col(0);
1226 // Sized by resize(), since braces would set the coefficients of a fixed-size vector.
1227 auto plus = OutputSamples{};
1228 auto minus = OutputSamples{};
1229 plus.resize(center.size(), n);
1230 minus.resize(center.size(), n);
1231 // With every coordinate affine there are no plus and minus points, at compile time.
1232 if constexpr (RestSize != 0)
1233 {
1234 for (Index j = 0; j < n; ++j)
1235 {
1236 plus.col(j) = outputs.col(1 + 2 * j);
1237 minus.col(j) = outputs.col(2 + 2 * j);
1238 pointWeights(1 + 2 * j) = weights.point;
1239 pointWeights(2 + 2 * j) = weights.point;
1240 }
1241 }
1242 auto sigma = sigmaMoments(problem, weights, center, plus, minus);
1243 auto [whitened, omega] = affineParts(jacobians, pointWeights);
1244 return std::tuple{
1245 std::move(sigma.mean),
1246 RestCross{std::move(sigma.whitened)},
1247 std::move(sigma.unexplained),
1248 std::move(whitened),
1249 std::move(omega)
1250 };
1251 }
1252 else
1253 {
1254 using Unit = UnitPoints<Scalar, RuleOf<Points>, RestSize, RestMaxSize>;
1255 using RestSpread = MatrixOf<Scalar, RestSize, Unit::Count, RestMaxSize, Unit::MaxCount>;
1256 using AffineSpread = MatrixOf<Scalar, AffineSize, Unit::Count, AffineMaxSize, Unit::MaxCount>;
1257 using Jacobians = MatrixOf<
1258 Scalar,
1259 OutputSize,
1260 sizeProduct<AffineSize, Unit::Count>,
1261 OutputMaxSize,
1262 sizeProduct<AffineMaxSize, Unit::MaxCount>>;
1263 Unit const unit = unitPoints<Scalar, RestSize, RestMaxSize>(ruleOf(method.points), n);
1264 Index const count = unit.weights.size();
1265 auto spread = RestSpread{};
1266 spread.resize(n, count);
1267 spread.noalias() = multiply(restRoot, unit.points);
1268 auto affineSpread = AffineSpread{};
1269 affineSpread.resize(l, count);
1270 affineSpread.noalias() = multiply(gain, spread);
1271 auto jacobians = Jacobians{};
1272 jacobians.resize(m, l * count);
1273 auto outputs = typename Problem::template OutputSamples<Unit::Count, Unit::MaxCount>{};
1274 auto const shiftAt = [&](Index j) {
1275 InputDifferenceVector shift = InputDifferenceVector::Zero(total);
1276 shift(rest) = spread.col(j);
1277 shift(affine) = affineSpread.col(j);
1278 return shift;
1279 };
1280 evaluateWithAffineJacobians(problem, mu, affine, count, shiftAt, outputs, jacobians);
1281 auto points = pointMoments(problem, unit.points, unit.weights, unit.reference, outputs, restRoot);
1282 auto [whitened, omega] = affineParts(jacobians, unit.weights);
1283 return std::tuple{
1284 std::move(points.mean),
1285 RestCross{std::move(points.whitened)},
1286 std::move(points.unexplained),
1287 std::move(whitened),
1288 std::move(omega)
1289 };
1290 }
1291 }();
1292
1293 // The joint root, with x_n = mu_n + R_n xi_n and x_a = mu_a + G R_n xi_n + L_c xi_a: block-diagonal
1294 // by input, since G and L_c are, and Psi's rows at x_n from the points and at x_a from L_c.
1295 typename Problem::InputMatrix root = Problem::InputMatrix::Zero(total, total);
1296 typename Problem::CrossMatrix whitened = Problem::CrossMatrix::Zero(total, m);
1297 if constexpr (RestSize != 0)
1298 {
1299 AffineRest const affineRoot = multiply(gain, restRoot);
1300 for (Index k = 0; k < n; ++k)
1301 {
1302 whitened.row(rest(k)) = restWhitened.row(k);
1303 for (Index j = 0; j < n; ++j)
1304 {
1305 root(rest(k), rest(j)) = restRoot(k, j);
1306 }
1307 for (Index j = 0; j < l; ++j)
1308 {
1309 root(affine(j), rest(k)) = affineRoot(j, k);
1310 }
1311 }
1312 }
1313 for (Index k = 0; k < l; ++k)
1314 {
1315 whitened.row(affine(k)) = affineWhitened.row(k);
1316 for (Index j = 0; j < l; ++j)
1317 {
1318 root(affine(k), affine(j)) = conditionalRoot(k, j);
1319 }
1320 }
1321 OutputMatrix unexplained = pointOmega + affineOmega;
1322 unexplained = (Scalar(0.5) * (unexplained + unexplained.transpose())).eval();
1323 return {std::move(mean), std::move(root), std::move(whitened), std::move(unexplained)};
1324}
1325
1331template <Space Space_, RealScalar Scalar_>
1332class PointInput
1333{
1334public:
1335
1336 using Scalar = Scalar_;
1337 using SpaceType = Space_;
1338 using MomentsType = Moments<Space_, Scalar_>;
1339 using VectorType = typename MomentsType::VectorType;
1340 using MatrixType = typename MomentsType::MatrixType;
1341
1343 static constexpr int ValueDimensionAtCompileTime = MomentsType::ValueDimensionAtCompileTime;
1344
1346 static constexpr int MaxValueDimensionAtCompileTime = MomentsType::MaxValueDimensionAtCompileTime;
1347
1349 static constexpr int DifferenceDimensionAtCompileTime = MomentsType::DifferenceDimensionAtCompileTime;
1350
1352 static constexpr int MaxDifferenceDimensionAtCompileTime = MomentsType::MaxDifferenceDimensionAtCompileTime;
1353
1355 PointInput(Space_ space, VectorType point)
1356 : m_space{std::move(space)}
1357 , m_point{std::move(point)}
1358 {}
1359
1361 [[nodiscard]] Space_ const &
1362 space() const
1363 {
1364 return m_space;
1365 }
1366
1368 [[nodiscard]] VectorType const &
1369 mean() const
1370 {
1371 return m_point;
1372 }
1373
1375 [[nodiscard]] MatrixType
1376 covarianceLower() const
1377 {
1378 return MatrixType::Zero(differenceDimension(), differenceDimension());
1379 }
1380
1382 [[nodiscard]] Eigen::Index
1383 valueDimension() const
1384 {
1385 return Emrys::valueDimension(m_space);
1386 }
1387
1389 [[nodiscard]] Eigen::Index
1390 differenceDimension() const
1391 {
1392 return Emrys::differenceDimension(m_space);
1393 }
1394
1395private:
1396
1397 EMRYS_FILTER_NO_UNIQUE_ADDRESS Space_ m_space;
1398 VectorType m_point;
1399};
1400
1410template <typename InputSpace, typename OutputSpace, typename Scalar>
1411struct LinearizationAbout
1412{
1414 using Linearization = StatisticalLinearization<InputSpace, OutputSpace, Scalar>;
1415
1416 using JacobianMatrix = typename Linearization::JacobianMatrix;
1417 using ErrorMatrix = typename Linearization::ErrorMatrix;
1418 using OutputVector = typename GaussianParams<OutputSpace, Scalar>::VectorType;
1419
1420 OutputSpace space;
1421 OutputVector value;
1422 JacobianMatrix jacobian;
1423 ErrorMatrix inputs;
1424
1428 ErrorMatrix unexplained;
1429
1431 [[nodiscard]] ErrorMatrix
1432 error() const
1433 {
1434 return inputs + unexplained;
1435 }
1436};
1437
1444template <typename InputSpace, typename Point, typename Base, typename OutputSpace, typename Scalar>
1445void
1446toChartOf(
1447 InputSpace const & space,
1448 Eigen::MatrixBase<Base> const & base,
1449 Point const & point,
1450 LinearizationAbout<InputSpace, OutputSpace, Scalar> & linearization
1451)
1452{
1453 using Linearization = LinearizationAbout<InputSpace, OutputSpace, Scalar>;
1454 using Difference = typename Moments<InputSpace, Scalar>::DifferenceVectorType;
1455 using OutputDifference = typename GaussianParams<OutputSpace, Scalar>::DifferenceVectorType;
1456 Difference const step = Emrys::minus(space, point, base);
1457 if constexpr (!hasIdentityReset<InputSpace>)
1458 {
1459 auto carried = typename Linearization::JacobianMatrix{};
1460 carried.resize(linearization.jacobian.rows(), linearization.jacobian.cols());
1461 carried.noalias() = multiply(linearization.jacobian, resetJacobian(space, base, step));
1462 linearization.jacobian = std::move(carried);
1463 }
1464 auto back = OutputDifference{};
1465 back.resize(linearization.jacobian.rows());
1466 back.noalias() = -multiply(linearization.jacobian, step);
1467 linearization.value = Emrys::plus(linearization.space, linearization.value, back);
1468}
1469
1479template <typename Method, typename About, typename InputSpace>
1480[[nodiscard]] consteval bool
1481checkReference()
1482{
1483 using Reference = typename About::ReferenceType;
1484 static_assert(
1485 !isIterated<Method>,
1486 "an Iterated update chooses its own points to linearize at, so give linearizedAbout() to a step by its "
1487 "inner method instead"
1488 );
1489 if constexpr (About::IsDistribution)
1490 {
1491 static_assert(
1492 std::is_same_v<typename std::remove_cvref_t<Reference>::SpaceType, InputSpace>,
1493 "linearizedAbout() takes a point or a distribution in the space of the input it is for"
1494 );
1495 }
1496 static_assert(
1497 isLinearMethod<Method> || About::IsDistribution,
1498 "a sigma-point method linearizes over a distribution, so give linearizedAbout() one, such as another "
1499 "filter's state, rather than a point"
1500 );
1501 return true;
1502}
1503
1517template <
1518 typename Method,
1519 typename F,
1520 typename OutputSpace,
1521 typename InputSpace,
1522 typename Base,
1523 typename Reference,
1524 typename... Args>
1525[[nodiscard]] auto
1526linearizeAbout(
1527 Method const & method,
1528 F & f,
1529 OutputSpace const & outputSpace,
1530 InputSpace const & space,
1531 Eigen::MatrixBase<Base> const & base,
1532 LinearizedAbout<Reference> const & about,
1533 Args const &... args
1534)
1535{
1536 using Scalar = typename Base::Scalar;
1537 using About = LinearizedAbout<Reference>;
1538 using Point = PointInput<InputSpace, Scalar>;
1539 using Result = LinearizationAbout<InputSpace, OutputSpace, Scalar>;
1540 static_assert(
1541 crossCount<Args...> == 0,
1542 "withCross() may mark only the input linearizedAbout() is for, which is the argument right after it"
1543 );
1544 static_assert(checkReference<Method, About, InputSpace>());
1545 typename Point::VectorType const point = about.mean();
1546 EMRYS_FILTER_ASSERT(
1547 point.size() == valueDimension(space) && "a point to linearize about must have the input's value dimension"
1548 );
1549 Eigen::Index const n = differenceDimension(space);
1550 auto result = [&] {
1551 if constexpr (isLinearMethod<Method>)
1552 {
1553 auto const input = Point{space, point};
1554 using Problem = Transform<F, OutputSpace, Point, Args...>;
1555 auto const problem = Problem{f, outputSpace, input, args...};
1556 auto const linearized = linearizeAt(method, problem);
1557 // The other inputs' share, J Sigma J^T over the joint input, whose block at the point is zero.
1558 typename Problem::CrossMatrix const cross =
1559 multiply(problem.covarianceLower(), linearized.jacobian.transpose());
1560 typename Result::ErrorMatrix inputs = multiply(linearized.jacobian, cross);
1561 inputs = (Scalar(0.5) * (inputs + inputs.transpose())).eval();
1562 Eigen::Index const m = inputs.rows();
1563 return Result{
1564 problem.outputSpace(),
1565 typename Result::OutputVector{linearized.mean},
1566 typename Result::JacobianMatrix{linearized.jacobian.middleCols(problem.differenceOffset(0), n)},
1567 std::move(inputs),
1568 Result::ErrorMatrix::Zero(m, m)
1569 };
1570 }
1571 else if constexpr (About::IsDistribution)
1572 {
1573 using Distribution = std::remove_cvref_t<Reference>;
1574 using Marked = CrossMarked<Distribution>;
1575 using Problem = Transform<F, OutputSpace, Marked, Args...>;
1576 auto const marked = Marked{about.reference};
1577 auto const problem = Problem{f, outputSpace, marked, args...};
1578 auto const predicted = problem.assemble(transformResult(method, problem));
1579 return Result{
1580 problem.outputSpace(),
1581 typename Result::OutputVector{predicted.output().mean()},
1582 predicted.jacobian(),
1583 predicted.inputsCovariance(),
1584 predicted.unexplainedCovariance()
1585 };
1586 }
1587 }();
1588 toChartOf(space, base, point, result);
1589 return result;
1590}
1591
1598template <typename Method, typename F, typename OutputSpace, typename Reference, typename X, typename... Args>
1599[[nodiscard]] auto
1600transformAbout(
1601 Method const & method,
1602 F & f,
1603 OutputSpace const & outputSpace,
1604 LinearizedAbout<Reference> const & about,
1605 X const & x,
1606 Args const &... args
1607)
1608{
1609 static_assert(ArgTraits<X>::isUncertain, "linearizedAbout() is followed by the uncertain input it is for");
1610 using Input = typename ArgTraits<X>::Input;
1611 using InputSpace = typename Input::SpaceType;
1612 using Scalar = typename Input::Scalar;
1613 using Output = GaussianParams<OutputSpace, Scalar>;
1614 using Covariance = typename Moments<InputSpace, Scalar>::MatrixType;
1615 using Cross = CrossMatrixOf<InputSpace, OutputSpace, Scalar>;
1616 auto const & input = argInput(x);
1617 auto linearization = linearizeAbout(method, f, outputSpace, input.space(), input.mean(), about, args...);
1618 Covariance const cov = symmetricFromLower<Covariance>(covarianceLowerOf(input));
1619 Cross cross = multiply(cov, linearization.jacobian.transpose());
1620 typename Output::MatrixType explained = multiply(linearization.jacobian, cross);
1621 explained = (Scalar(0.5) * (explained + explained.transpose())).eval();
1622 if constexpr (ArgTraits<X>::hasCross)
1623 {
1624 return OutputWithCross<InputSpace, OutputSpace, JacobianForm, Scalar>{
1625 std::move(linearization.space),
1626 std::move(linearization.value),
1627 std::move(linearization.jacobian),
1628 std::move(cross),
1629 std::move(explained),
1630 std::move(linearization.inputs),
1631 std::move(linearization.unexplained)
1632 };
1633 }
1634 else
1635 {
1636 typename Output::MatrixType outputCov = explained + linearization.inputs + linearization.unexplained;
1637 return Output{linearization.space, linearization.value, std::move(outputCov)};
1638 }
1639}
1640
1641} // namespace internal
1642
1647template <UncertainInput Input>
1648[[nodiscard]] CrossMarked<Input>
1649withCross(Input const & input)
1650{
1651 return {input};
1652}
1653
1658template <UncertainInput Input>
1659void
1660withCross(Input const && input) = delete;
1661
1686template <UncertainInput Distribution>
1688linearizedAbout(Distribution const & distribution)
1689{
1690 return {distribution};
1691}
1692
1697template <UncertainInput Distribution>
1698void
1699linearizedAbout(Distribution const && distribution) = delete;
1700
1706template <typename Derived>
1708linearizedAbout(Eigen::MatrixBase<Derived> const & point)
1709{
1710 static_assert(Derived::ColsAtCompileTime == 1, "a point to linearize at is a column vector");
1711 return {typename Derived::PlainObject{point}};
1712}
1713
1714namespace internal
1715{
1716
1730template <typename Method, typename F, typename OutputSpace, typename... Args>
1731 requires(IsSpace<OutputSpace> && crossCount<Args...> <= 1)
1732[[nodiscard]] auto
1733transformBy(Method const & method, F && f, OutputSpace const & outputSpace, Args const &... args)
1734{
1735 if constexpr (leadsWithReference<Args...>)
1736 {
1737 return transformAbout(method, f, outputSpace, args...);
1738 }
1739 else
1740 {
1741 using Problem = Transform<std::remove_reference_t<F>, OutputSpace, Args...>;
1742 Problem const problem{f, outputSpace, args...};
1743 return problem.assemble(transformResult(method, problem));
1744 }
1745}
1746
1750template <typename T>
1751struct TransformInputTagOf
1752{
1753 using type = void;
1754};
1755
1757template <typename T>
1758 requires(ArgTraits<T>::isUncertain)
1759struct TransformInputTagOf<T>
1760{
1761 using type = typename ArgTraits<T>::Input::SpaceType;
1762};
1763
1764template <typename S, typename Vector>
1765struct TransformInputTagOf<Sure<S, Vector>>
1766{
1767 using type = S;
1768};
1769
1771
1773template <typename T>
1774using TransformInputTag = typename TransformInputTagOf<T>::type;
1775
1779template <typename... Args>
1780inline constexpr std::size_t leadingPolicies = [] {
1781 constexpr std::array<bool, sizeof...(Args) + 1> policies{ModelPolicy<Args>..., false};
1782 std::size_t n = 0;
1783 while (policies[n])
1784 {
1785 ++n;
1786 }
1787 return n;
1788}();
1789
1791template <typename... Args>
1792inline constexpr bool policyAfterInput = [] {
1793 constexpr std::array<bool, sizeof...(Args) + 1> policies{ModelPolicy<Args>..., false};
1794 for (std::size_t i = leadingPolicies<Args...>; i < sizeof...(Args); ++i)
1795 {
1796 if (policies[i])
1797 {
1798 return true;
1799 }
1800 }
1801 return false;
1802}();
1803
1805template <typename... Ts>
1806struct FirstTypeOf
1807{
1808 using type = void;
1809};
1810
1812template <typename First, typename... Rest>
1813struct FirstTypeOf<First, Rest...>
1814{
1815 using type = std::conditional_t<std::is_void_v<First>, typename FirstTypeOf<Rest...>::type, First>;
1816};
1817
1819
1823template <bool Uncertain, typename A>
1824struct UncertainScalarOf
1825{
1826 using type = void;
1827};
1828
1830template <typename A>
1831struct UncertainScalarOf<true, A>
1832{
1833 using type = typename A::Scalar;
1834};
1835
1837
1842template <typename Model, typename... Inputs>
1843[[nodiscard]] consteval auto
1844transformReadsPolicy()
1845{
1846 if constexpr (readsWholeState<Model>)
1847 {
1848 static_assert(
1849 sizeof...(Inputs) == 1 && (ArgTraits<Inputs>::isUncertain && ...),
1850 "transform() of a function without reads() takes one uncertain input, the vector the function reads; "
1851 "name several inputs with reads(), each over a tagged space"
1852 );
1853 return std::type_identity<ReadsWhole<TransformInputTag<std::tuple_element_t<0, std::tuple<Inputs...>>>>>{};
1854 }
1855 else
1856 {
1857 return std::type_identity<typename Model::template Policy<PolicyKind::reads>>{};
1858 }
1859}
1860
1865template <typename Model>
1866[[nodiscard]] consteval bool
1867transformOutputFromNoise()
1868{
1869 if constexpr (Model::template has<PolicyKind::outputSpace> || Model::template has<PolicyKind::writes>)
1870 {
1871 return false;
1872 }
1873 else if constexpr (Model::template has<PolicyKind::additiveNoise>)
1874 {
1875 return Model::template Policy<PolicyKind::additiveNoise>::HasOwnSpace;
1876 }
1877 else
1878 {
1879 return false;
1880 }
1881}
1882
1889template <typename Model, typename In, bool OutputFromNoise>
1890[[nodiscard]] consteval auto
1891transformOutputSpace()
1892{
1893 using Function = typename Model::FunctionType;
1894 if constexpr (Model::template has<PolicyKind::outputSpace>)
1895 {
1896 using Policy = typename Model::template Policy<PolicyKind::outputSpace>;
1897 return std::type_identity<decltype(std::declval<Policy const &>().in(std::declval<In const &>()))>{};
1898 }
1899 else if constexpr (Model::template has<PolicyKind::writes>)
1900 {
1901 return std::type_identity<typename Model::template Policy<PolicyKind::writes>::OutputSpace>{};
1902 }
1903 else if constexpr (OutputFromNoise)
1904 {
1905 return std::type_identity<
1906 typename Model::template Policy<PolicyKind::additiveNoise>::Distribution::SpaceType>{};
1907 }
1908 else if constexpr (requires { typename Function::OutputSpaceType; })
1909 {
1910 return std::type_identity<typename Function::OutputSpaceType>{};
1911 }
1912 else
1913 {
1914 using Returned = decltype(vectorReturned(callForm(
1915 std::declval<Function const &>(),
1916 std::declval<In const &>(),
1917 std::declval<NoOutputSpace const &>(),
1918 std::declval<typename Moments<In, double>::VectorType const &>()
1919 )));
1920 using Evaluated = std::remove_cvref_t<decltype(valueOf(std::declval<Returned>()).eval())>;
1921 static_assert(Evaluated::ColsAtCompileTime == 1, "a transformed function must return a column vector");
1922 return std::type_identity<Linear<Evaluated::RowsAtCompileTime, Evaluated::MaxRowsAtCompileTime>>{};
1923 }
1924}
1925
1933template <typename Model, typename... Inputs>
1934class ModelTransform
1935{
1936public:
1937
1939 static constexpr bool Whole = readsWholeState<Model>;
1940
1941 static constexpr std::size_t InputCount = sizeof...(Inputs);
1942
1944 template <std::size_t J>
1945 using InputAt = std::tuple_element_t<J, std::tuple<Inputs...>>;
1946
1948 template <std::size_t J>
1949 using TagAt = TransformInputTag<InputAt<J>>;
1950
1951private:
1952
1954 [[nodiscard]] static consteval bool
1955 uncertain(std::size_t j)
1956 {
1957 constexpr std::array<bool, InputCount + 1> flags{ArgTraits<Inputs>::isUncertain..., false};
1958 return flags[j];
1959 }
1960
1961 static_assert(
1962 ((!std::is_void_v<TransformInputTag<Inputs>>) && ...),
1963 "an input of transform() is an uncertain input, one marked by withCross(), or sure<Tag>(value), after a "
1964 "linearizedAbout() reference if there is one; its output space is a policy, outputSpace(space), before the "
1965 "inputs"
1966 );
1967 static_assert(
1968 distinctTypes<TransformInputTag<Inputs>...>(),
1969 "two inputs of transform() have one tag; an input's tag names it once"
1970 );
1971
1973 using ReadsPolicy = typename decltype(transformReadsPolicy<Model, Inputs...>())::type;
1974
1975public:
1976
1977 static constexpr std::size_t ItemCount = ReadsPolicy::Count;
1978
1980 template <std::size_t I>
1981 using Item = typename ReadsPolicy::template Item<I>;
1982
1984 template <std::size_t I>
1985 using Name = typename Item<I>::NameType;
1986
1988 using In = typename ReadsPolicy::InputSpace;
1989
1991 template <typename T>
1992 using InVector = typename Moments<In, T>::VectorType;
1993
1994private:
1995
1997 template <std::size_t I>
1998 [[nodiscard]] static consteval std::size_t
1999 inputOfItem()
2000 {
2001 if constexpr (Whole)
2002 {
2003 return 0;
2004 }
2005 else
2006 {
2007 using Path = typename Item<I>::PathType;
2008 static_assert(
2009 std::is_same_v<Path, TagPath<PathTag<Path>>>,
2010 "transform() matches each item of reads() to an input by its one tag, path<Tag> or "
2011 "on<Name>(path<Tag>); "
2012 "it has no state for a path of several tags to lie in"
2013 );
2014 constexpr std::array<bool, InputCount + 1> matches{
2015 std::is_same_v<TransformInputTag<Inputs>, PathTag<Path>>...,
2016 false
2017 };
2018 std::size_t j = 0;
2019 while (j < InputCount && !matches[j])
2020 {
2021 ++j;
2022 }
2023 return j;
2024 }
2025 }
2026
2028 [[nodiscard]] static consteval std::size_t
2029 inputOf(std::size_t i)
2030 {
2031 return []<std::size_t... I>(std::size_t item, std::index_sequence<I...>) {
2032 std::size_t found = InputCount;
2033 ((I == item ? (found = inputOfItem<I>(), 0) : 0), ...);
2034 return found;
2035 }(i, std::make_index_sequence<ItemCount>{});
2036 }
2037
2039 template <std::size_t... I>
2040 [[nodiscard]] static consteval bool
2041 itemsGiven(std::index_sequence<I...>)
2042 {
2043 return ((inputOfItem<I>() < InputCount) && ...);
2044 }
2045
2046public:
2047
2049 static constexpr bool ItemsGiven = itemsGiven(std::make_index_sequence<ItemCount>{});
2050
2051private:
2052
2054 [[nodiscard]] static consteval bool
2055 inputRead(std::size_t j)
2056 {
2057 for (std::size_t i = 0; i < ItemCount; ++i)
2058 {
2059 if (inputOf(i) == j)
2060 {
2061 return true;
2062 }
2063 }
2064 return false;
2065 }
2066
2068 template <std::size_t... J>
2069 [[nodiscard]] static consteval bool
2070 inputsRead(std::index_sequence<J...>)
2071 {
2072 return (inputRead(J) && ...);
2073 }
2074
2075public:
2076
2078 static constexpr bool InputsRead = inputsRead(std::make_index_sequence<InputCount>{});
2079
2080private:
2081
2083 [[nodiscard]] static consteval std::size_t
2084 uncertainIndex(std::size_t j)
2085 {
2086 std::size_t k = 0;
2087 for (std::size_t i = 0; i < j; ++i)
2088 {
2089 if (uncertain(i))
2090 {
2091 ++k;
2092 }
2093 }
2094 return k;
2095 }
2096
2098 static constexpr std::size_t UncertainCount = (std::size_t{ArgTraits<Inputs>::isUncertain} + ... + 0);
2099
2100public:
2101
2103 using Function = typename Model::FunctionType;
2104
2106 static constexpr bool OutputFromNoise = transformOutputFromNoise<Model>();
2107
2111 using Out = typename decltype(transformOutputSpace<Model, In, OutputFromNoise>())::type;
2112
2114 explicit ModelTransform(Model const & model, Inputs const &... inputs)
2115 : m_model{model}
2116 , m_inputs{inputs...}
2117 , m_in{makeIn()}
2118 , m_out{makeOut()}
2119 {
2120 if constexpr (Whole)
2121 {
2122 m_valueRows[0] = BlockRows{0, valueDimension(m_in)};
2123 m_differenceRows[0] = BlockRows{0, differenceDimension(m_in)};
2124 }
2125 else
2126 {
2127 [&]<std::size_t... I>(std::index_sequence<I...>) {
2128 ((m_valueRows[I] = blockRows<Name<I>>(m_in), m_differenceRows[I] = differenceBlockRows<Name<I>>(m_in)),
2129 ...);
2130 }(std::make_index_sequence<ItemCount>{});
2131 }
2132 }
2133
2135 [[nodiscard]] Out const &
2136 out() const
2137 {
2138 return m_out;
2139 }
2140
2145 template <typename... A>
2146 [[nodiscard]] auto
2147 operator()(A const &... args) const
2148 {
2149 static_assert(sizeof...(A) == InputCount);
2150 using T = typename FirstTypeOf<typename UncertainScalarOf<ArgTraits<Inputs>::isUncertain, A>::type...>::type;
2151 auto const arguments = std::tuple<A const &...>{args...};
2152 auto x = InVector<T>{};
2153 x.resize(valueDimension(m_in));
2154 [&]<std::size_t... I>(std::index_sequence<I...>) {
2155 (gatherItem<I, T>(x, std::get<inputOfItem<I>()>(arguments)), ...);
2156 }(std::make_index_sequence<ItemCount>{});
2157 return split(vectorReturned(callForm(m_model.function(), m_in, m_out, x)));
2158 }
2159
2162 static constexpr bool EvaluatesBatches = takesBatches<Function>;
2163
2168 template <typename Y, typename... A>
2169 void
2170 evaluateBatch(Y & y, A const &... args) const
2171 requires EvaluatesBatches
2172 {
2173 callBatched(m_model.function(), m_in, m_out, gatherColumns<Y>(y.cols(), args...), y);
2174 }
2175
2180 template <typename Y, typename J, typename... A>
2181 void
2182 evaluateBatchWithAffineJacobians(Y & y, J & jacobians, A const &... args) const
2183 requires EvaluatesBatches
2184 {
2185 auto wanted = std::array<bool, std::tuple_size_v<InputBlocks<In>>>{};
2186 [&]<std::size_t... I>(std::index_sequence<I...>) {
2187 ((wanted[I] = itemFactored<I>()), ...);
2188 }(std::make_index_sequence<ItemCount>{});
2189 auto const x = gatherColumns<Y>(y.cols(), args...);
2190 callBatchedWithJacobians(m_model.function(), m_in, m_out, x, wanted, y, jacobians);
2191 }
2192
2197 template <typename Y, typename... A>
2198 [[nodiscard]] auto
2199 gatherColumns(Eigen::Index count, A const &... args) const
2200 {
2201 static_assert(sizeof...(A) == InputCount);
2202 using T = typename Y::Scalar;
2203 using X = MatrixOf<
2204 T,
2206 Y::ColsAtCompileTime,
2208 Y::MaxColsAtCompileTime>;
2209 auto const arguments = std::tuple<A const &...>{args...};
2210 auto x = X{};
2211 x.resize(valueDimension(m_in), count);
2212 auto column = InVector<T>{};
2213 column.resize(valueDimension(m_in));
2214 for (Eigen::Index j = 0; j < count; ++j)
2215 {
2216 [&]<std::size_t... I>(std::index_sequence<I...>) {
2217 (gatherItem<I, T>(column, columnOf<inputOfItem<I>()>(std::get<inputOfItem<I>()>(arguments), j)), ...);
2218 }(std::make_index_sequence<ItemCount>{});
2219 x.col(j) = column;
2220 }
2221 return x;
2222 }
2223
2227 template <std::size_t I>
2228 [[nodiscard]] static consteval bool
2229 itemFactored()
2230 {
2231 using Affine = typename Model::template Policy<PolicyKind::affineIn>;
2232 return affineNames<Affine, Name<I>> && isLinear<Name<I>> && uncertain(inputOfItem<I>());
2233 }
2234
2236 [[nodiscard]] static consteval bool
2237 factorsAffine()
2238 {
2239 return []<std::size_t... I>(std::index_sequence<I...>) {
2240 return (itemFactored<I>() || ... || false);
2241 }(std::make_index_sequence<ItemCount>{});
2242 }
2243
2245 [[nodiscard]] static consteval int
2246 affineSize()
2247 {
2248 return []<std::size_t... I>(std::index_sequence<I...>) {
2249 return sizeSum<(itemFactored<I>() ? valueDimensionAtCompileTime<DifferenceSpace<Name<I>>> : 0)...>;
2250 }(std::make_index_sequence<ItemCount>{});
2251 }
2252
2254 [[nodiscard]] static consteval int
2255 affineMaxSize()
2256 {
2257 return []<std::size_t... I>(std::index_sequence<I...>) {
2258 return sizeSum<(itemFactored<I>() ? maxValueDimensionAtCompileTime<DifferenceSpace<Name<I>>> : 0)...>;
2259 }(std::make_index_sequence<ItemCount>{});
2260 }
2261
2266 [[nodiscard]] IndexVector<affineSize(), affineMaxSize()>
2267 affineCoordinates() const
2268 {
2269 std::array<Eigen::Index, InputCount + 1> starts{};
2270 [&]<std::size_t... J>(std::index_sequence<J...>) {
2271 ((starts[J + 1] = starts[J] + (uncertain(J) ? argInput(std::get<J>(m_inputs)).differenceDimension() : 0)),
2272 ...);
2273 }(std::make_index_sequence<InputCount>{});
2274 Eigen::Index count = 0;
2275 [&]<std::size_t... I>(std::index_sequence<I...>) {
2276 ((count += itemFactored<I>() ? m_differenceRows[I].size : 0), ...);
2277 }(std::make_index_sequence<ItemCount>{});
2278 auto result = IndexVector<affineSize(), affineMaxSize()>{};
2279 result.resize(count);
2280 Eigen::Index next = 0;
2281 [&]<std::size_t... I>(std::index_sequence<I...>) {
2282 (
2283 [&] {
2284 if constexpr (itemFactored<I>())
2285 {
2286 for (Eigen::Index r = 0; r < m_differenceRows[I].size; ++r)
2287 {
2288 result(next++) = starts[inputOfItem<I>()] + r;
2289 }
2290 }
2291 }(),
2292 ...
2293 );
2294 }(std::make_index_sequence<ItemCount>{});
2295 return result;
2296 }
2297
2302 template <typename Result>
2303 [[nodiscard]] auto
2304 withNoise(Result && result) const
2305 {
2306 if constexpr (!Model::template has<PolicyKind::additiveNoise>)
2307 {
2308 return std::forward<Result>(result);
2309 }
2310 else
2311 {
2312 auto const placed = placeNoise(m_out, m_model.template policy<PolicyKind::additiveNoise>());
2313 auto const & noise = placed.distribution();
2314 static_assert(
2315 !isGaussianMixture<std::remove_cvref_t<decltype(noise)>>,
2316 "transform() adds one Gaussian noise; mixture noise gives a branch per component, which update() keeps"
2317 );
2318 return plusNoise(result, noise);
2319 }
2320 }
2321
2322private:
2323
2325 [[nodiscard]] In
2326 makeIn() const
2327 {
2328 if constexpr (Whole)
2329 {
2330 return argInput(std::get<0>(m_inputs)).space();
2331 }
2332 else
2333 {
2334 return [&]<std::size_t... I>(std::index_sequence<I...>) {
2335 return In{nameOf<Name<I>>(spaceOf<inputOfItem<I>()>())...};
2336 }(std::make_index_sequence<ItemCount>{});
2337 }
2338 }
2339
2341 template <std::size_t J>
2342 [[nodiscard]] auto
2343 spaceOf() const
2344 {
2345 auto const & input = std::get<J>(m_inputs);
2346 if constexpr (isSure<std::remove_cvref_t<decltype(input)>>)
2347 {
2348 return input.space;
2349 }
2350 else
2351 {
2352 return argInput(input).space();
2353 }
2354 }
2355
2360 [[nodiscard]] Out
2361 makeOut() const
2362 {
2363 if constexpr (Model::template has<PolicyKind::outputSpace>)
2364 {
2365 return m_model.template policy<PolicyKind::outputSpace>().in(m_in);
2366 }
2367 else if constexpr (OutputFromNoise)
2368 {
2369 return m_model.template policy<PolicyKind::additiveNoise>().distribution().space();
2370 }
2371 else if constexpr (!FixedSizeSpace<Out> && requires(Function const & f) { f.outputRows(); })
2372 {
2373 return Out{m_model.function().outputRows()};
2374 }
2375 else
2376 {
2377 return Out{};
2378 }
2379 }
2380
2384 template <std::size_t I, typename T, typename Argument>
2385 void
2386 gatherItem(InVector<T> & x, Argument const & argument) const
2387 {
2388 constexpr int Rows = valueDimensionAtCompileTime<Name<I>>;
2389 auto target = middleRowsOf<Rows>(x, m_valueRows[I].start, m_valueRows[I].size);
2390 if constexpr (uncertain(inputOfItem<I>()))
2391 {
2392 target = argument;
2393 }
2394 else
2395 {
2396 target = argument.value.template cast<T>();
2397 }
2398 }
2399
2403 template <std::size_t J, typename Argument>
2404 [[nodiscard]] static decltype(auto)
2405 columnOf(Argument const & argument, Eigen::Index j)
2406 {
2407 if constexpr (uncertain(J))
2408 {
2409 return argument.col(j);
2410 }
2411 else
2412 {
2413 return (argument);
2414 }
2415 }
2416
2420 template <typename R>
2421 [[nodiscard]] auto
2422 split(R && returned) const
2423 {
2424 using Plain = std::remove_cvref_t<R>;
2425 if constexpr (!isWithJacobian<Plain> || Whole)
2426 {
2427 return std::forward<R>(returned);
2428 }
2429 else
2430 {
2431 using J = std::remove_cvref_t<decltype(returned.jacobian)>;
2432 static_assert(
2433 MatrixExpression<J>,
2434 "a transformed function returns its Jacobian as one matrix over its input's differences, "
2435 "withJacobian(y, J)"
2436 );
2437 auto const & jacobian = returned.jacobian;
2438 EMRYS_FILTER_ASSERT(
2439 jacobian.cols() == differenceDimension(m_in)
2440 && "a returned Jacobian has a column per difference coordinate of the function's input"
2441 );
2442 auto blocks = [&]<std::size_t... J_>(std::index_sequence<J_...>) {
2443 return std::tuple_cat(inputBlock<J_>(jacobian)...);
2444 }(std::make_index_sequence<InputCount>{});
2445 if constexpr (UncertainCount == 1)
2446 {
2447 return withJacobian(std::move(returned.value), std::move(std::get<0>(blocks)));
2448 }
2449 else
2450 {
2451 return withJacobian(std::move(returned.value), std::move(blocks));
2452 }
2453 }
2454 }
2455
2459 template <std::size_t J, typename Jacobian>
2460 [[nodiscard]] auto
2461 inputBlock(Jacobian const & jacobian) const
2462 {
2463 if constexpr (!uncertain(J))
2464 {
2465 return std::tuple<>{};
2466 }
2467 else
2468 {
2469 using Traits = ArgTraits<InputAt<J>>;
2470 using Block = MatrixOf<
2471 typename Jacobian::Scalar,
2472 Jacobian::RowsAtCompileTime,
2473 Traits::differenceSize,
2474 Jacobian::MaxRowsAtCompileTime,
2475 Traits::differenceMaxSize>;
2476 Block block = Block::Zero(jacobian.rows(), argInput(std::get<J>(m_inputs)).differenceDimension());
2477 [&]<std::size_t... I>(std::index_sequence<I...>) {
2478 (
2479 [&] {
2480 if constexpr (inputOfItem<I>() == J)
2481 {
2482 block += jacobian.middleCols(m_differenceRows[I].start, m_differenceRows[I].size);
2483 }
2484 }(),
2485 ...
2486 );
2487 }(std::make_index_sequence<ItemCount>{});
2488 return std::tuple{std::move(block)};
2489 }
2490 }
2491
2492 Model const & m_model;
2493 std::tuple<Inputs const &...> m_inputs;
2494 In m_in;
2496 std::array<BlockRows, ItemCount> m_valueRows{};
2497 std::array<BlockRows, ItemCount> m_differenceRows{};
2498};
2499
2503template <typename Space, typename Scalar, typename Noise>
2504[[nodiscard]] GaussianParams<Space, Scalar>
2505plusNoise(GaussianParams<Space, Scalar> const & result, Noise const & noise)
2506{
2507 using Matrix = typename GaussianParams<Space, Scalar>::MatrixType;
2508 Matrix cov = symmetricFromLower<Matrix>(result.covarianceLower());
2509 cov += symmetricFromLower<Matrix>(noise.covarianceLower().template cast<Scalar>());
2510 return GaussianParams<Space, Scalar>{
2511 result.space(),
2512 Emrys::plus(result.space(), result.mean(), noise.mean().template cast<Scalar>()),
2513 std::move(cov)
2514 };
2515}
2516
2520template <typename InputSpace, typename Space, typename Form, typename Scalar, typename Noise>
2521[[nodiscard]] OutputWithCross<InputSpace, Space, Form, Scalar>
2522plusNoise(OutputWithCross<InputSpace, Space, Form, Scalar> const & result, Noise const & noise)
2523{
2524 using Result = OutputWithCross<InputSpace, Space, Form, Scalar>;
2525 using Matrix = typename Result::OutputMatrix;
2526 Matrix inputs = result.inputsCovariance();
2527 inputs += symmetricFromLower<Matrix>(noise.covarianceLower().template cast<Scalar>());
2528 auto const & space = result.output().space();
2529 typename Result::OutputVector mean =
2530 Emrys::plus(space, result.output().mean(), noise.mean().template cast<Scalar>());
2531 if constexpr (std::is_same_v<Form, RootForm>)
2532 {
2533 return Result{
2534 space,
2535 std::move(mean),
2536 typename Result::RootMatrix{result.root()},
2537 typename Result::CrossMatrix{result.whitenedCross()},
2538 Matrix{result.explainedCovariance()},
2539 std::move(inputs),
2540 Matrix{result.unexplainedCovariance()}
2541 };
2542 }
2543 else
2544 {
2545 return Result{
2546 space,
2547 std::move(mean),
2548 typename Result::JacobianMatrix{result.jacobian()},
2549 typename Result::CrossMatrix{result.crossCovariance()},
2550 Matrix{result.explainedCovariance()},
2551 std::move(inputs),
2552 Matrix{result.unexplainedCovariance()}
2553 };
2554 }
2555}
2556
2561template <typename G>
2562[[nodiscard]] consteval bool
2563inputsMatch()
2564{
2565 static_assert(
2566 G::ItemsGiven,
2567 "the function reads an input that transform() was not given: give each item of reads() as an uncertain "
2568 "input over its tag's space, or as sure<Tag>(value)"
2569 );
2570 static_assert(
2571 G::InputsRead,
2572 "transform() was given an input the function does not read: reads() names each input by the tag of its "
2573 "space, and an untagged input is read only by a function without reads()"
2574 );
2575 return G::ItemsGiven && G::InputsRead;
2576}
2577
2582template <typename Method, typename Model, typename... Inputs>
2583[[nodiscard]] auto
2584transformModel(Method const & method, Model const & model, Inputs const &... inputs)
2585{
2586 static_assert(
2587 Model::HasFunction,
2588 "a model known by its likelihood alone has no function for transform() to push its inputs through"
2589 );
2590 if constexpr (leadsWithReference<Inputs...>)
2591 {
2592 return [&]<typename About, typename... Rest>(About const & about, Rest const &... rest) {
2593 using G = ModelTransform<Model, Rest...>;
2594 if constexpr (inputsMatch<G>())
2595 {
2596 auto const g = G{model, rest...};
2597 return g.withNoise(transformBy(method, g, g.out(), about, rest...));
2598 }
2599 }(inputs...);
2600 }
2601 else if constexpr (!inputsMatch<ModelTransform<Model, Inputs...>>())
2602 {
2603 return;
2604 }
2605 else
2606 {
2607 using G = ModelTransform<Model, Inputs...>;
2608 auto const g = G{model, inputs...};
2609 if constexpr (isSigmaPointMethod<Method> && G::factorsAffine())
2610 {
2611 auto const factored =
2612 FactoredAt<G::affineSize(), G::affineMaxSize(), Method>{method, g.affineCoordinates()};
2613 return g.withNoise(transformBy(factored, g, g.out(), inputs...));
2614 }
2615 else
2616 {
2617 return g.withNoise(transformBy(method, g, g.out(), inputs...));
2618 }
2619 }
2620}
2621
2622} // namespace internal
2623
2658template <typename Method, typename F, typename... Args>
2659 requires(!MeasurementModel<F> && !internal::isModel<F> && internal::crossCount<Args...> <= 1)
2660[[nodiscard]] auto
2661transform(Method const & method, F const & f, Args const &... args)
2662{
2663 static_assert(
2664 !internal::policyAfterInput<Args...>,
2665 "transform() takes its policies before its inputs: transform(method, f, reads(...), additiveNoise(...), "
2666 "inputs...)"
2667 );
2668 constexpr std::size_t Policies = internal::leadingPolicies<Args...>;
2669 auto const all = std::tuple<Args const &...>{args...};
2670 return [&]<std::size_t... P, std::size_t... I>(std::index_sequence<P...>, std::index_sequence<I...>) {
2671 auto const model = measurementModel(f, std::get<P>(all)...);
2672 return internal::transformModel(method, model, std::get<Policies + I>(all)...);
2673 }(std::make_index_sequence<Policies>{}, std::make_index_sequence<sizeof...(Args) - Policies>{});
2674}
2675
2683template <typename Method, typename Model, typename... Inputs>
2684 requires((MeasurementModel<Model> || internal::isModel<Model>) && internal::crossCount<Inputs...> <= 1)
2685[[nodiscard]] auto
2686transform(Method const & method, Model const & model, Inputs const &... inputs)
2687{
2688 static_assert(
2690 "transform() pushes its inputs through a measurement model or a function; a process model's step is "
2691 "predict()'s"
2692 );
2693 static_assert(
2694 internal::leadingPolicies<Inputs...> == 0,
2695 "transform() of a model takes the model's own policies, so give the inputs alone after it"
2696 );
2697 return internal::transformModel(method, model, inputs...);
2698}
2699
2700} // namespace Emrys
Batched functions (decisions.md [batched-functions]): batched(), the mark that a function takes every...
The Kalman conditioning of a marked input on an observation of a transform's output (decisions....
The coordinate types (decisions.md [no-bounded-coordinates]): Linear, plain coordinates,...
How a function that stores a matrix argument takes it (decisions.md [forwarded-matrices]): the concep...
The Gaussian: its parameters GaussianParams, the Gaussian that evaluates the density,...
What marginals and conditionals are built from: resolving Eigen index sets against a space; the share...
What a transform returns and what a measurement update reads from it (decisions.md [linearization-for...
The macros every header includes first.
Mathematical constants and scalar functions that work in every scalar type the library accepts,...
Measurement models and their observations (decisions.md [model-policies], [sensors],...
The transform methods, as policies made by their factories (decisions.md [policy-factories],...
Products of fixed-size matrices by Eigen's fixed-size product at every size: multiply() is by Eigen'...
The covariance reset (decisions.md [covariance-reset]): hasIdentityReset<>, resetJacobian(),...
How a model's reads() meet a state and a call, all internal (decisions.md [model-policies],...
SampleSet: a matrix whose columns are samples in a space, which the samplers return and the unscented...
Drawing samples: SamplesWithLogDensity and SampleWithLogDensity, the Gaussian draws drawSamples(),...
Solving by a factorization of a small matrix: transposedSolve() gives , the shape of a Kalman gain an...
The Space concept and what every continuous space shares (decisions.md [sizes-and-dimensions]): the t...
Subspaces, and growing and shrinking blocks (decisions.md [several-resizable-blocks]).
Tag paths and the index utilities (decisions.md [tag-addressing], [index-names]): TagPath and TagPath...
The arguments of transform() and the joint input they form (decisions.md [prediction-inputs],...
typename MomentsType::MatrixType MatrixType
a matrix over differences
Definition Gaussian.h:267
typename MomentsType::VectorType VectorType
a value, such as the mean
Definition Gaussian.h:265
typename MomentsType::DifferenceVectorType DifferenceVectorType
a difference, such as a residual
Definition Gaussian.h:266
DifferenceMatrix< Space_, Space_, Scalar > MatrixType
Definition Moments.h:76
DifferenceVector< Space_, Scalar > DifferenceVectorType
a difference, such as a residual
Definition Moments.h:75
ValueVector< Space_, Scalar > VectorType
Definition Moments.h:74
static constexpr int MaxDifferenceDimensionAtCompileTime
Definition Moments.h:72
static constexpr int DifferenceDimensionAtCompileTime
Definition Moments.h:69
static constexpr int MaxValueDimensionAtCompileTime
Definition Moments.h:66
static constexpr int ValueDimensionAtCompileTime
Definition Moments.h:63
A measurement model: what measurementModel() returns, or a composition of such models (decisions....
Definition MeasurementModel.h:231
#define EMRYS_FILTER_NO_UNIQUE_ADDRESS
Assertion used for programmer errors (misuse of the API).
Definition Macros.h:38
OutputSpace< std::remove_cvref_t< X > > outputSpace(X &&x)
Definition Policies.h:718
auto measurementModel(F &&function, Policies &&... policies)
Definition MeasurementModel.h:913
constexpr Residual< Inner > residual(Inner inner={})
Definition Resampling.h:544
Eigen::Index valueDimension(S const &space)
Definition Space.h:272
auto plus(S const &space, Eigen::MatrixBase< XDerived > const &x, Eigen::MatrixBase< DDerived > const &d)
Definition Space.h:675
auto block(Space auto const &space, X &&x)
Definition TagPath.h:979
auto minus(S const &space, Eigen::MatrixBase< ADerived > const &a, Eigen::MatrixBase< BDerived > const &b)
Definition Space.h:715
auto resetJacobian(S const &space, Eigen::MatrixBase< XDerived > const &x, Eigen::MatrixBase< DDerived > const &d)
Definition Reset.h:377
constexpr bool hasIdentityReset
Whether the reset Jacobian of S is the identity, so that a covariance needs no change when its mean m...
Definition Reset.h:100
Eigen::Index differenceDimension(S const &space)
Definition Space.h:284
constexpr bool isLinear
Whether S is linear: its values and its differences are both plain vectors.
Definition Space.h:321
constexpr int valueDimensionAtCompileTime
The value dimension of Space at compile time, or Eigen::Dynamic if it is resizable.
Definition Space.h:251
constexpr int maxValueDimensionAtCompileTime
The largest valid value dimension of Space, or Eigen::Dynamic if it is unbounded.
Definition Space.h:258
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
auto transform(Method const &method, F const &f, Args const &... args)
Definition Transform.h:2661
CrossMarked< Input > withCross(Input const &input)
Marks the uncertain input, usually the state, whose cross-covariance with the output transform() also...
Definition Transform.h:1649
constexpr Spread< Rule, A > spread(Rule rule, A axes)
Definition Methods.h:750
constexpr Linearized linearized()
Definition Methods.h:333
WithJacobian< internal::PlainResult< std::remove_cvref_t< Value > >, internal::PlainResult< std::remove_cvref_t< Jacobian > > > withJacobian(Value &&value, Jacobian &&jacobian)
Definition Methods.h:222
LinearizedAbout< Distribution const & > linearizedAbout(Distribution const &distribution)
Definition Transform.h:1688
An uncertain input marked by withCross().
Definition TransformArgs.h:143
Where a step linearizes its input: a point, or a distribution, in place of the input's own mean and s...
Definition TransformArgs.h:156
internal::MatrixOf< Scalar, valueDimensionAtCompileTime< DifferenceSpace< OutputSpace > >, valueDimensionAtCompileTime< DifferenceSpace< InputSpace > >, maxValueDimensionAtCompileTime< DifferenceSpace< OutputSpace > >, maxValueDimensionAtCompileTime< DifferenceSpace< InputSpace > > > JacobianMatrix
Definition Condition.h:159
internal::MatrixOf< Scalar, valueDimensionAtCompileTime< DifferenceSpace< OutputSpace > >, valueDimensionAtCompileTime< DifferenceSpace< OutputSpace > >, maxValueDimensionAtCompileTime< DifferenceSpace< OutputSpace > >, maxValueDimensionAtCompileTime< DifferenceSpace< OutputSpace > > > ErrorMatrix
Definition Condition.h:167