1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
|
#pragma once
#include <algorithm>
#include <opencv2/core.hpp>
#include <opencv2/core/base.hpp>
#include <opencv2/core/quaternion.hpp>
#include <cmath>
#include <vector>
#include "opencv_contrib.h"
namespace ukf_cv
{
using namespace cvcontrib;
template<int dim, int otherdim = dim>
using SigmaPoints = std::array<cv::Vec<float,otherdim>,dim*2+1>;
// Ported from
// https://filterpy.readthedocs.io/en/latest/_modules/filterpy/kalman/sigma_points.html
// Excerpt from the original docu:
// "
// Generates sigma points and weights according to Van der Merwe's
// 2004 dissertation[1] for the UnscentedKalmanFilter class.. It
// parametizes the sigma points using alpha, beta, kappa terms, and
// is the version seen in most publications.
// Unless you know better, this should be your default choice.
// alpha : float
// Determins the spread of the sigma points around the mean.
// Usually a small positive value (1e-3) according to [3].
// beta : float
// Incorporates prior knowledge of the distribution of the mean. For
// Gaussian x beta=2 is optimal, according to [3].
// kappa : float, default=0.0
// Secondary scaling parameter usually set to 0 according to [4],
// or to 3-n according to [5].
// Reference
// .. [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
// Inference in Dynamic State-Space Models" (Doctoral dissertation)
// "
template<int dim>
class MerweScaledSigmaPoints
{
public:
static constexpr int num_sigmas = 2*dim+1;
using Vector = cv::Vec<float,dim>;
using Matrix = cv::Matx<float,dim,dim>;
MerweScaledSigmaPoints(float alpha = 0.01, float beta = 2., int kappa = 3-dim)
{
lambda = alpha*alpha * (dim + kappa) - dim;
const float c = .5 / (dim + lambda);
Wc_i = c;
Wm_i = c;
Wm_0 = lambda / (dim+lambda);
Wc_0 = Wm_0 + (1.-alpha*alpha + beta);
}
SigmaPoints<dim> compute_sigmas(const Vector &mu, const Matrix &mat, bool is_tril_factor) const
{
const Matrix triu_factor = is_tril_factor ? mat.t() : cholesky(mat).t();
const Matrix U = triu_factor*std::sqrt(lambda+dim);
SigmaPoints<dim> sigmas;
sigmas[0] = mu;
for (int k=0; k<dim; ++k)
{
sigmas[k+1] = to_vec(mu + U.row(k).t());
sigmas[dim+k+1] = to_vec(mu - U.row(k).t());
}
return sigmas;
}
template<int otherdim>
std::tuple<cv::Vec<float,otherdim> , cv::Matx<float,otherdim,otherdim>> compute_statistics(const SigmaPoints<dim,otherdim> &sigmas) const
{
cv::Vec<float,otherdim> mu{}; // Zero initializes
for (size_t i=0; i<sigmas.size(); ++i)
{
mu += to_vec((i==0 ? Wm_0 : Wm_i) * sigmas[i]);
}
cv::Matx<float,otherdim,otherdim> cov{};
for (size_t i=0; i<sigmas.size(); ++i)
{
const auto p = sigmas[i] - mu;
cov += (i==0 ? Wc_0 : Wc_i)*p*p.t();
}
return { mu, cov };
}
template<int otherdim>
cv::Matx<float,dim,otherdim> compute_cov(const SigmaPoints<dim,dim> &sigmas, const SigmaPoints<dim,otherdim> &othersigmas) const
{
cv::Vec<float,dim> mu{}; // Zero initializes
cv::Vec<float,otherdim> mu_other{}; // Zero initializes
for (size_t i=0; i<sigmas.size(); ++i)
{
mu += to_vec((i==0 ? Wm_0 : Wm_i) * sigmas[i]);
mu_other += to_vec((i==0 ? Wm_0 : Wm_i) * othersigmas[i]);
}
cv::Matx<float,dim,otherdim> cov{};
for (size_t i=0; i<sigmas.size(); ++i)
{
const auto p = sigmas[i] - mu;
const auto q = othersigmas[i] - mu_other;
cov += (i==0 ? Wc_0 : Wc_i)*p*q.t();
}
return cov;
}
private:
float Wc_i, Wm_i, Wm_0, Wc_0, lambda;
};
} // namespace ukf_cv
|
