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
|
#pragma once
/* Copyright (c) 2016 Michael Welter <mw.pub@welter-4d.de>
*
* 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 "compat/timer.hpp"
#include "api/plugin-api.hpp"
#include "options/options.hpp"
using namespace options;
// Eigen can't check for SSE3 on MSVC
#if defined _MSC_VER && defined __SSE2__
# define EIGEN_VECTORIZE_SSE3
// this hardware is 10 years old
# define EIGEN_VECTORIZE_SSE4_1
# define EIGEN_VECTORIZE_SSE4_2
#endif
#include <Eigen/Core>
#include <Eigen/LU>
#include "ui_ftnoir_kalman_filtercontrols.h"
#include <QString>
#include <QWidget>
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);
};
struct KalmanProcessNoiseScaler
{
MeasureMatrix
innovation_cov_estimate;
StateMatrix
base_cov; // baseline (unscaled) process noise covariance matrix
void init();
void update(KalmanFilter &kf, double dt);
};
struct DeadzoneFilter
{
PoseVector last_output { PoseVector::Zero() },
dz_size { PoseVector::Zero() };
DeadzoneFilter() = default;
void reset();
PoseVector filter(const PoseVector &input);
};
struct settings : opts {
value<slider_value> noise_rot_slider_value { b, "noise-rotation-slider", { .5, 0, 1 } };
value<slider_value> noise_pos_slider_value { b, "noise-position-slider", { .5, 0, 1 } };
static constexpr double adaptivity_window_length = 0.25; // seconds
static constexpr double deadzone_scale = 8;
static constexpr double deadzone_exponent = 2.0;
static constexpr double process_sigma_pos = 0.5;
static constexpr double process_sigma_rot = 0.5;
static double map_slider_value(const slider_value &v);
settings() : opts("kalman-filter") {}
};
class kalman : 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:
kalman();
void reset();
void filter(const double *input, double *output) override;
void center() override { reset(); }
module_status initialize() override { return status_ok(); }
double dt_since_last_input;
PoseVector last_input;
KalmanFilter kf;
KalmanProcessNoiseScaler kf_adaptive_process_noise_cov;
DeadzoneFilter dz_filter;
settings s;
slider_value prev_slider_pos[2] {
*s.noise_pos_slider_value,
*s.noise_rot_slider_value,
};
Timer timer;
bool first_run = true;
};
class kalmanDll : public Metadata
{
Q_OBJECT
QString name() override { return tr("Kalman"); }
QIcon icon() override { return QIcon(":/images/filter-16.png"); }
};
class dialog_kalman: public IFilterDialog
{
Q_OBJECT
public:
dialog_kalman();
Ui::KalmanUICdialog_kalman ui;
void register_filter(IFilter*) override {}
void unregister_filter() override {}
settings s;
kalman *filter;
public slots:
void doOK();
void doCancel();
void updateLabels(const slider_value&);
};
|
