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
|
/* Copyright (c) 2019, Stephane Lenclud <github@lenclud.com>
* 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-filter-pose.h"
namespace EasyTracker
{
KalmanFilterPose::KalmanFilterPose()
{
}
void KalmanFilterPose::Init(int nStates, int nMeasurements, int nInputs, double dt)
{
iMeasurements = cv::Mat(nMeasurements, 1, CV_64FC1);
init(nStates, nMeasurements, nInputs, CV_64F); // init Kalman Filter
// TODO: Use parameters instead of magic numbers
setIdentity(processNoiseCov, cv::Scalar::all(1)); //1e-5 // set process noise
setIdentity(measurementNoiseCov, cv::Scalar::all(1)); //1e-2 // set measurement noise
setIdentity(errorCovPost, cv::Scalar::all(1)); // error covariance
/** DYNAMIC MODEL **/
// [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0]
// [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
// [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
// [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
// position
transitionMatrix.at<double>(0, 3) = dt;
transitionMatrix.at<double>(1, 4) = dt;
transitionMatrix.at<double>(2, 5) = dt;
transitionMatrix.at<double>(3, 6) = dt;
transitionMatrix.at<double>(4, 7) = dt;
transitionMatrix.at<double>(5, 8) = dt;
transitionMatrix.at<double>(0, 6) = 0.5*pow(dt, 2);
transitionMatrix.at<double>(1, 7) = 0.5*pow(dt, 2);
transitionMatrix.at<double>(2, 8) = 0.5*pow(dt, 2);
// orientation
transitionMatrix.at<double>(9, 12) = dt;
transitionMatrix.at<double>(10, 13) = dt;
transitionMatrix.at<double>(11, 14) = dt;
transitionMatrix.at<double>(12, 15) = dt;
transitionMatrix.at<double>(13, 16) = dt;
transitionMatrix.at<double>(14, 17) = dt;
transitionMatrix.at<double>(9, 15) = 0.5*pow(dt, 2);
transitionMatrix.at<double>(10, 16) = 0.5*pow(dt, 2);
transitionMatrix.at<double>(11, 17) = 0.5*pow(dt, 2);
/** MEASUREMENT MODEL **/
// [1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0]
// [0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0]
// Those will scale the filtered values, 2 will give you half the raw input
measurementMatrix.at<double>(0, 0) = 1; // x
measurementMatrix.at<double>(1, 1) = 1; // y
measurementMatrix.at<double>(2, 2) = 1; // z
measurementMatrix.at<double>(3, 9) = 1; // roll
measurementMatrix.at<double>(4, 10) = 1; // pitch
measurementMatrix.at<double>(5, 11) = 1; // yaw
}
void KalmanFilterPose::Update(double& aX, double& aY, double& aZ, double& aRoll, double& aPitch, double& aYaw)
{
// Set measurement to predict
iMeasurements.at<double>(0) = aX; // x
iMeasurements.at<double>(1) = aY; // y
iMeasurements.at<double>(2) = aZ; // z
iMeasurements.at<double>(3) = aRoll; // roll
iMeasurements.at<double>(4) = aPitch; // pitch
iMeasurements.at<double>(5) = aYaw; // yaw
// First predict, to update the internal statePre variable
cv::Mat prediction = predict();
// The "correct" phase that is going to use the predicted value and our measurement
cv::Mat estimated = correct(iMeasurements);
// Estimated translation
aX = estimated.at<double>(0);
aY = estimated.at<double>(1);
aZ = estimated.at<double>(2);
// Estimated euler angles
aRoll = estimated.at<double>(9);
aPitch = estimated.at<double>(10);
aYaw = estimated.at<double>(11);
}
}
|
