test/kalman_filter.cc

Code
Comments
Other
Rev Date Author Line
4287 30 Jan 23 peter 1 // $Id$
4287 30 Jan 23 peter 2
4287 30 Jan 23 peter 3 /*
4287 30 Jan 23 peter 4   Copyright (C) 2023 Peter Johansson
4287 30 Jan 23 peter 5
4287 30 Jan 23 peter 6   This file is part of the yat library, https://dev.thep.lu.se/yat
4287 30 Jan 23 peter 7
4287 30 Jan 23 peter 8   The yat library is free software; you can redistribute it and/or
4287 30 Jan 23 peter 9   modify it under the terms of the GNU General Public License as
4287 30 Jan 23 peter 10   published by the Free Software Foundation; either version 3 of the
4287 30 Jan 23 peter 11   License, or (at your option) any later version.
4287 30 Jan 23 peter 12
4287 30 Jan 23 peter 13   The yat library is distributed in the hope that it will be useful,
4287 30 Jan 23 peter 14   but WITHOUT ANY WARRANTY; without even the implied warranty of
4287 30 Jan 23 peter 15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
4287 30 Jan 23 peter 16   General Public License for more details.
4287 30 Jan 23 peter 17
4287 30 Jan 23 peter 18   You should have received a copy of the GNU General Public License
4287 30 Jan 23 peter 19   along with yat. If not, see <https://www.gnu.org/licenses/>.
4287 30 Jan 23 peter 20 */
4287 30 Jan 23 peter 21
4287 30 Jan 23 peter 22 #include <config.h>
4287 30 Jan 23 peter 23
4287 30 Jan 23 peter 24 #include "Suite.h"
4287 30 Jan 23 peter 25
4287 30 Jan 23 peter 26 #include <yat/statistics/KalmanFilter.h>
4287 30 Jan 23 peter 27 #include <yat/random/random.h>
4287 30 Jan 23 peter 28
4287 30 Jan 23 peter 29 #include <cassert>
4287 30 Jan 23 peter 30
4287 30 Jan 23 peter 31 using namespace theplu;
4287 30 Jan 23 peter 32 using namespace yat;
4287 30 Jan 23 peter 33 using statistics::KalmanFilter;
4287 30 Jan 23 peter 34
4287 30 Jan 23 peter 35 void test1(yat::test::Suite& suite);
4287 30 Jan 23 peter 36
4287 30 Jan 23 peter 37
4287 30 Jan 23 peter 38 int main(int argc, char* argv[])
4287 30 Jan 23 peter 39 {
4287 30 Jan 23 peter 40   yat::test::Suite suite(argc, argv);
4287 30 Jan 23 peter 41   try {
4287 30 Jan 23 peter 42     test1(suite);
4287 30 Jan 23 peter 43   }
4287 30 Jan 23 peter 44   catch (std::runtime_error& e) {
4287 30 Jan 23 peter 45     suite.err() << "error: " << e.what() << "\n";
4287 30 Jan 23 peter 46     return EXIT_FAILURE;
4287 30 Jan 23 peter 47   }
4287 30 Jan 23 peter 48   return suite.return_value();
4287 30 Jan 23 peter 49 }
4287 30 Jan 23 peter 50
4287 30 Jan 23 peter 51
4287 30 Jan 23 peter 52 void test1(yat::test::Suite& suite)
4287 30 Jan 23 peter 53 {
4287 30 Jan 23 peter 54   size_t n = 3;
4287 30 Jan 23 peter 55   utility::Vector x(n);
4287 30 Jan 23 peter 56   utility::Matrix P(n, n, 0.0);
4287 30 Jan 23 peter 57   for (size_t i=0; i<n; ++i)
4287 30 Jan 23 peter 58     P(i, i) = 1e9;
4287 30 Jan 23 peter 59
4287 30 Jan 23 peter 60   utility::Matrix F(n, n);
4287 30 Jan 23 peter 61   F(0,0) = 1.0;
4287 30 Jan 23 peter 62   F(0,1) = 1.0;
4287 30 Jan 23 peter 63   F(1,1) = 1.0;
4287 30 Jan 23 peter 64   F(1,2) = 1.0;
4287 30 Jan 23 peter 65   F(2,1) = -0.1;
4287 30 Jan 23 peter 66   // x0 = x0 + x1
4287 30 Jan 23 peter 67   // x1 = x1 + x2
4287 30 Jan 23 peter 68   // x2 = -0.1*x1
4287 30 Jan 23 peter 69
4287 30 Jan 23 peter 70   utility::Matrix Q(n, n);
4287 30 Jan 23 peter 71   Q(2, 2) = 1.0;
4287 30 Jan 23 peter 72
4287 30 Jan 23 peter 73   size_t m = 1;
4287 30 Jan 23 peter 74   utility::Vector z(m, 0.0);
4287 30 Jan 23 peter 75   utility::Matrix H(m, n); // { 1, 0, 0}
4287 30 Jan 23 peter 76   H(0, 0) = 1.0;
4287 30 Jan 23 peter 77   utility::Matrix R(m, m, 1.0);
4287 30 Jan 23 peter 78
4287 30 Jan 23 peter 79   random::Gaussian gauss;
4287 30 Jan 23 peter 80
4287 30 Jan 23 peter 81   KalmanFilter kalman(x, P);
4287 30 Jan 23 peter 82   for (size_t time = 0; time<1000; ++time) {
4287 30 Jan 23 peter 83     kalman.predict(F, Q)(0);
4287 30 Jan 23 peter 84     x = F*x;
4287 30 Jan 23 peter 85     x(2) += gauss();
4287 30 Jan 23 peter 86
4287 30 Jan 23 peter 87     z(0) = x(0) + gauss();
4287 30 Jan 23 peter 88     kalman.update(z, H, R);
4287 30 Jan 23 peter 89     suite.out() << time
4287 30 Jan 23 peter 90                 << "\t" << z(0)
4287 30 Jan 23 peter 91                 << "\t" << x(0)
4287 30 Jan 23 peter 92                 << "\t" << kalman.xhat()(0)
4287 30 Jan 23 peter 93                 << "; " << std::sqrt(kalman.Phat()(0,0))
4287 30 Jan 23 peter 94                 << "\t" << kalman.xhat()(1)
4287 30 Jan 23 peter 95                 << "; " << std::sqrt(kalman.Phat()(1,1))
4287 30 Jan 23 peter 96                 << "\t" << kalman.xhat()(2)
4287 30 Jan 23 peter 97                 << "; " << std::sqrt(kalman.Phat()(2,2))
4287 30 Jan 23 peter 98                 << "\n";
4287 30 Jan 23 peter 99   }
4287 30 Jan 23 peter 100 }

Generated on Sat Oct 14 02:21:02 2023 (UTC) by svndigest 0.11.2pre (dev build 1672)