summaryrefslogtreecommitdiffhomepage
path: root/filter-kalman
diff options
context:
space:
mode:
Diffstat (limited to 'filter-kalman')
-rw-r--r--filter-kalman/CMakeLists.txt5
-rw-r--r--filter-kalman/ftnoir_kalman_filtercontrols.ui170
-rw-r--r--filter-kalman/kalman.cpp323
-rw-r--r--filter-kalman/kalman.h159
-rw-r--r--filter-kalman/kalman_simulation.py76
-rw-r--r--filter-kalman/lang/nl_NL.ts38
-rw-r--r--filter-kalman/lang/ru_RU.ts38
-rw-r--r--filter-kalman/lang/stub.ts38
-rw-r--r--filter-kalman/lang/zh_CN.ts38
9 files changed, 0 insertions, 885 deletions
diff --git a/filter-kalman/CMakeLists.txt b/filter-kalman/CMakeLists.txt
deleted file mode 100644
index b6f8100e..00000000
--- a/filter-kalman/CMakeLists.txt
+++ /dev/null
@@ -1,5 +0,0 @@
-find_package(Eigen3 QUIET)
-if(EIGEN3_FOUND)
- otr_module(filter-kalman)
- target_include_directories(opentrack-filter-kalman SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
-endif()
diff --git a/filter-kalman/ftnoir_kalman_filtercontrols.ui b/filter-kalman/ftnoir_kalman_filtercontrols.ui
deleted file mode 100644
index 83ee8e96..00000000
--- a/filter-kalman/ftnoir_kalman_filtercontrols.ui
+++ /dev/null
@@ -1,170 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>KalmanUICdialog_kalman</class>
- <widget class="QWidget" name="KalmanUICdialog_kalman">
- <property name="windowModality">
- <enum>Qt::NonModal</enum>
- </property>
- <property name="geometry">
- <rect>
- <x>0</x>
- <y>0</y>
- <width>438</width>
- <height>141</height>
- </rect>
- </property>
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="windowTitle">
- <string>Kalman settings</string>
- </property>
- <property name="windowIcon">
- <iconset resource="../gui/opentrack-res.qrc">
- <normaloff>:/images/filter-16.png</normaloff>:/images/filter-16.png</iconset>
- </property>
- <property name="layoutDirection">
- <enum>Qt::LeftToRight</enum>
- </property>
- <property name="autoFillBackground">
- <bool>false</bool>
- </property>
- <property name="styleSheet">
- <string notr="true"/>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout">
- <item>
- <widget class="QGroupBox" name="groupBox_2">
- <property name="title">
- <string>Measurement noise</string>
- </property>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="0" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Rotation</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QSlider" name="noiseRotSlider">
- <property name="maximum">
- <number>400</number>
- </property>
- <property name="singleStep">
- <number>1</number>
- </property>
- <property name="pageStep">
- <number>100</number>
- </property>
- <property name="tracking">
- <bool>true</bool>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="invertedAppearance">
- <bool>false</bool>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBelow</enum>
- </property>
- <property name="tickInterval">
- <number>100</number>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_4">
- <property name="text">
- <string>Position</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QSlider" name="noisePosSlider">
- <property name="maximum">
- <number>400</number>
- </property>
- <property name="pageStep">
- <number>100</number>
- </property>
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="tickPosition">
- <enum>QSlider::TicksBelow</enum>
- </property>
- <property name="tickInterval">
- <number>100</number>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="noiseRotLabel">
- <property name="minimumSize">
- <size>
- <width>65</width>
- <height>0</height>
- </size>
- </property>
- <property name="text">
- <string>°</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="noisePosLabel">
- <property name="minimumSize">
- <size>
- <width>65</width>
- <height>0</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item>
- <widget class="QDialogButtonBox" name="buttonBox">
- <property name="standardButtons">
- <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <resources>
- <include location="../gui/opentrack-res.qrc"/>
- </resources>
- <connections/>
- <designerdata>
- <property name="gridDeltaX">
- <number>10</number>
- </property>
- <property name="gridDeltaY">
- <number>10</number>
- </property>
- <property name="gridSnapX">
- <bool>false</bool>
- </property>
- <property name="gridSnapY">
- <bool>false</bool>
- </property>
- <property name="gridVisible">
- <bool>true</bool>
- </property>
- </designerdata>
- <slots>
- <slot>startEngineClicked()</slot>
- <slot>stopEngineClicked()</slot>
- <slot>cameraSettingsClicked()</slot>
- </slots>
-</ui>
diff --git a/filter-kalman/kalman.cpp b/filter-kalman/kalman.cpp
deleted file mode 100644
index 1960e0f6..00000000
--- a/filter-kalman/kalman.cpp
+++ /dev/null
@@ -1,323 +0,0 @@
-/* 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 "kalman.h"
-#include <cmath>
-#include <QDebug>
-
-void KalmanFilter::init()
-{
- // allocate and initialize matrices
- measurement_noise_cov = MeasureMatrix::Zero();
- process_noise_cov = StateMatrix::Zero();
- state_cov = StateMatrix::Zero();
- state_cov_prior = StateMatrix::Zero();
- transition_matrix = StateMatrix::Zero();
- measurement_matrix = StateToMeasureMatrix::Zero();
- kalman_gain = MeasureToStateMatrix::Zero();
- // initialize state variables
- state = StateVector::Zero();
- state_prior = StateVector::Zero();
- innovation = PoseVector::Zero();
-}
-
-
-void KalmanFilter::time_update()
-{
- state_prior = transition_matrix * state;
- state_cov_prior = transition_matrix * state_cov * transition_matrix.transpose() + process_noise_cov;
-}
-
-
-void KalmanFilter::measurement_update(const PoseVector &measurement)
-{
- MeasureMatrix tmp = measurement_matrix * state_cov_prior * measurement_matrix.transpose() + measurement_noise_cov;
- MeasureMatrix tmp_inv = tmp.inverse();
- kalman_gain = state_cov_prior * measurement_matrix.transpose() * tmp_inv;
- innovation = measurement - measurement_matrix * state_prior;
- state = state_prior + kalman_gain * innovation;
- state_cov = state_cov_prior - kalman_gain * measurement_matrix * state_cov_prior;
-}
-
-
-
-void KalmanProcessNoiseScaler::init()
-{
- base_cov = StateMatrix::Zero(NUM_STATE_DOF, NUM_STATE_DOF);
- innovation_cov_estimate = MeasureMatrix::Zero(NUM_MEASUREMENT_DOF, NUM_MEASUREMENT_DOF);
-}
-
-
-/* Uses
- innovation, measurement_matrix, measurement_noise_cov, and state_cov_prior
- found in KalmanFilter. It sets
- process_noise_cov
-*/
-void KalmanProcessNoiseScaler::update(KalmanFilter &kf, double dt)
-{
- MeasureMatrix ddT = kf.innovation * kf.innovation.transpose();
- double f = dt / (dt + settings::adaptivity_window_length);
- innovation_cov_estimate =
- f * ddT + (1. - f) * innovation_cov_estimate;
-
- double T1 = (innovation_cov_estimate - kf.measurement_noise_cov).trace();
- double T2 = (kf.measurement_matrix * kf.state_cov_prior * kf.measurement_matrix.transpose()).trace();
- double alpha = 0.001;
- if (T2 > 0. && T1 > 0.)
- {
- alpha = T1 / T2;
- alpha = std::sqrt(alpha);
- alpha = std::fmin(1000., std::fmax(0.001, alpha));
- }
- kf.process_noise_cov = alpha * base_cov;
- //qDebug() << "alpha = " << alpha;
-}
-
-void DeadzoneFilter::reset()
-{
- last_output = PoseVector::Zero();
-}
-
-PoseVector DeadzoneFilter::filter(const PoseVector &input)
-{
- PoseVector out;
- for (int i = 0; i < input.rows(); ++i)
- {
- const double dz = dz_size[i];
- if (dz > 0.)
- {
- const double delta = input[i] - last_output[i];
- const double f = std::pow(std::fabs(delta) / dz, settings::deadzone_exponent);
- const double response = f / (f + 1.) * delta;
- out[i] = last_output[i] + response;
- }
- else
- out[i] = input[i];
- last_output[i] = out[i];
- }
- return out;
-}
-
-
-void kalman::fill_transition_matrix(double dt)
-{
- for (int i = 0; i < 6; ++i)
- {
- kf.transition_matrix(i, i + 6) = dt;
- }
-}
-
-void kalman::fill_process_noise_cov_matrix(StateMatrix &target, double dt) const
-{
- // This model is like movement at fixed velocity plus superimposed
- // brownian motion. Unlike standard models for tracking of objects
- // with a very well predictable trajectory (e.g.
- // https://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical)
- double sigma_pos = settings::process_sigma_pos;
- double sigma_angle = settings::process_sigma_rot;
- double a_pos = sigma_pos * sigma_pos * dt;
- double a_ang = sigma_angle * sigma_angle * dt;
- constexpr double b = 20;
- constexpr double c = 1.;
- for (int i = 0; i < 3; ++i)
- {
- target(i, i) = a_pos;
- target(i, i + 6) = a_pos * c;
- target(i + 6, i) = a_pos * c;
- target(i + 6, i + 6) = a_pos * b;
- }
- for (int i = 3; i < 6; ++i)
- {
- target(i, i) = a_ang;
- target(i, i + 6) = a_ang * c;
- target(i + 6, i) = a_ang * c;
- target(i + 6, i + 6) = a_ang * b;
- }
-}
-
-
-PoseVector kalman::do_kalman_filter(const PoseVector &input, double dt, bool new_input)
-{
- if (new_input)
- {
- dt = dt_since_last_input;
- fill_transition_matrix(dt);
- fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, dt);
- kf_adaptive_process_noise_cov.update(kf, dt);
- kf.time_update();
- kf.measurement_update(input);
- }
- return kf.state.head(6);
-}
-
-
-
-kalman::kalman()
-{
- reset();
-}
-
-// The original code was written by Donovan Baarda <abo@minkirri.apana.org.au>
-// https://sourceforge.net/p/facetracknoir/discussion/1150909/thread/418615e1/?limit=25#af75/084b
-void kalman::reset()
-{
- kf.init();
- kf_adaptive_process_noise_cov.init();
- for (int i = 0; i < 6; ++i)
- {
- // initialize part of the transition matrix that do not change.
- kf.transition_matrix(i, i) = 1.;
- kf.transition_matrix(i + 6, i + 6) = 1.;
- // "extract" positions, i.e. the first 6 state dof.
- kf.measurement_matrix(i, i) = 1.;
- }
-
- double noise_variance_position = settings::map_slider_value(s.noise_pos_slider_value);
- double noise_variance_angle = settings::map_slider_value(s.noise_rot_slider_value);
- for (int i = 0; i < 3; ++i)
- {
- kf.measurement_noise_cov(i , i ) = noise_variance_position;
- kf.measurement_noise_cov(i + 3, i + 3) = noise_variance_angle;
- }
-
- fill_transition_matrix(0.03);
- fill_process_noise_cov_matrix(kf_adaptive_process_noise_cov.base_cov, 0.03);
-
- kf.process_noise_cov = kf_adaptive_process_noise_cov.base_cov;
- kf.state_cov = kf.process_noise_cov;
-
- for (int i = 0; i < 6; i++) {
- last_input[i] = 0;
- }
- dt_since_last_input = 0;
-
- prev_slider_pos[0] = s.noise_pos_slider_value;
- prev_slider_pos[1] = s.noise_rot_slider_value;
-
- dz_filter.reset();
-}
-
-
-void kalman::filter(const double* input_, double *output_)
-{
- // almost non-existent cost, so might as well ...
- Eigen::Map<const PoseVector> input(input_, PoseVector::RowsAtCompileTime, 1);
- Eigen::Map<PoseVector> output(output_, PoseVector::RowsAtCompileTime, 1);
-
- if (!(prev_slider_pos[0] == s.noise_pos_slider_value &&
- prev_slider_pos[1] == s.noise_rot_slider_value))
- {
- reset();
- }
-
- // Start the timer on first filter evaluation.
- if (first_run)
- {
- timer.start();
- first_run = false;
- return;
- }
-
- // Note this is a terrible way to detect when there is a new
- // frame of tracker input, but it is the best we have.
- bool new_input = input.cwiseNotEqual(last_input).any();
-
- // Get the time in seconds since last run and restart the timer.
- const double dt = timer.elapsed_seconds();
- dt_since_last_input += dt;
- timer.start();
-
- output = do_kalman_filter(input, dt, new_input);
-
- {
- // Compute deadzone size base on estimated state variance.
- // Given a constant input plus measurement noise, KF should converge to the true input.
- // This works well. That is the output pose becomes very still afte some time.
- // The QScaling adaptive filter makes the state cov vary depending on the estimated noise
- // and the measured noise of the innovation sequence. After a sudden movement it peaks
- // and then decays asymptotically to some constant value taken in stationary state.
- // We can use this to calculate the size of the deadzone, so that in the stationary state the
- // deadzone size is small. Thus the tracking error due to the dz-filter becomes also small.
- PoseVector variance = kf.state_cov.diagonal().head(6);
- dz_filter.dz_size = variance.cwiseSqrt() * settings::deadzone_scale;
- }
- output = dz_filter.filter(output);
-
- if (new_input)
- {
- dt_since_last_input = 0;
- last_input = input;
- }
-}
-
-
-
-dialog_kalman::dialog_kalman()
- : filter(nullptr)
-{
- ui.setupUi(this);
- connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
- connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
-
- tie_setting(s.noise_rot_slider_value, ui.noiseRotSlider);
- tie_setting(s.noise_pos_slider_value, ui.noisePosSlider);
-
- connect(&s.noise_rot_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
- connect(&s.noise_pos_slider_value, SIGNAL(valueChanged(const slider_value&)), this, SLOT(updateLabels(const slider_value&)));
-
- updateLabels(slider_value());
-}
-
-
-void dialog_kalman::updateLabels(const slider_value&)
-{
- this->ui.noiseRotLabel->setText(
- QString::number(settings::map_slider_value(s.noise_rot_slider_value), 'f', 3) + "°");
-
- this->ui.noisePosLabel->setText(
- QString::number(settings::map_slider_value(s.noise_pos_slider_value), 'f', 3) + " cm");
-}
-
-
-void dialog_kalman::doOK() {
- s.b->save();
- close();
-}
-
-
-void dialog_kalman::doCancel()
-{
- close();
-}
-
-double settings::map_slider_value(const slider_value& v_)
-{
- const double v = v_;
-#if 0
- //return std::pow(10., v * 4. - 3.);
-#else
- constexpr int min_log10 = -3;
- constexpr int max_log10 = 1;
- constexpr int num_divisions = max_log10 - min_log10;
- /* ascii art representation of slider
- // ----- // ------// ------// ------- // 4 divisions
- -3 - 2 -1 0 1 power of 10
- | |
- | f + left_side_log10
- |
- left_side_log10
- */
- const int k = int(v * num_divisions); // in which division are we?!
- const double f = v * num_divisions - k; // where in the division are we?!
- const double ff = f * 9. + 1.;
- const double multiplier = int(ff * 10.) / 10.;
- const int left_side_log10 = min_log10 + k;
- const double val = std::pow(10., left_side_log10) * multiplier;
- return val;
-#endif
-}
-
-OPENTRACK_DECLARE_FILTER(kalman, dialog_kalman, kalmanDll)
diff --git a/filter-kalman/kalman.h b/filter-kalman/kalman.h
deleted file mode 100644
index 1e6e2ac4..00000000
--- a/filter-kalman/kalman.h
+++ /dev/null
@@ -1,159 +0,0 @@
-#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
-
-// nodiscard for placement new
-#ifdef _MSC_VER
-# pragma warning(push)
-# pragma warning(disable : 4834)
-#endif
-
-#include <Eigen/Core>
-#include <Eigen/LU>
-
-#ifdef _MSC_VER
-# pragma warning(pop)
-#endif
-
-#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&);
-};
diff --git a/filter-kalman/kalman_simulation.py b/filter-kalman/kalman_simulation.py
deleted file mode 100644
index 3e792212..00000000
--- a/filter-kalman/kalman_simulation.py
+++ /dev/null
@@ -1,76 +0,0 @@
-# -*- coding: utf-8 -*-
-import numpy as np
-import numpy.matlib as mt
-import matplotlib.pyplot as plt
-
-M = 2
-N = 500
-dt = 1.0 / 30.
-sigma_measure = 0.1
-# x = 1/2 a t**2
-# assume x = 0.5 in 0.5, i.e. head turns half maximal movement range in 0.5 sec.
-# so a = 2 x / t**2 = 4.0
-sigma_accel = 4.0
-
-x = np.zeros(N, dtype = np.float)
-x[N/3:] = 1.0 # step input
-measurement = x.copy()
-measurement += np.random.normal(0., sigma_measure, N)
-
-A = np.matrix([
- [ 1. , dt ],
- [ 0. , 1. ]
-])
-
-R = np.matrix([[ sigma_measure**2 ]])
-
-dv = sigma_accel * dt
-dp = sigma_accel * 0.5 * dt * dt
-Q = np.matrix([
- [ dp*dp, dp*dv ],
- [ dv*dp, dv*dv ]
-])
-
-H = np.matrix([
- [ 1., 0. ],
-])
-
-I = mt.identity(M, dtype = np.float)
-
-def arrayOfMatrices(n, shape):
- return np.asarray([mt.zeros(shape, dtype = np.float) for i in xrange(n)])
-
-# Base on the scipy-cookbook http://scipy-cookbook.readthedocs.io/items/KalmanFiltering.html
-sz_state = (M, 1)
-sz_cov = (M, M)
-sz_K = (M, 1)
-xhat= arrayOfMatrices(N, sz_state) # a posteri estimate of x
-P=arrayOfMatrices(N, sz_cov) # a posteri error estimate
-xhatminus=arrayOfMatrices(N, sz_state) # a priori estimate of x
-Pminus=arrayOfMatrices(N, sz_cov) # a priori error estimate
-K=arrayOfMatrices(N, sz_K) # gain or blending factor
-
-P[0] = mt.ones((M,M)) * 100
-xhat[0] = measurement[0]
-
-for k in range(1,N):
- # time update
- xhatminus[k] = A * xhat[k-1]
- Pminus[k] = A * P[k-1] * A.T + Q
-
- # measurement update
- K[k] = Pminus[k] * H.T * np.linalg.inv( H * Pminus[k] * H.T + R )
- xhat[k] = xhatminus[k] + K[k] * (measurement[k] - H * xhatminus[k])
- P[k] = ( I - K[k]*H ) * Pminus[k]
-
-t = np.arange(N) * dt
-plt.figure()
-plt.subplot(2,1,1)
-plt.plot(t, measurement,'k+',label='noisy measurements')
-plt.plot(t, xhat[:,0,0],'b-',label='position estimate')
-plt.plot(t, x, 'r-', label='ground truth')
-
-plt.subplot(2,1,2)
-plt.plot(t, xhat[:,1,0],'g-',label='velocity estimate')
-
-plt.show()
diff --git a/filter-kalman/lang/nl_NL.ts b/filter-kalman/lang/nl_NL.ts
deleted file mode 100644
index e3c61bbb..00000000
--- a/filter-kalman/lang/nl_NL.ts
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!DOCTYPE TS>
-<TS version="2.1" language="nl_NL">
-<context>
- <name>KalmanUICdialog_kalman</name>
- <message>
- <source>Kalman settings</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Measurement noise</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Rotation</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Position</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>°</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>-</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-<context>
- <name>kalmanDll</name>
- <message>
- <source>Kalman</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-</TS>
diff --git a/filter-kalman/lang/ru_RU.ts b/filter-kalman/lang/ru_RU.ts
deleted file mode 100644
index 6fda97be..00000000
--- a/filter-kalman/lang/ru_RU.ts
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!DOCTYPE TS>
-<TS version="2.1" language="ru_RU">
-<context>
- <name>KalmanUICdialog_kalman</name>
- <message>
- <source>Kalman settings</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Measurement noise</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Rotation</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Position</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>°</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>-</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-<context>
- <name>kalmanDll</name>
- <message>
- <source>Kalman</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-</TS>
diff --git a/filter-kalman/lang/stub.ts b/filter-kalman/lang/stub.ts
deleted file mode 100644
index 29fae5c0..00000000
--- a/filter-kalman/lang/stub.ts
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!DOCTYPE TS>
-<TS version="2.1">
-<context>
- <name>KalmanUICdialog_kalman</name>
- <message>
- <source>Kalman settings</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Measurement noise</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Rotation</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>Position</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>°</source>
- <translation type="unfinished"></translation>
- </message>
- <message>
- <source>-</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-<context>
- <name>kalmanDll</name>
- <message>
- <source>Kalman</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-</TS>
diff --git a/filter-kalman/lang/zh_CN.ts b/filter-kalman/lang/zh_CN.ts
deleted file mode 100644
index d8de6cf2..00000000
--- a/filter-kalman/lang/zh_CN.ts
+++ /dev/null
@@ -1,38 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!DOCTYPE TS>
-<TS version="2.1" language="zh_CN">
-<context>
- <name>KalmanUICdialog_kalman</name>
- <message>
- <source>Kalman settings</source>
- <translation>Kalman 设置</translation>
- </message>
- <message>
- <source>Measurement noise</source>
- <translation type="unfinished">衡量噪声</translation>
- </message>
- <message>
- <source>Rotation</source>
- <translation>旋转</translation>
- </message>
- <message>
- <source>Position</source>
- <translation>方位</translation>
- </message>
- <message>
- <source>°</source>
- <translation></translation>
- </message>
- <message>
- <source>-</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-<context>
- <name>kalmanDll</name>
- <message>
- <source>Kalman</source>
- <translation type="unfinished"></translation>
- </message>
-</context>
-</TS>