Go to the documentation of this file.00001
00008 #ifndef UNSCENTED_KALMAN_FILTER_H_
00009 #define UNSCENTED_KALMAN_FILTER_H_
00010
00011 #include <vector>
00012 #include <vnl/vnl_matrix.h>
00013 #include <vnl/vnl_vector.h>
00014 #include <vnl/vnl_vector_ref.h>
00015 #include <vnl/algo/vnl_cholesky.h>
00016 #include <vnl/algo/vnl_solve_qp.h>
00017
00018 struct FilterModel;
00019
00021 typedef std::vector<double> State;
00022
00027 class UnscentedKalmanFilter
00028 {
00029 public:
00030
00036 UnscentedKalmanFilter(FilterModel *filter_model);
00037
00047 void Filter(const State& x,
00048 const vnl_matrix<double>& p,
00049 const std::vector<double>& z,
00050 State& x_new,
00051 vnl_matrix<double>& p_new,
00052 double& dNormMSE
00053 );
00054
00055 private:
00057 void SigmaPoints(const State& x, const vnl_matrix<double>& p,
00058 vnl_matrix<double>& x_spread);
00059
00066 void Constrain(vnl_matrix<double>& X, const vnl_matrix<double>& W);
00072 void Constrain(vnl_vector<double>& x, const vnl_matrix<double>& W);
00073
00075 bool violatesContraints(vnl_vector<double>& x);
00076
00078 FilterModel *_filter_model;
00079
00081 int _state_dim;
00082
00084 double _scale;
00085
00087 std::vector<double> _w;
00088
00090 vnl_matrix<double> _w_;
00091
00093 double _k;
00094
00095
00096 vnl_matrix<double> dim_dimext;
00097 vnl_matrix<double> signaldim_dimext;
00098 vnl_matrix<double> dim_dim;
00099 vnl_matrix<double> dim_signaldim;
00100 vnl_matrix<double> signaldim_dim;
00101
00102 vnl_matrix<double> X_;
00103
00105 vnl_matrix<double> X;
00106
00107
00109 vnl_matrix<double> Y;
00110
00112 vnl_matrix<double> Pxy;
00113
00115 vnl_matrix<double> Pyy;
00116
00118 vnl_matrix<double> K;
00119
00121 vnl_vector<double> X_hat;
00122
00124 vnl_vector<double> Y_hat;
00125 };
00126
00127 #endif // UNSCENTED_KALMAN_FILTER_H_