KalmanFilter/test.cpp at master · MichaelMathieu/KalmanFilter · GitHub

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
#include"kalman.hpp"
#include<iostream>
usingnamespacestd;

classTestKalman : publicKalmanFilter<float> {
public:
TestKalman()
 :KalmanFilter<float>(1, 0.5f, 0.5f) {
 };
virtual~TestKalman() {};
protected:
virtualintnCommandParams() const { return1;};
virtualintnNoise1Params() const { return1;};
virtualintnObsParams() const { return1;};
virtualintnNoise2Params() const { return1;};
public:
virtual matr f(const matr & u, const matr & w, constvoid* p = NULL) const;
virtual matr h(const matr & v, constvoid* = NULL) const;
};

TestKalman::matr TestKalman::f(const matr & u, const matr & w, constvoid*) const {
returnmatr(1, 1, x(0) + 2*u(0) + 0.1*w(0));
};

TestKalman::matr TestKalman::h(const matr & v, constvoid*) const {
returnmatr(1, 1, 0.7*x(0) + 0.2*v(0));
};


intmain () {
 TestKalman test;
 cout << (int)test.testDerivatives() << endl;
 cout << test.getX() << "" << test.getCov() << endl;
 test.update(TestKalman::matr(1,1,0.5f), TestKalman::matr(1,1,0.8f*0.7f));
 cout << test.getX() << "" << test.getCov() << endl;
 test.update(TestKalman::matr(1,1,0.0f), TestKalman::matr(1,1,0.9f*0.7f));
 cout << test.getX() << "" << test.getCov() << endl;
 test.update(TestKalman::matr(1,1,1.0f), TestKalman::matr(1,1,2.7f*0.7f));
 cout << test.getX() << "" << test.getCov() << endl;
return0;
}