44 class VECTORLIKE1,
class MATLIKE1,
class USERPARAM,
class VECTORLIKE2,
45 class VECTORLIKE3,
class MATLIKE2>
47 const VECTORLIKE1& x_mean,
const MATLIKE1& x_cov,
49 const VECTORLIKE1& x,
const USERPARAM& fixed_param, VECTORLIKE3& y),
50 const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
51 const bool* elem_do_wrap2pi =
nullptr,
const double alpha = 1e-3,
52 const double K = 0,
const double beta = 2.0)
55 const size_t Nx = x_mean.size();
56 const double lambda = alpha * alpha * (Nx + K) - Nx;
57 const double c = Nx + lambda;
60 const double Wi = 0.5 / c;
61 std::vector<double> W_mean(1 + 2 * Nx, Wi), W_cov(1 + 2 * Nx, Wi);
62 W_mean[0] = lambda / c;
63 W_cov[0] = W_mean[0] + (1 - alpha * alpha + beta);
67 const bool valid = x_cov.chol(L);
69 throw std::runtime_error(
70 "transform_gaussian_unscented: Singular covariance matrix in "
77 std::vector<VECTORLIKE3> Y(1 + 2 * Nx);
78 VECTORLIKE1 X = x_mean;
79 functor(X, fixed_param, Y[0]);
83 for (
size_t i = 0; i < Nx; i++)
85 for (
size_t k = 0; k < Nx; k++) delta[k] = L(i, k);
88 functor(X, fixed_param, Y[row++]);
91 functor(X, fixed_param, Y[row++]);
96 Y, y_cov, y_mean, &W_mean, &W_cov, elem_do_wrap2pi);
112 class VECTORLIKE1,
class MATLIKE1,
class USERPARAM,
class VECTORLIKE2,
113 class VECTORLIKE3,
class MATLIKE2>
115 const VECTORLIKE1& x_mean,
const MATLIKE1& x_cov,
117 const VECTORLIKE1& x,
const USERPARAM& fixed_param, VECTORLIKE3& y),
118 const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
119 const size_t num_samples = 1000,
120 std::vector<VECTORLIKE3>* out_samples_y =
nullptr)
123 std::vector<VECTORLIKE1> samples_x;
125 samples_x, num_samples, x_cov, &x_mean);
126 std::vector<VECTORLIKE3> samples_y(num_samples);
127 for (
size_t i = 0; i < num_samples; i++)
128 functor(samples_x[i], fixed_param, samples_y[i]);
132 out_samples_y->clear();
133 samples_y.swap(*out_samples_y);
150 class VECTORLIKE1,
class MATLIKE1,
class USERPARAM,
class VECTORLIKE2,
151 class VECTORLIKE3,
class MATLIKE2>
153 const VECTORLIKE1& x_mean,
const MATLIKE1& x_cov,
155 const VECTORLIKE1& x,
const USERPARAM& fixed_param, VECTORLIKE3& y),
156 const USERPARAM& fixed_param, VECTORLIKE2& y_mean, MATLIKE2& y_cov,
157 const VECTORLIKE1& x_increments)
161 functor(x_mean, fixed_param, y_mean);
164 double, VECTORLIKE3::RowsAtCompileTime, VECTORLIKE1::RowsAtCompileTime>
169 const VECTORLIKE1& x,
const USERPARAM& fixed_param,
170 VECTORLIKE3& y)>(functor),
171 x_increments, fixed_param, H);