Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Rather than using separate 1D filters for each of x,y,vx,vy, use a 2x2 matrix to represent position and velocity in a more proper kalman filter.
- Loading branch information
Showing
3 changed files
with
199 additions
and
55 deletions.
There are no files selected for viewing
158 changes: 158 additions & 0 deletions
158
src/platformsupport/input/evdevtouch/qevdevtouchfilter_p.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,158 @@ | ||
| /**************************************************************************** | ||
| ** | ||
| ** Copyright (C) 2016 Jolla Ltd, author: <gunnar.sletta@jollamobile.com> | ||
| ** Contact: http://www.qt.io/licensing/ | ||
| ** | ||
| ** This file is part of the plugins module of the Qt Toolkit. | ||
| ** | ||
| ** $QT_BEGIN_LICENSE:LGPL21$ | ||
| ** Commercial License Usage | ||
| ** Licensees holding valid commercial Qt licenses may use this file in | ||
| ** accordance with the commercial license agreement provided with the | ||
| ** Software or, alternatively, in accordance with the terms contained in | ||
| ** a written agreement between you and The Qt Company. For licensing terms | ||
| ** and conditions see http://www.qt.io/terms-conditions. For further | ||
| ** information use the contact form at http://www.qt.io/contact-us. | ||
| ** | ||
| ** GNU Lesser General Public License Usage | ||
| ** Alternatively, this file may be used under the terms of the GNU Lesser | ||
| ** General Public License version 2.1 or version 3 as published by the Free | ||
| ** Software Foundation and appearing in the file LICENSE.LGPLv21 and | ||
| ** LICENSE.LGPLv3 included in the packaging of this file. Please review the | ||
| ** following information to ensure the GNU Lesser General Public License | ||
| ** requirements will be met: https://www.gnu.org/licenses/lgpl.html and | ||
| ** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html. | ||
| ** | ||
| ** As a special exception, The Qt Company gives you certain additional | ||
| ** rights. These rights are described in The Qt Company LGPL Exception | ||
| ** version 1.1, included in the file LGPL_EXCEPTION.txt in this package. | ||
| ** | ||
| ** $QT_END_LICENSE$ | ||
| ** | ||
| ****************************************************************************/ | ||
|
|
||
| #include <qglobal.h> | ||
|
|
||
| QT_BEGIN_NAMESPACE | ||
|
|
||
| struct QEvdevTouchFilter | ||
| { | ||
| QEvdevTouchFilter(); | ||
|
|
||
| void initialize(float pos, float velocity); | ||
| void update(float pos, float velocity, float timeDelta); | ||
|
|
||
| float position() const { return x.x; } | ||
| float velocity() const { return x.y; } | ||
|
|
||
| private: | ||
| struct vec2 { | ||
| vec2(float x = 0.0f, float y = 0.0f) : x(x), y(y) { } | ||
| float x, y; | ||
|
|
||
| vec2 operator-(vec2 v) { | ||
| return vec2(x - v.x, y - v.y); | ||
| } | ||
|
|
||
| vec2 operator+(vec2 v) { | ||
| return vec2(x + v.x, y + v.y); | ||
| } | ||
| }; | ||
|
|
||
| struct mat2 { | ||
| float a, b, c, d; | ||
| mat2(float a = 1.0f, float b = 0.0f, float c = 0.0f, float d = 1.0f) | ||
| : a(a) | ||
| , b(b) | ||
| , c(c) | ||
| , d(d) | ||
| { | ||
| } | ||
|
|
||
| mat2 transposed() const { | ||
| return mat2(a, c, | ||
| b, d); | ||
| } | ||
|
|
||
| mat2 inverted() const { | ||
| float det = 1.0f / (a * d - b * c); | ||
| return mat2( d * det, -b * det, | ||
| -c * det, a * det); | ||
| } | ||
|
|
||
| mat2 operator+(mat2 m) const { | ||
| return mat2(a + m.a, b + m.b, | ||
| c + m.c, d + m.d); | ||
| } | ||
|
|
||
| mat2 operator-(mat2 m) const { | ||
| return mat2(a - m.a, b - m.b, | ||
| c - m.c, d - m.d); | ||
| } | ||
|
|
||
| vec2 operator*(vec2 v) const { | ||
| return vec2(a * v.x + b * v.y, | ||
| c * v.x + d * v.y); | ||
| } | ||
|
|
||
| mat2 operator*(mat2 M) const { | ||
| return mat2(a * M.a + b * M.c, | ||
| a * M.b + b * M.d, | ||
| c * M.a + d * M.c, | ||
| c * M.b + d * M.d); | ||
| } | ||
| }; | ||
|
|
||
| vec2 x; | ||
| mat2 A; | ||
| mat2 P; | ||
| mat2 Q; | ||
| mat2 R; | ||
| mat2 H; | ||
| }; | ||
|
|
||
| inline QEvdevTouchFilter::QEvdevTouchFilter() | ||
| { | ||
| } | ||
|
|
||
| inline void QEvdevTouchFilter::initialize(float pos, float velocity) | ||
| { | ||
| x = vec2(pos, velocity); | ||
|
|
||
| P = mat2(0.0f, 0.0f, | ||
| 0.0f, 0.0f); | ||
|
|
||
| Q = mat2(0.0f, 0.0f, | ||
| 0.0f, 0.1f); | ||
| R = mat2(0.1f, 0.0f, | ||
| 0.0f, 0.1f); | ||
| } | ||
|
|
||
| inline void QEvdevTouchFilter::update(float pos, float velocity, float dT) | ||
| { | ||
| A.b = dT; | ||
|
|
||
| // Prediction setp | ||
| x = A * x; | ||
| P = A * P * A.transposed() + Q; | ||
|
|
||
| // Correction step (complete with H) | ||
| // mat2 S = H * P * H.transposed() + R; | ||
| // mat2 K = P * H.transposed() * S.inverted(); | ||
| // vec2 m(pos, velocity); | ||
| // vec2 y = m - H * x; | ||
| // x = x + K * y; | ||
| // P = (mat2() - K * H) * P; | ||
|
|
||
| // Correction step (without H as H is currently set to I, so we can ignore | ||
| // it in the calculations...) | ||
| mat2 S = P + R; | ||
| mat2 K = P * S.inverted(); | ||
| vec2 m(pos, velocity); | ||
| vec2 y = m - x; | ||
| x = x + K * y; | ||
| P = (mat2() - K) * P; | ||
|
|
||
| } | ||
|
|
||
| QT_END_NAMESPACE |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters