-
Notifications
You must be signed in to change notification settings - Fork 134
/
kalman-test.cpp
77 lines (62 loc) · 2.5 KB
/
kalman-test.cpp
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
/**
* Test for the KalmanFilter class with 1D projectile motion.
*
* @author: Hayk Martirosyan
* @date: 2014.11.15
*/
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "kalman.hpp"
int main(int argc, char* argv[]) {
int n = 3; // Number of states
int m = 1; // Number of measurements
double dt = 1.0/30; // Time step
Eigen::MatrixXd A(n, n); // System dynamics matrix
Eigen::MatrixXd C(m, n); // Output matrix
Eigen::MatrixXd Q(n, n); // Process noise covariance
Eigen::MatrixXd R(m, m); // Measurement noise covariance
Eigen::MatrixXd P(n, n); // Estimate error covariance
// Discrete LTI projectile motion, measuring position only
A << 1, dt, 0, 0, 1, dt, 0, 0, 1;
C << 1, 0, 0;
// Reasonable covariance matrices
Q << .05, .05, .0, .05, .05, .0, .0, .0, .0;
R << 5;
P << .1, .1, .1, .1, 10000, 10, .1, 10, 100;
std::cout << "A: \n" << A << std::endl;
std::cout << "C: \n" << C << std::endl;
std::cout << "Q: \n" << Q << std::endl;
std::cout << "R: \n" << R << std::endl;
std::cout << "P: \n" << P << std::endl;
// Construct the filter
KalmanFilter kf(dt,A, C, Q, R, P);
// List of noisy position measurements (y)
std::vector<double> measurements = {
1.04202710058, 1.10726790452, 1.2913511148, 1.48485250951, 1.72825901034,
1.74216489744, 2.11672039768, 2.14529225112, 2.16029641405, 2.21269371128,
2.57709350237, 2.6682215744, 2.51641839428, 2.76034056782, 2.88131780617,
2.88373786518, 2.9448468727, 2.82866600131, 3.0006601946, 3.12920591669,
2.858361783, 2.83808170354, 2.68975330958, 2.66533185589, 2.81613499531,
2.81003612051, 2.88321849354, 2.69789264832, 2.4342229249, 2.23464791825,
2.30278776224, 2.02069770395, 1.94393985809, 1.82498398739, 1.52526230354,
1.86967808173, 1.18073207847, 1.10729605087, 0.916168349913, 0.678547664519,
0.562381751596, 0.355468474885, -0.155607486619, -0.287198661013, -0.602973173813
};
// Best guess of initial states
Eigen::VectorXd x0(n);
double t = 0;
x0 << measurements[0], 0, -9.81;
kf.init(t, x0);
// Feed measurements into filter, output estimated states
Eigen::VectorXd y(m);
std::cout << "t = " << t << ", " << "x_hat[0]: " << kf.state().transpose() << std::endl;
for(int i = 0; i < measurements.size(); i++) {
t += dt;
y << measurements[i];
kf.update(y);
std::cout << "t = " << t << ", " << "y[" << i << "] = " << y.transpose()
<< ", x_hat[" << i << "] = " << kf.state().transpose() << std::endl;
}
return 0;
}