00001
00007 #ifndef FILTER_MODEL_H_
00008 #define FILTER_MODEL_H_
00009
00010 #include <cassert>
00011 #include <vector>
00012 #include <iostream>
00013 #include "linalg.h"
00014 #include "ISignalData.h"
00015 #include "unscented_kalman_filter.h"
00016
00024 struct FilterModel {
00025
00027 FilterModel(int state_dim, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00028 : _state_dim(state_dim), _rs(rs), _signal_dim(0), _signal_data(NULL), weights_on_tensors_(weights_on_tensors),
00029 _constrained(constrained) {
00030
00031 _Q.set_size(_state_dim, _state_dim);
00032 _Q.fill(0);
00033
00034 if (constrained)
00035 std::cout << "Using constrained filter\n";
00036 else
00037 std::cout << "Using unconstrained filter\n";
00038
00039 }
00040
00042 virtual ~FilterModel() { }
00043
00045 virtual void F(vnl_matrix<double>& X) = 0;
00046
00048 virtual void H(const vnl_matrix<double>& X,
00049 vnl_matrix<double>& Y) = 0;
00050
00051
00052
00053
00054
00056 virtual void State2Tensor(const State& x, vec_t& m, vec_t& l) {
00057 assert(!"Not implemented");
00058 }
00059
00061 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00062 vec_t& l1, vec_t& m2, vec_t& l2) {
00063 assert(!"Not implemented");
00064 }
00065
00067 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00068 vec_t& l1, vec_t& m2, vec_t& l2, vec_t& m3,
00069 vec_t& l3) {
00070 assert(!"Not implemented");
00071 }
00072
00074 int state_dim() const {
00075 return _state_dim;
00076 }
00077
00079 void set_signal_dim(int dim) {
00080 _signal_dim = dim;
00081
00082 _R.set_size(_signal_dim, _signal_dim);
00083 _R.fill(0);
00084
00085 for (int i = 0; i < _signal_dim; ++i) {
00086 _R(i, i) = _rs;
00087 }
00088
00089 }
00090
00092 int signal_dim() const {
00093 return _signal_dim;
00094 }
00095
00097 const vnl_matrix<double>& Q() const {
00098 return _Q;
00099 }
00100
00102 const vnl_matrix<double>& R() const {
00103 return _R;
00104 }
00105
00106
00107
00109 const vnl_matrix<double>& D() {
00110 return _D;
00111 }
00112
00114 const vnl_vector<double>& d() {
00115 return _d;
00116 }
00117
00119 bool isConstrained() {
00120 return _constrained;
00121 }
00122
00124 void set_signal_data(ISignalData *signal_data) {
00125 _signal_data = signal_data;
00126 }
00127
00128 protected:
00129
00131 double CheckZero(const double & d);
00132
00134 const int _state_dim;
00135
00137 double _rs;
00138
00140 int _signal_dim;
00141
00143 ISignalData *_signal_data;
00144
00146 vnl_matrix<double> _Q;
00147
00149 vnl_matrix<double> _R;
00150
00152 vnl_matrix<double> _D;
00153
00155 vnl_vector<double> _d;
00156
00158 const std::vector<double> weights_on_tensors_ ;
00159
00161 bool _constrained;
00162 };
00163
00171 struct Full1T_FW : public FilterModel {
00172 Full1T_FW(double qs, double ql, double qw, double rs, const std::vector<double>& weights_on_tensors, bool constrained, const double diff_fw)
00173 : FilterModel(7, rs, weights_on_tensors, constrained), _lambda_min(100.0), _d_iso(diff_fw) {
00174 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00175 _Q(3, 3) = _Q(4, 4) = _Q(5, 5) = ql;
00176 _Q(6, 6) = qw;
00177
00178 _D.set_size(7, 5);
00179 _D.fill(0);
00180
00181 _d.set_size(5);
00182
00183
00184 _D(6, 0) = -1;
00185 _d(0) = 1;
00186 _D(6, 1) = 1;
00187 _d(1) = 0;
00188 _D(3, 2) = 1;
00189 _d(2) = 0;
00190 _D(4, 3) = 1;
00191 _d(3) = 0;
00192 _D(5, 4) = 1;
00193 _d(4) = 0;
00194
00195 }
00196
00197 virtual ~Full1T_FW() { }
00198
00199 virtual void F(vnl_matrix<double>& X);
00200 virtual void H(const vnl_matrix<double>& X,
00201 vnl_matrix<double>& Y);
00202
00203 virtual void State2Tensor(const State& x, vec_t& m, vec_t& l);
00204
00206 const double _lambda_min;
00207
00209 const double _d_iso;
00210
00211
00212 };
00213
00221 struct Full1T : public FilterModel {
00222 Full1T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00223 : FilterModel(6, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00224 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00225 _Q(3, 3) = _Q(4, 4) = _Q(5, 5) = ql;
00226 }
00227
00228 virtual ~Full1T() { }
00229
00230 virtual void F(vnl_matrix<double>& X);
00231 virtual void H(const vnl_matrix<double>& X,
00232 vnl_matrix<double>& Y);
00233
00234 virtual void State2Tensor(const State& x, vec_t& m, vec_t& l);
00235
00237 const double _lambda_min;
00238 };
00239
00247 struct Full2T : public FilterModel {
00248 Full2T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00249 : FilterModel(12, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00250 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = _Q(6, 6) = _Q(7, 7) = _Q(8, 8) = qs;
00251 _Q(3, 3) = _Q(4, 4) = _Q(5, 5) = _Q(9, 9) = _Q(10, 10) = _Q(11, 11) = ql;
00252 }
00253
00254 virtual ~Full2T() { }
00255
00256 virtual void F(vnl_matrix<double>& X);
00257 virtual void H(const vnl_matrix<double>& X,
00258 vnl_matrix<double>& Y);
00259
00260 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00261 vec_t& l1, vec_t& m2, vec_t& l2);
00262
00264 const double _lambda_min;
00265 };
00266
00274 struct Full2T_FW : public FilterModel {
00275 Full2T_FW(double qs, double ql, double qw, double rs, const std::vector<double>& weights_on_tensors, bool constrained, const double diff_fw)
00276 : FilterModel(13, rs, weights_on_tensors, constrained), _lambda_min(100.0), _d_iso(diff_fw) {
00277 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = _Q(6, 6) = _Q(7, 7) = _Q(8, 8) = qs;
00278 _Q(3, 3) = _Q(4, 4) = _Q(5, 5) = _Q(9, 9) = _Q(10, 10) = _Q(11, 11) = ql;
00279 _Q(12, 12) = qw;
00280
00281 _D.set_size(13, 8);
00282 _D.fill(0);
00283
00284 _d.set_size(8);
00285
00286
00287 _D(12, 0) = -1;
00288 _d(0) = 1;
00289 _D(12, 1) = 1;
00290 _d(1) = 0;
00291 _D(3, 2) = 1;
00292 _d(2) = 0;
00293 _D(4, 3) = 1;
00294 _d(3) = 0;
00295 _D(5, 4) = 1;
00296 _d(4) = 0;
00297 _D(9, 5) = 1;
00298 _d(5) = 0;
00299 _D(10, 6) = 1;
00300 _d(6) = 0;
00301 _D(11, 7) = 1;
00302 _d(7) = 0;
00303 }
00304
00305 virtual ~Full2T_FW() { }
00306
00307 virtual void F(vnl_matrix<double>& X);
00308 virtual void H(const vnl_matrix<double>& X,
00309 vnl_matrix<double>& Y);
00310
00311 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00312 vec_t& l1, vec_t& m2, vec_t& l2);
00313
00315 const double _lambda_min;
00316
00318 const double _d_iso;
00319
00320
00321 };
00322
00330 struct Full3T : public FilterModel {
00331 Full3T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00332 : FilterModel(18, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00333 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00334 _Q(6, 6) = _Q(7, 7) = _Q(8, 8) = qs;
00335 _Q(12, 12) = _Q(13, 13) = _Q(14, 14) = qs;
00336
00337 _Q(3, 3) = _Q(4, 4) = _Q(5, 5) = ql;
00338 _Q(9, 9) = _Q(10, 10) = _Q(11, 11) = ql;
00339 _Q(15, 15) = _Q(16, 16) = _Q(17, 17) = ql;
00340 }
00341
00342 virtual ~Full3T() { }
00343
00344 virtual void F(vnl_matrix<double>& X);
00345 virtual void H(const vnl_matrix<double>& X,
00346 vnl_matrix<double>& Y);
00347
00348 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00349 vec_t& l1, vec_t& m2, vec_t& l2, vec_t& m3,
00350 vec_t& l3);
00351
00353 const double _lambda_min;
00354
00355
00356 };
00357
00365 struct Simple1T_FW : public FilterModel {
00366 Simple1T_FW(double qs, double ql, double qw, double rs, const std::vector<double>& weights_on_tensors, bool constrained, const double diff_fw)
00367 : FilterModel(6, rs, weights_on_tensors, constrained), _lambda_min(100.0), _d_iso(diff_fw) {
00368
00369 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00370 _Q(3, 3) = _Q(4, 4) = ql;
00371 _Q(5, 5) = qw;
00372
00373 _D.set_size(6, 4);
00374 _D.fill(0);
00375
00376 _d.set_size(4);
00377
00378
00379 _D(5, 0) = -1;
00380 _d(0) = 1;
00381 _D(5, 1) = 1;
00382 _d(1) = 0;
00383 _D(3, 2) = 1;
00384 _d(2) = 0;
00385 _D(4, 3) = 1;
00386 _d(3) = 0;
00387
00388 }
00389
00390 virtual ~Simple1T_FW() { }
00391
00392 virtual void F(vnl_matrix<double>& X);
00393 virtual void H(const vnl_matrix<double>& X,
00394 vnl_matrix<double>& Y);
00395
00396
00397 virtual void State2Tensor(const State& x, vec_t& m, vec_t& l);
00398
00400 const double _lambda_min;
00401
00403 const double _d_iso;
00404
00405 };
00406
00413 struct Simple1T : public FilterModel {
00414 Simple1T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00415 : FilterModel(5, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00416 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00417 _Q(3, 3) = _Q(4, 4) = ql;
00418 }
00419
00420 virtual ~Simple1T() { }
00421
00422 virtual void F(vnl_matrix<double>& X);
00423 virtual void H(const vnl_matrix<double>& X,
00424 vnl_matrix<double>& Y);
00425
00426 virtual void State2Tensor(const State& x, vec_t& m, vec_t& l);
00427
00428 const double _lambda_min;
00429 };
00430
00437 struct Simple2T : public FilterModel {
00438 Simple2T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00439 : FilterModel(10, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00440 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = _Q(5, 5) = _Q(6, 6) = _Q(7, 7) = qs;
00441 _Q(3, 3) = _Q(4, 4) = _Q(8, 8) = _Q(9, 9) = ql;
00442 }
00443
00444 virtual ~Simple2T() { }
00445
00446 virtual void F(vnl_matrix<double>& X);
00447 virtual void H(const vnl_matrix<double>& X,
00448 vnl_matrix<double>& Y);
00449
00450 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00451 vec_t& l1, vec_t& m2, vec_t& l2);
00452
00454 const double _lambda_min;
00455
00456 };
00457
00465 struct Simple2T_FW : public FilterModel {
00466 Simple2T_FW(double qs, double ql, double qw, double rs, const std::vector<double>& weights_on_tensors, bool constrained, const double diff_fw)
00467 : FilterModel(11, rs, weights_on_tensors, constrained), _lambda_min(100.0), _d_iso(diff_fw) {
00468 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = _Q(5, 5) = _Q(6, 6) = _Q(7, 7) = qs;
00469 _Q(3, 3) = _Q(4, 4) = _Q(8, 8) = _Q(9, 9) = ql;
00470 _Q(10, 10) = qw;
00471
00472 _D.set_size(11, 6);
00473 _D.fill(0);
00474
00475 _d.set_size(6);
00476
00477
00478 _D(10, 0) = -1;
00479 _d(0) = 1;
00480 _D(10, 1) = 1;
00481 _d(1) = 0;
00482 _D(3, 2) = 1;
00483 _d(2) = 0;
00484 _D(4, 3) = 1;
00485 _d(3) = 0;
00486 _D(8, 4) = 1;
00487 _d(4) = 0;
00488 _D(9, 5) = 1;
00489 _d(5) = 0;
00490 }
00491
00492 virtual ~Simple2T_FW() { }
00493
00494 virtual void F(vnl_matrix<double>& X);
00495 virtual void H(const vnl_matrix<double>& X,
00496 vnl_matrix<double>& Y);
00497
00498 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00499 vec_t& l1, vec_t& m2, vec_t& l2);
00500
00502 const double _lambda_min;
00503
00505 const double _d_iso;
00506 };
00507
00514 struct Simple3T : public FilterModel {
00515 Simple3T(double qs, double ql, double rs, const std::vector<double>& weights_on_tensors, bool constrained)
00516 : FilterModel(15, rs, weights_on_tensors, constrained), _lambda_min(100.0) {
00517 _Q(0, 0) = _Q(1, 1) = _Q(2, 2) = qs;
00518 _Q(5, 5) = _Q(6, 6) = _Q(7, 7) = qs;
00519 _Q(10, 10) = _Q(11, 11) = _Q(12, 12) = qs;
00520
00521 _Q(3, 3) = _Q(4, 4) = _Q(8, 8) = _Q(9, 9) = _Q(13, 13) = _Q(14, 14) = ql;
00522 }
00523
00524 virtual ~Simple3T() { }
00525
00526 virtual void F(vnl_matrix<double>& X);
00527 virtual void H(const vnl_matrix<double>& X,
00528 vnl_matrix<double>& Y);
00529
00530 virtual void State2Tensor(const State& x, const vec_t& old_m, vec_t& m1,
00531 vec_t& l1, vec_t& m2, vec_t& l2, vec_t& m3,
00532 vec_t& l3);
00533
00535 const double _lambda_min;
00536
00537 };
00538
00539 #endif // FILTER_MODEL_H_