27#include <Eigen/Cholesky>
28#include <Eigen/Eigenvalues>
29#include <unsupported/Eigen/AutoDiff>
157template <
typename Problem>
158struct LinearizationAt
160 typename Problem::template OutputVector<typename Problem::Scalar> mean;
161 typename Problem::JacobianMatrix jacobian;
168template <
typename Problem>
170normalizeLinearization(Problem
const & problem, LinearizationAt<Problem> & result)
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 &>()
186template <
typename Problem>
187using DualOf = Eigen::AutoDiffScalar<Eigen::Matrix<
188 typename Problem::Scalar,
189 Problem::InputDifferenceSize,
192 Problem::InputDifferenceMaxSize,
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>;
208template <
typename Problem>
209[[nodiscard]] LinearizationAt<Problem>
210linearizeReturned(Problem
const & problem)
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())
221 result.mean.size() ==
valueDimension(problem.outputSpace()) &&
"the output does not fit the output space"
223 problem.forEachUncertain([&]<std::size_t I>(
auto const & input) {
224 auto const &
block = [&]() ->
auto const & {
225 if constexpr (isArgumentJacobians<Blocks>)
227 return std::get<I>(returned.jacobian.blocks);
231 return jacobianBlock<Problem::ArgInfo::ordinal[I]>(returned.jacobian);
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;
239 normalizeLinearization(problem, result);
247template <
typename Problem>
248[[nodiscard]] LinearizationAt<Problem>
249linearizeByAutoDiff(Problem
const & problem)
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;
258 for (Eigen::Index i = 0; i < n; ++i)
260 delta(i) = Dual{Scalar(0), int(n), int(i)};
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)};
265 auto result = LinearizationAt<Problem>{
266 Problem::template OutputVector<Scalar>::Zero(y.size()),
267 Problem::JacobianMatrix::Zero(problem.outputDifferenceSize(), n)
269 for (Eigen::Index r = 0; r < y.size(); ++r)
271 result.mean(r) = y(r).value();
273 auto const deviation = [&] {
274 if constexpr (HasMinus<OutputSpace>)
276 return typename Problem::template OutputDifferenceVector<Dual>{
277 Emrys::minus(problem.outputSpace(), y, result.mean.template cast<Dual>())
285 EMRYS_FILTER_ASSERT(deviation.size() == result.jacobian.rows());
286 for (Eigen::Index r = 0; r < deviation.size(); ++r)
289 if (deviation(r).derivatives().size() > 0)
291 result.jacobian.row(r) = deviation(r).derivatives().transpose();
294 normalizeLinearization(problem, result);
299template <
typename Problem>
300[[nodiscard]] LinearizationAt<Problem>
301linearizeAt(methods::Linearized
const &, Problem
const & problem)
303 if constexpr (returnsJacobian<Problem>)
305 return linearizeReturned(problem);
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()"
315 if constexpr (returnsDuals<Problem>)
317 return linearizeByAutoDiff(problem);
329template <
typename Method,
typename Problem>
330 requires isLinearMethod<Method>
331[[nodiscard]] LinearizationAt<Problem>
332transformResult(Method
const & method, Problem
const & problem)
334 return linearizeAt(method, problem);
340template <
typename Method,
typename Scalar,
typename Problem>
341[[nodiscard]]
typename Problem::RootResult
342transformResult(methods::Iterated<Method, Scalar>
const & , Problem
const & )
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"
353template <
typename Scalar>
366template <
typename Scalar, UnscentedRule Rule,
int N>
367[[nodiscard]]
consteval SigmaWeights<Scalar>
370 if constexpr (N == 0)
372 return SigmaWeights<Scalar>{Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
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)),
385 Scalar(center + 1 - alphaSquared + params.beta),
386 Scalar(1 / (2 * (n + lambda)))
400template <
typename Scalar,
int N, UnscentedRule Rule>
401[[nodiscard]] SigmaWeights<Scalar>
402sigmaWeights(Rule
const & , Eigen::Index n)
404 if constexpr (N != Eigen::Dynamic && takesDoubleConstants<Scalar>)
406 EMRYS_FILTER_ASSERT(n == N);
407 return unscentedWeights<Scalar, Rule, N>();
413 return {Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
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);
421 Eigen::numext::sqrt(Scalar(n) + lambda),
423 center + Scalar(1) - alphaSquared + params.beta,
424 Scalar(1) / (Scalar(2) * (Scalar(n) + lambda))
432template <
typename Problem,
int Rows,
int MaxRows>
434 MatrixOf<typename Problem::Scalar, Rows, Problem::OutputDifferenceSize, MaxRows, Problem::OutputDifferenceMaxSize>;
440template <
typename Problem,
typename Deviations>
443 typename Problem::template OutputVector<typename Problem::Scalar> mean;
446 WhitenedOf<Problem, Deviations::ColsAtCompileTime, Deviations::MaxColsAtCompileTime> whitened;
448 typename Problem::OutputMatrix unexplained;
451 typename Problem::template OutputDifferenceVector<typename Problem::Scalar> center;
463template <
typename Problem,
typename Samples>
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
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);
481 Emrys::plus(space, center, weights.point * (plusDeviations.rowwise().sum() + minusDeviations.rowwise().sum()));
483 typename Problem::template OutputDifferenceVector<Scalar>
const centerDeviation =
Emrys::minus(space, center, mean);
485 minusDeviations =
Emrys::minus(space, minusPoints, mean);
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)
495 unexplained.noalias() += (Scalar(2) * weights.point) * multiply(residual,
residual.transpose());
497 unexplained = (Scalar(0.5) * (unexplained + unexplained.transpose())).eval();
501 std::move(unexplained),
502 std::move(plusDeviations),
503 std::move(minusDeviations),
518 Problem::InputDifferenceSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * Problem::InputDifferenceSize + 1,
520 Problem::InputDifferenceMaxSize == Eigen::Dynamic ? Eigen::Dynamic : 2 * Problem::InputDifferenceMaxSize + 1>
524 static constexpr int Points = Points_;
527 static constexpr int MaxPoints = MaxPoints_;
530 using RootMatrix =
typename Problem::template OutputDeviations<Points, MaxPoints>;
532 typename Problem::template OutputVector<typename Problem::Scalar> mean;
534 bool centerNegative =
false;
540template <
typename Problem>
543 typename Problem::template OutputVector<typename Problem::Scalar> center;
544 typename Problem::template OutputSamples<> plus;
545 typename Problem::template OutputSamples<> minus;
553template <
typename Problem,
typename Spread>
554[[nodiscard]] SigmaOutputs<Problem>
556 Problem
const & problem,
557 typename Problem::template InputVector<typename Problem::Scalar>
const & mu,
558 Spread
const & spread
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(
572 [&](Eigen::Index k) -> InputVector {
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));
582 auto result = SigmaOutputs<Problem>{};
583 result.center = outputs.col(0);
585 result.plus.resize(outputs.rows(), n);
586 result.minus.resize(outputs.rows(), n);
588 if constexpr (N != 0)
590 for (Eigen::Index j = 0; j < n; ++j)
592 result.plus.col(j) = outputs.col(1 + 2 * j);
593 result.minus.col(j) = outputs.col(2 + 2 * j);
604template <UnscentedRule Rule,
typename Problem,
typename A = roots::DefaultAxes>
605[[nodiscard]] SigmaRoot<Problem>
606transformRoot(Rule
const & method, Problem
const & problem, A
const & axes = {})
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);
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);
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)};
632template <UnscentedRule Rule,
typename Problem,
typename A = roots::DefaultAxes>
633[[nodiscard]]
typename Problem::RootResult
634transformResult(Rule
const & method, Problem
const & problem, A
const & axes = {})
636 using Scalar =
typename Problem::Scalar;
637 Eigen::Index
const n = problem.inputDifferenceSize();
638 SigmaWeights<Scalar>
const weights = sigmaWeights<Scalar, Problem::InputDifferenceSize>(method, n);
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);
645 auto moments = sigmaMoments(problem, weights, outputs.center, outputs.plus, outputs.minus);
647 std::move(moments.mean),
649 typename Problem::CrossMatrix{std::move(moments.whitened)},
650 std::move(moments.unexplained)
658template <Po
intSet Po
ints>
659[[nodiscard]]
consteval int
662 if (n == Eigen::Dynamic)
664 return Eigen::Dynamic;
670 return int(std::min<Eigen::Index>(Points::pointCount(n), MaxPointCount + 1));
674template <Po
intSet Po
ints>
675[[nodiscard]] Eigen::Index
676pointCountAt(Eigen::Index n)
678 return n == 0 ? Eigen::Index(1) : Points::pointCount(n);
685template <
typename Scalar,
typename Po
ints,
int N,
int NMax>
688 static constexpr int Count = pointCount<Points>(N);
689 static constexpr int MaxCount = pointCount<Points>(NMax);
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"
698 MatrixOf<Scalar, N, Count, NMax, MaxCount> points;
699 MatrixOf<Scalar, Count, 1, MaxCount, 1> weights;
700 Eigen::Index reference;
707template <
typename Scalar,
int N,
int NMax, Po
intSet Po
ints>
708[[nodiscard]] UnitPoints<Scalar, Points, N, NMax>
709unitPoints(Points
const & , Eigen::Index n)
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;
719 result.weights(0) = Scalar(1);
722 result.reference = Points::unitPoints(n, result.points, result.weights);
724 result.reference >= 0 && result.reference < count
725 &&
"the point the mean is taken about must be one of the points"
733template <
typename Problem,
typename Deviations,
typename Whitened>
736 typename Problem::template OutputVector<typename Problem::Scalar> mean;
738 typename Problem::OutputMatrix unexplained;
739 Deviations deviations;
755template <
typename Problem,
typename Samples,
typename Po
ints,
typename Weights,
typename Root>
758 Problem
const & problem,
759 Points
const & points,
760 Weights
const & weights,
761 Eigen::Index reference,
762 Samples
const & outputs,
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);
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)
782 if ((root.col(j).array() == Scalar(0)).all())
784 whitened.row(j).setZero();
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>{
794 std::move(unexplained),
795 std::move(deviations)
803template <
typename Problem,
typename Unit,
typename Spread>
805evaluatePoints(Problem
const & problem, Unit
const & unit, Spread
const & spread)
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(
813 [&](Eigen::Index j) {
814 return problem.plusInputs(mu, spread.col(j));
825template <
typename Po
ints,
typename Problem,
typename A = roots::DefaultAxes>
826 requires isPointSet<Points>
828transformRoot(Points
const & set, Problem
const & problem, A
const & axes = {})
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>;
834 MatrixOf<Scalar, Problem::InputDifferenceSize, Unit::Count, Problem::InputDifferenceMaxSize, Unit::MaxCount>;
835 Unit
const unit = unitPoints<Scalar, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>(
837 problem.inputDifferenceSize()
839 typename Problem::InputMatrix
const inputRoot = problem.root(axes);
841 spread.resize(unit.points.rows(), unit.points.cols());
842 spread.noalias() = multiply(inputRoot, unit.points);
843 auto moments = pointMoments(
848 evaluatePoints(problem, unit, spread),
851 typename Result::RootMatrix root = moments.deviations * unit.weights.cwiseSqrt().asDiagonal();
852 return Result{std::move(moments.mean), std::move(root),
false};
860template <
typename Po
ints,
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 = {})
865 using Scalar =
typename Problem::Scalar;
866 using Unit = UnitPoints<Scalar, Points, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>;
868 MatrixOf<Scalar, Problem::InputDifferenceSize, Unit::Count, Problem::InputDifferenceMaxSize, Unit::MaxCount>;
869 Unit
const unit = unitPoints<Scalar, Problem::InputDifferenceSize, Problem::InputDifferenceMaxSize>(
871 problem.inputDifferenceSize()
873 typename Problem::InputMatrix root = problem.root(axes);
875 spread.resize(unit.points.rows(), unit.points.cols());
876 spread.noalias() = multiply(root, unit.points);
878 pointMoments(problem, unit.points, unit.weights, unit.reference, evaluatePoints(problem, unit, spread), root);
880 std::move(moments.mean),
882 typename Problem::CrossMatrix{std::move(moments.whitened)},
883 std::move(moments.unexplained)
888template <
typename Rule,
typename A,
typename Problem>
889[[nodiscard]]
typename Problem::RootResult
890transformResult(methods::Spread<Rule, A>
const & method, Problem
const & problem)
892 return transformResult(method.rule, problem, method.axes);
896template <
typename Rule,
typename A,
typename Problem>
898transformRoot(methods::Spread<Rule, A>
const & method, Problem
const & problem)
900 return transformRoot(method.rule, problem, method.axes);
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)
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>)
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>)
924 return std::get<I>(returned.jacobian.blocks);
928 return jacobianBlock<Problem::ArgInfo::ordinal[I]>(returned.jacobian);
931 Index
const start = problem.differenceOffset(I);
932 Index
const size = input.differenceDimension();
933 for (Index j = 0; j < l; ++j)
935 if (affine(j) >= start && affine(j) < start + size)
937 columns.col(j) =
block.col(affine(j) - start);
941 OutputVector y = std::move(returned.value);
943 y.size() ==
valueDimension(problem.outputSpace()) &&
"the output does not fit the output space"
950 Eigen::Matrix<Scalar, Affine::SizeAtCompileTime, 1, Eigen::ColMajor, Affine::MaxSizeAtCompileTime, 1>;
951 using Dual = Eigen::AutoDiffScalar<Derivatives>;
953 returnsDuals<Problem, Dual>,
954 "a function that a sigma-point method factors affine coordinates out of returns its Jacobian, or is "
956 "in its scalar, to be differentiated in those coordinates"
958 using OutputSpace =
typename Problem::OutputSpaceType;
959 Index
const total = problem.inputDifferenceSize();
960 typename Problem::template InputDifferenceVector<Dual> delta;
962 for (Index i = 0; i < total; ++i)
964 delta(i) = Dual{Scalar(0), Derivatives::Zero(l)};
966 for (Index j = 0; j < l; ++j)
968 delta(affine(j)) = Dual{Scalar(0), int(l), int(j)};
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)
975 value(r) = y(r).value();
977 auto const deviation = [&] {
978 if constexpr (HasMinus<OutputSpace>)
980 return typename Problem::template OutputDifferenceVector<Dual>{
981 Emrys::minus(problem.outputSpace(), y, value.template cast<Dual>())
989 for (Index r = 0; r < deviation.size(); ++r)
992 if (deviation(r).derivatives().size() > 0)
994 columns.row(r) = deviation(r).derivatives().transpose();
998 columns.row(r).setZero();
1014template <
typename Problem,
typename Affine,
typename ShiftAt,
typename Outputs,
typename Jacobians>
1016evaluateWithAffineJacobians(
1017 Problem
const & problem,
1018 typename Problem::template InputVector<typename Problem::Scalar>
const & mu,
1019 Affine
const & affine,
1021 ShiftAt
const & shiftAt,
1023 Jacobians & jacobians
1026 using Index = Eigen::Index;
1027 Index
const l = affine.size();
1028 if constexpr (Problem::EvaluatesBatches)
1030 EMRYS_FILTER_ASSERT(jacobians.cols() == l * count &&
"a Jacobian over the affine coordinates per point");
1031 problem.evaluateEachWithAffineJacobians(
1034 return problem.plusInputs(mu, shiftAt(k));
1042 for (Index k = 0; k < count; ++k)
1044 auto const y = valueWithAffineJacobian(
1046 problem.plusInputs(mu, shiftAt(k)),
1048 jacobians.middleCols(k * l, l)
1052 outputs.resize(y.size(), count);
1067template <
typename Problem,
int AffineSize,
int AffineMaxSize,
typename Po
ints>
1068[[nodiscard]]
typename Problem::RootResult
1069transformResult(FactoredAt<AffineSize, AffineMaxSize, Points>
const & method, Problem
const & problem)
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;
1080 constexpr int RestSize = Problem::InputDifferenceSize == Eigen::Dynamic || AffineSize == 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>;
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"
1102 auto rest = RestIndices{};
1104 for (Index i = 0, k = 0; i < total; ++i)
1106 if ((affine.array() != i).all())
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)
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)));
1130 conditional = (Scalar(0.5) * (conditional + conditional.transpose())).eval();
1132 AffineMatrix conditionalRoot = AffineMatrix::Zero(l, l);
1133 if constexpr (AffineSize != 0)
1137 conditionalRoot = defaultRoot(conditional);
1143 Index
const m = problem.outputDifferenceSize();
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)
1152 meanJacobian += pointWeights(k) * jacobians.middleCols(k * l, l);
1154 OutputMatrix omega = OutputMatrix::Zero(m, m);
1155 for (Index k = 0; k < count; ++k)
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);
1161 AffineCross whitened = multiply(conditionalRoot.transpose(), meanJacobian.transpose());
1162 return std::pair{std::move(whitened), std::move(omega)};
1167 auto [mean, restWhitened, pointOmega, affineWhitened, affineOmega] = [&] {
1168 if constexpr (isUnscented<RuleOf<Points>> || RestSize == 0)
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<
1175 sizeProduct<AffineSize, Count>,
1177 sizeProduct<AffineMaxSize, MaxCount>>;
1178 using PointWeights = MatrixOf<Scalar, Count, 1, MaxCount, 1>;
1179 SigmaWeights<Scalar>
const weights = [&] {
1180 if constexpr (RestSize == 0)
1182 return SigmaWeights<Scalar>{Scalar(0), Scalar(1), Scalar(1), Scalar(0)};
1186 return sigmaWeights<Scalar, RestSize>(ruleOf(method.points), n);
1189 RestMatrix
const spread = weights.scale * restRoot;
1190 AffineRest
const affineSpread = [&] {
1191 if constexpr (RestSize != 0)
1193 return AffineRest{multiply(gain, spread)};
1197 return AffineRest{};
1201 auto const shiftAt = [&]([[maybe_unused]] Index k) {
1202 InputDifferenceVector shift = InputDifferenceVector::Zero(total);
1203 if constexpr (RestSize != 0)
1207 Index
const j = (k - 1) / 2;
1208 shift(rest) =
spread.col(j);
1209 shift(affine) = affineSpread.col(j);
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);
1227 auto plus = OutputSamples{};
1228 auto minus = OutputSamples{};
1229 plus.resize(center.size(), n);
1230 minus.resize(center.size(), n);
1232 if constexpr (RestSize != 0)
1234 for (Index j = 0; j < n; ++j)
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;
1242 auto sigma = sigmaMoments(problem, weights, center,
plus,
minus);
1243 auto [whitened, omega] = affineParts(jacobians, pointWeights);
1245 std::move(sigma.mean),
1246 RestCross{std::move(sigma.whitened)},
1247 std::move(sigma.unexplained),
1248 std::move(whitened),
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<
1260 sizeProduct<AffineSize, Unit::Count>,
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{};
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);
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);
1284 std::move(points.mean),
1285 RestCross{std::move(points.whitened)},
1286 std::move(points.unexplained),
1287 std::move(whitened),
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)
1299 AffineRest
const affineRoot = multiply(gain, restRoot);
1300 for (Index k = 0; k < n; ++k)
1302 whitened.row(rest(k)) = restWhitened.row(k);
1303 for (Index j = 0; j < n; ++j)
1305 root(rest(k), rest(j)) = restRoot(k, j);
1307 for (Index j = 0; j < l; ++j)
1309 root(affine(j), rest(k)) = affineRoot(j, k);
1313 for (Index k = 0; k < l; ++k)
1315 whitened.row(affine(k)) = affineWhitened.row(k);
1316 for (Index j = 0; j < l; ++j)
1318 root(affine(k), affine(j)) = conditionalRoot(k, j);
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)};
1331template <Space Space_, RealScalar Scalar_>
1336 using Scalar = Scalar_;
1337 using SpaceType = Space_;
1338 using MomentsType = Moments<Space_, Scalar_>;
1355 PointInput(Space_ space, VectorType point)
1356 : m_space{std::move(space)}
1357 , m_point{std::move(point)}
1361 [[nodiscard]] Space_
const &
1368 [[nodiscard]] VectorType
const &
1375 [[nodiscard]] MatrixType
1376 covarianceLower()
const
1378 return MatrixType::Zero(differenceDimension(), differenceDimension());
1382 [[nodiscard]] Eigen::Index
1383 valueDimension()
const
1389 [[nodiscard]] Eigen::Index
1390 differenceDimension()
const
1410template <
typename InputSpace,
typename OutputSpace,
typename Scalar>
1411struct LinearizationAbout
1414 using Linearization = StatisticalLinearization<InputSpace, OutputSpace, Scalar>;
1422 JacobianMatrix jacobian;
1428 ErrorMatrix unexplained;
1431 [[nodiscard]] ErrorMatrix
1434 return inputs + unexplained;
1444template <
typename InputSpace,
typename Po
int,
typename Base,
typename OutputSpace,
typename Scalar>
1447 InputSpace
const & space,
1448 Eigen::MatrixBase<Base>
const & base,
1449 Point
const & point,
1450 LinearizationAbout<InputSpace, OutputSpace, Scalar> & linearization
1453 using Linearization = LinearizationAbout<InputSpace, OutputSpace, Scalar>;
1456 Difference
const step =
Emrys::minus(space, point, base);
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);
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);
1479template <
typename Method,
typename About,
typename InputSpace>
1480[[nodiscard]]
consteval bool
1483 using Reference =
typename About::ReferenceType;
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"
1489 if constexpr (About::IsDistribution)
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"
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"
1520 typename OutputSpace,
1521 typename InputSpace,
1527 Method
const & method,
1529 OutputSpace
const & outputSpace,
1530 InputSpace
const & space,
1531 Eigen::MatrixBase<Base>
const & base,
1532 LinearizedAbout<Reference>
const & about,
1533 Args
const &... args
1536 using Scalar =
typename Base::Scalar;
1537 using About = LinearizedAbout<Reference>;
1538 using Point = PointInput<InputSpace, Scalar>;
1539 using Result = LinearizationAbout<InputSpace, OutputSpace, Scalar>;
1541 crossCount<Args...> == 0,
1542 "withCross() may mark only the input linearizedAbout() is for, which is the argument right after it"
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"
1551 if constexpr (isLinearMethod<Method>)
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);
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();
1564 problem.outputSpace(),
1565 typename Result::OutputVector{
linearized.mean},
1566 typename Result::JacobianMatrix{
linearized.jacobian.middleCols(problem.differenceOffset(0), n)},
1568 Result::ErrorMatrix::Zero(m, m)
1571 else if constexpr (About::IsDistribution)
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));
1580 problem.outputSpace(),
1581 typename Result::OutputVector{predicted.output().mean()},
1582 predicted.jacobian(),
1583 predicted.inputsCovariance(),
1584 predicted.unexplainedCovariance()
1588 toChartOf(space, base, point, result);
1598template <
typename Method,
typename F,
typename OutputSpace,
typename Reference,
typename X,
typename... Args>
1601 Method
const & method,
1603 OutputSpace
const & outputSpace,
1604 LinearizedAbout<Reference>
const & about,
1606 Args
const &... args
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>;
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)
1624 return OutputWithCross<InputSpace, OutputSpace, JacobianForm, Scalar>{
1625 std::move(linearization.space),
1626 std::move(linearization.value),
1627 std::move(linearization.jacobian),
1629 std::move(explained),
1630 std::move(linearization.inputs),
1631 std::move(linearization.unexplained)
1636 typename Output::MatrixType outputCov = explained + linearization.inputs + linearization.unexplained;
1637 return Output{linearization.space, linearization.value, std::move(outputCov)};
1647template <UncertainInput Input>
1658template <UncertainInput Input>
1686template <UncertainInput Distribution>
1690 return {distribution};
1697template <UncertainInput Distribution>
1706template <
typename Derived>
1710 static_assert(Derived::ColsAtCompileTime == 1,
"a point to linearize at is a column vector");
1711 return {
typename Derived::PlainObject{point}};
1730template <
typename Method,
typename F,
typename OutputSpace,
typename... Args>
1731 requires(IsSpace<OutputSpace> && crossCount<Args...> <= 1)
1733transformBy(Method
const & method, F && f, OutputSpace
const &
outputSpace, Args
const &... args)
1735 if constexpr (leadsWithReference<Args...>)
1737 return transformAbout(method, f,
outputSpace, args...);
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));
1750template <
typename T>
1751struct TransformInputTagOf
1757template <
typename T>
1758 requires(ArgTraits<T>::isUncertain)
1759struct TransformInputTagOf<T>
1761 using type =
typename ArgTraits<T>::Input::SpaceType;
1764template <
typename S,
typename Vector>
1765struct TransformInputTagOf<Sure<S, Vector>>
1773template <
typename T>
1774using TransformInputTag =
typename TransformInputTagOf<T>::type;
1779template <
typename... Args>
1780inline constexpr std::size_t leadingPolicies = [] {
1781 constexpr std::array<bool,
sizeof...(Args) + 1> policies{ModelPolicy<Args>...,
false};
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)
1805template <
typename... Ts>
1812template <
typename First,
typename... Rest>
1813struct FirstTypeOf<First, Rest...>
1815 using type = std::conditional_t<std::is_void_v<First>,
typename FirstTypeOf<Rest...>::type, First>;
1823template <
bool Uncertain,
typename A>
1824struct UncertainScalarOf
1830template <
typename A>
1831struct UncertainScalarOf<true, A>
1833 using type =
typename A::Scalar;
1842template <
typename Model,
typename... Inputs>
1843[[nodiscard]]
consteval auto
1844transformReadsPolicy()
1846 if constexpr (readsWholeState<Model>)
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"
1853 return std::type_identity<ReadsWhole<TransformInputTag<std::tuple_element_t<0, std::tuple<Inputs...>>>>>{};
1857 return std::type_identity<typename Model::template Policy<PolicyKind::reads>>{};
1865template <
typename Model>
1866[[nodiscard]]
consteval bool
1867transformOutputFromNoise()
1869 if constexpr (Model::template has<PolicyKind::outputSpace> || Model::template has<PolicyKind::writes>)
1873 else if constexpr (Model::template has<PolicyKind::additiveNoise>)
1875 return Model::template Policy<PolicyKind::additiveNoise>::HasOwnSpace;
1889template <
typename Model,
typename In,
bool OutputFromNoise>
1890[[nodiscard]]
consteval auto
1891transformOutputSpace()
1893 using Function =
typename Model::FunctionType;
1894 if constexpr (Model::template has<PolicyKind::outputSpace>)
1896 using Policy =
typename Model::template Policy<PolicyKind::outputSpace>;
1897 return std::type_identity<decltype(std::declval<Policy const &>().in(std::declval<In const &>()))>{};
1899 else if constexpr (Model::template has<PolicyKind::writes>)
1901 return std::type_identity<typename Model::template Policy<PolicyKind::writes>::OutputSpace>{};
1903 else if constexpr (OutputFromNoise)
1905 return std::type_identity<
1906 typename Model::template Policy<PolicyKind::additiveNoise>::Distribution::SpaceType>{};
1908 else if constexpr (
requires {
typename Function::OutputSpaceType; })
1910 return std::type_identity<typename Function::OutputSpaceType>{};
1914 using Returned =
decltype(vectorReturned(callForm(
1915 std::declval<Function const &>(),
1916 std::declval<In const &>(),
1917 std::declval<NoOutputSpace const &>(),
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>>{};
1933template <
typename Model,
typename... Inputs>
1939 static constexpr bool Whole = readsWholeState<Model>;
1941 static constexpr std::size_t InputCount =
sizeof...(Inputs);
1944 template <std::
size_t J>
1945 using InputAt = std::tuple_element_t<J, std::tuple<Inputs...>>;
1948 template <std::
size_t J>
1949 using TagAt = TransformInputTag<InputAt<J>>;
1954 [[nodiscard]]
static consteval bool
1955 uncertain(std::size_t j)
1957 constexpr std::array<bool, InputCount + 1> flags{ArgTraits<Inputs>::isUncertain...,
false};
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 "
1968 distinctTypes<TransformInputTag<Inputs>...>(),
1969 "two inputs of transform() have one tag; an input's tag names it once"
1973 using ReadsPolicy =
typename decltype(transformReadsPolicy<Model, Inputs...>())::type;
1977 static constexpr std::size_t ItemCount = ReadsPolicy::Count;
1980 template <std::
size_t I>
1981 using Item =
typename ReadsPolicy::template Item<I>;
1984 template <std::
size_t I>
1985 using Name =
typename Item<I>::NameType;
1988 using In =
typename ReadsPolicy::InputSpace;
1991 template <
typename T>
1997 template <std::
size_t I>
1998 [[nodiscard]]
static consteval std::size_t
2001 if constexpr (Whole)
2007 using Path =
typename Item<I>::PathType;
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"
2014 constexpr std::array<bool, InputCount + 1> matches{
2015 std::is_same_v<TransformInputTag<Inputs>, PathTag<Path>>...,
2019 while (j < InputCount && !matches[j])
2028 [[nodiscard]]
static consteval std::size_t
2029 inputOf(std::size_t i)
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), ...);
2035 }(i, std::make_index_sequence<ItemCount>{});
2039 template <std::size_t... I>
2040 [[nodiscard]]
static consteval bool
2041 itemsGiven(std::index_sequence<I...>)
2043 return ((inputOfItem<I>() < InputCount) && ...);
2049 static constexpr bool ItemsGiven = itemsGiven(std::make_index_sequence<ItemCount>{});
2054 [[nodiscard]]
static consteval bool
2055 inputRead(std::size_t j)
2057 for (std::size_t i = 0; i < ItemCount; ++i)
2059 if (inputOf(i) == j)
2068 template <std::size_t... J>
2069 [[nodiscard]]
static consteval bool
2070 inputsRead(std::index_sequence<J...>)
2072 return (inputRead(J) && ...);
2078 static constexpr bool InputsRead = inputsRead(std::make_index_sequence<InputCount>{});
2083 [[nodiscard]]
static consteval std::size_t
2084 uncertainIndex(std::size_t j)
2087 for (std::size_t i = 0; i < j; ++i)
2098 static constexpr std::size_t UncertainCount = (std::size_t{ArgTraits<Inputs>::isUncertain} + ... + 0);
2103 using Function =
typename Model::FunctionType;
2106 static constexpr bool OutputFromNoise = transformOutputFromNoise<Model>();
2111 using Out =
typename decltype(transformOutputSpace<Model, In, OutputFromNoise>())::type;
2114 explicit ModelTransform(Model
const & model, Inputs
const &... inputs)
2116 , m_inputs{inputs...}
2120 if constexpr (Whole)
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)),
2130 }(std::make_index_sequence<ItemCount>{});
2135 [[nodiscard]] Out
const &
2145 template <
typename... A>
2147 operator()(A
const &... args)
const
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>{};
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)));
2162 static constexpr bool EvaluatesBatches = takesBatches<Function>;
2168 template <
typename Y,
typename... A>
2170 evaluateBatch(Y & y, A
const &... args)
const
2171 requires EvaluatesBatches
2173 callBatched(m_model.function(), m_in, m_out, gatherColumns<Y>(y.cols(), args...), y);
2180 template <
typename Y,
typename J,
typename... A>
2182 evaluateBatchWithAffineJacobians(Y & y, J & jacobians, A
const &... args)
const
2183 requires EvaluatesBatches
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);
2197 template <
typename Y,
typename... A>
2199 gatherColumns(Eigen::Index count, A
const &... args)
const
2201 static_assert(
sizeof...(A) == InputCount);
2202 using T =
typename Y::Scalar;
2206 Y::ColsAtCompileTime,
2208 Y::MaxColsAtCompileTime>;
2209 auto const arguments = std::tuple<A
const &...>{args...};
2212 auto column = InVector<T>{};
2214 for (Eigen::Index j = 0; j < count; ++j)
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>{});
2227 template <std::
size_t I>
2228 [[nodiscard]]
static consteval bool
2231 using Affine =
typename Model::template Policy<PolicyKind::affineIn>;
2232 return affineNames<Affine, Name<I>> &&
isLinear<Name<I>> && uncertain(inputOfItem<I>());
2236 [[nodiscard]]
static consteval bool
2239 return []<std::size_t... I>(std::index_sequence<I...>) {
2240 return (itemFactored<I>() || ... ||
false);
2241 }(std::make_index_sequence<ItemCount>{});
2245 [[nodiscard]]
static consteval int
2248 return []<std::size_t... I>(std::index_sequence<I...>) {
2250 }(std::make_index_sequence<ItemCount>{});
2254 [[nodiscard]]
static consteval int
2257 return []<std::size_t... I>(std::index_sequence<I...>) {
2259 }(std::make_index_sequence<ItemCount>{});
2266 [[nodiscard]] IndexVector<affineSize(), affineMaxSize()>
2267 affineCoordinates()
const
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)),
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...>) {
2284 if constexpr (itemFactored<I>())
2286 for (Eigen::Index r = 0; r < m_differenceRows[I].size; ++r)
2288 result(next++) = starts[inputOfItem<I>()] + r;
2294 }(std::make_index_sequence<ItemCount>{});
2302 template <
typename Result>
2304 withNoise(Result && result)
const
2306 if constexpr (!Model::template has<PolicyKind::additiveNoise>)
2308 return std::forward<Result>(result);
2312 auto const placed = placeNoise(m_out, m_model.template policy<PolicyKind::additiveNoise>());
2313 auto const & noise = placed.distribution();
2315 !isGaussianMixture<std::remove_cvref_t<
decltype(noise)>>,
2316 "transform() adds one Gaussian noise; mixture noise gives a branch per component, which update() keeps"
2318 return plusNoise(result, noise);
2328 if constexpr (Whole)
2330 return argInput(std::get<0>(m_inputs)).space();
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>{});
2341 template <std::
size_t J>
2345 auto const & input = std::get<J>(m_inputs);
2346 if constexpr (isSure<std::remove_cvref_t<
decltype(input)>>)
2352 return argInput(input).space();
2363 if constexpr (Model::template has<PolicyKind::outputSpace>)
2365 return m_model.template policy<PolicyKind::outputSpace>().in(m_in);
2367 else if constexpr (OutputFromNoise)
2369 return m_model.template policy<PolicyKind::additiveNoise>().distribution().space();
2371 else if constexpr (!FixedSizeSpace<Out> &&
requires(Function
const & f) { f.outputRows(); })
2373 return Out{m_model.function().outputRows()};
2384 template <std::
size_t I,
typename T,
typename Argument>
2386 gatherItem(InVector<T> & x, Argument
const & argument)
const
2389 auto target = middleRowsOf<Rows>(x, m_valueRows[I].start, m_valueRows[I].size);
2390 if constexpr (uncertain(inputOfItem<I>()))
2396 target = argument.value.template cast<T>();
2403 template <std::
size_t J,
typename Argument>
2404 [[nodiscard]]
static decltype(
auto)
2405 columnOf(Argument
const & argument, Eigen::Index j)
2407 if constexpr (uncertain(J))
2409 return argument.col(j);
2420 template <
typename R>
2422 split(R && returned)
const
2424 using Plain = std::remove_cvref_t<R>;
2425 if constexpr (!isWithJacobian<Plain> || Whole)
2427 return std::forward<R>(returned);
2431 using J = std::remove_cvref_t<
decltype(returned.jacobian)>;
2433 MatrixExpression<J>,
2434 "a transformed function returns its Jacobian as one matrix over its input's differences, "
2435 "withJacobian(y, J)"
2437 auto const & jacobian = returned.jacobian;
2438 EMRYS_FILTER_ASSERT(
2440 &&
"a returned Jacobian has a column per difference coordinate of the function's input"
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)
2447 return withJacobian(std::move(returned.value), std::move(std::get<0>(blocks)));
2451 return withJacobian(std::move(returned.value), std::move(blocks));
2459 template <std::
size_t J,
typename Jacobian>
2461 inputBlock(Jacobian
const & jacobian)
const
2463 if constexpr (!uncertain(J))
2465 return std::tuple<>{};
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>;
2477 [&]<std::size_t... I>(std::index_sequence<I...>) {
2480 if constexpr (inputOfItem<I>() == J)
2482 block += jacobian.middleCols(m_differenceRows[I].start, m_differenceRows[I].size);
2487 }(std::make_index_sequence<ItemCount>{});
2488 return std::tuple{std::move(
block)};
2492 Model
const & m_model;
2493 std::tuple<Inputs
const &...> m_inputs;
2496 std::array<BlockRows, ItemCount> m_valueRows{};
2497 std::array<BlockRows, ItemCount> m_differenceRows{};
2503template <
typename Space,
typename Scalar,
typename Noise>
2504[[nodiscard]] GaussianParams<Space, Scalar>
2505plusNoise(GaussianParams<Space, Scalar>
const & result, Noise
const & noise)
2508 Matrix cov = symmetricFromLower<Matrix>(result.covarianceLower());
2509 cov += symmetricFromLower<Matrix>(noise.covarianceLower().template cast<Scalar>());
2510 return GaussianParams<Space, Scalar>{
2512 Emrys::plus(result.space(), result.mean(), noise.mean().template cast<Scalar>()),
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)
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>)
2536 typename Result::RootMatrix{result.root()},
2537 typename Result::CrossMatrix{result.whitenedCross()},
2538 Matrix{result.explainedCovariance()},
2540 Matrix{result.unexplainedCovariance()}
2548 typename Result::JacobianMatrix{result.jacobian()},
2549 typename Result::CrossMatrix{result.crossCovariance()},
2550 Matrix{result.explainedCovariance()},
2552 Matrix{result.unexplainedCovariance()}
2561template <
typename G>
2562[[nodiscard]]
consteval bool
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)"
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()"
2575 return G::ItemsGiven && G::InputsRead;
2582template <
typename Method,
typename Model,
typename... Inputs>
2584transformModel(Method
const & method, Model
const & model, Inputs
const &... inputs)
2588 "a model known by its likelihood alone has no function for transform() to push its inputs through"
2590 if constexpr (leadsWithReference<Inputs...>)
2592 return [&]<
typename About,
typename... Rest>(About
const & about, Rest
const &... rest) {
2593 using G = ModelTransform<Model, Rest...>;
2594 if constexpr (inputsMatch<G>())
2596 auto const g = G{model, rest...};
2597 return g.withNoise(transformBy(method, g, g.out(), about, rest...));
2601 else if constexpr (!inputsMatch<ModelTransform<Model, Inputs...>>())
2607 using G = ModelTransform<Model, Inputs...>;
2608 auto const g = G{model, inputs...};
2609 if constexpr (isSigmaPointMethod<Method> && G::factorsAffine())
2611 auto const factored =
2612 FactoredAt<G::affineSize(), G::affineMaxSize(), Method>{method, g.affineCoordinates()};
2613 return g.withNoise(transformBy(factored, g, g.out(), inputs...));
2617 return g.withNoise(transformBy(method, g, g.out(), inputs...));
2658template <
typename Method,
typename F,
typename... Args>
2659 requires(!
MeasurementModel<F> && !internal::isModel<F> && internal::crossCount<Args...> <= 1)
2661transform(Method
const & method, F
const & f, Args
const &... args)
2664 !internal::policyAfterInput<Args...>,
2665 "transform() takes its policies before its inputs: transform(method, f, reads(...), additiveNoise(...), "
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...>) {
2672 return internal::transformModel(method, model, std::get<Policies + I>(all)...);
2673 }(std::make_index_sequence<Policies>{}, std::make_index_sequence<
sizeof...(Args) - Policies>{});
2683template <
typename Method,
typename Model,
typename... Inputs>
2684 requires((MeasurementModel<Model> || internal::isModel<Model>) && internal::crossCount<Inputs...> <= 1)
2686transform(Method
const & method, Model
const & model, Inputs
const &... inputs)
2690 "transform() pushes its inputs through a measurement model or a function; a process model's step is "
2694 internal::leadingPolicies<Inputs...> == 0,
2695 "transform() of a model takes the model's own policies, so give the inputs alone after it"
2697 return internal::transformModel(method, model, inputs...);
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...
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
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