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
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
#pragma once
/* Copyright (c) 2013 Stanislaw Halik <sthalik@misaki.pl>
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*/
#include "ui_ftnoir_kalman_filtercontrols.h"
#include "opentrack/plugin-api.hpp"
#include "opentrack-compat/options.hpp"
using namespace options;
#include "opentrack-compat/timer.hpp"
#include <Eigen/Core>
#include <Eigen/LU>
#include <QString>
#include <QWidget>
#include <atomic>
static constexpr int NUM_STATE_DOF = 12;
static constexpr int NUM_MEASUREMENT_DOF = 6;
// These vectors are compile time fixed size, stack allocated
using StateToMeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_STATE_DOF>;
using StateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_STATE_DOF>;
using MeasureToStateMatrix = Eigen::Matrix<double, NUM_STATE_DOF, NUM_MEASUREMENT_DOF>;
using MeasureMatrix = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF>;
using StateVector = Eigen::Matrix<double, NUM_STATE_DOF, 1>;
using PoseVector = Eigen::Matrix<double, NUM_MEASUREMENT_DOF, 1>;
struct KalmanFilter
{
MeasureMatrix
measurement_noise_cov;
StateMatrix
process_noise_cov,
state_cov,
state_cov_prior,
transition_matrix;
MeasureToStateMatrix
kalman_gain;
StateToMeasureMatrix
measurement_matrix;
StateVector
state,
state_prior;
PoseVector
innovation;
void init();
void time_update();
void measurement_update(const PoseVector &measurement);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct KalmanProcessNoiseScaler
{
MeasureMatrix
innovation_cov_estimate;
StateMatrix
base_cov; // baseline (unscaled) process noise covariance matrix
void init();
void update(KalmanFilter &kf, double dt);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct DeadzoneFilter
{
PoseVector
last_output,
dz_size;
DeadzoneFilter() :
last_output(PoseVector::Zero()),
dz_size(PoseVector::Zero())
{}
void reset() {
last_output = PoseVector::Zero();
}
PoseVector filter(const PoseVector &input);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct settings : opts {
value<int> noise_rot_slider_value;
value<int> noise_pos_slider_value;
static constexpr double adaptivity_window_length = 0.5; // seconds
static constexpr double deadzone_scale = 2.;
static constexpr double deadzone_exponent = 4.0;
// these values worked best for me (MW) when taken with acompanying measured noise stddev of ca 0.1 (rot) and 0.01 (pos).
static constexpr double process_sigma_pos = 0.05;
static constexpr double process_simga_rot = 0.5;
static double map_slider_value(int v)
{
return std::pow(10., v * 0.04 - 3.);
}
settings() :
opts("kalman-filter"),
noise_rot_slider_value(b, "noise-rotation-slider", 40),
noise_pos_slider_value(b, "noise-position-slider", 40)
{}
};
class FTNoIR_Filter : public IFilter
{
PoseVector do_kalman_filter(const PoseVector &input, double dt, bool new_input);
void fill_transition_matrix(double dt);
void fill_process_noise_cov_matrix(StateMatrix &target, double dt) const;
public:
FTNoIR_Filter();
void reset();
void filter(const double *input, double *output);
PoseVector last_input;
Timer timer;
bool first_run;
double dt_since_last_input;
settings s;
KalmanFilter kf;
KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
PoseVector minimal_state_var;
DeadzoneFilter dz_filter;
int prev_slider_pos[2];
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class FTNoIR_FilterDll : public Metadata
{
public:
QString name() { return QString("Kalman"); }
QIcon icon() { return QIcon(":/images/filter-16.png"); }
};
class FilterControls: public IFilterDialog
{
Q_OBJECT
public:
FilterControls();
Ui::KalmanUICFilterControls ui;
void register_filter(IFilter*) override {}
void unregister_filter() override {}
settings s;
FTNoIR_Filter *filter;
public slots:
void doOK();
void doCancel();
};
|
